Category Archives: PCL

Equivalent ways to register two point clouds with known pairing in PCL

Given the 2 point clouds:

pcl::PointCloud<pcl::PointXYZ>::Ptr XPm1 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr XPm2 (new pcl::PointCloud<pcl::PointXYZ>);

the following three ways to register the two point clouds by means of a rigid transformation are equivalent :

1) Singular Value Decomposition-based estimation of the rigid transform

pcl::registration::TransformationEstimationSVD <pcl::PointXYZ, pcl::PointXYZ> te;
Eigen::Matrix4f T;

te.estimateRigidTransformation (*XPm2, *XPm1, T);

2) explicit Levenberg Marquardt-based estimation used in the ICP algorithm

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

icp.setInputSource (XPm2);
icp.setInputTarget (XPm1);

typedef pcl::registration::TransformationEstimationLM <pcl::PointXYZ, pcl::PointXYZ> te;
boost::shared_ptr<te> teLM (new te);
icp.setTransformationEstimation (teLM);

pcl::PointCloud<pcl::PointXYZ> Final;
icp.align (Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;

T = icp.getFinalTransformation();

3) implicit Levenberg Marquardt-based estimation used in the ICP algorithm

pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> icp;

icp.setInputSource (XPm2);
icp.setInputTarget (XPm1);

pcl::PointCloud<pcl::PointXYZ> Final;
icp.align (Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;

T = icp.getFinalTransformation();

Advertisements