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_ = poses;
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().maxRange()>0.0f)
309  {
310  maxRange = cloudMaxDepth_ < node.sensorData().laserScanRaw().maxRange()?cloudMaxDepth_:node.sensorData().laserScanRaw().maxRange();
311  }
312  else if(scan2dUnknownSpaceFilled_ && node.sensorData().laserScanRaw().maxRange()>0.0f)
313  {
314  maxRange = node.sensorData().laserScanRaw().maxRange();
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  uInsert(cache_, std::make_pair(nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
683 }
684 
685 void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
686 {
687  UTimer timer;
688  UDEBUG("Update (poses=%d addedNodes_=%d)", (int)posesIn.size(), (int)addedNodes_.size());
689 
690  float margin = cellSize_*10.0f+(footprintRadius_>cellSize_*1.5f?float(int(footprintRadius_/cellSize_)+1):0.0f)*cellSize_;
691 
692  float minX=-minMapSize_/2.0f;
693  float minY=-minMapSize_/2.0f;
694  float maxX=minMapSize_/2.0f;
695  float maxY=minMapSize_/2.0f;
696  bool undefinedSize = minMapSize_ == 0.0f;
697  std::map<int, cv::Mat> emptyLocalMaps;
698  std::map<int, cv::Mat> occupiedLocalMaps;
699 
700  // First, check of the graph has changed. If so, re-create the map by moving all occupied nodes (fullUpdate==false).
701  bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
702  bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
703  std::map<int, Transform> transforms;
704  float updateErrorSqrd = updateError_*updateError_;
705  for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
706  {
707  std::map<int, Transform>::const_iterator jter = posesIn.find(iter->first);
708  if(jter != posesIn.end())
709  {
710  graphChanged = false;
711 
712  UASSERT(!iter->second.isNull() && !jter->second.isNull());
713  Transform t = Transform::getIdentity();
714  if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
715  {
716  t = jter->second * iter->second.inverse();
717  graphOptimized = true;
718  }
719  transforms.insert(std::make_pair(jter->first, t));
720 
721  float x = jter->second.x();
722  float y =jter->second.y();
723  if(undefinedSize)
724  {
725  minX = maxX = x;
726  minY = maxY = y;
727  undefinedSize = false;
728  }
729  else
730  {
731  if(minX > x)
732  minX = x;
733  else if(maxX < x)
734  maxX = x;
735 
736  if(minY > y)
737  minY = y;
738  else if(maxY < y)
739  maxY = y;
740  }
741  }
742  else
743  {
744  UDEBUG("Updated pose for node %d is not found, some points may not be copied if graph has changed.", iter->first);
745  }
746  }
747 
748  bool assembledGroundUpdated = false;
749  bool assembledObstaclesUpdated = false;
750  bool assembledEmptyCellsUpdated = false;
751 
752  if(graphOptimized || graphChanged)
753  {
754  if(graphChanged)
755  {
756  UWARN("Graph has changed! The whole map should be rebuilt.");
757  }
758  else
759  {
760  UINFO("Graph optimized!");
761  }
762 
763  if(cloudAssembling_)
764  {
765  assembledGround_->clear();
766  assembledObstacles_->clear();
767  }
768 
769  if(!fullUpdate_ && !graphChanged && !map_.empty()) // incremental, just move cells
770  {
771  // 1) recreate all local maps
772  UASSERT(map_.cols == mapInfo_.cols &&
773  map_.rows == mapInfo_.rows);
774  std::map<int, std::pair<int, int> > tmpIndices;
775  for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
776  {
777  if(!uContains(cache_, iter->first) && transforms.find(iter->first) != transforms.end())
778  {
779  if(iter->second.first)
780  {
781  emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
782  }
783  if(iter->second.second)
784  {
785  occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
786  }
787  tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
788  }
789  }
790  for(int y=0; y<map_.rows; ++y)
791  {
792  for(int x=0; x<map_.cols; ++x)
793  {
794  float * info = mapInfo_.ptr<float>(y,x);
795  int nodeId = (int)info[0];
796  if(nodeId > 0 && map_.at<char>(y,x) >= 0)
797  {
798  if(tmpIndices.find(nodeId)!=tmpIndices.end())
799  {
800  std::map<int, Transform>::iterator tter = transforms.find(nodeId);
801  UASSERT(tter != transforms.end());
802 
803  cv::Point3f pt(info[1], info[2], 0.0f);
804  pt = util3d::transformPoint(pt, tter->second);
805 
806  if(minX > pt.x)
807  minX = pt.x;
808  else if(maxX < pt.x)
809  maxX = pt.x;
810 
811  if(minY > pt.y)
812  minY = pt.y;
813  else if(maxY < pt.y)
814  maxY = pt.y;
815 
816  std::map<int, std::pair<int, int> >::iterator jter = tmpIndices.find(nodeId);
817  if(map_.at<char>(y, x) == 0)
818  {
819  // ground
820  std::map<int, cv::Mat>::iterator iter = emptyLocalMaps.find(nodeId);
821  UASSERT(iter != emptyLocalMaps.end());
822  UASSERT(jter->second.first < iter->second.cols);
823  float * ptf = iter->second.ptr<float>(0,jter->second.first++);
824  ptf[0] = pt.x;
825  ptf[1] = pt.y;
826  }
827  else
828  {
829  // obstacle
830  std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(nodeId);
831  UASSERT(iter != occupiedLocalMaps.end());
832  UASSERT(iter!=occupiedLocalMaps.end());
833  UASSERT(jter->second.second < iter->second.cols);
834  float * ptf = iter->second.ptr<float>(0,jter->second.second++);
835  ptf[0] = pt.x;
836  ptf[1] = pt.y;
837  }
838  }
839  }
840  else if(nodeId > 0)
841  {
842  UERROR("Cell referred b node %d is unknown!?", nodeId);
843  }
844  }
845  }
846 
847  //verify if all cells were added
848  for(std::map<int, std::pair<int, int> >::iterator iter=tmpIndices.begin(); iter!=tmpIndices.end(); ++iter)
849  {
850  std::map<int, cv::Mat>::iterator jter = emptyLocalMaps.find(iter->first);
851  UASSERT_MSG((iter->second.first == 0 && (jter==emptyLocalMaps.end() || jter->second.empty())) ||
852  (iter->second.first != 0 && jter!=emptyLocalMaps.end() && jter->second.cols == iter->second.first),
853  uFormat("iter->second.first=%d jter->second.cols=%d", iter->second.first, jter!=emptyLocalMaps.end()?jter->second.cols:-1).c_str());
854  jter = occupiedLocalMaps.find(iter->first);
855  UASSERT_MSG((iter->second.second == 0 && (jter==occupiedLocalMaps.end() || jter->second.empty())) ||
856  (iter->second.second != 0 && jter!=occupiedLocalMaps.end() && jter->second.cols == iter->second.second),
857  uFormat("iter->second.first=%d jter->second.cols=%d", iter->second.first, jter!=emptyLocalMaps.end()?jter->second.cols:-1).c_str());
858  }
859 
860  UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
861  }
862 
863  addedNodes_.clear();
864  map_ = cv::Mat();
865  mapInfo_ = cv::Mat();
866  cellCount_.clear();
867  xMin_ = 0.0f;
868  yMin_ = 0.0f;
869  }
870  else if(!map_.empty())
871  {
872  // update
873  minX=xMin_+margin+cellSize_/2.0f;
874  minY=yMin_+margin+cellSize_/2.0f;
875  maxX=xMin_+float(map_.cols)*cellSize_ - margin;
876  maxY=yMin_+float(map_.rows)*cellSize_ - margin;
877  undefinedSize = false;
878  }
879 
880  bool incrementalGraphUpdate = graphOptimized && !fullUpdate_ && !graphChanged;
881 
882  std::list<std::pair<int, Transform> > poses;
883  int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
884  UDEBUG("Last id = %d", lastId);
885 
886  // add old poses that were not in the current map (they were just retrieved from LTM)
887  for(std::map<int, Transform>::const_iterator iter=posesIn.upper_bound(0); iter!=posesIn.end(); ++iter)
888  {
889  if(addedNodes_.find(iter->first) == addedNodes_.end())
890  {
891  UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first);
892  poses.push_back(*iter);
893  }
894  }
895 
896  // insert negative after
897  for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
898  {
899  if(iter->first < 0)
900  {
901  poses.push_back(*iter);
902  }
903  else
904  {
905  break;
906  }
907  }
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  value = 100; // obstacles
1339 
1340  // update odds
1341  if(nodeId != kter->first)
1342  {
1343  info[3] += probHit_;
1344  if (info[3] < probClampingMin_)
1345  {
1346  info[3] = probClampingMin_;
1347  }
1348  if (info[3] > probClampingMax_)
1349  {
1350  info[3] = probClampingMax_;
1351  }
1352  }
1353  }
1354  }
1355  }
1356  }
1357 
1358  if(footprintRadius_ >= cellSize_*1.5f || incrementalGraphUpdate)
1359  {
1360  for(int i=1; i<map.rows-1; ++i)
1361  {
1362  for(int j=1; j<map.cols-1; ++j)
1363  {
1364  char & value = map.at<char>(i, j);
1365  if(value == -2)
1366  {
1367  value = 0;
1368  }
1369 
1370  if(incrementalGraphUpdate && value == -1)
1371  {
1372  float * info = mapInfo.ptr<float>(i, j);
1373 
1374  // fill obstacle
1375  if(map.at<char>(i+1, j) == 100 && map.at<char>(i-1, j) == 100)
1376  {
1377  value = 100;
1378  // associate with the nearest pose
1379  if(mapInfo.ptr<float>(i+1, j)[0]>0.0f)
1380  {
1381  info[0] = mapInfo.ptr<float>(i+1, j)[0];
1382  info[1] = float(j) * cellSize_ + xMin;
1383  info[2] = float(i) * cellSize_ + yMin;
1384  std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
1385  UASSERT(cter!=cellCount_.end());
1386  cter->second.second+=1;
1387  }
1388  else if(mapInfo.ptr<float>(i-1, j)[0]>0.0f)
1389  {
1390  info[0] = mapInfo.ptr<float>(i-1, j)[0];
1391  info[1] = float(j) * cellSize_ + xMin;
1392  info[2] = float(i) * cellSize_ + yMin;
1393  std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
1394  UASSERT(cter!=cellCount_.end());
1395  cter->second.second+=1;
1396  }
1397  }
1398  else if(map.at<char>(i, j+1) == 100 && map.at<char>(i, j-1) == 100)
1399  {
1400  value = 100;
1401  // associate with the nearest pose
1402  if(mapInfo.ptr<float>(i, j+1)[0]>0.0f)
1403  {
1404  info[0] = mapInfo.ptr<float>(i, j+1)[0];
1405  info[1] = float(j) * cellSize_ + xMin;
1406  info[2] = float(i) * cellSize_ + yMin;
1407  std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
1408  UASSERT(cter!=cellCount_.end());
1409  cter->second.second+=1;
1410  }
1411  else if(mapInfo.ptr<float>(i, j-1)[0]>0.0f)
1412  {
1413  info[0] = mapInfo.ptr<float>(i, j-1)[0];
1414  info[1] = float(j) * cellSize_ + xMin;
1415  info[2] = float(i) * cellSize_ + yMin;
1416  std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
1417  UASSERT(cter!=cellCount_.end());
1418  cter->second.second+=1;
1419  }
1420  }
1421  else
1422  {
1423  // fill empty
1424  char sum = (map.at<char>(i+1, j) == 0?1:0) +
1425  (map.at<char>(i-1, j) == 0?1:0) +
1426  (map.at<char>(i, j+1) == 0?1:0) +
1427  (map.at<char>(i, j-1) == 0?1:0);
1428  if(sum >=3)
1429  {
1430  value = 0;
1431  // associate with the nearest pose, only check two cases (as 3 are required)
1432  if(map.at<char>(i+1, j) != -1 && mapInfo.ptr<float>(i+1, j)[0]>0.0f)
1433  {
1434  info[0] = mapInfo.ptr<float>(i+1, j)[0];
1435  info[1] = float(j) * cellSize_ + xMin;
1436  info[2] = float(i) * cellSize_ + yMin;
1437  std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
1438  UASSERT(cter!=cellCount_.end());
1439  cter->second.first+=1;
1440  }
1441  else if(map.at<char>(i-1, j) != -1 && mapInfo.ptr<float>(i-1, j)[0]>0.0f)
1442  {
1443  info[0] = mapInfo.ptr<float>(i-1, j)[0];
1444  info[1] = float(j) * cellSize_ + xMin;
1445  info[2] = float(i) * cellSize_ + yMin;
1446  std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
1447  UASSERT(cter!=cellCount_.end());
1448  cter->second.first+=1;
1449  }
1450  }
1451  }
1452  }
1453 
1454  //float * info = mapInfo.ptr<float>(i,j);
1455  //if(info[0] > 0)
1456  //{
1457  // cloud->at(oi).x = info[1];
1458  // cloud->at(oi).y = info[2];
1459  // oi++;
1460  //}
1461  }
1462  }
1463  }
1464  //if(graphChanged)
1465  //{
1466  // cloud->resize(oi);
1467  // pcl::io::savePCDFileBinary("mapInfo.pcd", *cloud);
1468  // UWARN("Saved mapInfo.pcd");
1469  //}
1470 
1471  map_ = map;
1472  mapInfo_ = mapInfo;
1473  xMin_ = xMin;
1474  yMin_ = yMin;
1475 
1476  // clean cellCount_
1477  for(std::map<int, std::pair<int, int> >::iterator iter= cellCount_.begin(); iter!=cellCount_.end();)
1478  {
1479  UASSERT(iter->second.first >= 0 && iter->second.second >= 0);
1480  if(iter->second.first == 0 && iter->second.second == 0)
1481  {
1482  cellCount_.erase(iter++);
1483  }
1484  else
1485  {
1486  ++iter;
1487  }
1488  }
1489  }
1490  }
1491 
1492  if(cloudAssembling_)
1493  {
1494  if(assembledGroundUpdated && assembledGround_->size() > 1)
1495  {
1496  assembledGround_ = util3d::voxelize(assembledGround_, cellSize_);
1497  }
1498  if(assembledObstaclesUpdated && assembledGround_->size() > 1)
1499  {
1500  assembledObstacles_ = util3d::voxelize(assembledObstacles_, cellSize_);
1501  }
1502  if(assembledEmptyCellsUpdated && assembledEmptyCells_->size() > 1)
1503  {
1504  assembledEmptyCells_ = util3d::voxelize(assembledEmptyCells_, cellSize_);
1505  }
1506  }
1507 
1508  if(!fullUpdate_ && !cloudAssembling_)
1509  {
1510  cache_.clear();
1511  }
1512  else
1513  {
1514  //clear only negative ids
1515  for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=cache_.begin(); iter!=cache_.end();)
1516  {
1517  if(iter->first < 0)
1518  {
1519  cache_.erase(iter++);
1520  }
1521  else
1522  {
1523  break;
1524  }
1525  }
1526  }
1527 
1528  UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
1529 }
1530 
1531 }
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true)
Definition: util3d.cpp:1266
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:446
rtabmap::util3d::erodeMap
cv::Mat RTABMAP_EXP erodeMap(const cv::Mat &map)
Definition: util3d_mapping.cpp:981
UTimer
Definition: UTimer.h:46
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:193
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:63
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:66
rtabmap::OccupancyGrid::roiRatios_
std::vector< float > roiRatios_
Definition: OccupancyGrid.h:112
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:73
f
f
rtabmap::OccupancyGrid::probability
static double probability(double logodds)
Definition: OccupancyGrid.h:48
rtabmap::LaserScan::maxRange
float maxRange() const
Definition: LaserScan.h:65
rtabmap::OctoMap::update
void update(const std::map< int, Transform > &poses)
Definition: OctoMap.cpp:367
pcl
Definition: CameraRGBD.h:59
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:364
rtabmap::util3d::laserScanToPointCloudRGBNormal
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=255, unsigned char g=255, unsigned char b=255)
Definition: util3d.cpp:2399
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:69
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:74
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:41
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:72
rtabmap::OccupancyGrid::setCellSize
void setCellSize(float cellSize)
Definition: OccupancyGrid.cpp:258
rtabmap::OccupancyGrid::addedNodes_
std::map< int, Transform > addedNodes_
Definition: OccupancyGrid.h:153
rtabmap::OccupancyGrid::update
void update(const std::map< int, Transform > &poses)
Definition: OccupancyGrid.cpp:685
UConversion.h
Some conversion functions.
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:163
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:70
UFATAL
#define UFATAL(...)
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2328
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:317
UASSERT
#define UASSERT(condition)
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:343
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:211
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:156
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) const
Definition: OctoMap.cpp:920
rtabmap::OccupancyGrid::cellCount_
std::map< int, std::pair< int, int > > cellCount_
Definition: OccupancyGrid.h:150
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:67
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:147
rtabmap::OccupancyGrid::assembledEmptyCells_
pcl::PointCloud< pcl::PointXYZRGB >::Ptr assembledEmptyCells_
Definition: OccupancyGrid.h:158
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:193
rtabmap::Signature::getPose
const Transform & getPose() const
Definition: Signature.h:129
rtabmap::OccupancyGrid::assembledObstacles_
pcl::PointCloud< pcl::PointXYZRGB >::Ptr assembledObstacles_
Definition: OccupancyGrid.h:157
UDEBUG
#define UDEBUG(...)
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:134
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2346
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
rtabmap::SensorData::stereoCameraModel
const StereoCameraModel & stereoCameraModel() const
Definition: SensorData.h:194
rtabmap::OccupancyGrid::parameters_
ParametersMap parameters_
Definition: OccupancyGrid.h:108
UWARN
#define UWARN(...)
rtabmap::util3d::rangeFiltering
LaserScan RTABMAP_EXP rangeFiltering(const LaserScan &scan, float rangeMin, float rangeMax)
Definition: util3d_filtering.cpp:270
UTimer::ticks
double ticks()
Definition: UTimer.cpp:110
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:407
rtabmap::StereoCameraModel::localTransform
const Transform & localTransform() const
Definition: StereoCameraModel.h:116
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:84
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=255, unsigned char g=255, unsigned char b=255)
Definition: util3d.cpp:2363
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)
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:32