minimal util3d_surface.h

corelib/include/rtabmap/core/util3d_surface.h

@@ -395,29 +395,73 @@ pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormal
 		float normalSmoothingSize = 10.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 
+/**
+ * @defgroup ComputeNormalsComplexity Compute Structural Complexity of a Point Cloud with Normals
+ * @brief Computes the complexity of surface normals in a point cloud using PCA.
+ *
+ * This function performs a Principal Component Analysis (PCA) on the normals of a point cloud
+ * and returns a scalar measure of their spread (complexity). A low value indicates that normals
+ * are aligned (e.g., flat surface), while a high value indicates variation in orientation (e.g., curved or rough surface).
+ *
+ * If a transformation is provided, the normals are rotated accordingly before PCA. The result is normalized
+ * to lie between 0 and 0.25, where 0 represents minimal complexity and 0.25 represents maximal complexity.
+ *
+ * @param cloud              The input point cloud or laser scan containing normals (pcl::PointNormal), or simply normals.
+ * @param t                  The transform to apply to the normals (only the rotation is used).
+ * @param is2d               Set to true if the data is 2D (normals will be analyzed in 2D space).
+ * @param pcaEigenVectors    (Optional) Output matrix containing the eigenvectors computed by PCA.
+ * @param pcaEigenValues     (Optional) Output matrix containing the eigenvalues computed by PCA.
+ *
+ * @return A float value between 0 and 0.25 representing the complexity of the normal distribution.
+ *         Returns 0 if not enough valid normals are available.
+ *
+ * @note Invalid normals (containing NaN or Inf) are automatically filtered out.
+ *       The result is based on the smallest eigenvalue from PCA (for 2D: 2nd eigenvalue, for 3D: 3rd eigenvalue).
+ *
+ */
+/**
+ * @ingroup ComputeNormalsComplexity
+ * @brief Computes the complexity of surface normals in a point cloud of type `LaserScan`.
+ */
 float RTABMAP_CORE_EXPORT computeNormalsComplexity(
 		const LaserScan & scan,
 		const Transform & t = Transform::getIdentity(),
 		cv::Mat * pcaEigenVectors = 0,
 		cv::Mat * pcaEigenValues = 0);
+/**
+ * @ingroup ComputeNormalsComplexity
+ * @brief Computes the complexity of surface normals in a point cloud of type `pcl::Normal`.
+ */
 float RTABMAP_CORE_EXPORT computeNormalsComplexity(
 		const pcl::PointCloud<pcl::Normal> & normals,
 		const Transform & t = Transform::getIdentity(),
 		bool is2d = false,
 		cv::Mat * pcaEigenVectors = 0,
 		cv::Mat * pcaEigenValues = 0);
+/**
+ * @ingroup ComputeNormalsComplexity
+ * @brief Computes the complexity of surface normals in a point cloud of type `pcl::PointNormal`.
+ */
 float RTABMAP_CORE_EXPORT computeNormalsComplexity(
 		const pcl::PointCloud<pcl::PointNormal> & cloud,
 		const Transform & t = Transform::getIdentity(),
 		bool is2d = false,
 		cv::Mat * pcaEigenVectors = 0,
 		cv::Mat * pcaEigenValues = 0);
+/**
+ * @ingroup ComputeNormalsComplexity
+ * @brief Computes the complexity of surface normals in a point cloud of type `pcl::PointXYZINormal`.
+ */
 float RTABMAP_CORE_EXPORT computeNormalsComplexity(
 		const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
 		const Transform & t = Transform::getIdentity(),
 		bool is2d = false,
 		cv::Mat * pcaEigenVectors = 0,
 		cv::Mat * pcaEigenValues = 0);
