From d48e2093f5407638aa1087ac526880d742d7432b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 2 Jun 2023 16:05:05 -0700 Subject: [PATCH 1/2] added fast normal estimation on obstacle segmentation --- corelib/include/rtabmap/core/OccupancyGrid.h | 1 + corelib/include/rtabmap/core/Parameters.h | 1 + .../rtabmap/core/impl/OccupancyGrid.hpp | 43 ++++++- .../rtabmap/core/impl/util3d_mapping.hpp | 70 ++++++++--- corelib/include/rtabmap/core/util3d_mapping.h | 6 +- corelib/include/rtabmap/core/util3d_surface.h | 11 ++ corelib/src/OccupancyGrid.cpp | 2 + corelib/src/util3d.cpp | 117 ++++++++++++++++-- corelib/src/util3d_surface.cpp | 76 ++++++++---- 9 files changed, 272 insertions(+), 55 deletions(-) diff --git a/corelib/include/rtabmap/core/OccupancyGrid.h b/corelib/include/rtabmap/core/OccupancyGrid.h index 04617c4bfe..f454f7474e 100644 --- a/corelib/include/rtabmap/core/OccupancyGrid.h +++ b/corelib/include/rtabmap/core/OccupancyGrid.h @@ -132,6 +132,7 @@ class RTABMAP_CORE_EXPORT OccupancyGrid bool normalsSegmentation_; bool grid3D_; bool groundIsObstacle_; + bool labelUndergroundObstaclesAsGround_; float noiseFilteringRadius_; int noiseFilteringMinNeighbors_; bool scan2dUnknownSpaceFilled_; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index f9eee94fad..67b041d85c 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -760,6 +760,7 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(Grid, 3D, bool, false, uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is 0.", kGridSensor().c_str())); #endif RTABMAP_PARAM(Grid, GroundIsObstacle, bool, false, uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str())); + RTABMAP_PARAM(Grid, UndergroundIsGround, bool, false, uFormat("[%s=true] Label all underground points under largest flat surface detected as ground.", kGridNormalsSegmentation().c_str())); RTABMAP_PARAM(Grid, NoiseFilteringRadius, float, 0.0, "Noise filtering radius (0=disabled). Done after segmentation."); RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int, 5, "Noise filtering minimum neighbors."); RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled, bool, false, uFormat("Unknown space filled. Only used with 2D laser scans. Use %s to set maximum range if laser scan max range is to set.", kGridRangeMax().c_str())); diff --git a/corelib/include/rtabmap/core/impl/OccupancyGrid.hpp b/corelib/include/rtabmap/core/impl/OccupancyGrid.hpp index a614a3d5a8..6d14cc44c7 100644 --- a/corelib/include/rtabmap/core/impl/OccupancyGrid.hpp +++ b/corelib/include/rtabmap/core/impl/OccupancyGrid.hpp @@ -54,6 +54,7 @@ typename pcl::PointCloud::Ptr OccupancyGrid::segmentCloud( typename pcl::PointCloud::Ptr cloud(new pcl::PointCloud); pcl::IndicesPtr indices(new std::vector); + UDEBUG("preVoxelFiltering=%d", preVoxelFiltering_?1:0); if(preVoxelFiltering_) { // voxelize to grid cell size @@ -127,6 +128,8 @@ typename pcl::PointCloud::Ptr OccupancyGrid::segmentCloud( UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0); UDEBUG("maxGroundHeight=%f", maxGroundHeight_); UDEBUG("groundNormalsUp=%f", groundNormalsUp_); + UDEBUG("labelUndergroundObstaclesAsGround=%d", labelUndergroundObstaclesAsGround_?1:0); + UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0)); util3d::segmentObstaclesFromGround( cloud, indices, @@ -140,8 +143,8 @@ typename pcl::PointCloud::Ptr OccupancyGrid::segmentCloud( maxGroundHeight_, flatObstacles, Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1), - groundNormalsUp_); - UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0)); + groundNormalsUp_, + labelUndergroundObstaclesAsGround_); //UWARN("Saving ground.pcd and obstacles.pcd"); //pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices); //pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices); @@ -165,6 +168,42 @@ typename pcl::PointCloud::Ptr OccupancyGrid::segmentCloud( UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size()); + if(!preVoxelFiltering_ && (!groundIndices->empty() || !obstaclesIndices->empty())) + { + // voxelize to grid cell size + typename pcl::PointCloud::Ptr cloudWithTransform = cloud; + cloud.reset(new pcl::PointCloud); + if(!groundIndices->empty()) + { + *cloud += *util3d::voxelize(cloudWithTransform, groundIndices, cellSize_); + groundIndices->resize(cloud->size()); + for(size_t i=0; isize(); ++i) + { + groundIndices->at(i) = i; + } + } + if(!obstaclesIndices->empty()) + { + int previousSize = cloud->size(); + *cloud += *util3d::voxelize(cloudWithTransform, obstaclesIndices, cellSize_); + obstaclesIndices->resize(cloud->size()-previousSize); + for(size_t i=0; isize(); ++i) + { + obstaclesIndices->at(i) = previousSize+i; + } + } + if(flatObstacles && !(*flatObstacles)->empty()) + { + int previousSize = cloud->size(); + *cloud += *util3d::voxelize(cloudWithTransform, *flatObstacles, cellSize_); + (*flatObstacles)->resize(cloud->size()-previousSize); + for(size_t i=0; i<(*flatObstacles)->size(); ++i) + { + (*flatObstacles)->at(i) = previousSize+i; + } + } + } + // Do radius filtering after voxel filtering ( a lot faster) if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0) { diff --git a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp index 7d82e4a47c..c7a7b819cf 100644 --- a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp +++ b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp @@ -64,7 +64,8 @@ void segmentObstaclesFromGround( float maxGroundHeight, pcl::IndicesPtr * flatObstacles, const Eigen::Vector4f & viewPoint, - float groundNormalsUp) + float groundNormalsUp, + bool labelUndergroundObstaclesAsGround) { ground.reset(new std::vector); obstacles.reset(new std::vector); @@ -84,10 +85,15 @@ void segmentObstaclesFromGround( normalKSearch, viewPoint, groundNormalsUp); + UDEBUG("%ld points on flat surfaces (input indices = %ld, total cloud=%ld)", + flatSurfaces->size(), indices->size(), cloud->size()); if(segmentFlatObstacles && flatSurfaces->size()) { int biggestFlatSurfaceIndex; + + //If cloud is orgazized, cluster/floodfill using intergral image (can use radius to limit the flood) + std::vector clusteredFlatSurfaces = extractClusters( cloud, flatSurfaces, @@ -125,6 +131,8 @@ void segmentObstaclesFromGround( if(biggestFlatSurfaceIndex>=0) { ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex); + UDEBUG("Biggest flat surface size = %ld (z min=%f max=%f)", + ground->size(), biggestSurfaceMin[2], biggestSurfaceMax[2]); } if(!ground->empty() && (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight)) @@ -135,7 +143,7 @@ void segmentObstaclesFromGround( { Eigen::Vector4f centroid(0,0,0,1); pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid); - if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= biggestSurfaceMax[2]) // epsilon + if(centroid[2] <= biggestSurfaceMax[2]) // relative to ground detected { ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i)); } @@ -171,25 +179,53 @@ void segmentObstaclesFromGround( notObstacles = util3d::extractIndices(cloud, indices, true); notObstacles = util3d::concatenate(notObstacles, ground); } - pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true); - // If ground height is set, remove obstacles under it - if(maxGroundHeight != 0.0f) + // If ground height is set and if we label obstacles under it as ground + if(labelUndergroundObstaclesAsGround) { - otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits::max()); + Eigen::Vector4f min,max; + if(!ground->empty()) + { + pcl::getMinMax3D(*cloud, *ground, min, max); + if(maxGroundHeight>0) + { + max[2] += maxGroundHeight; + } + } + else + { + max[2] = maxGroundHeight; + } + + pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true); + pcl::IndicesPtr underground = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", (float)std::numeric_limits::min(), max[2]); + if(!underground->empty()) + { + ground = util3d::concatenate(ground, underground); + notObstacles = util3d::concatenate(underground, notObstacles); + } } + pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true); + //Cluster remaining stuff (obstacles) if(otherStuffIndices->size()) { - std::vector clusteredObstaclesSurfaces = util3d::extractClusters( - cloud, - otherStuffIndices, - clusterRadius, - minClusterSize); - - // merge indices - obstacles = util3d::concatenate(clusteredObstaclesSurfaces); + if(minClusterSize>1) + { + std::vector clusteredObstaclesSurfaces = util3d::extractClusters( + cloud, + otherStuffIndices, + clusterRadius, + minClusterSize); + + // merge indices + obstacles = util3d::concatenate(clusteredObstaclesSurfaces); + } + else + { + obstacles = otherStuffIndices; + } } } } @@ -208,7 +244,8 @@ void segmentObstaclesFromGround( float maxGroundHeight, pcl::IndicesPtr * flatObstacles, const Eigen::Vector4f & viewPoint, - float groundNormalsUp) + float groundNormalsUp, + bool labelUndergroundObstaclesAsGround) { pcl::IndicesPtr indices(new std::vector); segmentObstaclesFromGround( @@ -224,7 +261,8 @@ void segmentObstaclesFromGround( maxGroundHeight, flatObstacles, viewPoint, - groundNormalsUp); + groundNormalsUp, + labelUndergroundObstaclesAsGround); } template diff --git a/corelib/include/rtabmap/core/util3d_mapping.h b/corelib/include/rtabmap/core/util3d_mapping.h index b1c865d7cd..700e6f1e7d 100644 --- a/corelib/include/rtabmap/core/util3d_mapping.h +++ b/corelib/include/rtabmap/core/util3d_mapping.h @@ -157,7 +157,8 @@ void segmentObstaclesFromGround( float maxGroundHeight = 0.0f, pcl::IndicesPtr * flatObstacles = 0, const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0), - float groundNormalsUp = 0); + float groundNormalsUp = 0, + bool labelUndergroundObstaclesAsGround = false); template void segmentObstaclesFromGround( const typename pcl::PointCloud::Ptr & cloud, @@ -171,7 +172,8 @@ void segmentObstaclesFromGround( float maxGroundHeight = 0.0f, pcl::IndicesPtr * flatObstacles = 0, const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0), - float groundNormalsUp = 0); + float groundNormalsUp = 0, + bool labelUndergroundObstaclesAsGround = false); template void occupancy2DFromGroundObstacles( diff --git a/corelib/include/rtabmap/core/util3d_surface.h b/corelib/include/rtabmap/core/util3d_surface.h index 080cd8b7e1..684336f6c2 100644 --- a/corelib/include/rtabmap/core/util3d_surface.h +++ b/corelib/include/rtabmap/core/util3d_surface.h @@ -381,6 +381,17 @@ pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormal float searchRadius = 0.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals( + const pcl::PointCloud::Ptr & cloud, + float maxDepthChangeFactor = 0.02f, + float normalSmoothingSize = 10.0f, + const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float maxDepthChangeFactor = 0.02f, + float normalSmoothingSize = 10.0f, + const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals( const pcl::PointCloud::Ptr & cloud, float maxDepthChangeFactor = 0.02f, diff --git a/corelib/src/OccupancyGrid.cpp b/corelib/src/OccupancyGrid.cpp index 5305700a07..66fed70fcc 100644 --- a/corelib/src/OccupancyGrid.cpp +++ b/corelib/src/OccupancyGrid.cpp @@ -66,6 +66,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) : normalsSegmentation_(Parameters::defaultGridNormalsSegmentation()), grid3D_(Parameters::defaultGrid3D()), groundIsObstacle_(Parameters::defaultGridGroundIsObstacle()), + labelUndergroundObstaclesAsGround_(Parameters::defaultGridUndergroundIsGround()), noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()), noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()), scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()), @@ -128,6 +129,7 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_); Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_); Parameters::parse(parameters, Parameters::kGridGroundIsObstacle(), groundIsObstacle_); + Parameters::parse(parameters, Parameters::kGridUndergroundIsGround(), labelUndergroundObstaclesAsGround_); Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_); Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_); Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_); diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index ecbed214ce..639913cd75 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -372,34 +372,127 @@ pcl::PointCloud::Ptr cloudFromDepth( float depthCx = model.cx() * rgbToDepthFactorX; float depthCy = model.cy() * rgbToDepthFactorY; - UDEBUG("depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d", + bool isMM = imageDepth.type() == CV_16UC1; + + UDEBUG("depth=%dx%d (isMM=%d) fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d", imageDepth.cols, imageDepth.rows, + isMM?1:0, model.fx(), model.fy(), model.cx(), model.cy(), rgbToDepthFactorX, rgbToDepthFactorY, decimation); + int decimationMode = 1; int oi = 0; - for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation) + if(isMM) { - for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation) + for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation) { - pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation)); - - pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false); - if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth)) + const unsigned short * rowPtr = imageDepth.ptr(h); + for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation) { - pt.x = ptXYZ.x; - pt.y = ptXYZ.y; - pt.z = ptXYZ.z; - if(validIndices) + pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation)); + + pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN(); + + if(decimationMode == 1 && decimation>1) + { + // project closest point + cv::Point2i closestPixel(w,h); + unsigned short closestDepthMM = 0; + for(int v = h; v < h + decimation; ++v) + { + const unsigned short * roiRowPtr = imageDepth.ptr(v); + for(int u = w; u < w + decimation; ++u) + { + const unsigned short & depthMM = roiRowPtr[u]; + if(depthMM > 0 && (depthMM < closestDepthMM || closestDepthMM == 0)) + { + closestDepthMM = depthMM; + closestPixel.x = u; + closestPixel.y = v; + } + } + } + if(closestDepthMM > 0) + { + float depth = ((float)closestDepthMM)/1000.0f; + if(depth>=minDepth && (maxDepth<=0.0f || depth <= maxDepth)) + { + // Fill in XYZ + pt.z = depth; + pt.x = ((float)closestPixel.x - depthCx) * pt.z / depthFx; + pt.y = ((float)closestPixel.y - depthCy) * pt.z / depthFy; + } + } + } + else if(rowPtr[w]>0) + { + float depth = ((float)rowPtr[w])/1000.0f; + if(depth>=minDepth && (maxDepth<=0.0f || depth <= maxDepth)) + { + // Fill in XYZ + pt.z = depth; + pt.x = ((float)w - depthCx) * pt.z / depthFx; + pt.y = ((float)h - depthCy) * pt.z / depthFy; + } + } + if(pcl::isFinite(pt) && validIndices) { validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation); } } - else + } + } + else + { + for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation) + { + const float * rowPtr = imageDepth.ptr(h); + for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation) { + pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation)); + pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN(); + + if(decimationMode == 1 && decimation>1) + { + // project closest point + cv::Point2i closestPixel(w,h); + float closestDepth = 0.0f; + for(int v = h; v < h + decimation; ++v) + { + const float * roiRowPtr = imageDepth.ptr(v); + for(int u = w; u < w + decimation; ++u) + { + const float & depth = roiRowPtr[u]; + if(depth > 0.0f && (depth < closestDepth || closestDepth == 0.0f)) + { + closestDepth = depth; + closestPixel.x = u; + closestPixel.y = v; + } + } + } + if(closestDepth > 0.0f && closestDepth>=minDepth && (maxDepth<=0.0f || closestDepth <= maxDepth)) + { + // Fill in XYZ + pt.z = closestDepth; + pt.x = ((float)closestPixel.x - depthCx) * pt.z / depthFx; + pt.y = ((float)closestPixel.y - depthCy) * pt.z / depthFy; + } + } + else if(rowPtr[w] > 0 && rowPtr[w]>=minDepth && (maxDepth<=0.0f || rowPtr[w] <= maxDepth)) + { + // Fill in XYZ + pt.z = rowPtr[w]; + pt.x = ((float)w - depthCx) * pt.z / depthFx; + pt.y = ((float)h - depthCy) * pt.z / depthFy; + } + if(pcl::isFinite(pt) && validIndices) + { + validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation); + } } } } diff --git a/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index 6bafca8a5b..685378a81f 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -3033,17 +3033,9 @@ pcl::PointCloud::Ptr computeFastOrganizedNormals2D( return computeFastOrganizedNormals2DImpl(cloud, searchK, searchRadius, viewPoint); } -pcl::PointCloud::Ptr computeFastOrganizedNormals( - const pcl::PointCloud::Ptr & cloud, - float maxDepthChangeFactor, - float normalSmoothingSize, - const Eigen::Vector3f & viewPoint) -{ - pcl::IndicesPtr indices(new std::vector); - return computeFastOrganizedNormals(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint); -} -pcl::PointCloud::Ptr computeFastOrganizedNormals( - const pcl::PointCloud::Ptr & cloud, +template +pcl::PointCloud::Ptr computeFastOrganizedNormalsImpl( + const typename pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float maxDepthChangeFactor, float normalSmoothingSize, @@ -3051,35 +3043,73 @@ pcl::PointCloud::Ptr computeFastOrganizedNormals( { UASSERT(cloud->isOrganized()); - pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); - if(indices->size()) - { - tree->setInputCloud(cloud, indices); - } - else - { - tree->setInputCloud (cloud); - } - // Normal estimation pcl::PointCloud::Ptr normals (new pcl::PointCloud); - pcl::IntegralImageNormalEstimation ne; + pcl::IntegralImageNormalEstimation ne; +#if PCL_VERSION_COMPARE(<, 1, 7, 0) ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT); + ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR); +#else + ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT); + ne.setBorderPolicy(ne.BORDER_POLICY_IGNORE); +#endif ne.setMaxDepthChangeFactor(maxDepthChangeFactor); ne.setNormalSmoothingSize(normalSmoothingSize); - ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR); ne.setInputCloud(cloud); // Commented: Keep the output normals size the same as the input cloud //if(indices->size()) //{ // ne.setIndices(indices); //} + + // create kdtree search tree (not used by IntegralImageNormalEstimation) to avoid + // "[pcl::OrganizedNeighbor::radiusSearch] Input dataset is not from a projective device!" + // on clouds smaller than regular organized clouds from camera (640x480) + typename pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); ne.setSearchMethod(tree); + ne.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]); ne.compute(*normals); return normals; } +pcl::PointCloud::Ptr computeFastOrganizedNormals( + const pcl::PointCloud::Ptr & cloud, + float maxDepthChangeFactor, + float normalSmoothingSize, + const Eigen::Vector3f & viewPoint) +{ + pcl::IndicesPtr indices(new std::vector); + return computeFastOrganizedNormals(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint); +} +pcl::PointCloud::Ptr computeFastOrganizedNormals( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float maxDepthChangeFactor, + float normalSmoothingSize, + const Eigen::Vector3f & viewPoint) +{ + return computeFastOrganizedNormalsImpl(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint); +} + +pcl::PointCloud::Ptr computeFastOrganizedNormals( + const pcl::PointCloud::Ptr & cloud, + float maxDepthChangeFactor, + float normalSmoothingSize, + const Eigen::Vector3f & viewPoint) +{ + pcl::IndicesPtr indices(new std::vector); + return computeFastOrganizedNormals(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint); +} +pcl::PointCloud::Ptr computeFastOrganizedNormals( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float maxDepthChangeFactor, + float normalSmoothingSize, + const Eigen::Vector3f & viewPoint) +{ + return computeFastOrganizedNormalsImpl(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint); +} float computeNormalsComplexity( const LaserScan & scan, From 0fcf5620428bf53ee70c2ee02640be17dba69ce7 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 7 Jun 2023 17:14:48 -0700 Subject: [PATCH 2/2] latest update --- .../rtabmap/core/impl/OccupancyGrid.hpp | 3 + .../rtabmap/core/impl/util3d_mapping.hpp | 157 +++++++++++++++--- corelib/src/util3d_mapping.cpp | 30 ++++ 3 files changed, 171 insertions(+), 19 deletions(-) diff --git a/corelib/include/rtabmap/core/impl/OccupancyGrid.hpp b/corelib/include/rtabmap/core/impl/OccupancyGrid.hpp index 6d14cc44c7..a9ef498fe3 100644 --- a/corelib/include/rtabmap/core/impl/OccupancyGrid.hpp +++ b/corelib/include/rtabmap/core/impl/OccupancyGrid.hpp @@ -44,6 +44,8 @@ typename pcl::PointCloud::Ptr OccupancyGrid::segmentCloud( pcl::IndicesPtr & obstaclesIndices, pcl::IndicesPtr * flatObstacles) const { + UDEBUG("cloudIn=%dx%d indicesIn=%ld", cloudIn->width, cloudIn->height, indicesIn->size()); + groundIndices.reset(new std::vector); obstaclesIndices.reset(new std::vector); if(flatObstacles) @@ -130,6 +132,7 @@ typename pcl::PointCloud::Ptr OccupancyGrid::segmentCloud( UDEBUG("groundNormalsUp=%f", groundNormalsUp_); UDEBUG("labelUndergroundObstaclesAsGround=%d", labelUndergroundObstaclesAsGround_?1:0); UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0)); + UDEBUG("cloud=%dx%d indices=%ld", cloud->width, cloud->height, indices->size()); util3d::segmentObstaclesFromGround( cloud, indices, diff --git a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp index c7a7b819cf..29a27fb217 100644 --- a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp +++ b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp @@ -50,6 +50,79 @@ typename pcl::PointCloud::Ptr projectCloudOnXYPlane( return output; } +void clusterIndicesFloodfill(std::vector & cluster, + float * visitedIndices, + int width, + int height, + float clusterRadius, + int currentIndex, + float previousHeight); + +/** + * @brief Cluster indices of an organized cloud + * + * @tparam PointT + * @param cloud + * @param indices + * @param minClusterSize + * @param maxClusterSize + * @param biggestClusterIndex + * @return std::vector + */ +template +std::vector clusterIndices( + const typename pcl::PointCloud::Ptr & cloud, + const typename pcl::IndicesPtr & indices, + float clusterRadius, + int minClusterSize, + int maxClusterSize, + int * biggestClusterIndex) +{ + std::vector clusters; + if(cloud->empty()) + { + return clusters; + } + + UASSERT(cloud->isOrganized()); + + cv::Mat visitedIndices = cv::Mat::zeros(cloud->height, cloud->width, CV_32FC1); + float * ptr = visitedIndices.ptr(); + // init search image + for(size_t i = 0; isize(); ++i) + { + ptr[indices->at(i)] = cloud->at(indices->at(i)).z; + } + + int largestCluster = -1; + int largestClusterSize = 0; + int sum = 0; + for(size_t i = 0; isize(); ++i) + { + if(ptr[indices->at(i)] != 0.0f) + { + pcl::IndicesPtr cluster(new pcl::Indices()); + clusterIndicesFloodfill(*cluster, ptr, visitedIndices.cols, visitedIndices.rows, clusterRadius, indices->at(i), ptr[indices->at(i)]); + if(cluster->size()>0 && (int)cluster->size()>=minClusterSize && (int)cluster->size()<=maxClusterSize) + { + clusters.push_back(cluster); + if((int)cluster->size() > largestClusterSize) + { + sum+=cluster->size(); + largestCluster = clusters.size()-1; + largestClusterSize = cluster->size(); + } + } + } + } + if(biggestClusterIndex) + { + *biggestClusterIndex = largestCluster; + } + + return clusters; +} + template void segmentObstaclesFromGround( const typename pcl::PointCloud::Ptr & cloud, @@ -76,6 +149,8 @@ void segmentObstaclesFromGround( if(cloud->size()) { + UDEBUG("Normal filtering.... cloud=%ld indices=%ld organized=%d", + cloud->size(), indices->size(), cloud->isOrganized()?1:0); // Find the ground pcl::IndicesPtr flatSurfaces = normalFiltering( cloud, @@ -88,24 +163,37 @@ void segmentObstaclesFromGround( UDEBUG("%ld points on flat surfaces (input indices = %ld, total cloud=%ld)", flatSurfaces->size(), indices->size(), cloud->size()); + Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax(0,0,0,0); if(segmentFlatObstacles && flatSurfaces->size()) { int biggestFlatSurfaceIndex; - //If cloud is orgazized, cluster/floodfill using intergral image (can use radius to limit the flood) - - std::vector clusteredFlatSurfaces = extractClusters( + std::vector clusteredFlatSurfaces; + if(cloud->isOrganized()) + { + clusteredFlatSurfaces = clusterIndices( cloud, flatSurfaces, clusterRadius, minClusterSize, std::numeric_limits::max(), &biggestFlatSurfaceIndex); + UDEBUG("clusteredFlatSurfaces=%ld", clusteredFlatSurfaces.size()); + } + else + { + clusteredFlatSurfaces = extractClusters( + cloud, + flatSurfaces, + clusterRadius, + minClusterSize, + std::numeric_limits::max(), + &biggestFlatSurfaceIndex); + } // cluster all surfaces for which the centroid is in the Z-range of the bigger surface if(clusteredFlatSurfaces.size()) { - Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax; if(maxGroundHeight != 0.0f) { // Search for biggest surface under max ground height @@ -131,11 +219,12 @@ void segmentObstaclesFromGround( if(biggestFlatSurfaceIndex>=0) { ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex); - UDEBUG("Biggest flat surface size = %ld (z min=%f max=%f)", - ground->size(), biggestSurfaceMin[2], biggestSurfaceMax[2]); + UDEBUG("Biggest flat surface size = %ld (%d%%) (z min=%f max=%f)", + ground->size(), 100*ground->size()/cloud->size(), biggestSurfaceMin[2], biggestSurfaceMax[2]); } - if(!ground->empty() && (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight)) + if(!ground->empty() && + (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight)) { for(unsigned int i=0; isize()/cloud->size(); + int minGroundRatio = 10; + if(minGroundRatio != 0 && groundRatioempty()?biggestSurfaceMin[2]:0.0f); + // passthrough filter + ground = rtabmap::util3d::passThrough(cloud, indices, "z", + std::numeric_limits::min(), + maxGroundHeight!=0.0f?maxGroundHeight:std::numeric_limits::max()); + + pcl::IndicesPtr notObstacles = ground; + if(indices->size()) + { + notObstacles = util3d::extractIndices(cloud, indices, true); + notObstacles = util3d::concatenate(notObstacles, ground); + } + obstacles = rtabmap::util3d::extractIndices(cloud, notObstacles, true); + return; + } + else + { + UWARN("Failed normal segmentation, ground surface is too small (ground ratio=%d%%, ground height=%f)!", + groundRatio, !ground->empty()?biggestSurfaceMin[2]:0.0f); + // reject ground! + ground.reset(new std::vector); + if(flatObstacles) + { + *flatObstacles = flatSurfaces; + } + } + } } else { + UWARN("Failed normal segmentation, could not detect the ground!"); // reject ground! ground.reset(new std::vector); if(flatObstacles) @@ -176,6 +302,7 @@ void segmentObstaclesFromGround( pcl::IndicesPtr notObstacles = ground; if(indices->size()) { + // This will ignore all points not in input indices for obstacles. notObstacles = util3d::extractIndices(cloud, indices, true); notObstacles = util3d::concatenate(notObstacles, ground); } @@ -183,22 +310,14 @@ void segmentObstaclesFromGround( // If ground height is set and if we label obstacles under it as ground if(labelUndergroundObstaclesAsGround) { - Eigen::Vector4f min,max; - if(!ground->empty()) - { - pcl::getMinMax3D(*cloud, *ground, min, max); - if(maxGroundHeight>0) - { - max[2] += maxGroundHeight; - } - } - else + float max = biggestSurfaceMax[2]; + if(maxGroundHeight > 0) { - max[2] = maxGroundHeight; + max += maxGroundHeight; } pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true); - pcl::IndicesPtr underground = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", (float)std::numeric_limits::min(), max[2]); + pcl::IndicesPtr underground = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", (float)std::numeric_limits::min(), max); if(!underground->empty()) { ground = util3d::concatenate(ground, underground); diff --git a/corelib/src/util3d_mapping.cpp b/corelib/src/util3d_mapping.cpp index 0e5c3928f6..34a004b6e8 100644 --- a/corelib/src/util3d_mapping.cpp +++ b/corelib/src/util3d_mapping.cpp @@ -1026,6 +1026,36 @@ cv::Mat erodeMap(const cv::Mat & map) return erodedMap; } +void clusterIndicesFloodfill(std::vector & cluster, + float * visitedIndices, + int width, + int height, + float clusterRadius, + int currentIndex, + float previousHeight) +{ + if(visitedIndices[currentIndex] == 0.0f || + (clusterRadius>0.0f && fabs(visitedIndices[currentIndex]-previousHeight)>clusterRadius)) + { + return; + } + + int y = currentIndex / width; + int x = currentIndex - y*width; + + if(x>=0 && x=0 && y