finished testing util3d_mapping.hpp

corelib/include/rtabmap/core/impl/util3d_mapping.hpp

@@ -85,7 +85,8 @@ void segmentObstaclesFromGround(
 				viewPoint,
 				groundNormalsUp);
 
-		if(segmentFlatObstacles && flatSurfaces->size())
+		if(flatSurfaces->size() && 
+		   (segmentFlatObstacles || maxGroundHeight != 0.0f || minClusterSize>1))
 		{
 			int biggestFlatSurfaceIndex;
 			std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
@@ -135,7 +136,9 @@ 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((maxGroundHeight!=0.0f && centroid[2] <= maxGroundHeight) ||
+								centroid[2] <= biggestSurfaceMax[2] ||
+								(maxGroundHeight==0.0f && !segmentFlatObstacles))
 							{
 								ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
 							}

corelib/include/rtabmap/core/util3d_mapping.h

@@ -311,11 +311,54 @@ cv::Mat RTABMAP_CORE_EXPORT convertImage8U2Map(const cv::Mat & map8U, bool pgmFo
  */
 cv::Mat RTABMAP_CORE_EXPORT erodeMap(const cv::Mat & map);
 
+// templated methods
+
+/**
+ * @brief Projects a point cloud onto the XY plane by setting all Z coordinates to zero.
+ *
+ * This function creates a copy of the input point cloud and modifies each point's Z coordinate
+ * to be zero, effectively projecting the entire cloud onto the XY plane.
+ *
+ * @tparam PointT The type of point used in the point cloud (e.g., pcl::PointXYZ).
+ * @param cloud The input point cloud to project.
+ * @return typename pcl::PointCloud<PointT>::Ptr A pointer to the projected point cloud
+ *         with Z coordinates set to zero.
+ */
 template<typename PointT>
 typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
 		const typename pcl::PointCloud<PointT> & cloud);
 
-// templated methods
+
+/**
+ * @brief Segments ground and obstacle indices from a point cloud using surface normals and clustering.
+ *
+ * This function analyzes a point cloud to identify flat surfaces (e.g., ground) and separates them
+ * from potential obstacles based on normal orientation, height constraints, and optional clustering.
+ * Optionally, flat obstacles (e.g., tables, ramps) can be segmented separately.
+ *
+ * @tparam PointT The type of point used in the point cloud (e.g., pcl::PointXYZ).
+ * 
+ * @param cloud The input point cloud.
+ * @param indices Optional input indices to consider from the cloud (e.g., from a prior ROI extraction).
+ * @param ground Output pointer where indices corresponding to ground points will be stored.
+ * @param obstacles Output pointer where indices corresponding to obstacle points will be stored.
+ * @param normalKSearch Number of neighbors to use for normal estimation.
+ * @param groundNormalAngle Maximum angle (in radians) between the estimated normal and the "up" direction
+ *                          for a surface to be considered ground.
+ * @param clusterRadius The Euclidean distance threshold for clustering flat surfaces and obstacles.
+ * @param minClusterSize The minimum number of points required to form a valid cluster.
+ * @param segmentFlatObstacles If true, flat but non-ground surfaces (e.g., tables) are detected and
+ *                             optionally returned via `flatObstacles`.
+ * @param maxGroundHeight Maximum Z-height for a surface to be considered ground (0 disables filtering). 
+ *                        Note that all obstacle points under that threshold will be ignored (i.e., won't 
+ *                        be returned in `obstacles`).
+ * @param flatObstacles Optional output pointer where indices corresponding to flat obstacles will be stored
+ *                      (only valid if `segmentFlatObstacles` is true).
+ * @param viewPoint The viewpoint to use for normal estimation (important for consistent orientation).
+ * @param groundNormalsUp Threshold (between 0 and 1) used to detect and flip ground-facing normals 
+ *        (set to 0.0f to disable). If the Z component of a normal is less than `-groundNormalsUp` 
+ *        and the corresponding point is below the viewpoint, the normal will be flipped.
+ */
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -331,6 +374,10 @@ void segmentObstaclesFromGround(
 		pcl::IndicesPtr * flatObstacles = 0,
 		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
 		float groundNormalsUp = 0);
+/**
+ * @brief Segments ground and obstacle indices from a point cloud using surface normals and clustering.
+ * @see `segmentObstaclesFromGround()` with indices
+ */
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -346,6 +393,33 @@ void segmentObstaclesFromGround(
 		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
 		float groundNormalsUp = 0);
 
+/**
+ * @brief Projects 3D ground and obstacle point clouds onto the 2D XY plane and voxelizes them into 2D occupancy data.
+ *
+ * This function takes two 3D point clouds—representing ground and obstacles—and performs the following steps:
+ * - Projects them onto the XY plane (setting Z = 0).
+ * - Voxelizes them based on the specified cell size.
+ * - Converts the resulting 2D points into OpenCV matrices (1-row, N-columns, `CV_32FC2`) where each element is a (x, y) coordinate.
+ *
+ * @tparam PointT The type of point in the input point clouds (e.g., pcl::PointXYZ).
+ * 
+ * @param groundCloud The input point cloud representing ground points.
+ * @param obstaclesCloud The input point cloud representing obstacle points.
+ * @param ground Output matrix containing 2D (x, y) coordinates of projected ground points (type: `CV_32FC2`).
+ * @param obstacles Output matrix containing 2D (x, y) coordinates of projected obstacle points (type: `CV_32FC2`).
+ * @param cellSize The size of each voxel/grid cell used for downsampling the projected cloud (in meters).
+ */
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & groundCloud,
+		const typename pcl::PointCloud<PointT>::Ptr & obstaclesCloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize);
+/**
+ * @brief Projects 3D ground and obstacle point clouds onto the 2D XY plane and voxelizes them into 2D occupancy data.
+ * @see occupancy2DFromGroundObstacles() without indices
+ */
 template<typename PointT>
 void occupancy2DFromGroundObstacles(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -355,17 +429,36 @@ void occupancy2DFromGroundObstacles(
 		cv::Mat & obstacles,
 		float cellSize);
 
-template<typename PointT>
-void occupancy2DFromGroundObstacles(
-		const typename pcl::PointCloud<PointT>::Ptr & groundCloud,
-		const typename pcl::PointCloud<PointT>::Ptr & obstaclesCloud,
-		cv::Mat & ground,
-		cv::Mat & obstacles,
-		float cellSize);
-
+/**
+ * @brief Generates 2D ground and obstacle occupancy data from a 3D point cloud.
+ *
+ * This function performs segmentation on a 3D point cloud to separate ground and obstacle points
+ * based on normal orientation and clustering, then projects both onto the XY plane to create
+ * 2D occupancy representations using voxelization.
+ *
+ * The resulting occupancy data is returned as two OpenCV matrices (`cv::Mat`) of type `CV_32FC2`,
+ * where each entry contains a 2D point (x, y) in meters corresponding to a ground or obstacle voxel.
+ *
+ * @tparam PointT The type of point used in the input point cloud (e.g., pcl::PointXYZ).
+ *
+ * @param cloud The input 3D point cloud.
+ * @param indices Optional subset of points from the cloud to use for processing (can be full cloud indices).
+ * @param ground Output matrix containing 2D ground points projected and voxelized (`CV_32FC2`).
+ * @param obstacles Output matrix containing 2D obstacle points projected and voxelized (`CV_32FC2`).
+ * @param cellSize The voxel size (in meters) for projecting and grouping points in 2D.
+ * @param groundNormalAngle Maximum allowable angle (in radians) between a point's normal and the vertical axis for it to be considered part of the ground.
+ * @param minClusterSize Minimum number of points required to form a valid obstacle cluster.
+ * @param segmentFlatObstacles Whether to separate flat horizontal surfaces (e.g., tables) from the ground and treat them as obstacles.
+ * @param maxGroundHeight Maximum Z value (in meters) for a surface to be considered ground. If 0, height filtering is disabled.
+ *
+ * @note Internally, this function calls:
+ * - `segmentObstaclesFromGround()` to classify ground vs. obstacle points.
+ * - `occupancy2DFromGroundObstacles()` to project and voxelize the classified data.
+ */
 template<typename PointT>
 void occupancy2DFromCloud3D(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
 		cv::Mat & ground,
 		cv::Mat & obstacles,
 		float cellSize = 0.05f,
@@ -373,10 +466,13 @@ void occupancy2DFromCloud3D(
 		int minClusterSize = 20,
 		bool segmentFlatObstacles = false,
 		float maxGroundHeight = 0.0f);
+/**
+ * @brief Generates 2D ground and obstacle occupancy data from a 3D point cloud.
+ * @see `occupancy2DFromCloud3D()` with indices
+ */
 template<typename PointT>
 void occupancy2DFromCloud3D(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		const pcl::IndicesPtr & indices,
 		cv::Mat & ground,
 		cv::Mat & obstacles,
 		float cellSize = 0.05f,

corelib/test/test_util3d_mapping.cpp

@@ -1,6 +1,7 @@
 #include "gtest/gtest.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_mapping.h"
+#include "rtabmap/core/util3d_surface.h"
 #include "rtabmap/core/CameraModel.h"
 #include "rtabmap/utilite/UException.h"
 #include "rtabmap/utilite/UConversion.h"
@@ -418,4 +419,264 @@ TEST(Util3dMapping, erodeMapNoErosionWithUnknown)
 
     // Obstacle should NOT be eroded because adjacent unknown cell (-1)
     EXPECT_EQ(erodedMap.at<signed char>(1,1), 100);
+}
+
+TEST(Util3dMapping, projectCloudOnXYPlaneZCoordinatesAreZero)
+{
+    // Create a test point cloud
+    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    input_cloud->push_back(pcl::PointXYZ(1.0, 2.0, 3.0));
+    input_cloud->push_back(pcl::PointXYZ(4.0, 5.0, -1.0));
+    input_cloud->push_back(pcl::PointXYZ(0.0, 0.0, 10.0));
+
+    // Project the cloud
+    auto projected_cloud = util3d::projectCloudOnXYPlane<pcl::PointXYZ>(*input_cloud);
+
+    // Check that the size remains the same
+    ASSERT_EQ(projected_cloud->size(), input_cloud->size());
+
+    // Check that x and y remain the same, and z is set to zero
+    for (size_t i = 0; i < projected_cloud->size(); ++i)
+    {
+        EXPECT_FLOAT_EQ(projected_cloud->at(i).x, input_cloud->at(i).x);
+        EXPECT_FLOAT_EQ(projected_cloud->at(i).y, input_cloud->at(i).y);
+        EXPECT_FLOAT_EQ(projected_cloud->at(i).z, 0.0f);
+    }
+}
+
+TEST(Util3dMapping, segmentObstaclesFromGround)
+{
+    // Create a cloud of a floor, then elevate some part of it to make a flat obstacle
+    pcl::IndicesPtr expected_ground(new std::vector<int>);
+    pcl::IndicesPtr expected_big_obstacles(new std::vector<int>);
+    pcl::IndicesPtr expected_small_obstacles(new std::vector<int>);
+    pcl::IndicesPtr expected_flat_obstacles(new std::vector<int>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    for(int i=0; i<10; ++i) {
+        for(int j=0; j<10; ++j) {
+            if(i>5 && j>5) {
+                expected_flat_obstacles->push_back(cloud->size());
+            }
+            else {
+                expected_ground->push_back(cloud->size());
+            }
+            cloud->push_back(pcl::PointXYZ(0.05f*i, 0.05f*j, 0.01f*i+(i>5 && j>5 ? 0.25f:0.0f)));
+        }
+    }
+    // Add a wall and a small obstacle
+    for(int i=0; i<10; ++i) {
+        for(int k=0; k<10; ++k) {
+            if(i>5 && k>5) {
+                expected_small_obstacles->push_back(cloud->size());
+                cloud->push_back(pcl::PointXYZ(0.05f*i, -0.35f, 0.05f*k));
+            }
+            expected_big_obstacles->push_back(cloud->size());
+            cloud->push_back(pcl::PointXYZ(0.05f*i, -0.15f, 0.05f*k));
+        }
+    }
+
+    pcl::IndicesPtr ground, obstacles, flatObs;
+
+    // test basic
+    float normalKSearch = 5;
+    float angleMax = 20.0f*M_PI/180.0f;  // Allow 20 degrees of deviation
+    float clusterRadius = 0.1f;
+    util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+        cloud,
+        ground,
+        obstacles,
+        normalKSearch,       
+        angleMax,     
+        clusterRadius,
+        1,        // minClusterSize
+        false,    // segmentFlatObstacles
+        0.0f,     // maxGroundHeight
+        &flatObs, // flatObstacles
+        Eigen::Vector4f(0,0,2,0), // viewpoint
+        0.8f      // groundNormalsUp
+    );
+
+    ASSERT_EQ(ground->size(), expected_ground->size() + expected_flat_obstacles->size());
+    ASSERT_EQ(obstacles->size(), expected_big_obstacles->size() + expected_small_obstacles->size());
+
+    // test indices
+    util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+        cloud,
+        expected_ground,
+        ground,
+        obstacles,
+        normalKSearch,
+        angleMax,
+        clusterRadius,
+        1,        // minClusterSize
+        false,    // segmentFlatObstacles
+        0.0f,     // maxGroundHeight
+        &flatObs, // flatObstacles
+        Eigen::Vector4f(0,0,2,0), // viewpoint
+        0.8f      // groundNormalsUp
+    );
+
+    ASSERT_EQ(ground->size(), expected_ground->size());
+    ASSERT_EQ(obstacles->size(), 0);
+
+    // test flat obstacles
+    util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+        cloud,
+        ground,
+        obstacles,
+        normalKSearch,
+        angleMax,
+        clusterRadius,
+        1,        // minClusterSize
+        true,    // segmentFlatObstacles
+        0.0f,     // maxGroundHeight
+        &flatObs,  // flatObstacles
+        Eigen::Vector4f(0,0,2,0), // viewpoint
+        0.8f      // groundNormalsUp
+    );
+
+    ASSERT_EQ(flatObs->size(), expected_flat_obstacles->size());
+    ASSERT_EQ(ground->size(), expected_ground->size());
+    ASSERT_EQ(obstacles->size(), expected_big_obstacles->size() + expected_small_obstacles->size() + expected_flat_obstacles->size());
+
+    // test flat obstacles with maxGroundHeight
+    for(int i=0; i<2; ++i) {
+        util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+            cloud,
+            ground,
+            obstacles,
+            normalKSearch,
+            angleMax,
+            clusterRadius,
+            1,        // minClusterSize
+            i==0,    // segmentFlatObstacles
+            0.1f,     // maxGroundHeight
+            &flatObs,  // flatObstacles
+            Eigen::Vector4f(0,0,2,0), // viewpoint
+            0.8f      // groundNormalsUp
+        );
+
+        ASSERT_EQ(flatObs->size(), expected_flat_obstacles->size());
+        ASSERT_EQ(ground->size(), expected_ground->size());
+        // all obstacles under maxGroundHeight are ignored
+        ASSERT_EQ(obstacles->size(), expected_big_obstacles->size() + expected_small_obstacles->size() + expected_flat_obstacles->size() - 20);
+    }
+
+    // test viewpoint (ceiling segmentation)
+    util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+        cloud,
+        ground,
+        obstacles,
+        normalKSearch,
+        angleMax,
+        clusterRadius,
+        1,        // minClusterSize
+        false,    // segmentFlatObstacles
+        0.0f,     // maxGroundHeight
+        &flatObs,  // flatObstacles
+        Eigen::Vector4f(0,0,0.15f,0), // viewpoint under the top flat obstacle
+        0.8f      // groundNormalsUp
+    );
+
+    ASSERT_EQ(ground->size(), expected_ground->size());
+    ASSERT_EQ(obstacles->size(), expected_big_obstacles->size() + expected_small_obstacles->size() + expected_flat_obstacles->size());
+
+    // test min cluster radius 
+    util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+        cloud,
+        ground,
+        obstacles,
+        normalKSearch,
+        angleMax,
+        clusterRadius,
+        17,        // minClusterSize
+        false,    // segmentFlatObstacles
+        0.0f,     // maxGroundHeight
+        &flatObs,  // flatObstacles
+        Eigen::Vector4f(0,0,2,0),
+        0.8f      // groundNormalsUp
+    );
+
+    ASSERT_EQ(ground->size(), expected_ground->size());
+    ASSERT_EQ(obstacles->size(), expected_big_obstacles->size());
+
+    // Everything obstacles
+    util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+        cloud,
+        ground,
+        obstacles,
+        normalKSearch,
+        angleMax,
+        clusterRadius,
+        1,        // minClusterSize
+        false,    // segmentFlatObstacles
+        -0.1f,     // maxGroundHeight
+        &flatObs,  // flatObstacles
+        Eigen::Vector4f(0,0,2,0),
+        0.8f      // groundNormalsUp
+    );
+
+    ASSERT_EQ(ground->size(), 0);
+    ASSERT_EQ(obstacles->size(), expected_ground->size() + expected_big_obstacles->size() + expected_small_obstacles->size() + expected_flat_obstacles->size());
+
+    // Everything ground or ignored
+    for(int i=0; i<2; ++i) {
+        util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+            cloud,
+            ground,
+            obstacles,
+            normalKSearch,
+            angleMax,
+            clusterRadius,
+            1,        // minClusterSize
+            i==0,    // segmentFlatObstacles
+            10,     // maxGroundHeight
+            &flatObs,  // flatObstacles
+            Eigen::Vector4f(0,0,2,0),
+            0.8f      // groundNormalsUp
+        );
+        // wether we segment or not, all flat surfaces are under 10 meters
+        ASSERT_EQ(ground->size(), expected_ground->size() + expected_flat_obstacles->size());
+        ASSERT_EQ(obstacles->size(), 0);
+    }
+}
+
+TEST(Util3dMapping, occupancy2DFromGroundObstaclesBasic)
+{
+    pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+    // Ground points (clustered near 0,0)
+    groundCloud->push_back(pcl::PointXYZ(0.05f, 0.05f, -0.2f));
+    groundCloud->push_back(pcl::PointXYZ(0.06f, 0.04f, -0.1f));
+    groundCloud->push_back(pcl::PointXYZ(0.5f, 0.5f, -0.2f)); // Separate voxel
+
+    // Obstacle points
+    obstaclesCloud->push_back(pcl::PointXYZ(1.0f, 1.0f, 1.0f));
+    obstaclesCloud->push_back(pcl::PointXYZ(1.02f, 1.01f, 1.2f)); // Same voxel
+    obstaclesCloud->push_back(pcl::PointXYZ(2.0f, 2.0f, 1.5f));
+
+    cv::Mat groundMat, obstaclesMat;
+    float cellSize = 0.1f;
+
+    util3d::occupancy2DFromGroundObstacles<pcl::PointXYZ>(groundCloud, obstaclesCloud, groundMat, obstaclesMat, cellSize);
+
+    // Check that points were voxelized and projected
+    EXPECT_EQ(groundMat.rows, 1);
+    EXPECT_EQ(groundMat.cols, 2); // Expect 2 distinct voxels
+    EXPECT_EQ(groundMat.type(), CV_32FC2);
+
+    EXPECT_NEAR(groundMat.at<cv::Vec2f>(0)[0], 0.055, 0.001);
+    EXPECT_NEAR(groundMat.at<cv::Vec2f>(0)[1], 0.045, 0.001);
+    EXPECT_NEAR(groundMat.at<cv::Vec2f>(1)[0], 0.5, 0.001);
+    EXPECT_NEAR(groundMat.at<cv::Vec2f>(1)[1], 0.5, 0.001);
+
+    EXPECT_EQ(obstaclesMat.rows, 1);
+    EXPECT_EQ(obstaclesMat.cols, 2); // Expect 2 voxels (1.0,1.0) and (2.0,2.0)
+    EXPECT_EQ(obstaclesMat.type(), CV_32FC2);
+
+    EXPECT_NEAR(obstaclesMat.at<cv::Vec2f>(0)[0], 1.01, 0.001);
+    EXPECT_NEAR(obstaclesMat.at<cv::Vec2f>(0)[1], 1.005, 0.001);
+    EXPECT_NEAR(obstaclesMat.at<cv::Vec2f>(1)[0], 2, 0.001);
+    EXPECT_NEAR(obstaclesMat.at<cv::Vec2f>(1)[1], 2, 0.001);
 }