LocalGridMaker.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2023, 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/LocalGridMaker.h>
29 #include <rtabmap/core/util3d.h>
30 #include <rtabmap/core/util3d_filtering.h>
31 #include <rtabmap/core/util3d_mapping.h>
32 #include <rtabmap/core/util2d.h>
33 #include <rtabmap/utilite/ULogger.h>
34 #include <rtabmap/utilite/UConversion.h>
35 #include <rtabmap/utilite/UStl.h>
36 #include <rtabmap/utilite/UTimer.h>
37 
38 #ifdef RTABMAP_OCTOMAP
39 #include <rtabmap/core/global_map/OctoMap.h>
40 #endif
41 
42 #include <pcl/io/pcd_io.h>
43 
44 namespace rtabmap {
45 
46 LocalGridMaker::LocalGridMaker(const ParametersMap & parameters) :
47  parameters_(parameters),
48  cloudDecimation_(Parameters::defaultGridDepthDecimation()),
49  rangeMax_(Parameters::defaultGridRangeMax()),
50  rangeMin_(Parameters::defaultGridRangeMin()),
51  //roiRatios_(Parameters::defaultGridDepthRoiRatios()), // initialized in parseParameters()
52  footprintLength_(Parameters::defaultGridFootprintLength()),
53  footprintWidth_(Parameters::defaultGridFootprintWidth()),
54  footprintHeight_(Parameters::defaultGridFootprintHeight()),
55  scanDecimation_(Parameters::defaultGridScanDecimation()),
56  cellSize_(Parameters::defaultGridCellSize()),
57  preVoxelFiltering_(Parameters::defaultGridPreVoxelFiltering()),
58  occupancySensor_(Parameters::defaultGridSensor()),
59  projMapFrame_(Parameters::defaultGridMapFrameProjection()),
60  maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
61  normalKSearch_(Parameters::defaultGridNormalK()),
62  groundNormalsUp_(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
63  maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()*M_PI/180.0f),
64  clusterRadius_(Parameters::defaultGridClusterRadius()),
65  minClusterSize_(Parameters::defaultGridMinClusterSize()),
66  flatObstaclesDetected_(Parameters::defaultGridFlatObstacleDetected()),
67  minGroundHeight_(Parameters::defaultGridMinGroundHeight()),
68  maxGroundHeight_(Parameters::defaultGridMaxGroundHeight()),
69  normalsSegmentation_(Parameters::defaultGridNormalsSegmentation()),
70  grid3D_(Parameters::defaultGrid3D()),
71  groundIsObstacle_(Parameters::defaultGridGroundIsObstacle()),
72  noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
73  noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
74  scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
75  rayTracing_(Parameters::defaultGridRayTracing())
76 {
77  this->parseParameters(parameters);
78 }
79 
80 LocalGridMaker::~LocalGridMaker()
81 {
82 }
83 
84 void LocalGridMaker::parseParameters(const ParametersMap & parameters)
85 {
86  uInsert(parameters_, parameters);
87 
88  Parameters::parse(parameters, Parameters::kGridSensor(), occupancySensor_);
89  Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_);
90  if(cloudDecimation_ == 0)
91  {
92  cloudDecimation_ = 1;
93  }
94  Parameters::parse(parameters, Parameters::kGridRangeMin(), rangeMin_);
95  Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_);
96  Parameters::parse(parameters, Parameters::kGridFootprintLength(), footprintLength_);
97  Parameters::parse(parameters, Parameters::kGridFootprintWidth(), footprintWidth_);
98  Parameters::parse(parameters, Parameters::kGridFootprintHeight(), footprintHeight_);
99  Parameters::parse(parameters, Parameters::kGridScanDecimation(), scanDecimation_);
100  Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize_);
101  UASSERT(cellSize_>0.0f);
102 
103  Parameters::parse(parameters, Parameters::kGridPreVoxelFiltering(), preVoxelFiltering_);
104  Parameters::parse(parameters, Parameters::kGridMapFrameProjection(), projMapFrame_);
105  Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
106  Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
107  Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
108  Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_);
109  Parameters::parse(parameters, Parameters::kIcpPointToPlaneGroundNormalsUp(), groundNormalsUp_);
110  if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_))
111  {
112  maxGroundAngle_ *= M_PI/180.0f;
113  }
114  Parameters::parse(parameters, Parameters::kGridClusterRadius(), clusterRadius_);
115  UASSERT_MSG(clusterRadius_ > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridClusterRadius().c_str()).c_str());
116  Parameters::parse(parameters, Parameters::kGridMinClusterSize(), minClusterSize_);
117  Parameters::parse(parameters, Parameters::kGridFlatObstacleDetected(), flatObstaclesDetected_);
118  Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_);
119  Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_);
120  Parameters::parse(parameters, Parameters::kGridGroundIsObstacle(), groundIsObstacle_);
121  Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
122  Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
123  Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
124  Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
125 
126  // convert ROI from string to vector
127  ParametersMap::const_iterator iter;
128  if((iter=parameters.find(Parameters::kGridDepthRoiRatios())) != parameters.end())
129  {
130  std::list<std::string> strValues = uSplit(iter->second, ' ');
131  if(strValues.size() != 4)
132  {
133  ULOGGER_ERROR("The number of values must be 4 (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
134  }
135  else
136  {
137  std::vector<float> tmpValues(4);
138  unsigned int i=0;
139  for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
140  {
141  tmpValues[i] = uStr2Float(*jter);
142  ++i;
143  }
144 
145  if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
146  tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
147  tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
148  tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
149  {
150  roiRatios_ = tmpValues;
151  }
152  else
153  {
154  ULOGGER_ERROR("The roi ratios are not valid (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
155  }
156  }
157  }
158 
159  if(maxGroundHeight_ == 0.0f && !normalsSegmentation_)
160  {
161  UWARN("\"%s\" should be not equal to 0 if not using normals "
162  "segmentation approach. Setting it to cell size (%f).",
163  Parameters::kGridMaxGroundHeight().c_str(), cellSize_);
164  maxGroundHeight_ = cellSize_;
165  }
166  if(maxGroundHeight_ != 0.0f &&
167  maxObstacleHeight_ != 0.0f &&
168  maxObstacleHeight_ < maxGroundHeight_)
169  {
170  UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
171  Parameters::kGridMaxGroundHeight().c_str(),
172  Parameters::kGridMaxObstacleHeight().c_str(),
173  Parameters::kGridMaxObstacleHeight().c_str());
174  maxObstacleHeight_ = 0;
175  }
176  if(maxGroundHeight_ != 0.0f &&
177  minGroundHeight_ != 0.0f &&
178  maxGroundHeight_ < minGroundHeight_)
179  {
180  UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
181  Parameters::kGridMinGroundHeight().c_str(),
182  Parameters::kGridMaxGroundHeight().c_str(),
183  Parameters::kGridMinGroundHeight().c_str());
184  minGroundHeight_ = 0;
185  }
186 }
187 
188 void LocalGridMaker::createLocalMap(
189  const Signature & node,
190  cv::Mat & groundCells,
191  cv::Mat & obstacleCells,
192  cv::Mat & emptyCells,
193  cv::Point3f & viewPoint)
194 {
195  UDEBUG("scan format=%s, occupancySensor_=%d normalsSegmentation_=%d grid3D_=%d",
196  node.sensorData().laserScanRaw().isEmpty()?"NA":node.sensorData().laserScanRaw().formatName().c_str(), occupancySensor_, normalsSegmentation_?1:0, grid3D_?1:0);
197 
198  if((node.sensorData().laserScanRaw().is2d()) && occupancySensor_ == 0)
199  {
200  UDEBUG("2D laser scan");
201  //2D
202  viewPoint = cv::Point3f(
203  node.sensorData().laserScanRaw().localTransform().x(),
204  node.sensorData().laserScanRaw().localTransform().y(),
205  node.sensorData().laserScanRaw().localTransform().z());
206 
207  LaserScan scan = node.sensorData().laserScanRaw();
208  if(rangeMin_ > 0.0f)
209  {
210  scan = util3d::rangeFiltering(scan, rangeMin_, 0.0f);
211  }
212 
213  float maxRange = rangeMax_;
214  if(rangeMax_>0.0f && node.sensorData().laserScanRaw().rangeMax()>0.0f)
215  {
216  maxRange = rangeMax_ < node.sensorData().laserScanRaw().rangeMax()?rangeMax_:node.sensorData().laserScanRaw().rangeMax();
217  }
218  else if(scan2dUnknownSpaceFilled_ && node.sensorData().laserScanRaw().rangeMax()>0.0f)
219  {
220  maxRange = node.sensorData().laserScanRaw().rangeMax();
221  }
222  util3d::occupancy2DFromLaserScan(
223  util3d::transformLaserScan(scan, node.sensorData().laserScanRaw().localTransform()).data(),
224  cv::Mat(),
225  viewPoint,
226  emptyCells,
227  obstacleCells,
228  cellSize_,
229  scan2dUnknownSpaceFilled_,
230  maxRange);
231 
232  UDEBUG("ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels());
233  }
234  else
235  {
236  // 3D
237  if(occupancySensor_ == 0 || occupancySensor_ == 2)
238  {
239  if(!node.sensorData().laserScanRaw().isEmpty())
240  {
241  UDEBUG("3D laser scan");
242  const Transform & t = node.sensorData().laserScanRaw().localTransform();
243  LaserScan scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
244 #ifdef RTABMAP_OCTOMAP
245  // If ray tracing enabled, clipping will be done in OctoMap or in occupancy2DFromLaserScan()
246  float maxRange = rayTracing_?0.0f:rangeMax_;
247 #else
248  // If ray tracing enabled, clipping will be done in occupancy2DFromLaserScan()
249  float maxRange = !grid3D_ && rayTracing_?0.0f:rangeMax_;
250 #endif
251  if(rangeMin_ > 0.0f || maxRange > 0.0f)
252  {
253  scan = util3d::rangeFiltering(scan, rangeMin_, maxRange);
254  }
255 
256  // update viewpoint
257  viewPoint = cv::Point3f(t.x(), t.y(), t.z());
258 
259  UDEBUG("scan format=%d", scan.format());
260 
261  bool normalSegmentationTmp = normalsSegmentation_;
262  float minGroundHeightTmp = minGroundHeight_;
263  float maxGroundHeightTmp = maxGroundHeight_;
264  if(scan.is2d())
265  {
266  // if 2D, assume the whole scan is obstacle
267  normalsSegmentation_ = false;
268  minGroundHeight_ = std::numeric_limits<int>::min();
269  maxGroundHeight_ = std::numeric_limits<int>::min()+100;
270  }
271 
272  createLocalMap(scan, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
273 
274  if(scan.is2d())
275  {
276  // restore
277  normalsSegmentation_ = normalSegmentationTmp;
278  minGroundHeight_ = minGroundHeightTmp;
279  maxGroundHeight_ = maxGroundHeightTmp;
280  }
281  }
282  else
283  {
284  UWARN("Cannot create local map from scan: scan is empty (node=%d, %s=%d).", node.id(), Parameters::kGridSensor().c_str(), occupancySensor_);
285  }
286  }
287 
288  if(occupancySensor_ >= 1)
289  {
290  pcl::IndicesPtr indices(new std::vector<int>);
291  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
292  UDEBUG("Depth image : decimation=%d max=%f min=%f",
293  cloudDecimation_,
294  rangeMax_,
295  rangeMin_);
296  cloud = util3d::cloudRGBFromSensorData(
297  node.sensorData(),
298  cloudDecimation_,
299 #ifdef RTABMAP_OCTOMAP
300  // If ray tracing enabled, clipping will be done in OctoMap or in occupancy2DFromLaserScan()
301  rayTracing_?0.0f:rangeMax_,
302 #else
303  // If ray tracing enabled, clipping will be done in occupancy2DFromLaserScan()
304  !grid3D_&&rayTracing_?0.0f:rangeMax_,
305 #endif
306  rangeMin_,
307  indices.get(),
308  parameters_,
309  roiRatios_);
310 
311  // update viewpoint
312  viewPoint = cv::Point3f(0,0,0);
313  if(node.sensorData().cameraModels().size())
314  {
315  // average of all local transforms
316  float sum = 0;
317  for(unsigned int i=0; i<node.sensorData().cameraModels().size(); ++i)
318  {
319  const Transform & t = node.sensorData().cameraModels()[i].localTransform();
320  if(!t.isNull())
321  {
322  viewPoint.x += t.x();
323  viewPoint.y += t.y();
324  viewPoint.z += t.z();
325  sum += 1.0f;
326  }
327  }
328  if(sum > 0.0f)
329  {
330  viewPoint.x /= sum;
331  viewPoint.y /= sum;
332  viewPoint.z /= sum;
333  }
334  }
335  else
336  {
337  // average of all local transforms
338  float sum = 0;
339  for(unsigned int i=0; i<node.sensorData().stereoCameraModels().size(); ++i)
340  {
341  const Transform & t = node.sensorData().stereoCameraModels()[i].localTransform();
342  if(!t.isNull())
343  {
344  viewPoint.x += t.x();
345  viewPoint.y += t.y();
346  viewPoint.z += t.z();
347  sum += 1.0f;
348  }
349  }
350  if(sum > 0.0f)
351  {
352  viewPoint.x /= sum;
353  viewPoint.y /= sum;
354  viewPoint.z /= sum;
355  }
356  }
357 
358  cv::Mat scanGroundCells;
359  cv::Mat scanObstacleCells;
360  cv::Mat scanEmptyCells;
361  if(occupancySensor_ == 2)
362  {
363  // backup
364  scanGroundCells = groundCells;
365  scanObstacleCells = obstacleCells;
366  scanEmptyCells = emptyCells;
367  groundCells = cv::Mat();
368  obstacleCells = cv::Mat();
369  emptyCells = cv::Mat();
370  }
371 
372  createLocalMap(LaserScan(util3d::laserScanFromPointCloud(*cloud, indices), 0, 0.0f), node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
373 
374  if(occupancySensor_ == 2)
375  {
376  if(grid3D_)
377  {
378  // We should convert scans to 4 channels (XYZRGB) to be compatible
379  scanGroundCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanGroundCells), Transform::getIdentity(), 255, 255, 255)).data();
380  scanObstacleCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanObstacleCells), Transform::getIdentity(), 255, 255, 255)).data();
381  scanEmptyCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanEmptyCells), Transform::getIdentity(), 255, 255, 255)).data();
382  }
383 
384  UDEBUG("groundCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", groundCells.cols, groundCells.channels(), scanGroundCells.cols, scanGroundCells.channels());
385  UDEBUG("obstacleCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", obstacleCells.cols, obstacleCells.channels(), scanObstacleCells.cols, scanObstacleCells.channels());
386  UDEBUG("emptyCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", emptyCells.cols, emptyCells.channels(), scanEmptyCells.cols, scanEmptyCells.channels());
387 
388  if(!groundCells.empty() && !scanGroundCells.empty())
389  cv::hconcat(groundCells, scanGroundCells, groundCells);
390  else if(!scanGroundCells.empty())
391  groundCells = scanGroundCells;
392 
393  if(!obstacleCells.empty() && !scanObstacleCells.empty())
394  cv::hconcat(obstacleCells, scanObstacleCells, obstacleCells);
395  else if(!scanObstacleCells.empty())
396  obstacleCells = scanObstacleCells;
397 
398  if(!emptyCells.empty() && !scanEmptyCells.empty())
399  cv::hconcat(emptyCells, scanEmptyCells, emptyCells);
400  else if(!scanEmptyCells.empty())
401  emptyCells = scanEmptyCells;
402  }
403  }
404  }
405 }
406 
407 void LocalGridMaker::createLocalMap(
408  const LaserScan & scan,
409  const Transform & pose,
410  cv::Mat & groundCells,
411  cv::Mat & obstacleCells,
412  cv::Mat & emptyCells,
413  cv::Point3f & viewPointInOut) const
414 {
415  if(projMapFrame_)
416  {
417  //we should rotate viewPoint in /map frame
418  float roll, pitch, yaw;
419  pose.getEulerAngles(roll, pitch, yaw);
420  Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z, 0,0,0);
421  viewPointInOut.x = viewpointRotated.x();
422  viewPointInOut.y = viewpointRotated.y();
423  viewPointInOut.z = viewpointRotated.z();
424  }
425 
426  if(scan.size())
427  {
428  pcl::IndicesPtr groundIndices(new std::vector<int>);
429  pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
430  cv::Mat groundCloud;
431  cv::Mat obstaclesCloud;
432 
433  if(scan.hasRGB() && scan.hasNormals())
434  {
435  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = util3d::laserScanToPointCloudRGBNormal(scan, scan.localTransform());
436  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZRGBNormal>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
437  UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
438  if(grid3D_)
439  {
440  groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
441  obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
442  }
443  else
444  {
445  util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGBNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
446  }
447  }
448  else if(scan.hasRGB())
449  {
450  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
451  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZRGB>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
452  UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
453  if(grid3D_)
454  {
455  groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
456  obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
457  }
458  else
459  {
460  util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
461  }
462  }
463  else if(scan.hasNormals())
464  {
465  pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(scan, scan.localTransform());
466  pcl::PointCloud<pcl::PointNormal>::Ptr cloudSegmented = segmentCloud<pcl::PointNormal>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
467  UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
468  if(grid3D_)
469  {
470  groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
471  obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
472  }
473  else
474  {
475  util3d::occupancy2DFromGroundObstacles<pcl::PointNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
476  }
477  }
478  else
479  {
480  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
481  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZ>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
482  UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
483  if(grid3D_)
484  {
485  groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
486  obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
487  }
488  else
489  {
490  util3d::occupancy2DFromGroundObstacles<pcl::PointXYZ>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
491  }
492  }
493 
494  if(grid3D_ && (!obstaclesCloud.empty() || !groundCloud.empty()))
495  {
496  UDEBUG("ground=%d obstacles=%d", groundCloud.cols, obstaclesCloud.cols);
497  if(groundIsObstacle_ && !groundCloud.empty())
498  {
499  if(obstaclesCloud.empty())
500  {
501  obstaclesCloud = groundCloud;
502  groundCloud = cv::Mat();
503  }
504  else
505  {
506  UASSERT(obstaclesCloud.type() == groundCloud.type());
507  cv::Mat merged(1,obstaclesCloud.cols+groundCloud.cols, obstaclesCloud.type());
508  obstaclesCloud.copyTo(merged(cv::Range::all(), cv::Range(0, obstaclesCloud.cols)));
509  groundCloud.copyTo(merged(cv::Range::all(), cv::Range(obstaclesCloud.cols, obstaclesCloud.cols+groundCloud.cols)));
510  }
511  }
512 
513  // transform back in base frame
514  float roll, pitch, yaw;
515  pose.getEulerAngles(roll, pitch, yaw);
516  Transform tinv = Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0).inverse();
517 
518  if(rayTracing_)
519  {
520 #ifdef RTABMAP_OCTOMAP
521  if(!groundCloud.empty() || !obstaclesCloud.empty())
522  {
523  //create local octomap
524  ParametersMap params;
525  params.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(cellSize_)));
526  params.insert(ParametersPair(Parameters::kGridRangeMax(), uNumber2Str(rangeMax_)));
527  params.insert(ParametersPair(Parameters::kGridRayTracing(), uNumber2Str(rayTracing_)));
528  LocalGridCache cache;
529  OctoMap octomap(&cache, params);
530  cache.add(1, groundCloud, obstaclesCloud, cv::Mat(), cellSize_, cv::Point3f(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z));
531  std::map<int, Transform> poses;
532  poses.insert(std::make_pair(1, Transform::getIdentity()));
533  octomap.update(poses);
534 
535  pcl::IndicesPtr groundIndices(new std::vector<int>);
536  pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
537  pcl::IndicesPtr emptyIndices(new std::vector<int>);
538  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithRayTracing = octomap.createCloud(0, obstaclesIndices.get(), emptyIndices.get(), groundIndices.get());
539  UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size());
540  if(scan.hasRGB())
541  {
542  groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv).data();
543  obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv).data();
544  emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv).data();
545  }
546  else
547  {
548  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithRayTracing2(new pcl::PointCloud<pcl::PointXYZ>);
549  pcl::copyPointCloud(*cloudWithRayTracing, *cloudWithRayTracing2);
550  groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, groundIndices, tinv).data();
551  obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, obstaclesIndices, tinv).data();
552  emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, emptyIndices, tinv).data();
553  }
554  }
555  }
556  else
557 #else
558  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());
559  }
560 #endif
561  {
562  groundCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(groundCloud), tinv).data();
563  obstacleCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(obstaclesCloud), tinv).data();
564  }
565 
566  }
567  else if(!grid3D_ && rayTracing_ && (!obstacleCells.empty() || !groundCells.empty()))
568  {
569  cv::Mat laserScan = obstacleCells;
570  cv::Mat laserScanNoHit = groundCells;
571  obstacleCells = cv::Mat();
572  groundCells = cv::Mat();
573  util3d::occupancy2DFromLaserScan(
574  laserScan,
575  laserScanNoHit,
576  viewPointInOut,
577  emptyCells,
578  obstacleCells,
579  cellSize_,
580  false, // don't fill unknown space
581  rangeMax_);
582  }
583  }
584  UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
585 }
586 
587 } // namespace rtabmap
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:127
rtabmap::LocalGridMaker::maxGroundHeight_
float maxGroundHeight_
Definition: LocalGridMaker.h:101
rtabmap::LocalGridMaker::groundIsObstacle_
bool groundIsObstacle_
Definition: LocalGridMaker.h:104
rtabmap::LocalGridMaker::noiseFilteringRadius_
float noiseFilteringRadius_
Definition: LocalGridMaker.h:105
rtabmap::LocalGridMaker::roiRatios_
std::vector< float > roiRatios_
Definition: LocalGridMaker.h:84
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
rtabmap::LaserScan::backwardCompatibility
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:133
rtabmap::LocalGridMaker::rangeMax_
float rangeMax_
Definition: LocalGridMaker.h:82
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:122
rtabmap::LocalGridMaker::LocalGridMaker
LocalGridMaker(const ParametersMap &parameters=ParametersMap())
Definition: LocalGridMaker.cpp:46
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:135
rtabmap::LocalGridMaker::~LocalGridMaker
virtual ~LocalGridMaker()
Definition: LocalGridMaker.cpp:80
rtabmap::LocalGridMaker::grid3D_
bool grid3D_
Definition: LocalGridMaker.h:103
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
size
Index size
rtabmap::LocalGridMaker::rangeMin_
float rangeMin_
Definition: LocalGridMaker.h:83
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:116
rtabmap::LocalGridMaker::normalKSearch_
int normalKSearch_
Definition: LocalGridMaker.h:94
rtabmap::util3d::transformLaserScan
LaserScan RTABMAP_CORE_EXPORT transformLaserScan(const LaserScan &laserScan, const Transform &transform)
Definition: util3d_transforms.cpp:39
rtabmap::LaserScan::formatName
static std::string formatName(const Format &format)
Definition: LaserScan.cpp:34
rtabmap::LocalGridMaker::footprintHeight_
float footprintHeight_
Definition: LocalGridMaker.h:87
rtabmap::LocalGridCache
Definition: LocalGrid.h:56
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:134
LocalGridMaker.h
util3d.h
rtabmap::LocalGridMaker::createLocalMap
void createLocalMap(const Signature &node, cv::Mat &groundCells, cv::Mat &obstacleCells, cv::Mat &emptyCells, cv::Point3f &viewPoint)
Definition: LocalGridMaker.cpp:188
rtabmap::LaserScan
Definition: LaserScan.h:37
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
Vector2::y
float y
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
UTimer.h
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:131
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:240
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:130
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:241
indices
indices
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:117
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:441
rtabmap::Transform::y
float & y()
Definition: Transform.h:93
rtabmap::Transform::z
float & z()
Definition: Transform.h:94
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT 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:1268
rtabmap::Transform::getEulerAngles
void getEulerAngles(float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:263
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
rtabmap::LocalGridMaker::cloudDecimation_
unsigned int cloudDecimation_
Definition: LocalGridMaker.h:81
rtabmap::LocalGridMaker::minClusterSize_
int minClusterSize_
Definition: LocalGridMaker.h:98
rtabmap::LocalGridMaker::maxObstacleHeight_
float maxObstacleHeight_
Definition: LocalGridMaker.h:93
uNumber2Str
std::string UTILITE_EXPORT uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::LocalGridMaker::noiseFilteringMinNeighbors_
int noiseFilteringMinNeighbors_
Definition: LocalGridMaker.h:106
rtabmap::LocalGridMaker::normalsSegmentation_
bool normalsSegmentation_
Definition: LocalGridMaker.h:102
UConversion.h
Some conversion functions.
util3d_filtering.h
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2322
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:133
all
static const Eigen::internal::all_t all
rtabmap::Transform::x
float & x()
Definition: Transform.h:92
rtabmap::LocalGridMaker::footprintWidth_
float footprintWidth_
Definition: LocalGridMaker.h:86
rtabmap::LocalGridMaker::rayTracing_
bool rayTracing_
Definition: LocalGridMaker.h:108
rtabmap::util3d::downsample
LaserScan RTABMAP_CORE_EXPORT downsample(const LaserScan &cloud, int step)
Definition: util3d_filtering.cpp:398
UASSERT
#define UASSERT(condition)
params
SmartProjectionParams params(gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY)
rtabmap::Parameters
Definition: Parameters.h:170
util3d_mapping.h
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::LocalGridMaker::maxGroundAngle_
float maxGroundAngle_
Definition: LocalGridMaker.h:96
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::LocalGridCache::add
void add(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty, float cellSize, const cv::Point3f &viewPoint=cv::Point3f(0, 0, 0))
Definition: LocalGrid.cpp:55
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::LocalGridMaker::occupancySensor_
int occupancySensor_
Definition: LocalGridMaker.h:91
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::LocalGridMaker::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: LocalGridMaker.cpp:84
M_PI
#define M_PI
Definition: tango-gl/include/tango-gl/util.h:66
rtabmap::Signature::getPose
const Transform & getPose() const
Definition: Signature.h:133
ULogger.h
ULogger class and convenient macros.
rtabmap::LocalGridMaker::minGroundHeight_
float minGroundHeight_
Definition: LocalGridMaker.h:100
rtabmap::Transform
Definition: Transform.h:41
rtabmap::LocalGridMaker::scan2dUnknownSpaceFilled_
bool scan2dUnknownSpaceFilled_
Definition: LocalGridMaker.h:107
rtabmap::LocalGridMaker::groundNormalsUp_
float groundNormalsUp_
Definition: LocalGridMaker.h:95
util2d.h
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:564
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::util3d::occupancy2DFromLaserScan
RTABMAP_DEPRECATED void RTABMAP_CORE_EXPORT occupancy2DFromLaserScan(const cv::Mat &scan, cv::Mat &empty, cv::Mat &occupied, float cellSize, bool unknownSpaceFilled=false, float scanMaxRange=0.0f)
Definition: util3d_mapping.cpp:50
OctoMap.h
iter
iterator iter(handle obj)
ULOGGER_ERROR
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
rtabmap::LocalGridMaker::clusterRadius_
float clusterRadius_
Definition: LocalGridMaker.h:97
rtabmap::LocalGridMaker::footprintLength_
float footprintLength_
Definition: LocalGridMaker.h:85
c_str
const char * c_str(Args &&...args)
UStl.h
Wrappers of STL for convenient functions.
UDEBUG
#define UDEBUG(...)
rtabmap::LocalGridMaker::preVoxelFiltering_
bool preVoxelFiltering_
Definition: LocalGridMaker.h:90
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2269
rtabmap::LocalGridMaker::cellSize_
float cellSize_
Definition: LocalGridMaker.h:89
octomap
rtabmap::OctoMap
Definition: global_map/OctoMap.h:175
rtabmap::util3d::rangeFiltering
LaserScan RTABMAP_CORE_EXPORT rangeFiltering(const LaserScan &scan, float rangeMin, float rangeMax)
Definition: util3d_filtering.cpp:347
Vector2::x
float x
uStr2Float
float UTILITE_EXPORT uStr2Float(const std::string &str)
Definition: UConversion.cpp:138
t
Point2 t(10, 10)
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:138
rtabmap::util3d::laserScanToPointCloudRGBNormal
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGBNormal(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2376
rtabmap::LocalGridMaker::parameters_
ParametersMap parameters_
Definition: LocalGridMaker.h:79
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2296
rtabmap::LocalGridMaker::scanDecimation_
int scanDecimation_
Definition: LocalGridMaker.h:88
rtabmap::LocalGridMaker::flatObstaclesDetected_
bool flatObstaclesDetected_
Definition: LocalGridMaker.h:99
i
int i
rtabmap::LocalGridMaker::projMapFrame_
bool projMapFrame_
Definition: LocalGridMaker.h:92
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::Signature
Definition: Signature.h:48

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jul 1 2024 02:42:29