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