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

Added util3d_registration tests

Browse Source
This commit is contained in:
matlabbe
2025-06-21 20:12:02 -07:00
parent 9335b9d816
commit cb6cdcae90
4 changed files with 611 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/util3d_registration.h
+149
View File
@@ -45,10 +45,59 @@ int RTABMAP_CORE_EXPORT getCorrespondencesCount(const pcl::PointCloud<pcl::Point
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
float maxDistance);
/**
* @brief Estimates the rigid 3D transformation between two point clouds using SVD.
*
* This function computes the transformation (rotation and translation) that best aligns
* `cloud2` to `cloud1` using Singular Value Decomposition (SVD) based on point correspondences.
* It assumes a one-to-one correspondence between points in the two clouds.
*
* Internally, it uses PCL's `TransformationEstimationSVD` to compute the 4x4 transformation matrix,
* which is then converted to a `Transform` object.
*
* @param cloud1 Target point cloud (reference frame).
* @param cloud2 Source point cloud to be aligned with `cloud1`.
* It must have the same number of points as `cloud1`, and the points should
* correspond to each other by index.
*
* @return A `Transform` representing the rigid-body transformation from `cloud1` to `cloud2`.
*
* @note This function does not perform any outlier rejection or correspondence estimation—
* it assumes that the input clouds are already matched appropriately.
*/
Transform RTABMAP_CORE_EXPORT transformFromXYZCorrespondencesSVD(
const pcl::PointCloud<pcl::PointXYZ> & cloud1,
const pcl::PointCloud<pcl::PointXYZ> & cloud2);
/**
* @brief Estimates a rigid transformation between two point clouds using RANSAC with optional refinement.
*
* This function finds a 3D rigid-body transform from `cloud1` to `cloud2` using one-to-one point correspondences.
* It applies a RANSAC-based outlier rejection and optionally refines the transformation with iterative model optimization.
*
* It also optionally returns the inlier indices used to compute the final model and an approximate 6x6 covariance matrix
* of the transform.
*
* @param cloud1 Target point cloud (reference frame). Must contain at least 3 points and match `cloud2` in size.
* @param cloud2 Source point cloud to align to `cloud1`. Must be the same size as `cloud1`.
* @param inlierThreshold Maximum Euclidean distance (in meters) between corresponding points for them to be considered inliers.
* @param iterations Number of RANSAC iterations to perform.
* @param refineIterations Number of refinement steps to perform after the initial RANSAC.
* If set to 0, no refinement is done.
* @param refineSigma Multiplier for standard deviation used to adjust the inlier threshold during refinement.
* @param inliersOut Optional pointer to a vector that will receive the indices of the inlier correspondences.
* @param covariance Optional pointer to a 6x6 covariance matrix of the estimated transform (as `CV_64FC1`).
* Will be identity if set and no inliers are found.
*
* @return A `Transform` representing the estimated pose from `cloud1` to `cloud2`.
* If no valid model is found, the returned transform will be identity.
*
* @note This function assumes a one-to-one correspondence between points in the two clouds
* (e.g., index `i` in `cloud1` corresponds to index `i` in `cloud2`).
* @note If fewer than 3 points are provided or point counts do not match, the identity transform is returned.
*
* @warning Inlier refinement is sensitive to oscillation and may stop early if alternating inlier counts are detected.
*/
Transform RTABMAP_CORE_EXPORT transformFromXYZCorrespondences(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud1,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
@@ -59,6 +108,36 @@ Transform RTABMAP_CORE_EXPORT transformFromXYZCorrespondences(
std::vector<int> * inliers = 0,
cv::Mat * variance = 0);
/**
* @defgroup ComputeVarianceAndCorrespondences Compute Variance and Correspondences of Two Point Clouds
* @brief Computes the geometric variance and number of valid correspondences between two point clouds.
*
* This function estimates correspondences between two point clouds and computes a variance
* measure (based on squared median error) for the inlier correspondences. Optionally filters correspondences by
* normal alignment angle if the point cloud has normals.
*
* The correspondence estimation uses PCL's reciprocal correspondence logic by default. The target and source clouds are automatically
* chosen based on their sizes (the larger becomes the target to ensure optimal matching behavior).
*
* @param cloudA First input point cloud (used interchangeably with cloudB for matching).
* @param cloudB Second input point cloud.
* @param maxCorrespondenceDistance Maximum allowable Euclidean distance between correspondences.
* @param maxCorrespondenceAngle Maximum angle (in radians) between normals for correspondences to be considered valid.
* If ≤ 0, angle filtering is disabled.
* @param[out] variance Output variance value (estimated using 2.1981 × median squared distance of inliers).
* @param[out] correspondencesOut Output number of correspondences that passed all filters (distance and optional angle).
* @param reciprocal If true, use reciprocal correspondences.
*
* @note This function chooses the target/source roles based on cloud size (the larger becomes the target).
* @note The normal comparison is only applied if `maxCorrespondenceAngle > 0`.
* @note The computed `variance` is based on a robust estimator using the median squared distance of correspondences.
*
* @see pcl::registration::CorrespondenceEstimation
*/
/**
* @ingroup ComputeVarianceAndCorrespondences
* @brief Compute with variance and correspondences of `pcl::PointNormal` point cloud type.
*/
void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
@@ -67,6 +146,10 @@ void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
double & variance,
int & correspondencesOut,
bool reciprocal);
/**
* @ingroup ComputeVarianceAndCorrespondences
* @brief Compute with variance and correspondences of `pcl::PointXYZINormal` point cloud type.
*/
void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudB,
@@ -75,6 +158,10 @@ void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
double & variance,
int & correspondencesOut,
bool reciprocal);
/**
* @ingroup ComputeVarianceAndCorrespondences
* @brief Compute with variance and correspondences of `pcl::PointXYZ` point cloud type.
*/
void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
@@ -82,6 +169,10 @@ void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
double & variance,
int & correspondencesOut,
bool reciprocal);
/**
* @ingroup ComputeVarianceAndCorrespondences
* @brief Compute with variance and correspondences of `pcl::PointXYZI` point cloud type.
*/
void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudB,
@@ -90,6 +181,31 @@ void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(
int & correspondencesOut,
bool reciprocal);
/**
* @brief Performs Iterative Closest Point (ICP) alignment between two point clouds and returns the resulting transform.
*
* This function aligns the `cloud_source` to the `cloud_target` using PCL's ICP algorithm. It optionally supports 2D ICP,
* which constrains the estimated transformation to the XY-plane with rotation about the Z-axis.
*
* The result is returned as a `Transform` representing the transformation from source to target.
* The aligned version of the source cloud is written into `cloud_source_registered`.
*
* @param cloud_source The input source point cloud to align.
* @param cloud_target The input target point cloud to align to.
* @param maxCorrespondenceDistance Maximum distance threshold for point correspondences.
* @param maximumIterations Maximum number of ICP iterations to perform.
* @param[out] hasConverged Set to true if ICP converged to a solution; false otherwise.
* @param[out] cloud_source_registered Output point cloud containing the source aligned to the target.
* @param epsilon Convergence threshold for transformation changes between iterations (applied as squared value).
* @param icp2D If true, enforces 2D ICP using only XY translation and Z rotation (ignores Z and X/Y rotation).
*
* @return Transform The estimated transformation from `cloud_source` to `cloud_target`.
*
* @note All input points in both clouds must be finite (i.e., no NaNs or infinite values).
*
* @see pcl::IterativeClosestPoint
* @see pcl::registration::TransformationEstimation2D
*/
Transform RTABMAP_CORE_EXPORT icp(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
@@ -99,6 +215,10 @@ Transform RTABMAP_CORE_EXPORT icp(
pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
float epsilon = 0.0f,
bool icp2D = false);
/**
* @brief Performs Iterative Closest Point (ICP) alignment between two point clouds and returns the resulting transform.
* @see util3d::icp()
*/
Transform RTABMAP_CORE_EXPORT icp(
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_target,
@@ -109,6 +229,31 @@ Transform RTABMAP_CORE_EXPORT icp(
float epsilon = 0.0f,
bool icp2D = false);
/**
* @brief Performs Iterative Closest Point (ICP) alignment using a point-to-plane error metric.
*
* This function aligns a source point cloud to a target point cloud using PCL's
* point-to-plane ICP implementation with a linear least squares estimator. It returns
* the estimated transformation from the source to the target.
*
* Optionally, if `icp2D` is true, the resulting transformation is projected to 3DoF (XY translation and rotation about Z).
*
* @param cloud_source Input source point cloud with normals.
* @param cloud_target Input target point cloud with normals.
* @param maxCorrespondenceDistance Maximum distance for considering point correspondences.
* @param maximumIterations Maximum number of ICP iterations to perform.
* @param[out] hasConverged Set to true if the ICP algorithm successfully converged.
* @param[out] cloud_source_registered Output cloud representing the aligned source.
* @param epsilon Convergence threshold for the transformation change (used as squared value).
* @param icp2D If true, the result is projected to 2D (XY + Yaw only).
*
* @return The transformation from the source to the target cloud.
*
* @note All points and normals in both input clouds must be finite (no NaNs or infinities).
*
* @see pcl::IterativeClosestPoint
* @see pcl::registration::TransformationEstimationPointToPlaneLLS
*/
Transform RTABMAP_CORE_EXPORT icpPointToPlane(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
@@ -118,6 +263,10 @@ Transform RTABMAP_CORE_EXPORT icpPointToPlane(
pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
float epsilon = 0.0f,
bool icp2D = false);
/**
* @briefPerforms Iterative Closest Point (ICP) alignment using a point-to-plane error metric.
* @see util3d::icpPointToPlane()
*/
Transform RTABMAP_CORE_EXPORT icpPointToPlane(
const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_target,
corelib/src/util3d_registration.cpp
+16 -3
View File
@@ -51,6 +51,7 @@ Transform transformFromXYZCorrespondencesSVD(
const pcl::PointCloud<pcl::PointXYZ> & cloud1,
const pcl::PointCloud<pcl::PointXYZ> & cloud2)
{
UASSERT(cloud1.size() == cloud2.size());
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> svd;
// Perform the alignment
@@ -256,7 +257,13 @@ void computeVarianceAndCorrespondencesImpl(
est->setInputTarget(target);
est->setInputSource(source);
pcl::Correspondences correspondences;
est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
if(reciprocal) {
est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
}
else {
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
}
if(correspondences.size())
{
@@ -340,7 +347,12 @@ void computeVarianceAndCorrespondencesImpl(
est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
pcl::Correspondences correspondences;
est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
if(reciprocal) {
est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
}
else {
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
}
if(correspondences.size()>=3)
{
@@ -411,7 +423,7 @@ Transform icpImpl(const typename pcl::PointCloud<PointT>::ConstPtr & cloud_sourc
// Set the transformation epsilon (criterion 2)
icp.setTransformationEpsilon (epsilon*epsilon);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
//icp.setEuclideanFitnessEpsilon (-std::numeric_limits<double>::max());
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
// Perform the alignment
@@ -486,6 +498,7 @@ Transform icpPointToPlaneImpl(
{
// FIXME probably an estimation approach already 2D like in icp() version above exists.
t = t.to3DoF();
pcl::transformPointCloudWithNormals(*cloud_source, cloud_source_registered, t.toEigen4f());
}
return t;
corelib/test/CMakeLists.txt
+6 -1
View File
@@ -19,4 +19,9 @@ gtest_discover_tests(test_util3d_transforms)
#util3d_filtering.h
add_executable(test_util3d_filtering test_util3d_filtering.cpp)
target_link_libraries(test_util3d_filtering gtest_main rtabmap_core)
gtest_discover_tests(test_util3d_filtering)
gtest_discover_tests(test_util3d_filtering)
#util3d_registration.h
add_executable(test_util3d_registration test_util3d_registration.cpp)
target_link_libraries(test_util3d_registration gtest_main rtabmap_core)
gtest_discover_tests(test_util3d_registration)
corelib/test/test_util3d_registration.cpp
+440
View File
@@ -0,0 +1,440 @@
#include "gtest/gtest.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/utilite/UException.h"
#include <pcl/common/impl/angles.hpp>
#include <pcl/common/io.h>
#include <pcl/io/pcd_io.h>
using namespace rtabmap;
TEST(Util3DRegistration, transformFromXYZCorrespondencesSVDIdentityTransform)
{
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
cloud1.push_back(pcl::PointXYZ(1, 2, 3));
cloud1.push_back(pcl::PointXYZ(4, 5, 6));
cloud1.push_back(pcl::PointXYZ(7, 8, 9));
cloud1.push_back(pcl::PointXYZ(10, 11, 12));
cloud1.push_back(pcl::PointXYZ(1, 6, 12));
cloud2 = cloud1; // Exact same points
Transform result = util3d::transformFromXYZCorrespondencesSVD(cloud1, cloud2);
Transform identity = Transform::getIdentity();
EXPECT_LT(result.getDistance(identity), 0.001f);
EXPECT_LT(result.getAngle(identity), 0.001f);
}
TEST(Util3DRegistration, transformFromXYZCorrespondencesSVDTranslationOnly)
{
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
cloud1.push_back(pcl::PointXYZ(0, 0, 0));
cloud1.push_back(pcl::PointXYZ(1, 0, 0));
cloud1.push_back(pcl::PointXYZ(0, 1, 0));
cloud2.push_back(pcl::PointXYZ(1, 2, 3));
cloud2.push_back(pcl::PointXYZ(2, 2, 3));
cloud2.push_back(pcl::PointXYZ(1, 3, 3));
Transform result = util3d::transformFromXYZCorrespondencesSVD(cloud1, cloud2);
EXPECT_LT(result.getAngle(Transform::getIdentity()), 0.001f);
EXPECT_NEAR(result.x(), 1.0f, 1e-4f);
EXPECT_NEAR(result.y(), 2.0f, 1e-4f);
EXPECT_NEAR(result.z(), 3.0f, 1e-4f);
}
TEST(Util3DRegistration, transformFromXYZCorrespondencesSVDRotationAndTranslation)
{
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
// Original triangle
cloud1.push_back(pcl::PointXYZ(1, 0, 0));
cloud1.push_back(pcl::PointXYZ(0, 1, 0));
cloud1.push_back(pcl::PointXYZ(0, 0, 1));
// Apply known rotation (90° about Z) and translation (1, 2, 3)
Eigen::Matrix3f R;
R = Eigen::AngleAxisf(M_PI_2, Eigen::Vector3f::UnitZ());
Eigen::Vector3f t(1, 2, 3);
for (const auto & pt : cloud1.points)
{
Eigen::Vector3f p(pt.x, pt.y, pt.z);
p = R * p + t;
cloud2.push_back(pcl::PointXYZ(p[0], p[1], p[2]));
}
Transform result = util3d::transformFromXYZCorrespondencesSVD(cloud1, cloud2);
Eigen::Matrix4f expected = Eigen::Matrix4f::Identity();
expected.block<3,3>(0,0) = R;
expected.block<3,1>(0,3) = t;
Transform expected_t = Transform::fromEigen4f(expected);
EXPECT_LT(result.getAngle(expected_t), 0.001f);
EXPECT_NEAR(result.x(), expected_t.x(), 1e-4f);
EXPECT_NEAR(result.y(), expected_t.y(), 1e-4f);
EXPECT_NEAR(result.z(), expected_t.z(), 1e-4f);
}
TEST(Util3DRegistration, transformFromXYZCorrespondencesSVDMismatchedSizes)
{
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
cloud1.push_back(pcl::PointXYZ(0, 0, 0));
cloud1.push_back(pcl::PointXYZ(1, 1, 1));
cloud2.push_back(pcl::PointXYZ(0, 0, 0)); // Only 1 point
EXPECT_THROW(util3d::transformFromXYZCorrespondencesSVD(cloud1, cloud2), UException);
}
TEST(Util3DRegistration, transformFromXYZCorrespondencesIdentityTransform)
{
auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
cloud1->push_back(pcl::PointXYZ(1, 0, 0));
cloud1->push_back(pcl::PointXYZ(0, 1, 0));
cloud1->push_back(pcl::PointXYZ(0, 0, 1));
auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>(*cloud1);
std::vector<int> inliers;
cv::Mat covariance;
Transform result = util3d::transformFromXYZCorrespondences(
cloud1, cloud2, 0.01, 100, 0, 1.0, &inliers, &covariance);
Transform identity = Transform::getIdentity();
EXPECT_LT(result.getDistance(identity), 0.001f);
EXPECT_LT(result.getAngle(identity), 0.001f);
EXPECT_EQ(inliers.size(), 3);
EXPECT_EQ(covariance.rows, 6);
EXPECT_EQ(covariance.cols, 6);
}
TEST(Util3DRegistration, transformFromXYZCorrespondencesTranslatedCloud)
{
auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
Eigen::Vector3f t(1.0f, 2.0f, 3.0f);
for (int i = 0; i < 3; ++i)
{
pcl::PointXYZ p(i, i * 2, i * 3);
cloud1->push_back(p);
cloud2->push_back(pcl::PointXYZ(p.x + t.x(), p.y + t.y(), p.z + t.z()));
}
std::vector<int> inliers;
cv::Mat covariance;
Transform result = util3d::transformFromXYZCorrespondences(
cloud1, cloud2, 0.1, 100, 0, 1.0, &inliers, &covariance);
EXPECT_LT(result.getAngle(Transform::getIdentity()), 0.001f);
EXPECT_NEAR(result.x(), t.x(), 1e-4f);
EXPECT_NEAR(result.y(), t.y(), 1e-4f);
EXPECT_NEAR(result.z(), t.z(), 1e-4f);
EXPECT_EQ(inliers.size(), 3);
}
TEST(Util3DRegistration, transformFromXYZCorrespondencesTooFewPoints)
{
auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
cloud1->push_back(pcl::PointXYZ(0, 0, 0));
cloud2->push_back(pcl::PointXYZ(1, 1, 1));
Transform result = util3d::transformFromXYZCorrespondences(
cloud1, cloud2, 0.1, 100, 0, 1.0, nullptr, nullptr);
EXPECT_TRUE(result.isNull());
}
TEST(Util3DRegistration, transformFromXYZCorrespondencesMismatchedPointCounts)
{
auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
cloud1->push_back(pcl::PointXYZ(0, 0, 0));
cloud1->push_back(pcl::PointXYZ(1, 0, 0));
cloud1->push_back(pcl::PointXYZ(0, 1, 0));
cloud2->push_back(pcl::PointXYZ(0, 0, 0)); // Only one point
Transform result = util3d::transformFromXYZCorrespondences(
cloud1, cloud2, 0.1, 100, 0, 1.0, nullptr, nullptr);
EXPECT_TRUE(result.isNull());
}
TEST(Util3DRegistration, computeVarianceAndCorrespondencesPerfectMatchNoAngleCheck)
{
auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
for (int i = 0; i < 5; ++i)
{
pcl::PointNormal pt;
pt.x = i; pt.y = i; pt.z = i;
pt.normal_x = 1; pt.normal_y = 0; pt.normal_z = 0;
cloud1->push_back(pt);
cloud2->push_back(pt); // identical
}
double variance = -1.0;
int correspondences = -1;
util3d::computeVarianceAndCorrespondences(
cloud1, cloud2, 0.1, -1.0, variance, correspondences, true);
EXPECT_EQ(correspondences, 5);
EXPECT_DOUBLE_EQ(variance, 0.0);
}
TEST(Util3DRegistration, computeVarianceAndCorrespondencesNormalMismatchFilteredByAngle)
{
auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
for (int i = 0; i < 5; ++i)
{
pcl::PointNormal a, b;
a.x = b.x = i; a.y = b.y = i; a.z = b.z = i;
a.normal_x = 1; a.normal_y = 0; a.normal_z = 0;
b.normal_x = 0; b.normal_y = 1; b.normal_z = 0; // orthogonal normals
cloud1->push_back(a);
cloud2->push_back(b);
}
double variance = -1.0;
int correspondences = -1;
util3d::computeVarianceAndCorrespondences(
cloud1, cloud2, 0.1, pcl::deg2rad(45.0), variance, correspondences, true);
EXPECT_EQ(correspondences, 0);
EXPECT_DOUBLE_EQ(variance, 1.0); // untouched default value
}
TEST(Util3DRegistration, computeVarianceAndCorrespondencesAnglePassWithLargeThreshold)
{
auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
for (int i = 0; i < 5; ++i)
{
pcl::PointNormal a, b;
a.x = b.x = i; a.y = b.y = i; a.z = b.z = i;
a.normal_x = 1; a.normal_y = 0; a.normal_z = 0;
b.normal_x = 0.7f; b.normal_y = 0.7f; b.normal_z = 0;
cloud1->push_back(a);
cloud2->push_back(b);
}
double variance = -1.0;
int correspondences = -1;
util3d::computeVarianceAndCorrespondences(
cloud1, cloud2, 0.1, pcl::deg2rad(90.0), variance, correspondences, true);
EXPECT_EQ(correspondences, 5);
EXPECT_GE(variance, 0.0);
}
TEST(Util3DRegistration, computeVarianceAndCorrespondencesNoCorrespondencesDueToDistance)
{
auto cloud1 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
auto cloud2 = std::make_shared<pcl::PointCloud<pcl::PointNormal> >();
for (int i = 0; i < 5; ++i)
{
pcl::PointNormal pt1, pt2;
pt1.x = pt2.x = i;
pt1.y = pt2.y = i;
pt1.z = pt2.z = i;
pt1.normal_x = pt2.normal_x = 1;
pt1.normal_y = pt2.normal_y = 0;
pt1.normal_z = pt2.normal_z = 0;
pt2.x += 100; // make them too far apart
cloud1->push_back(pt1);
cloud2->push_back(pt2);
}
double variance = -1.0;
int correspondences = -1;
util3d::computeVarianceAndCorrespondences(
cloud1, cloud2, 0.5, 0.0, variance, correspondences, true);
EXPECT_EQ(correspondences, 0);
EXPECT_DOUBLE_EQ(variance, 1.0); // default untouched
}
TEST(Util3DRegistration, icpIdentityTransformConverges)
{
auto cloud_source = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
for (float i = 0; i < 5; ++i)
{
cloud_source->emplace_back(i, i * 2.0f, 0.0f);
}
auto cloud_target = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >(*cloud_source); // identical
bool hasConverged = false;
pcl::PointCloud<pcl::PointXYZ> aligned;
Transform result = util3d::icp(
cloud_source, cloud_target,
0.1, // max correspondence distance
50, // max iterations
hasConverged,
aligned,
1e-6f, // epsilon
false // 3D ICP
);
EXPECT_TRUE(hasConverged);
// ICP should return identity transform for identical clouds
Eigen::Matrix4f identity = Eigen::Matrix4f::Identity();
EXPECT_TRUE(result.toEigen4f().isApprox(identity, 1e-4));
}
TEST(Util3DRegistration, icpTranslatedTransformConverges)
{
auto cloud_source = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
for (float i = 0; i < 5; ++i)
{
cloud_source->emplace_back(i, i * 2.0f, 0);
}
// Translate the cloud
Transform transformGT(0.025, 0, 0.0f ,0,0,0);
auto cloud_target = util3d::transformPointCloud(cloud_source, transformGT);
bool hasConverged = false;
pcl::PointCloud<pcl::PointXYZ> aligned;
Transform result = util3d::icp(
cloud_source, cloud_target,
0.05, 100, hasConverged, aligned,
1e-6f,
false
);
EXPECT_TRUE(hasConverged);
std::cout << result << std::endl;
std::cout << transformGT << std::endl;
Eigen::Matrix4f estimated = result.toEigen4f();
Eigen::Matrix4f expected = transformGT.toEigen4f();
EXPECT_TRUE(estimated.isApprox(expected, 1e-2));
}
TEST(Util3DRegistration, icp2DAlignsFlatClouds)
{
pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
auto cloud_source = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
for (float x = 0; x < 5; ++x)
{
for (float y = 0; y < 5; ++y)
{
if(y == 0 || y == 2 || y == 4 || x==0 || x==2 || x== 4){
cloud_source->emplace_back(x*0.05, y*0.05, (int)x%2==0?0.01:-0.01);
}
}
}
Transform transformGT(0.075, 0.05, 0.01f, 0,0, M_PI / 8);
auto cloud_target = util3d::transformPointCloud(cloud_source, transformGT);
bool hasConverged = false;
pcl::PointCloud<pcl::PointXYZ> aligned;
Transform result = util3d::icp(
cloud_source, cloud_target,
0.15, 100, hasConverged, aligned,
1e-6f,
true // use ICP 2D
);
transformGT.z() = 0;
EXPECT_TRUE(hasConverged);
Eigen::Matrix4f estimated = result.toEigen4f();
Eigen::Matrix4f expected = transformGT.toEigen4f();
EXPECT_TRUE(estimated.isApprox(expected, 1e-4));
}
TEST(Util3DRegistration, icpPointToPlaneAlignsTranslatedPlane)
{
// Create a plane point cloud
auto cloud_source_raw = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
for (float x = -0.5f; x <= 0.5f; x += 0.1f)
{
for (float y = -0.5f; y <= 0.5f; y += 0.1f)
{
cloud_source_raw->emplace_back(x, y, int(x*10)%2==0&&int(y*10)%2==0?0.01:-0.01);
}
}
// Compute normals
auto normals = util3d::computeNormals(cloud_source_raw, 20, 0, Eigen::Vector3f(0,0,1));
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_source(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud_source_raw, *normals, *cloud_source);
// Apply known transformation
Transform gtTransform(0.2f, -0.1f, 0.05f, 0.1f, 0, 0.1f);
auto cloud_target = util3d::transformPointCloud(cloud_source, gtTransform);
bool hasConverged = false;
pcl::PointCloud<pcl::PointNormal> aligned;
Transform estimated = util3d::icpPointToPlane(
cloud_source,
cloud_target,
0.2, // maxCorrespondenceDistance
50, // iterations
hasConverged,
aligned,
1e-6f, // epsilon
false // icp2D
);
std::cout << gtTransform << std::endl;
std::cout << estimated << std::endl;
EXPECT_TRUE(hasConverged);
Eigen::Matrix4f estimatedMatrix = estimated.toEigen4f();
Eigen::Matrix4f expectedMatrix = gtTransform.toEigen4f();
EXPECT_TRUE(estimatedMatrix.isApprox(expectedMatrix, 1e-4));
hasConverged = false;
estimated = util3d::icpPointToPlane(
cloud_source,
cloud_target,
0.2, // maxCorrespondenceDistance
50, // iterations
hasConverged,
aligned,
1e-6f, // epsilon
true // icp2D
);
// remove z and roll
gtTransform = Transform(0.2f, -0.1f, 0, 0, 0, 0.1f);
EXPECT_TRUE(hasConverged);
estimatedMatrix = estimated.toEigen4f();
expectedMatrix = gtTransform.toEigen4f();
EXPECT_TRUE(estimatedMatrix.isApprox(expectedMatrix, 1e-4));
}