OccupancyGrid.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/core/OccupancyGrid.h>
29 #include <rtabmap/core/util3d.h>
30 #include <rtabmap/utilite/ULogger.h>
31 #include <rtabmap/utilite/UConversion.h>
32 #include <rtabmap/utilite/UStl.h>
33 #include <rtabmap/utilite/UTimer.h>
34 
35 #ifdef RTABMAP_OCTOMAP
36 #include <rtabmap/core/OctoMap.h>
37 #endif
38 
39 #include <pcl/io/pcd_io.h>
40 
41 namespace rtabmap {
42 
43 OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
44  parameters_(parameters),
45  cloudDecimation_(Parameters::defaultGridDepthDecimation()),
46  cloudMaxDepth_(Parameters::defaultGridRangeMax()),
47  cloudMinDepth_(Parameters::defaultGridRangeMin()),
48  //roiRatios_(Parameters::defaultGridDepthRoiRatios()), // initialized in parseParameters()
49  footprintLength_(Parameters::defaultGridFootprintLength()),
50  footprintWidth_(Parameters::defaultGridFootprintWidth()),
51  footprintHeight_(Parameters::defaultGridFootprintHeight()),
52  scanDecimation_(Parameters::defaultGridScanDecimation()),
53  cellSize_(Parameters::defaultGridCellSize()),
54  preVoxelFiltering_(Parameters::defaultGridPreVoxelFiltering()),
55  occupancyFromDepth_(Parameters::defaultGridFromDepth()),
56  projMapFrame_(Parameters::defaultGridMapFrameProjection()),
57  maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
58  normalKSearch_(Parameters::defaultGridNormalK()),
59  maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()*M_PI/180.0f),
60  clusterRadius_(Parameters::defaultGridClusterRadius()),
61  minClusterSize_(Parameters::defaultGridMinClusterSize()),
62  flatObstaclesDetected_(Parameters::defaultGridFlatObstacleDetected()),
63  minGroundHeight_(Parameters::defaultGridMinGroundHeight()),
64  maxGroundHeight_(Parameters::defaultGridMaxGroundHeight()),
65  normalsSegmentation_(Parameters::defaultGridNormalsSegmentation()),
66  grid3D_(Parameters::defaultGrid3D()),
67  groundIsObstacle_(Parameters::defaultGridGroundIsObstacle()),
68  noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
69  noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
70  scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
71  rayTracing_(Parameters::defaultGridRayTracing()),
72  fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
73  minMapSize_(Parameters::defaultGridGlobalMinSize()),
74  erode_(Parameters::defaultGridGlobalEroded()),
75  footprintRadius_(Parameters::defaultGridGlobalFootprintRadius()),
76  updateError_(Parameters::defaultGridGlobalUpdateError()),
77  occupancyThr_(Parameters::defaultGridGlobalOccupancyThr()),
78  probHit_(logodds(Parameters::defaultGridGlobalProbHit())),
79  probMiss_(logodds(Parameters::defaultGridGlobalProbMiss())),
80  probClampingMin_(logodds(Parameters::defaultGridGlobalProbClampingMin())),
81  probClampingMax_(logodds(Parameters::defaultGridGlobalProbClampingMax())),
82  xMin_(0.0f),
83  yMin_(0.0f),
84  cloudAssembling_(false),
85  assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
86  assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
87  assembledEmptyCells_(new pcl::PointCloud<pcl::PointXYZRGB>)
88 {
89  this->parseParameters(parameters);
90 }
91 
92 void OccupancyGrid::parseParameters(const ParametersMap & parameters)
93 {
94  Parameters::parse(parameters, Parameters::kGridFromDepth(), occupancyFromDepth_);
95  Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_);
96  if(cloudDecimation_ == 0)
97  {
98  cloudDecimation_ = 1;
99  }
100  Parameters::parse(parameters, Parameters::kGridRangeMin(), cloudMinDepth_);
101  Parameters::parse(parameters, Parameters::kGridRangeMax(), cloudMaxDepth_);
102  Parameters::parse(parameters, Parameters::kGridFootprintLength(), footprintLength_);
103  Parameters::parse(parameters, Parameters::kGridFootprintWidth(), footprintWidth_);
104  Parameters::parse(parameters, Parameters::kGridFootprintHeight(), footprintHeight_);
105  Parameters::parse(parameters, Parameters::kGridScanDecimation(), scanDecimation_);
106  float cellSize = cellSize_;
107  if(Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize))
108  {
109  this->setCellSize(cellSize);
110  }
111 
112  Parameters::parse(parameters, Parameters::kGridPreVoxelFiltering(), preVoxelFiltering_);
113  Parameters::parse(parameters, Parameters::kGridMapFrameProjection(), projMapFrame_);
114  Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
115  Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
116  Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
117  Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_);
118  if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_))
119  {
120  maxGroundAngle_ *= M_PI/180.0f;
121  }
122  Parameters::parse(parameters, Parameters::kGridClusterRadius(), clusterRadius_);
123  UASSERT_MSG(clusterRadius_ > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridClusterRadius().c_str()).c_str());
124  Parameters::parse(parameters, Parameters::kGridMinClusterSize(), minClusterSize_);
125  Parameters::parse(parameters, Parameters::kGridFlatObstacleDetected(), flatObstaclesDetected_);
126  Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_);
127  Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_);
128  Parameters::parse(parameters, Parameters::kGridGroundIsObstacle(), groundIsObstacle_);
129  Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
130  Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
131  Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
132  Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
133  Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
134  Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
135  Parameters::parse(parameters, Parameters::kGridGlobalEroded(), erode_);
136  Parameters::parse(parameters, Parameters::kGridGlobalFootprintRadius(), footprintRadius_);
137  Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
138 
139  Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr_);
140  if(Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), probHit_))
141  {
142  probHit_ = logodds(probHit_);
143  UASSERT_MSG(probHit_ >= 0.0f, uFormat("probHit_=%f",probHit_).c_str());
144  }
145  if(Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), probMiss_))
146  {
147  probMiss_ = logodds(probMiss_);
148  UASSERT_MSG(probMiss_ <= 0.0f, uFormat("probMiss_=%f",probMiss_).c_str());
149  }
150  if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), probClampingMin_))
151  {
152  probClampingMin_ = logodds(probClampingMin_);
153  }
154  if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), probClampingMax_))
155  {
156  probClampingMax_ = logodds(probClampingMax_);
157  }
158  UASSERT(probClampingMax_ > probClampingMin_);
159 
160  UASSERT(minMapSize_ >= 0.0f);
161 
162  // convert ROI from string to vector
163  ParametersMap::const_iterator iter;
164  if((iter=parameters.find(Parameters::kGridDepthRoiRatios())) != parameters.end())
165  {
166  std::list<std::string> strValues = uSplit(iter->second, ' ');
167  if(strValues.size() != 4)
168  {
169  ULOGGER_ERROR("The number of values must be 4 (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
170  }
171  else
172  {
173  std::vector<float> tmpValues(4);
174  unsigned int i=0;
175  for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
176  {
177  tmpValues[i] = uStr2Float(*jter);
178  ++i;
179  }
180 
181  if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
182  tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
183  tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
184  tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
185  {
186  roiRatios_ = tmpValues;
187  }
188  else
189  {
190  ULOGGER_ERROR("The roi ratios are not valid (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
191  }
192  }
193  }
194 
195  if(maxGroundHeight_ == 0.0f && !normalsSegmentation_)
196  {
197  UWARN("\"%s\" should be not equal to 0 if not using normals "
198  "segmentation approach. Setting it to cell size (%f).",
199  Parameters::kGridMaxGroundHeight().c_str(), cellSize_);
200  maxGroundHeight_ = cellSize_;
201  }
202  if(maxGroundHeight_ != 0.0f &&
203  maxObstacleHeight_ != 0.0f &&
204  maxObstacleHeight_ < maxGroundHeight_)
205  {
206  UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
207  Parameters::kGridMaxGroundHeight().c_str(),
208  Parameters::kGridMaxObstacleHeight().c_str(),
209  Parameters::kGridMaxObstacleHeight().c_str());
210  maxObstacleHeight_ = 0;
211  }
212  if(maxGroundHeight_ != 0.0f &&
213  minGroundHeight_ != 0.0f &&
214  maxGroundHeight_ < minGroundHeight_)
215  {
216  UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
217  Parameters::kGridMinGroundHeight().c_str(),
218  Parameters::kGridMaxGroundHeight().c_str(),
219  Parameters::kGridMinGroundHeight().c_str());
220  minGroundHeight_ = 0;
221  }
222 }
223 
224 void OccupancyGrid::setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, Transform> & poses)
225 {
226  UDEBUG("map=%d/%d xMin=%f yMin=%f cellSize=%f poses=%d",
227  map.cols, map.rows, xMin, yMin, cellSize, (int)poses.size());
228  this->clear();
229  if(!poses.empty() && !map.empty())
230  {
231  UASSERT(cellSize > 0.0f);
232  UASSERT(map.type() == CV_8SC1);
233  map_ = map.clone();
234  mapInfo_ = cv::Mat::zeros(map.size(), CV_32FC4);
235  for(int i=0; i<map_.rows; ++i)
236  {
237  for(int j=0; j<map_.cols; ++j)
238  {
239  const char value = map_.at<char>(i,j);
240  float * info = mapInfo_.ptr<float>(i,j);
241  if(value == 0)
242  {
243  info[3] = probClampingMin_;
244  }
245  else if(value == 100)
246  {
247  info[3] = probClampingMax_;
248  }
249  }
250  }
251  xMin_ = xMin;
252  yMin_ = yMin;
253  cellSize_ = cellSize;
254  addedNodes_.insert(poses.lower_bound(1), poses.end());
255  }
256 }
257 
258 void OccupancyGrid::setCellSize(float cellSize)
259 {
260  UASSERT_MSG(cellSize > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridCellSize().c_str()).c_str());
261  if(cellSize_ != cellSize)
262  {
263  if(!map_.empty())
264  {
265  UWARN("Grid cell size has changed, the map is cleared!");
266  }
267  this->clear();
268  cellSize_ = cellSize;
269  }
270 }
271 
272 void OccupancyGrid::setCloudAssembling(bool enabled)
273 {
274  cloudAssembling_ = enabled;
275  if(!cloudAssembling_)
276  {
277  assembledGround_->clear();
278  assembledObstacles_->clear();
279  }
280 }
281 
282 void OccupancyGrid::createLocalMap(
283  const Signature & node,
284  cv::Mat & groundCells,
285  cv::Mat & obstacleCells,
286  cv::Mat & emptyCells,
287  cv::Point3f & viewPoint) const
288 {
289  UDEBUG("scan format=%d, occupancyFromDepth_=%d normalsSegmentation_=%d grid3D_=%d",
290  node.sensorData().laserScanRaw().isEmpty()?0:node.sensorData().laserScanRaw().format(), occupancyFromDepth_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);
291 
292  if((node.sensorData().laserScanRaw().is2d()) && !occupancyFromDepth_)
293  {
294  UDEBUG("2D laser scan");
295  //2D
296  viewPoint = cv::Point3f(
297  node.sensorData().laserScanRaw().localTransform().x(),
298  node.sensorData().laserScanRaw().localTransform().y(),
299  node.sensorData().laserScanRaw().localTransform().z());
300 
301  LaserScan scan = node.sensorData().laserScanRaw();
302  if(cloudMinDepth_ > 0.0f)
303  {
304  scan = util3d::rangeFiltering(scan, cloudMinDepth_, 0.0f);
305  }
306 
307  float maxRange = cloudMaxDepth_;
308  if(cloudMaxDepth_>0.0f && node.sensorData().laserScanRaw().rangeMax()>0.0f)
309  {
310  maxRange = cloudMaxDepth_ < node.sensorData().laserScanRaw().rangeMax()?cloudMaxDepth_:node.sensorData().laserScanRaw().rangeMax();
311  }
312  else if(scan2dUnknownSpaceFilled_ && node.sensorData().laserScanRaw().rangeMax()>0.0f)
313  {
314  maxRange = node.sensorData().laserScanRaw().rangeMax();
315  }
316  util3d::occupancy2DFromLaserScan(
317  util3d::transformLaserScan(scan, node.sensorData().laserScanRaw().localTransform()).data(),
318  cv::Mat(),
319  viewPoint,
320  emptyCells,
321  obstacleCells,
322  cellSize_,
323  scan2dUnknownSpaceFilled_,
324  maxRange);
325 
326  UDEBUG("ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels());
327  }
328  else
329  {
330  // 3D
331  if(!occupancyFromDepth_)
332  {
333  if(!node.sensorData().laserScanRaw().isEmpty())
334  {
335  UDEBUG("3D laser scan");
336  const Transform & t = node.sensorData().laserScanRaw().localTransform();
337  LaserScan scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
338 #ifdef RTABMAP_OCTOMAP
339  // clipping will be done in OctoMap
340  float maxRange = grid3D_&&rayTracing_?0.0f:cloudMaxDepth_;
341 #else
342  float maxRange = cloudMaxDepth_;
343 #endif
344  if(cloudMinDepth_ > 0.0f || maxRange > 0.0f)
345  {
346  scan = util3d::rangeFiltering(scan, cloudMinDepth_, maxRange);
347  }
348 
349  // update viewpoint
350  viewPoint = cv::Point3f(t.x(), t.y(), t.z());
351 
352  UDEBUG("scan format=%d", scan.format());
353  createLocalMap(scan, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
354  }
355  else
356  {
357  UWARN("Cannot create local map, scan is empty (node=%d).", node.id());
358  }
359  }
360  else
361  {
362  pcl::IndicesPtr indices(new std::vector<int>);
363  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
364  UDEBUG("Depth image : decimation=%d max=%f min=%f",
365  cloudDecimation_,
366  cloudMaxDepth_,
367  cloudMinDepth_);
368  cloud = util3d::cloudRGBFromSensorData(
369  node.sensorData(),
370  cloudDecimation_,
371 #ifdef RTABMAP_OCTOMAP
372  // clipping will be done in OctoMap
373  grid3D_&&rayTracing_?0.0f:cloudMaxDepth_,
374 #else
375  cloudMaxDepth_,
376 #endif
377  cloudMinDepth_,
378  indices.get(),
379  parameters_,
380  roiRatios_);
381 
382  // update viewpoint
383  if(node.sensorData().cameraModels().size())
384  {
385  // average of all local transforms
386  float sum = 0;
387  for(unsigned int i=0; i<node.sensorData().cameraModels().size(); ++i)
388  {
389  const Transform & t = node.sensorData().cameraModels()[i].localTransform();
390  if(!t.isNull())
391  {
392  viewPoint.x += t.x();
393  viewPoint.y += t.y();
394  viewPoint.z += t.z();
395  sum += 1.0f;
396  }
397  }
398  if(sum > 0.0f)
399  {
400  viewPoint.x /= sum;
401  viewPoint.y /= sum;
402  viewPoint.z /= sum;
403  }
404  }
405  else
406  {
407  const Transform & t = node.sensorData().stereoCameraModel().localTransform();
408  viewPoint = cv::Point3f(t.x(), t.y(), t.z());
409  }
410  createLocalMap(LaserScan(util3d::laserScanFromPointCloud(*cloud, indices), 0, 0.0f, LaserScan::kXYZRGB), node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
411  }
412  }
413 }
414 
415 void OccupancyGrid::createLocalMap(
416  const LaserScan & scan,
417  const Transform & pose,
418  cv::Mat & groundCells,
419  cv::Mat & obstacleCells,
420  cv::Mat & emptyCells,
421  cv::Point3f & viewPointInOut) const
422 {
423  if(projMapFrame_)
424  {
425  //we should rotate viewPoint in /map frame
426  float roll, pitch, yaw;
427  pose.getEulerAngles(roll, pitch, yaw);
428  Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z, 0,0,0);
429  viewPointInOut.x = viewpointRotated.x();
430  viewPointInOut.y = viewpointRotated.y();
431  viewPointInOut.z = viewpointRotated.z();
432  }
433 
434  if(scan.size())
435  {
436  pcl::IndicesPtr groundIndices(new std::vector<int>);
437  pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
438  cv::Mat groundCloud;
439  cv::Mat obstaclesCloud;
440 
441  if(scan.hasRGB() && scan.hasNormals())
442  {
443  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = util3d::laserScanToPointCloudRGBNormal(scan, scan.localTransform());
444  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZRGBNormal>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
445  UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
446  if(grid3D_)
447  {
448  groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
449  obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
450  }
451  else
452  {
453  util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGBNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
454  }
455  }
456  else if(scan.hasRGB())
457  {
458  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
459  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZRGB>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
460  UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
461  if(grid3D_)
462  {
463  groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
464  obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
465  }
466  else
467  {
468  util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
469  }
470  }
471  else if(scan.hasNormals())
472  {
473  pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(scan, scan.localTransform());
474  pcl::PointCloud<pcl::PointNormal>::Ptr cloudSegmented = segmentCloud<pcl::PointNormal>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
475  UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
476  if(grid3D_)
477  {
478  groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
479  obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
480  }
481  else
482  {
483  util3d::occupancy2DFromGroundObstacles<pcl::PointNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
484  }
485  }
486  else
487  {
488  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
489  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZ>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
490  UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
491  if(grid3D_)
492  {
493  groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
494  obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
495  }
496  else
497  {
498  util3d::occupancy2DFromGroundObstacles<pcl::PointXYZ>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
499  }
500  }
501 
502  if(grid3D_ && (!obstaclesCloud.empty() || !groundCloud.empty()))
503  {
504  UDEBUG("ground=%d obstacles=%d", groundCloud.cols, obstaclesCloud.cols);
505  if(groundIsObstacle_ && !groundCloud.empty())
506  {
507  if(obstaclesCloud.empty())
508  {
509  obstaclesCloud = groundCloud;
510  groundCloud = cv::Mat();
511  }
512  else
513  {
514  UASSERT(obstaclesCloud.type() == groundCloud.type());
515  cv::Mat merged(1,obstaclesCloud.cols+groundCloud.cols, obstaclesCloud.type());
516  obstaclesCloud.copyTo(merged(cv::Range::all(), cv::Range(0, obstaclesCloud.cols)));
517  groundCloud.copyTo(merged(cv::Range::all(), cv::Range(obstaclesCloud.cols, obstaclesCloud.cols+groundCloud.cols)));
518  }
519  }
520 
521  // transform back in base frame
522  float roll, pitch, yaw;
523  pose.getEulerAngles(roll, pitch, yaw);
524  Transform tinv = Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0).inverse();
525 
526  if(rayTracing_)
527  {
528 #ifdef RTABMAP_OCTOMAP
529  if(!groundCloud.empty() || !obstaclesCloud.empty())
530  {
531  //create local octomap
532  OctoMap octomap(cellSize_);
533  octomap.setMaxRange(cloudMaxDepth_);
534  octomap.addToCache(1, groundCloud, obstaclesCloud, cv::Mat(), cv::Point3f(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z));
535  std::map<int, Transform> poses;
536  poses.insert(std::make_pair(1, Transform::getIdentity()));
537  octomap.update(poses);
538 
539  pcl::IndicesPtr groundIndices(new std::vector<int>);
540  pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
541  pcl::IndicesPtr emptyIndices(new std::vector<int>);
542  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithRayTracing = octomap.createCloud(0, obstaclesIndices.get(), emptyIndices.get(), groundIndices.get());
543  UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size());
544  if(scan.hasRGB())
545  {
546  groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv);
547  obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv);
548  emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv);
549  }
550  else
551  {
552  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithRayTracing2(new pcl::PointCloud<pcl::PointXYZ>);
553  pcl::copyPointCloud(*cloudWithRayTracing, *cloudWithRayTracing2);
554  groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, groundIndices, tinv);
555  obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, obstaclesIndices, tinv);
556  emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, emptyIndices, tinv);
557  }
558  }
559  }
560  else
561 #else
562  UWARN("RTAB-Map is not built with OctoMap dependency, 3D ray tracing is ignored. Set \"%s\" to false to avoid this warning.", Parameters::kGridRayTracing().c_str());
563  }
564 #endif
565  {
566  groundCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(groundCloud), tinv).data();
567  obstacleCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(obstaclesCloud), tinv).data();
568  }
569 
570  }
571  else if(!grid3D_ && rayTracing_ && (!obstacleCells.empty() || !groundCells.empty()))
572  {
573  cv::Mat laserScan = obstacleCells;
574  cv::Mat laserScanNoHit = groundCells;
575  obstacleCells = cv::Mat();
576  groundCells = cv::Mat();
577  util3d::occupancy2DFromLaserScan(
578  laserScan,
579  laserScanNoHit,
580  viewPointInOut,
581  emptyCells,
582  obstacleCells,
583  cellSize_,
584  false, // don't fill unknown space
585  cloudMaxDepth_);
586  }
587  }
588  UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
589 }
590 
591 void OccupancyGrid::clear()
592 {
593  cache_.clear();
594  map_ = cv::Mat();
595  mapInfo_ = cv::Mat();
596  cellCount_.clear();
597  xMin_ = 0.0f;
598  yMin_ = 0.0f;
599  addedNodes_.clear();
600  assembledGround_->clear();
601  assembledObstacles_->clear();
602 }
603 
604 cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
605 {
606  xMin = xMin_;
607  yMin = yMin_;
608 
609  cv::Mat map = map_;
610 
611  UTimer t;
612  if(occupancyThr_ != 0.0f && !map.empty())
613  {
614  float occThr = logodds(occupancyThr_);
615  map = cv::Mat(map.size(), map.type());
616  UASSERT(mapInfo_.cols == map.cols && mapInfo_.rows == map.rows);
617  for(int i=0; i<map.rows; ++i)
618  {
619  for(int j=0; j<map.cols; ++j)
620  {
621  const float * info = mapInfo_.ptr<float>(i, j);
622  if(info[3] == 0.0f)
623  {
624  map.at<char>(i, j) = -1; // unknown
625  }
626  else if(info[3] >= occThr)
627  {
628  map.at<char>(i, j) = 100; // unknown
629  }
630  else
631  {
632  map.at<char>(i, j) = 0; // empty
633  }
634  }
635  }
636  UDEBUG("Converting map from probabilities (thr=%f) = %fs", occupancyThr_, t.ticks());
637  }
638 
639  if(erode_ && !map.empty())
640  {
641  map = util3d::erodeMap(map);
642  UDEBUG("Eroding map = %fs", t.ticks());
643  }
644  return map;
645 }
646 
647 cv::Mat OccupancyGrid::getProbMap(float & xMin, float & yMin) const
648 {
649  xMin = xMin_;
650  yMin = yMin_;
651 
652  cv::Mat map;
653  if(!mapInfo_.empty())
654  {
655  map = cv::Mat(mapInfo_.size(), map_.type());
656  for(int i=0; i<map.rows; ++i)
657  {
658  for(int j=0; j<map.cols; ++j)
659  {
660  const float * info = mapInfo_.ptr<float>(i, j);
661  if(info[3] == 0.0f)
662  {
663  map.at<char>(i, j) = -1; // unknown
664  }
665  else
666  {
667  map.at<char>(i, j) = char(probability(info[3])*100.0f); // empty
668  }
669  }
670  }
671  }
672  return map;
673 }
674 
675 void OccupancyGrid::addToCache(
676  int nodeId,
677  const cv::Mat & ground,
678  const cv::Mat & obstacles,
679  const cv::Mat & empty)
680 {
681  UDEBUG("nodeId=%d", nodeId);
682  if(nodeId < 0)
683  {
684  UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
685  return;
686  }
687  uInsert(cache_, std::make_pair(nodeId==0?-1:nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
688 }
689 
690 bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
691 {
692  UTimer timer;
693  UDEBUG("Update (poses=%d addedNodes_=%d)", (int)posesIn.size(), (int)addedNodes_.size());
694 
695  float margin = cellSize_*10.0f+(footprintRadius_>cellSize_*1.5f?float(int(footprintRadius_/cellSize_)+1):0.0f)*cellSize_;
696 
697  float minX=-minMapSize_/2.0f;
698  float minY=-minMapSize_/2.0f;
699  float maxX=minMapSize_/2.0f;
700  float maxY=minMapSize_/2.0f;
701  bool undefinedSize = minMapSize_ == 0.0f;
702  std::map<int, cv::Mat> emptyLocalMaps;
703  std::map<int, cv::Mat> occupiedLocalMaps;
704 
705  // First, check of the graph has changed. If so, re-create the map by moving all occupied nodes (fullUpdate==false).
706  bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
707  bool graphChanged = !addedNodes_.empty(); // If the new map doesn't have any node from the previous map
708  std::map<int, Transform> transforms;
709  float updateErrorSqrd = updateError_*updateError_;
710  for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
711  {
712  std::map<int, Transform>::const_iterator jter = posesIn.find(iter->first);
713  if(jter != posesIn.end())
714  {
715  graphChanged = false;
716 
717  UASSERT(!iter->second.isNull() && !jter->second.isNull());
718  Transform t = Transform::getIdentity();
719  if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
720  {
721  t = jter->second * iter->second.inverse();
722  graphOptimized = true;
723  }
724  transforms.insert(std::make_pair(jter->first, t));
725 
726  float x = jter->second.x();
727  float y =jter->second.y();
728  if(undefinedSize)
729  {
730  minX = maxX = x;
731  minY = maxY = y;
732  undefinedSize = false;
733  }
734  else
735  {
736  if(minX > x)
737  minX = x;
738  else if(maxX < x)
739  maxX = x;
740 
741  if(minY > y)
742  minY = y;
743  else if(maxY < y)
744  maxY = y;
745  }
746  }
747  else
748  {
749  UDEBUG("Updated pose for node %d is not found, some points may not be copied if graph has changed.", iter->first);
750  }
751  }
752 
753  bool assembledGroundUpdated = false;
754  bool assembledObstaclesUpdated = false;
755  bool assembledEmptyCellsUpdated = false;
756 
757  if(graphOptimized || graphChanged)
758  {
759  if(graphChanged)
760  {
761  UWARN("Graph has changed! The whole map should be rebuilt.");
762  }
763  else
764  {
765  UINFO("Graph optimized!");
766  }
767 
768  if(cloudAssembling_)
769  {
770  assembledGround_->clear();
771  assembledObstacles_->clear();
772  }
773 
774  if(!fullUpdate_ && !graphChanged && !map_.empty()) // incremental, just move cells
775  {
776  // 1) recreate all local maps
777  UASSERT(map_.cols == mapInfo_.cols &&
778  map_.rows == mapInfo_.rows);
779  std::map<int, std::pair<int, int> > tmpIndices;
780  for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
781  {
782  if(!uContains(cache_, iter->first) && transforms.find(iter->first) != transforms.end())
783  {
784  if(iter->second.first)
785  {
786  emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
787  }
788  if(iter->second.second)
789  {
790  occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
791  }
792  tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
793  }
794  }
795  for(int y=0; y<map_.rows; ++y)
796  {
797  for(int x=0; x<map_.cols; ++x)
798  {
799  float * info = mapInfo_.ptr<float>(y,x);
800  int nodeId = (int)info[0];
801  if(nodeId > 0 && map_.at<char>(y,x) >= 0)
802  {
803  if(tmpIndices.find(nodeId)!=tmpIndices.end())
804  {
805  std::map<int, Transform>::iterator tter = transforms.find(nodeId);
806  UASSERT(tter != transforms.end());
807 
808  cv::Point3f pt(info[1], info[2], 0.0f);
809  pt = util3d::transformPoint(pt, tter->second);
810 
811  if(minX > pt.x)
812  minX = pt.x;
813  else if(maxX < pt.x)
814  maxX = pt.x;
815 
816  if(minY > pt.y)
817  minY = pt.y;
818  else if(maxY < pt.y)
819  maxY = pt.y;
820 
821  std::map<int, std::pair<int, int> >::iterator jter = tmpIndices.find(nodeId);
822  if(map_.at<char>(y, x) == 0)
823  {
824  // ground
825  std::map<int, cv::Mat>::iterator iter = emptyLocalMaps.find(nodeId);
826  UASSERT(iter != emptyLocalMaps.end());
827  UASSERT(jter->second.first < iter->second.cols);
828  float * ptf = iter->second.ptr<float>(0,jter->second.first++);
829  ptf[0] = pt.x;
830  ptf[1] = pt.y;
831  }
832  else
833  {
834  // obstacle
835  std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(nodeId);
836  UASSERT(iter != occupiedLocalMaps.end());
837  UASSERT(iter!=occupiedLocalMaps.end());
838  UASSERT(jter->second.second < iter->second.cols);
839  float * ptf = iter->second.ptr<float>(0,jter->second.second++);
840  ptf[0] = pt.x;
841  ptf[1] = pt.y;
842  }
843  }
844  }
845  else if(nodeId > 0)
846  {
847  UERROR("Cell referred b node %d is unknown!?", nodeId);
848  }
849  }
850  }
851 
852  //verify if all cells were added
853  for(std::map<int, std::pair<int, int> >::iterator iter=tmpIndices.begin(); iter!=tmpIndices.end(); ++iter)
854  {
855  std::map<int, cv::Mat>::iterator jter = emptyLocalMaps.find(iter->first);
856  UASSERT_MSG((iter->second.first == 0 && (jter==emptyLocalMaps.end() || jter->second.empty())) ||
857  (iter->second.first != 0 && jter!=emptyLocalMaps.end() && jter->second.cols == iter->second.first),
858  uFormat("iter->second.first=%d jter->second.cols=%d", iter->second.first, jter!=emptyLocalMaps.end()?jter->second.cols:-1).c_str());
859  jter = occupiedLocalMaps.find(iter->first);
860  UASSERT_MSG((iter->second.second == 0 && (jter==occupiedLocalMaps.end() || jter->second.empty())) ||
861  (iter->second.second != 0 && jter!=occupiedLocalMaps.end() && jter->second.cols == iter->second.second),
862  uFormat("iter->second.first=%d jter->second.cols=%d", iter->second.first, jter!=emptyLocalMaps.end()?jter->second.cols:-1).c_str());
863  }
864 
865  UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
866  }
867 
868  addedNodes_.clear();
869  map_ = cv::Mat();
870  mapInfo_ = cv::Mat();
871  cellCount_.clear();
872  xMin_ = 0.0f;
873  yMin_ = 0.0f;
874  }
875  else if(!map_.empty())
876  {
877  // update
878  minX=xMin_+margin+cellSize_/2.0f;
879  minY=yMin_+margin+cellSize_/2.0f;
880  maxX=xMin_+float(map_.cols)*cellSize_ - margin;
881  maxY=yMin_+float(map_.rows)*cellSize_ - margin;
882  undefinedSize = false;
883  }
884 
885  bool incrementalGraphUpdate = graphOptimized && !fullUpdate_ && !graphChanged;
886 
887  std::list<std::pair<int, Transform> > poses;
888  int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
889  UDEBUG("Last id = %d", lastId);
890 
891  // add old poses that were not in the current map (they were just retrieved from LTM)
892  for(std::map<int, Transform>::const_iterator iter=posesIn.lower_bound(1); iter!=posesIn.end(); ++iter)
893  {
894  if(addedNodes_.find(iter->first) == addedNodes_.end())
895  {
896  UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first);
897  poses.push_back(*iter);
898  }
899  }
900 
901  // insert zero after
902  if(posesIn.find(0) != posesIn.end())
903  {
904  poses.push_back(std::make_pair(-1, posesIn.at(0)));
905  }
906 
907  if(!poses.empty())
908  {
909  for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
910  {
911  UASSERT(!iter->second.isNull());
912 
913  float x = iter->second.x();
914  float y =iter->second.y();
915  if(undefinedSize)
916  {
917  minX = maxX = x;
918  minY = maxY = y;
919  undefinedSize = false;
920  }
921  else
922  {
923  if(minX > x)
924  minX = x;
925  else if(maxX < x)
926  maxX = x;
927 
928  if(minY > y)
929  minY = y;
930  else if(maxY < y)
931  maxY = y;
932  }
933  }
934 
935  if(!cache_.empty())
936  {
937  for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
938  {
939  if(uContains(cache_, iter->first))
940  {
941  const std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> & pair = cache_.at(iter->first);
942 
943  UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, pair.first.first.cols, pair.first.second.cols, pair.second.cols);
944 
945  //ground
946  if(pair.first.first.cols)
947  {
948  if(pair.first.first.rows > 1 && pair.first.first.cols == 1)
949  {
950  UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.first.rows, pair.first.first.cols);
951  }
952  cv::Mat ground(1, pair.first.first.cols, CV_32FC2);
953  for(int i=0; i<ground.cols; ++i)
954  {
955  const float * vi = pair.first.first.ptr<float>(0,i);
956  float * vo = ground.ptr<float>(0,i);
957  cv::Point3f vt;
958  if(pair.first.first.channels() != 2 && pair.first.first.channels() != 5)
959  {
960  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
961  }
962  else
963  {
964  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
965  }
966  vo[0] = vt.x;
967  vo[1] = vt.y;
968  if(minX > vo[0])
969  minX = vo[0];
970  else if(maxX < vo[0])
971  maxX = vo[0];
972 
973  if(minY > vo[1])
974  minY = vo[1];
975  else if(maxY < vo[1])
976  maxY = vo[1];
977  }
978  uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
979 
980  if(cloudAssembling_)
981  {
982  *assembledGround_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(pair.first.first), iter->second, 0, 255, 0);
983  assembledGroundUpdated = true;
984  }
985  }
986 
987  //empty
988  if(pair.second.cols)
989  {
990  if(pair.second.rows > 1 && pair.second.cols == 1)
991  {
992  UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
993  }
994  cv::Mat ground(1, pair.second.cols, CV_32FC2);
995  for(int i=0; i<ground.cols; ++i)
996  {
997  const float * vi = pair.second.ptr<float>(0,i);
998  float * vo = ground.ptr<float>(0,i);
999  cv::Point3f vt;
1000  if(pair.second.channels() != 2 && pair.second.channels() != 5)
1001  {
1002  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
1003  }
1004  else
1005  {
1006  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
1007  }
1008  vo[0] = vt.x;
1009  vo[1] = vt.y;
1010  if(minX > vo[0])
1011  minX = vo[0];
1012  else if(maxX < vo[0])
1013  maxX = vo[0];
1014 
1015  if(minY > vo[1])
1016  minY = vo[1];
1017  else if(maxY < vo[1])
1018  maxY = vo[1];
1019  }
1020  uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
1021 
1022  if(cloudAssembling_)
1023  {
1024  *assembledEmptyCells_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(pair.second), iter->second, 0, 255, 0);
1025  assembledEmptyCellsUpdated = true;
1026  }
1027  }
1028 
1029  //obstacles
1030  if(pair.first.second.cols)
1031  {
1032  if(pair.first.second.rows > 1 && pair.first.second.cols == 1)
1033  {
1034  UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.second.rows, pair.first.second.cols);
1035  }
1036  cv::Mat obstacles(1, pair.first.second.cols, CV_32FC2);
1037  for(int i=0; i<obstacles.cols; ++i)
1038  {
1039  const float * vi = pair.first.second.ptr<float>(0,i);
1040  float * vo = obstacles.ptr<float>(0,i);
1041  cv::Point3f vt;
1042  if(pair.first.second.channels() != 2 && pair.first.second.channels() != 5)
1043  {
1044  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
1045  }
1046  else
1047  {
1048  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
1049  }
1050  vo[0] = vt.x;
1051  vo[1] = vt.y;
1052  if(minX > vo[0])
1053  minX = vo[0];
1054  else if(maxX < vo[0])
1055  maxX = vo[0];
1056 
1057  if(minY > vo[1])
1058  minY = vo[1];
1059  else if(maxY < vo[1])
1060  maxY = vo[1];
1061  }
1062  uInsert(occupiedLocalMaps, std::make_pair(iter->first, obstacles));
1063 
1064  if(cloudAssembling_)
1065  {
1066  *assembledObstacles_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(pair.first.second), iter->second, 255, 0, 0);
1067  assembledObstaclesUpdated = true;
1068  }
1069  }
1070  }
1071  }
1072  }
1073 
1074  cv::Mat map;
1075  cv::Mat mapInfo;
1076  if(minX != maxX && minY != maxY)
1077  {
1078  //Get map size
1079  float xMin = minX-margin;
1080  xMin -= cellSize_/2.0f;
1081  float yMin = minY-margin;
1082  yMin -= cellSize_/2.0f;
1083  float xMax = maxX+margin;
1084  float yMax = maxY+margin;
1085 
1086  if(fabs((yMax - yMin) / cellSize_) > 99999 ||
1087  fabs((xMax - xMin) / cellSize_) > 99999)
1088  {
1089  UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
1090  "There's maybe an error with the poses provided! The map will not be created!",
1091  xMin, yMin, xMax, yMax);
1092  }
1093  else
1094  {
1095  UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, xMin_, yMin_, xMax, yMax);
1096  cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f);
1097  if(map_.empty())
1098  {
1099  UDEBUG("Map empty!");
1100  map = cv::Mat::ones(newMapSize, CV_8S)*-1;
1101  mapInfo = cv::Mat::zeros(newMapSize, CV_32FC4);
1102  }
1103  else
1104  {
1105  if(xMin == xMin_ && yMin == yMin_ &&
1106  newMapSize.width == map_.cols &&
1107  newMapSize.height == map_.rows)
1108  {
1109  // same map size and origin, don't do anything
1110  UDEBUG("Map same size!");
1111  map = map_;
1112  mapInfo = mapInfo_;
1113  }
1114  else
1115  {
1116  UASSERT_MSG(xMin <= xMin_+cellSize_/2, uFormat("xMin=%f, xMin_=%f, cellSize_=%f", xMin, xMin_, cellSize_).c_str());
1117  UASSERT_MSG(yMin <= yMin_+cellSize_/2, uFormat("yMin=%f, yMin_=%f, cellSize_=%f", yMin, yMin_, cellSize_).c_str());
1118  UASSERT_MSG(xMax >= xMin_+float(map_.cols)*cellSize_ - cellSize_/2, uFormat("xMin=%f, xMin_=%f, cols=%d cellSize_=%f", xMin, xMin_, map_.cols, cellSize_).c_str());
1119  UASSERT_MSG(yMax >= yMin_+float(map_.rows)*cellSize_ - cellSize_/2, uFormat("yMin=%f, yMin_=%f, cols=%d cellSize_=%f", yMin, yMin_, map_.rows, cellSize_).c_str());
1120 
1121  UDEBUG("Copy map");
1122  // copy the old map in the new map
1123  // make sure the translation is cellSize
1124  int deltaX = 0;
1125  if(xMin < xMin_)
1126  {
1127  deltaX = (xMin_ - xMin) / cellSize_ + 1.0f;
1128  xMin = xMin_-float(deltaX)*cellSize_;
1129  }
1130  int deltaY = 0;
1131  if(yMin < yMin_)
1132  {
1133  deltaY = (yMin_ - yMin) / cellSize_ + 1.0f;
1134  yMin = yMin_-float(deltaY)*cellSize_;
1135  }
1136  UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
1137  newMapSize.width = (xMax - xMin) / cellSize_+0.5f;
1138  newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
1139  UDEBUG("%d/%d -> %d/%d", map_.cols, map_.rows, newMapSize.width, newMapSize.height);
1140  UASSERT(newMapSize.width >= map_.cols && newMapSize.height >= map_.rows);
1141  UASSERT(newMapSize.width >= map_.cols+deltaX && newMapSize.height >= map_.rows+deltaY);
1142  UASSERT(deltaX>=0 && deltaY>=0);
1143  map = cv::Mat::ones(newMapSize, CV_8S)*-1;
1144  mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type());
1145  map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
1146  mapInfo_.copyTo(mapInfo(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
1147  }
1148  }
1149  UASSERT(map.cols == mapInfo.cols && map.rows == mapInfo.rows);
1150  UDEBUG("map %d %d", map.cols, map.rows);
1151  if(poses.size())
1152  {
1153  UDEBUG("first pose= %d last pose=%d", poses.begin()->first, poses.rbegin()->first);
1154  }
1155  for(std::list<std::pair<int, Transform> >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
1156  {
1157  if(kter->first > 0)
1158  {
1159  uInsert(addedNodes_, *kter);
1160  }
1161  std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
1162  std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
1163  std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
1164  if(cter == cellCount_.end() && kter->first > 0)
1165  {
1166  cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
1167  }
1168  if(iter!=emptyLocalMaps.end())
1169  {
1170  for(int i=0; i<iter->second.cols; ++i)
1171  {
1172  float * ptf = iter->second.ptr<float>(0,i);
1173  cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
1174  UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols,
1175  uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
1176  kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
1177  char & value = map.at<char>(pt.y, pt.x);
1178  if(value != -2 && (!incrementalGraphUpdate || value==-1))
1179  {
1180  float * info = mapInfo.ptr<float>(pt.y, pt.x);
1181  int nodeId = (int)info[0];
1182  if(value != -1)
1183  {
1184  if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
1185  {
1186  // cannot rewrite on cells referred by more recent nodes
1187  continue;
1188  }
1189  if(nodeId > 0)
1190  {
1191  std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
1192  UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
1193  if(value == 0)
1194  {
1195  eter->second.first -= 1;
1196  }
1197  else if(value == 100)
1198  {
1199  eter->second.second -= 1;
1200  }
1201  if(kter->first < 0)
1202  {
1203  eter->second.first += 1;
1204  }
1205  }
1206  }
1207  if(kter->first > 0)
1208  {
1209  info[0] = (float)kter->first;
1210  info[1] = ptf[0];
1211  info[2] = ptf[1];
1212  cter->second.first+=1;
1213  }
1214  value = 0; // free space
1215 
1216  // update odds
1217  if(nodeId != kter->first)
1218  {
1219  info[3] += probMiss_;
1220  if (info[3] < probClampingMin_)
1221  {
1222  info[3] = probClampingMin_;
1223  }
1224  if (info[3] > probClampingMax_)
1225  {
1226  info[3] = probClampingMax_;
1227  }
1228  }
1229  }
1230  }
1231  }
1232 
1233  if(footprintRadius_ >= cellSize_*1.5f)
1234  {
1235  // place free space under the footprint of the robot
1236  cv::Point2i ptBegin((kter->second.x()-footprintRadius_-xMin)/cellSize_, (kter->second.y()-footprintRadius_-yMin)/cellSize_);
1237  cv::Point2i ptEnd((kter->second.x()+footprintRadius_-xMin)/cellSize_, (kter->second.y()+footprintRadius_-yMin)/cellSize_);
1238  if(ptBegin.x < 0)
1239  ptBegin.x = 0;
1240  if(ptEnd.x >= map.cols)
1241  ptEnd.x = map.cols-1;
1242 
1243  if(ptBegin.y < 0)
1244  ptBegin.y = 0;
1245  if(ptEnd.y >= map.rows)
1246  ptEnd.y = map.rows-1;
1247  for(int i=ptBegin.x; i<ptEnd.x; ++i)
1248  {
1249  for(int j=ptBegin.y; j<ptEnd.y; ++j)
1250  {
1251  UASSERT(j < map.rows && i < map.cols);
1252  char & value = map.at<char>(j, i);
1253  float * info = mapInfo.ptr<float>(j, i);
1254  int nodeId = (int)info[0];
1255  if(value != -1)
1256  {
1257  if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
1258  {
1259  // cannot rewrite on cells referred by more recent nodes
1260  continue;
1261  }
1262  if(nodeId>0)
1263  {
1264  std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
1265  UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
1266  if(value == 0)
1267  {
1268  eter->second.first -= 1;
1269  }
1270  else if(value == 100)
1271  {
1272  eter->second.second -= 1;
1273  }
1274  if(kter->first < 0)
1275  {
1276  eter->second.first += 1;
1277  }
1278  }
1279  }
1280  if(kter->first > 0)
1281  {
1282  info[0] = (float)kter->first;
1283  info[1] = float(i) * cellSize_ + xMin;
1284  info[2] = float(j) * cellSize_ + yMin;
1285  cter->second.first+=1;
1286  }
1287  value = -2; // free space (footprint)
1288  }
1289  }
1290  }
1291 
1292  if(jter!=occupiedLocalMaps.end())
1293  {
1294  for(int i=0; i<jter->second.cols; ++i)
1295  {
1296  float * ptf = jter->second.ptr<float>(0,i);
1297  cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
1298  UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols,
1299  uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
1300  kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
1301  char & value = map.at<char>(pt.y, pt.x);
1302  if(value != -2)
1303  {
1304  float * info = mapInfo.ptr<float>(pt.y, pt.x);
1305  int nodeId = (int)info[0];
1306  if(value != -1)
1307  {
1308  if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
1309  {
1310  // cannot rewrite on cells referred by more recent nodes
1311  continue;
1312  }
1313  if(nodeId>0)
1314  {
1315  std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
1316  UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
1317  if(value == 0)
1318  {
1319  eter->second.first -= 1;
1320  }
1321  else if(value == 100)
1322  {
1323  eter->second.second -= 1;
1324  }
1325  if(kter->first < 0)
1326  {
1327  eter->second.second += 1;
1328  }
1329  }
1330  }
1331  if(kter->first > 0)
1332  {
1333  info[0] = (float)kter->first;
1334  info[1] = ptf[0];
1335  info[2] = ptf[1];
1336  cter->second.second+=1;
1337  }
1338 
1339  // update odds
1340  if(nodeId != kter->first || value!=100)
1341  {
1342  info[3] += probHit_;
1343  if (info[3] < probClampingMin_)
1344  {
1345  info[3] = probClampingMin_;
1346  }
1347  if (info[3] > probClampingMax_)
1348  {
1349  info[3] = probClampingMax_;
1350  }
1351  }
1352 
1353  value = 100; // obstacles
1354  }
1355  }
1356  }
1357  }
1358 
1359  if(footprintRadius_ >= cellSize_*1.5f || incrementalGraphUpdate)
1360  {
1361  for(int i=1; i<map.rows-1; ++i)
1362  {
1363  for(int j=1; j<map.cols-1; ++j)
1364  {
1365  char & value = map.at<char>(i, j);
1366  if(value == -2)
1367  {
1368  value = 0;
1369  }
1370 
1371  if(incrementalGraphUpdate && value == -1)
1372  {
1373  float * info = mapInfo.ptr<float>(i, j);
1374 
1375  // fill obstacle
1376  if(map.at<char>(i+1, j) == 100 && map.at<char>(i-1, j) == 100)
1377  {
1378  value = 100;
1379  // associate with the nearest pose
1380  if(mapInfo.ptr<float>(i+1, j)[0]>0.0f)
1381  {
1382  info[0] = mapInfo.ptr<float>(i+1, j)[0];
1383  info[1] = float(j) * cellSize_ + xMin;
1384  info[2] = float(i) * cellSize_ + yMin;
1385  std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
1386  UASSERT(cter!=cellCount_.end());
1387  cter->second.second+=1;
1388  }
1389  else if(mapInfo.ptr<float>(i-1, j)[0]>0.0f)
1390  {
1391  info[0] = mapInfo.ptr<float>(i-1, j)[0];
1392  info[1] = float(j) * cellSize_ + xMin;
1393  info[2] = float(i) * cellSize_ + yMin;
1394  std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
1395  UASSERT(cter!=cellCount_.end());
1396  cter->second.second+=1;
1397  }
1398  }
1399  else if(map.at<char>(i, j+1) == 100 && map.at<char>(i, j-1) == 100)
1400  {
1401  value = 100;
1402  // associate with the nearest pose
1403  if(mapInfo.ptr<float>(i, j+1)[0]>0.0f)
1404  {
1405  info[0] = mapInfo.ptr<float>(i, j+1)[0];
1406  info[1] = float(j) * cellSize_ + xMin;
1407  info[2] = float(i) * cellSize_ + yMin;
1408  std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
1409  UASSERT(cter!=cellCount_.end());
1410  cter->second.second+=1;
1411  }
1412  else if(mapInfo.ptr<float>(i, j-1)[0]>0.0f)
1413  {
1414  info[0] = mapInfo.ptr<float>(i, j-1)[0];
1415  info[1] = float(j) * cellSize_ + xMin;
1416  info[2] = float(i) * cellSize_ + yMin;
1417  std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
1418  UASSERT(cter!=cellCount_.end());
1419  cter->second.second+=1;
1420  }
1421  }
1422  else
1423  {
1424  // fill empty
1425  char sum = (map.at<char>(i+1, j) == 0?1:0) +
1426  (map.at<char>(i-1, j) == 0?1:0) +
1427  (map.at<char>(i, j+1) == 0?1:0) +
1428  (map.at<char>(i, j-1) == 0?1:0);
1429  if(sum >=3)
1430  {
1431  value = 0;
1432  // associate with the nearest pose, only check two cases (as 3 are required)
1433  if(map.at<char>(i+1, j) != -1 && mapInfo.ptr<float>(i+1, j)[0]>0.0f)
1434  {
1435  info[0] = mapInfo.ptr<float>(i+1, j)[0];
1436  info[1] = float(j) * cellSize_ + xMin;
1437  info[2] = float(i) * cellSize_ + yMin;
1438  std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
1439  UASSERT(cter!=cellCount_.end());
1440  cter->second.first+=1;
1441  }
1442  else if(map.at<char>(i-1, j) != -1 && mapInfo.ptr<float>(i-1, j)[0]>0.0f)
1443  {
1444  info[0] = mapInfo.ptr<float>(i-1, j)[0];
1445  info[1] = float(j) * cellSize_ + xMin;
1446  info[2] = float(i) * cellSize_ + yMin;
1447  std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
1448  UASSERT(cter!=cellCount_.end());
1449  cter->second.first+=1;
1450  }
1451  }
1452  }
1453  }
1454 
1455  //float * info = mapInfo.ptr<float>(i,j);
1456  //if(info[0] > 0)
1457  //{
1458  // cloud->at(oi).x = info[1];
1459  // cloud->at(oi).y = info[2];
1460  // oi++;
1461  //}
1462  }
1463  }
1464  }
1465  //if(graphChanged)
1466  //{
1467  // cloud->resize(oi);
1468  // pcl::io::savePCDFileBinary("mapInfo.pcd", *cloud);
1469  // UWARN("Saved mapInfo.pcd");
1470  //}
1471 
1472  map_ = map;
1473  mapInfo_ = mapInfo;
1474  xMin_ = xMin;
1475  yMin_ = yMin;
1476 
1477  // clean cellCount_
1478  for(std::map<int, std::pair<int, int> >::iterator iter= cellCount_.begin(); iter!=cellCount_.end();)
1479  {
1480  UASSERT(iter->second.first >= 0 && iter->second.second >= 0);
1481  if(iter->second.first == 0 && iter->second.second == 0)
1482  {
1483  cellCount_.erase(iter++);
1484  }
1485  else
1486  {
1487  ++iter;
1488  }
1489  }
1490  }
1491  }
1492 
1493  if(cloudAssembling_)
1494  {
1495  if(assembledGroundUpdated && assembledGround_->size() > 1)
1496  {
1497  assembledGround_ = util3d::voxelize(assembledGround_, cellSize_);
1498  }
1499  if(assembledObstaclesUpdated && assembledGround_->size() > 1)
1500  {
1501  assembledObstacles_ = util3d::voxelize(assembledObstacles_, cellSize_);
1502  }
1503  if(assembledEmptyCellsUpdated && assembledEmptyCells_->size() > 1)
1504  {
1505  assembledEmptyCells_ = util3d::voxelize(assembledEmptyCells_, cellSize_);
1506  }
1507  }
1508  }
1509 
1510  if(!fullUpdate_ && !cloudAssembling_)
1511  {
1512  cache_.clear();
1513  }
1514  else
1515  {
1516  //clear only negative ids
1517  for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=cache_.begin(); iter!=cache_.end();)
1518  {
1519  if(iter->first < 0)
1520  {
1521  cache_.erase(iter++);
1522  }
1523  else
1524  {
1525  break;
1526  }
1527  }
1528  }
1529 
1530  bool updated = !poses.empty() || graphOptimized || graphChanged;
1531  UDEBUG("Occupancy Grid update time = %f s (updated=%s)", timer.ticks(), updated?"true":"false");
1532  return updated;
1533 }
1534 
1535 unsigned long OccupancyGrid::getMemoryUsed() const
1536 {
1537  unsigned long memoryUsage = sizeof(OccupancyGrid);
1538  memoryUsage += parameters_.size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap);
1539 
1540  memoryUsage += cache_.size()*(sizeof(int) + sizeof(std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat>) + sizeof(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator)) + sizeof(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >);
1541  for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::const_iterator iter=cache_.begin(); iter!=cache_.end(); ++iter)
1542  {
1543  memoryUsage += iter->second.first.first.total() * iter->second.first.first.elemSize();
1544  memoryUsage += iter->second.first.second.total() * iter->second.first.second.elemSize();
1545  memoryUsage += iter->second.second.total() * iter->second.second.elemSize();
1546  }
1547  memoryUsage += map_.total() * map_.elemSize();
1548  memoryUsage += mapInfo_.total() * mapInfo_.elemSize();
1549  memoryUsage += cellCount_.size()*(sizeof(int)*3 + sizeof(std::pair<int, int>) + sizeof(std::map<int, std::pair<int, int> >::iterator)) + sizeof(std::map<int, std::pair<int, int> >);
1550  memoryUsage += addedNodes_.size()*(sizeof(int) + sizeof(Transform)+ sizeof(float)*12 + sizeof(std::map<int, Transform>::iterator)) + sizeof(std::map<int, Transform>);
1551 
1552  if(assembledGround_.get())
1553  {
1554  memoryUsage += assembledGround_->points.size() * sizeof(pcl::PointXYZRGB);
1555  }
1556  if(assembledObstacles_.get())
1557  {
1558  memoryUsage += assembledObstacles_->points.size() * sizeof(pcl::PointXYZRGB);
1559  }
1560  if(assembledEmptyCells_.get())
1561  {
1562  memoryUsage += assembledEmptyCells_->points.size() * sizeof(pcl::PointXYZRGB);
1563  }
1564  return memoryUsage;
1565 }
1566 
1567 }
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:476
rtabmap::OccupancyGrid::update
bool update(const std::map< int, Transform > &poses)
Definition: OccupancyGrid.cpp:690
rtabmap::util3d::erodeMap
cv::Mat RTABMAP_EXP erodeMap(const cv::Mat &map)
Definition: util3d_mapping.cpp:1010
UTimer
Definition: UTimer.h:46
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:88
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:89
rtabmap::OccupancyGrid::roiRatios_
std::vector< float > roiRatios_
Definition: OccupancyGrid.h:114
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:105
f
f
rtabmap::OccupancyGrid::probability
static double probability(double logodds)
Definition: OccupancyGrid.h:48
rtabmap::util3d::laserScanToPointCloudRGBNormal
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2453
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:380
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:94
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:101
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:106
uStr2Float
float UTILITE_EXP uStr2Float(const std::string &str)
Definition: UConversion.cpp:123
glm::all
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::util3d::occupancy2DFromLaserScan
void RTABMAP_EXP occupancy2DFromLaserScan(const cv::Mat &scanHit, const cv::Mat &scanNoHit, const cv::Point3f &viewpoint, cv::Mat &empty, cv::Mat &occupied, float cellSize, bool unknownSpaceFilled=false, float scanMaxRange=0.0f)
Definition: util3d_mapping.cpp:82
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:104
rtabmap::OccupancyGrid::setCellSize
void setCellSize(float cellSize)
Definition: OccupancyGrid.cpp:258
rtabmap::OccupancyGrid::addedNodes_
std::map< int, Transform > addedNodes_
Definition: OccupancyGrid.h:155
UConversion.h
Some conversion functions.
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:166
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:102
UFATAL
#define UFATAL(...)
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2382
rtabmap::OccupancyGrid::setMap
void setMap(const cv::Mat &map, float xMin, float yMin, float cellSize, const std::map< int, Transform > &poses)
Definition: OccupancyGrid.cpp:224
uSplit
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
Definition: UStl.h:566
rtabmap::util3d::downsample
LaserScan RTABMAP_EXP downsample(const LaserScan &cloud, int step)
Definition: util3d_filtering.cpp:368
UASSERT
#define UASSERT(condition)
rtabmap::OctoMap::update
bool update(const std::map< int, Transform > &poses)
Definition: OctoMap.cpp:382
UStl.h
Wrappers of STL for convenient functions.
rtabmap::OctoMap::addToCache
void addToCache(int nodeId, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &ground, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &obstacles, const pcl::PointXYZ &viewPoint)
Definition: OctoMap.cpp:348
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2417
rtabmap::OccupancyGrid::createLocalMap
void createLocalMap(const Signature &node, cv::Mat &groundCells, cv::Mat &obstacleCells, cv::Mat &emptyCells, cv::Point3f &viewPoint) const
Definition: OccupancyGrid.cpp:282
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::OccupancyGrid::OccupancyGrid
OccupancyGrid(const ParametersMap &parameters=ParametersMap())
Definition: OccupancyGrid.cpp:43
rtabmap::util3d::transformLaserScan
LaserScan RTABMAP_EXP transformLaserScan(const LaserScan &laserScan, const Transform &transform)
Definition: util3d_transforms.cpp:39
rtabmap::Transform::getEulerAngles
void getEulerAngles(float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:232
rtabmap::OccupancyGrid::addToCache
void addToCache(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty)
Definition: OccupancyGrid.cpp:675
rtabmap::OccupancyGrid::logodds
static float logodds(double probability)
Definition: OccupancyGrid.h:43
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP cloudRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1031
rtabmap::OccupancyGrid::assembledGround_
pcl::PointCloud< pcl::PointXYZRGB >::Ptr assembledGround_
Definition: OccupancyGrid.h:158
rtabmap::OccupancyGrid::cellCount_
std::map< int, std::pair< int, int > > cellCount_
Definition: OccupancyGrid.h:152
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:98
rtabmap::OccupancyGrid::getProbMap
cv::Mat getProbMap(float &xMin, float &yMin) const
Definition: OccupancyGrid.cpp:647
rtabmap::OccupancyGrid::cache_
std::map< int, std::pair< std::pair< cv::Mat, cv::Mat >, cv::Mat > > cache_
Definition: OccupancyGrid.h:149
rtabmap::OccupancyGrid::assembledEmptyCells_
pcl::PointCloud< pcl::PointXYZRGB >::Ptr assembledEmptyCells_
Definition: OccupancyGrid.h:160
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::OctoMap::setMaxRange
void setMaxRange(float value)
Definition: OctoMap.h:213
false
#define false
Definition: ConvertUTF.c:56
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:215
rtabmap::Signature::getPose
const Transform & getPose() const
Definition: Signature.h:132
rtabmap::OctoMap::createCloud
pcl::PointCloud< pcl::PointXYZRGB >::Ptr createCloud(unsigned int treeDepth=0, std::vector< int > *obstacleIndices=0, std::vector< int > *emptyIndices=0, std::vector< int > *groundIndices=0, bool originalRefPoints=true, std::vector< int > *frontierIndices=0, std::vector< double > *cloudProb=0) const
Definition: OctoMap.cpp:890
rtabmap::OccupancyGrid::assembledObstacles_
pcl::PointCloud< pcl::PointXYZRGB >::Ptr assembledObstacles_
Definition: OccupancyGrid.h:159
UDEBUG
#define UDEBUG(...)
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:137
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2400
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
rtabmap::SensorData::stereoCameraModel
const StereoCameraModel & stereoCameraModel() const
Definition: SensorData.h:216
rtabmap::OccupancyGrid::parameters_
ParametersMap parameters_
Definition: OccupancyGrid.h:110
UWARN
#define UWARN(...)
rtabmap::util3d::rangeFiltering
LaserScan RTABMAP_EXP rangeFiltering(const LaserScan &scan, float rangeMin, float rangeMax)
Definition: util3d_filtering.cpp:317
rtabmap::OccupancyGrid::getMemoryUsed
unsigned long getMemoryUsed() const
Definition: OccupancyGrid.cpp:1535
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
rtabmap::OccupancyGrid::parseParameters
void parseParameters(const ParametersMap &parameters)
Definition: OccupancyGrid.cpp:92
logodds
float logodds(double probability)
ULOGGER_ERROR
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:519
rtabmap::StereoCameraModel::localTransform
const Transform & localTransform() const
Definition: StereoCameraModel.h:119
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::OccupancyGrid::getMap
cv::Mat getMap(float &xMin, float &yMin) const
Definition: OccupancyGrid.cpp:604
rtabmap::LaserScan::backwardCompatibility
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:125
rtabmap::OccupancyGrid::setCloudAssembling
void setCloudAssembling(bool enabled)
Definition: OccupancyGrid.cpp:272
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true, bool is2D=false, const Transform &transform=Transform())
Definition: util3d.cpp:1266
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59