38 #ifdef RTABMAP_OCTOMAP
42 #include <pcl/io/pcd_io.h>
47 parameters_(parameters),
48 cloudDecimation_(
Parameters::defaultGridDepthDecimation()),
52 footprintLength_(
Parameters::defaultGridFootprintLength()),
53 footprintWidth_(
Parameters::defaultGridFootprintWidth()),
54 footprintHeight_(
Parameters::defaultGridFootprintHeight()),
55 scanDecimation_(
Parameters::defaultGridScanDecimation()),
57 preVoxelFiltering_(
Parameters::defaultGridPreVoxelFiltering()),
58 occupancySensor_(
Parameters::defaultGridSensor()),
59 projMapFrame_(
Parameters::defaultGridMapFrameProjection()),
60 maxObstacleHeight_(
Parameters::defaultGridMaxObstacleHeight()),
61 normalKSearch_(
Parameters::defaultGridNormalK()),
62 groundNormalsUp_(
Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
64 clusterRadius_(
Parameters::defaultGridClusterRadius()),
65 minClusterSize_(
Parameters::defaultGridMinClusterSize()),
66 flatObstaclesDetected_(
Parameters::defaultGridFlatObstacleDetected()),
67 minGroundHeight_(
Parameters::defaultGridMinGroundHeight()),
68 maxGroundHeight_(
Parameters::defaultGridMaxGroundHeight()),
69 normalsSegmentation_(
Parameters::defaultGridNormalsSegmentation()),
71 groundIsObstacle_(
Parameters::defaultGridGroundIsObstacle()),
72 noiseFilteringRadius_(
Parameters::defaultGridNoiseFilteringRadius()),
73 noiseFilteringMinNeighbors_(
Parameters::defaultGridNoiseFilteringMinNeighbors()),
74 scan2dUnknownSpaceFilled_(
Parameters::defaultGridScan2dUnknownSpaceFilled()),
75 rayTracing_(
Parameters::defaultGridRayTracing())
127 ParametersMap::const_iterator
iter;
128 if((
iter=parameters.find(Parameters::kGridDepthRoiRatios())) != parameters.end())
130 std::list<std::string> strValues =
uSplit(
iter->second,
' ');
131 if(strValues.size() != 4)
133 ULOGGER_ERROR(
"The number of values must be 4 (%s=\"%s\")",
iter->first.c_str(),
iter->second.c_str());
137 std::vector<float> tmpValues(4);
139 for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
145 if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0
f-tmpValues[1] &&
146 tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0
f-tmpValues[0] &&
147 tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0
f-tmpValues[3] &&
148 tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0
f-tmpValues[2])
154 ULOGGER_ERROR(
"The roi ratios are not valid (%s=\"%s\")",
iter->first.c_str(),
iter->second.c_str());
161 UWARN(
"\"%s\" should be not equal to 0 if not using normals "
162 "segmentation approach. Setting it to cell size (%f).",
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());
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());
190 cv::Mat & groundCells,
191 cv::Mat & obstacleCells,
192 cv::Mat & emptyCells,
193 cv::Point3f & viewPoint)
195 UDEBUG(
"scan format=%s, occupancySensor_=%d normalsSegmentation_=%d grid3D_=%d",
202 viewPoint = cv::Point3f(
232 UDEBUG(
"ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels());
244 #ifdef RTABMAP_OCTOMAP
257 viewPoint = cv::Point3f(
t.
x(),
t.
y(),
t.z());
284 UWARN(
"Cannot create local map from scan: scan is empty (node=%d, %s=%d).", node.
id(), Parameters::kGridSensor().c_str(),
occupancySensor_);
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",
299 #ifdef RTABMAP_OCTOMAP
312 viewPoint = cv::Point3f(0,0,0);
322 viewPoint.x +=
t.
x();
323 viewPoint.y +=
t.
y();
324 viewPoint.z +=
t.z();
344 viewPoint.x +=
t.
x();
345 viewPoint.y +=
t.
y();
346 viewPoint.z +=
t.z();
358 cv::Mat scanGroundCells;
359 cv::Mat scanObstacleCells;
360 cv::Mat scanEmptyCells;
364 scanGroundCells = groundCells;
365 scanObstacleCells = obstacleCells;
366 scanEmptyCells = emptyCells;
367 groundCells = cv::Mat();
368 obstacleCells = cv::Mat();
369 emptyCells = cv::Mat();
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());
388 if(!groundCells.empty() && !scanGroundCells.empty())
389 cv::hconcat(groundCells, scanGroundCells, groundCells);
390 else if(!scanGroundCells.empty())
391 groundCells = scanGroundCells;
393 if(!obstacleCells.empty() && !scanObstacleCells.empty())
394 cv::hconcat(obstacleCells, scanObstacleCells, obstacleCells);
395 else if(!scanObstacleCells.empty())
396 obstacleCells = scanObstacleCells;
398 if(!emptyCells.empty() && !scanEmptyCells.empty())
399 cv::hconcat(emptyCells, scanEmptyCells, emptyCells);
400 else if(!scanEmptyCells.empty())
401 emptyCells = scanEmptyCells;
410 cv::Mat & groundCells,
411 cv::Mat & obstacleCells,
412 cv::Mat & emptyCells,
413 cv::Point3f & viewPointInOut)
const
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();
428 pcl::IndicesPtr groundIndices(
new std::vector<int>);
429 pcl::IndicesPtr obstaclesIndices(
new std::vector<int>);
431 cv::Mat obstaclesCloud;
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());
445 util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGBNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells,
cellSize_);
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());
460 util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells,
cellSize_);
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());
475 util3d::occupancy2DFromGroundObstacles<pcl::PointNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells,
cellSize_);
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());
490 util3d::occupancy2DFromGroundObstacles<pcl::PointXYZ>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells,
cellSize_);
494 if(
grid3D_ && (!obstaclesCloud.empty() || !groundCloud.empty()))
496 UDEBUG(
"ground=%d obstacles=%d", groundCloud.cols, obstaclesCloud.cols);
499 if(obstaclesCloud.empty())
501 obstaclesCloud = groundCloud;
502 groundCloud = cv::Mat();
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)));
520 #ifdef RTABMAP_OCTOMAP
521 if(!groundCloud.empty() || !obstaclesCloud.empty())
530 cache.
add(1, groundCloud, obstaclesCloud, cv::Mat(),
cellSize_, cv::Point3f(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z));
531 std::map<int, Transform> poses;
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());
548 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithRayTracing2(
new pcl::PointCloud<pcl::PointXYZ>);
549 pcl::copyPointCloud(*cloudWithRayTracing, *cloudWithRayTracing2);
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());
569 cv::Mat laserScan = obstacleCells;
570 cv::Mat laserScanNoHit = groundCells;
571 obstacleCells = cv::Mat();
572 groundCells = cv::Mat();
584 UDEBUG(
"ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());