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

Added RegistrationIcp tests

Browse Source
This commit is contained in:
matlabbe
2026-06-06 18:43:51 -07:00
parent 0fe9718c28
commit 92f75c76b2
13 changed files with 1528 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Parameters.h
+2 -1
View File
@@ -832,7 +832,8 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Icp, PointToPlaneRadius, float, 0.0, "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
RTABMAP_PARAM(Icp, PointToPlaneGroundNormalsUp, float, 0.0, "Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans.");
RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.02, uFormat("Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when %s=true.", kIcpPointToPlaneLowComplexityStrategy().c_str(), kIcpPointToPlane().c_str()));
RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept \"as is\" the transform computed by PointToPoint.", kIcpPointToPlaneMinComplexity().c_str()));
RTABMAP_PARAM(Icp, PointToPlaneComplexityCentered, bool, false, uFormat("If false (default), the complexity metric uses the uncentered second-moment matrix (1/N) * sum(n_i * n_i^T), whose smallest eigenvalue directly measures how well the surface normals span R^N. If true, uses centered PCA (cv::PCA covariance) for backwards compatibility -- but the centered metric is known to mis-classify perpendicular-surface scenes as degenerate when normals are consistently viewpoint-flipped (only N distinct directions in N-D collapse to rank N-1 after centering). For true degeneracies (parallel surfaces, e.g. corridors) the two metrics agree because the normal mean is zero. The %s threshold of 0.02 works under either setting.", kIcpPointToPlaneMinComplexity().c_str()));
RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 to so that the transform is automatically rejected, set to 1 to keep the PointToPlane transform and limit its correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to recompute the transform with PointToPoint and accept it \"as is\", set to 3 for the legacy behavior of recomputing with PointToPoint then applying the same constraint as strategy 1.", kIcpPointToPlaneMinComplexity().c_str()));
RTABMAP_PARAM(Icp, OutlierRatio, float, 0.85, uFormat("Outlier ratio. For libpointmatcher (%s=1), sets TrimmedDistOutlierFilter/ratio for convenience when configuration file is not set. For CCCoreLib (%s=2), sets \"finalOverlapRatio\". For PCL (%s=0), if 0<value<1, installs a RANSAC correspondence rejector with inlier threshold = value * %s. The value should be between 0 and 1.", kIcpStrategy().c_str(), kIcpStrategy().c_str(), kIcpStrategy().c_str(), kIcpMaxCorrespondenceDistance().c_str()));
RTABMAP_PARAM_STR(Icp, DebugExportFormat, "", "Export scans used for ICP in the specified format (a warning on terminal will be shown with the file paths used). Supported formats are \"pcd\", \"ply\" or \"vtk\". If logger level is debug, from and to scans will stamped, so previous files won't be overwritten.");
corelib/include/rtabmap/core/RegistrationIcp.h
+27 -1
View File
@@ -39,8 +39,33 @@ namespace rtabmap {
class RTABMAP_CORE_EXPORT RegistrationIcp : public Registration
{
public:
enum IcpStrategy {
kIcpUndef = -1, /**< Undefined / invalid type. */
kIcpPCL = 0, /**< Point Cloud Library. */
kIcpPointMatcher = 1, /**< libpointmatcher. */
kIcpCCCoreLib = 2, /**< CCCoreLib (Cloud Compare). */
kIcpEnd = 3 /**< Sentinel: always keep last. Used to iterate through strategies. */
};
public:
/**
* @brief Returns true if @p strategy is built into this rtabmap binary.
*
* @ref kPCL is always available; @ref kPointMatcher requires the
* RTABMAP_POINTMATCHER build flag; @ref kCCCoreLib requires
* RTABMAP_CCCORELIB.
*/
static bool available(IcpStrategy strategy);
/**
* @brief Human-readable name for @p strategy ("PCL", "libpointmatcher",
* "CCCoreLib", or "Unknown").
*/
static const char * strategyName(IcpStrategy strategy);
// take ownership of child
RegistrationIcp(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
RegistrationIcp(IcpStrategy strategy, const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
virtual ~RegistrationIcp();
virtual void parseParameters(const ParametersMap & parameters);
@@ -56,7 +81,7 @@ protected:
virtual float getMinGeometryCorrespondencesRatioImpl() const {return _correspondenceRatio;}
private:
int _strategy;
IcpStrategy _strategy;
float _maxTranslation;
float _maxRotation;
float _voxelSize;
@@ -75,6 +100,7 @@ private:
float _pointToPlaneRadius;
float _pointToPlaneGroundNormalsUp;
float _pointToPlaneMinComplexity;
bool _pointToPlaneComplexityCentered;
int _pointToPlaneLowComplexityStrategy;
std::string _libpointmatcherConfig;
int _libpointmatcherKnn;
corelib/include/rtabmap/core/RegistrationInfo.h
+4 -1
View File
@@ -61,7 +61,8 @@ public:
icpStructuralComplexity(0.0f),
icpStructuralDistribution(0.0f),
icpCorrespondences(0),
icpRMS(0)
icpRMS(0),
icpIterations(-1)
{
}
@@ -87,6 +88,7 @@ public:
output.icpStructuralDistribution = icpStructuralDistribution;
output.icpCorrespondences = icpCorrespondences;
output.icpRMS = icpRMS;
output.icpIterations = icpIterations;
return output;
}
@@ -115,6 +117,7 @@ public:
float icpStructuralDistribution; /**< Distribution of structural features in the overlap. */
int icpCorrespondences; /**< Number of ICP point correspondences. */
float icpRMS; /**< Root-mean-square error of ICP correspondences (m). */
int icpIterations; /**< Number of ICP iterations actually performed (-1 if unavailable from backend). */
};
}
corelib/include/rtabmap/core/util3d_registration.h
+8 -4
View File
@@ -215,7 +215,8 @@ Transform RTABMAP_CORE_EXPORT icp(
pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
float epsilon = 0.0f,
bool icp2D = false,
float ransacOutlierRatio = 0.0f);
float ransacOutlierRatio = 0.0f,
int * iterationsDone = nullptr);
/**
* @brief Performs Iterative Closest Point (ICP) alignment between two point clouds and returns the resulting transform.
* @see util3d::icp()
@@ -233,7 +234,8 @@ Transform RTABMAP_CORE_EXPORT icp(
pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
float epsilon = 0.0f,
bool icp2D = false,
float ransacOutlierRatio = 0.0f);
float ransacOutlierRatio = 0.0f,
int * iterationsDone = nullptr);
/**
* @brief Performs Iterative Closest Point (ICP) alignment using a point-to-plane error metric.
@@ -269,7 +271,8 @@ Transform RTABMAP_CORE_EXPORT icpPointToPlane(
pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
float epsilon = 0.0f,
bool icp2D = false,
float ransacOutlierRatio = 0.0f);
float ransacOutlierRatio = 0.0f,
int * iterationsDone = nullptr);
/**
* @briefPerforms Iterative Closest Point (ICP) alignment using a point-to-plane error metric.
* @see util3d::icpPointToPlane()
@@ -287,7 +290,8 @@ Transform RTABMAP_CORE_EXPORT icpPointToPlane(
pcl::PointCloud<pcl::PointXYZINormal> & cloud_source_registered,
float epsilon = 0.0f,
bool icp2D = false,
float ransacOutlierRatio = 0.0f);
float ransacOutlierRatio = 0.0f,
int * iterationsDone = nullptr);
} // namespace util3d
} // namespace rtabmap
corelib/include/rtabmap/core/util3d_surface.h
+16 -5
View File
@@ -427,17 +427,25 @@ float RTABMAP_CORE_EXPORT computeNormalsComplexity(
const LaserScan & scan,
const Transform & t = Transform::getIdentity(),
cv::Mat * pcaEigenVectors = 0,
cv::Mat * pcaEigenValues = 0);
cv::Mat * pcaEigenValues = 0,
bool centered = true);
/**
* @ingroup ComputeNormalsComplexity
* @brief Computes the complexity of surface normals in a point cloud of type `pcl::Normal`.
*
* @param centered When true (default), use covariance PCA (centered at mean).
* Use false for the uncentered second-moment matrix `M = (1/N) sum(n_i * n_i^T)`,
* which measures span of normal directions and correctly identifies degeneracy
* even when there are only N (rather than N+1) distinct viewpoint-flipped
* normal directions in N-D space.
*/
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);
cv::Mat * pcaEigenValues = 0,
bool centered = true);
/**
* @ingroup ComputeNormalsComplexity
* @brief Computes the complexity of surface normals in a point cloud of type `pcl::PointNormal`.
@@ -447,7 +455,8 @@ float RTABMAP_CORE_EXPORT computeNormalsComplexity(
const Transform & t = Transform::getIdentity(),
bool is2d = false,
cv::Mat * pcaEigenVectors = 0,
cv::Mat * pcaEigenValues = 0);
cv::Mat * pcaEigenValues = 0,
bool centered = true);
/**
* @ingroup ComputeNormalsComplexity
* @brief Computes the complexity of surface normals in a point cloud of type `pcl::PointXYZINormal`.
@@ -457,7 +466,8 @@ float RTABMAP_CORE_EXPORT computeNormalsComplexity(
const Transform & t = Transform::getIdentity(),
bool is2d = false,
cv::Mat * pcaEigenVectors = 0,
cv::Mat * pcaEigenValues = 0);
cv::Mat * pcaEigenValues = 0,
bool centered = true);
/**
* @ingroup ComputeNormalsComplexity
* @brief Computes the complexity of surface normals in a point cloud of type `pcl::PointXYZRGBNormal`.
@@ -467,7 +477,8 @@ float RTABMAP_CORE_EXPORT computeNormalsComplexity(
const Transform & t = Transform::getIdentity(),
bool is2d = false,
cv::Mat * pcaEigenVectors = 0,
cv::Mat * pcaEigenValues = 0);
cv::Mat * pcaEigenValues = 0,
bool centered = true);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_CORE_EXPORT mls(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,