i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-03 16:47:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

minimal util3d_surface.h

Browse Source
This commit is contained in:
matlabbe
2025-07-06 12:02:09 -07:00
parent d558c3a93b
commit b8d253129c
3 changed files with 169 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/util3d_surface.h
+44
View File
@@ -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
+6 -1
View File
@@ -44,4 +44,9 @@ gtest_discover_tests(test_util3d_mapping)
#util3d_motion_estimation.h
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)
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
+119
View File
@@ -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
}