+/**
+ * @ingroup ComputeNormalsComplexity
+ * @brief Computes the complexity of surface normals in a point cloud of type `pcl::PointXYZRGBNormal`.
+ */
 float RTABMAP_CORE_EXPORT computeNormalsComplexity(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
 		const Transform & t = Transform::getIdentity(),

corelib/test/CMakeLists.txt

@@ -45,3 +45,8 @@ gtest_discover_tests(test_util3d_mapping)
 add_executable(test_util3d_motion_estimation test_util3d_motion_estimation.cpp)
 target_link_libraries(test_util3d_motion_estimation gtest_main rtabmap_core)
 gtest_discover_tests(test_util3d_motion_estimation)
+
+#util3d_surface.h
+add_executable(test_util3d_surface test_util3d_surface.cpp)
+target_link_libraries(test_util3d_surface gtest_main rtabmap_core)
+gtest_discover_tests(test_util3d_surface)

corelib/test/test_util3d_surface.cpp

@@ -0,0 +1,119 @@
+#include "gtest/gtest.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_surface.h"
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/utilite/UException.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/core/Version.h"
+#include <pcl/io/pcd_io.h>
+
+using namespace rtabmap;
+
+// Utility to generate a flat plane of normals pointing up
+pcl::PointCloud<pcl::PointNormal> createFlatNormalCloud(int count, const cv::Point3f& normal)
+{
+    pcl::PointCloud<pcl::PointNormal> cloud;
+    cloud.resize(count);
+    for (int i = 0; i < count; ++i)
+    {
+        cloud[i].normal_x = normal.x;
+        cloud[i].normal_y = normal.y;
+        cloud[i].normal_z = normal.z;
+    }
+    return cloud;
+}
+
+TEST(Util3dSurface, computeNormalsComplexityVaryingNormals3D)
+{
+    auto floor = createFlatNormalCloud(100, cv::Point3f(0.0f, 0.0f, 1.0f));
+    auto wallA = createFlatNormalCloud(100, cv::Point3f(0.0f, 1.0f, 0.0f));
+    auto wallB = createFlatNormalCloud(100, cv::Point3f(1.0f, 0.0f, 0.0f));
+    auto smallWallB = createFlatNormalCloud(10, cv::Point3f(1.0f, 0.0f, 0.0f));
+
+    // One flat surface
+    float complexity = util3d::computeNormalsComplexity(floor);
+    EXPECT_NEAR(complexity, 0.0f, 1e-3);
+
+    pcl::PointCloud<pcl::PointNormal> cloudA;
+    pcl::concatenate(floor, wallA, cloudA);
+    // Two perpendicular surfaces
+    complexity = util3d::computeNormalsComplexity(cloudA);
+    EXPECT_NEAR(complexity, 0.0f, 1e-3);
+
+    // Three perpendicular surfaces
+    pcl::PointCloud<pcl::PointNormal> cloudB;
+    pcl::concatenate(cloudA, wallB, cloudB);
+    
+    complexity = util3d::computeNormalsComplexity(cloudB);
+    EXPECT_NEAR(complexity, 0.25f, 1e-3);
+
+    // Three perpendicular surfaces (one small)
+    pcl::PointCloud<pcl::PointNormal> smallCloudB;
+    pcl::concatenate(cloudA, smallWallB, smallCloudB);
+    
+    complexity = util3d::computeNormalsComplexity(smallCloudB);
+    EXPECT_LT(complexity, 0.25f);
+    EXPECT_GT(complexity, 0.01f);
+}
+
+TEST(Util3dSurface, computeNormalsComplexityIdentityVsRotated)
+{
+    auto cloud = createFlatNormalCloud(50, cv::Point3f(0.0f, 1.0f, 0.0f));
+    Transform identity = Transform::getIdentity();
+    Transform rotated = Transform(0,0,0,0,M_PI / 4,0);
+
+    float c1 = util3d::computeNormalsComplexity(cloud, identity, false, nullptr, nullptr);
+    float c2 = util3d::computeNormalsComplexity(cloud, rotated, false, nullptr, nullptr);
+
+    EXPECT_NEAR(c1, c2, 1e-5); // rotation should not affect complexity
+}
+
+TEST(Util3dSurface, computeNormalsComplexityEmptyOrInvalidNormals)
+{
+    pcl::PointCloud<pcl::PointNormal> cloud;
+    pcl::PointNormal pt;
+    pt.normal_x = std::numeric_limits<float>::quiet_NaN();
+    pt.normal_y = 0.0f;
+    pt.normal_z = 0.0f;
+    cloud.push_back(pt);
+
+    float complexity = util3d::computeNormalsComplexity(cloud, Transform(), false, nullptr, nullptr);
+    EXPECT_EQ(complexity, 0.0f); // Should return 0 when all normals are invalid
+}
+
+TEST(Util3dSurface, computeNormalsComplexityVaryingNormals2D)
+{
+    auto wallA = createFlatNormalCloud(100, cv::Point3f(0.0f, 1.0f, 0.0f));
+    auto negWallA = createFlatNormalCloud(100, cv::Point3f(0.0f, -1.0f, 0.0f));
+    auto wallB = createFlatNormalCloud(100, cv::Point3f(1.0f, 0.0f, 0.0f));
+    auto smalllWallB = createFlatNormalCloud(10, cv::Point3f(1.0f, 0.0f, 0.0f));
+
+    // One flat surface
+    float complexity = util3d::computeNormalsComplexity(wallA, Transform(), true);
+    EXPECT_NEAR(complexity, 0.0f, 1e-3);
+
+    complexity = util3d::computeNormalsComplexity(wallB, Transform(), true);
+    EXPECT_NEAR(complexity, 0.0f, 1e-3);
+
+    pcl::PointCloud<pcl::PointNormal> cloud;
+    pcl::concatenate(wallA, wallB, cloud);
+    // Two perpendicular surfaces
+    complexity = util3d::computeNormalsComplexity(cloud, Transform(), true);
+    EXPECT_NEAR(complexity, 0.25f, 1e-3);
+
+    pcl::PointCloud<pcl::PointNormal> cloudB;
+    pcl::concatenate(wallA, smalllWallB, cloudB);
+    // Two perpendicular surfaces (one small)
+    complexity = util3d::computeNormalsComplexity(cloudB, Transform(), true);
+    EXPECT_LT(complexity, 0.25f);
+    EXPECT_GT(complexity, 0.01f);
+
+    pcl::PointCloud<pcl::PointNormal> corridorLikeCloud;
+    pcl::concatenate(wallA, negWallA, corridorLikeCloud);
+    // Two parallel surfaces simulating a corridor
+    cv::Mat vector,values;
+    complexity = util3d::computeNormalsComplexity(corridorLikeCloud, Transform(), true, &vector, &values);
+    EXPECT_NEAR(complexity, 0.0f, 1e-3);
+    EXPECT_NEAR(vector.at<float>(0,0), 0, 1e-3);
+    EXPECT_NEAR(vector.at<float>(0,1), 1, 1e-3); // first eigen vector should be aligned with the normals
+}