I am a beginner in PCL. I'm trying to use the Iterative Closest Point algorithm from PCL_ICP_exemple.
However, I've got irrelevant values in my final transformation Matrix. Moreover, the algorithm seems to converge into a local minimum so that the 2 point clouds are not correclty aligned.
I've try to follow the suggestion of this post ICP_stackOverFlow_issue, but don't seem to be better.
It would be much appreciated to have some help to find what I've done wrong. In parallel, I'm trying also to educate myself about registration algorithms, I've find this pdf: Leihui_Li_pdf
If you have other good resources about this subject, don't hesitate !
Here you can see still a shift in the alignment, but what is stranger is the transformation matrix
ros2 run pcl_process ICP
[pcl::PCDReader::read] Read a binary compressed file with 7676009 bytes compressed and 33177600 original.
[pcl::PCDReader::read] Loaded /home/luca/ros2_ws/smart_robot2/src/pcl_process/data/1189_kinect_v2_scene_1.pcd as a non-dense cloud in 57.2576 ms with 2073600 points. Available dimensions: x y z rgb.
Loaded 2073600 data points from the Scanned cloud file.
[pcl::PCDReader::read] Loaded /home/luca/ros2_ws/smart_robot2/src/pcl_process/data/1189_kinect_v2_scene_2_rottranslated.pcd as a dense cloud in 19.6859 ms with 1536984 points. Available dimensions: rgb x y z _.
Loaded 1536984 data points from the CAD cloud file.
Removed 1091988 NaN points from the cloud filtered cloud.
FLANN is not optimized for current index type. Will incur extra allocations and copy
Downsampled cloud size: 20156
Applying this rigid transformation to: cloud_in -> cloud_icp
Rotation matrix :
| 1.000 0.000 0.000 |
R = | 0.000 1.000 0.000 |
| 0.000 0.000 1.000 |
Translation vector :
t = < 0.000, 0.000, 0.000 >
FLANN is not optimized for current index type. Will incur extra allocations and copy
[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 1 out of 20.
[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 0.902629 rotation (cosine) and 5.978394 translation.
[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: 179769313486231570814527423731704356798070567525844996598917476803157260780028538760589558632766878171540458953514382464234321326889464182768467546703537516986049910576551282076245490090389328944075868508455133942304583236903222948165808559332123348274797826204144723168738177180919299881250404026184124858368.000000 / 4.493558.
[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 2 out of 20.
[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 0.996368 rotation (cosine) and 0.008724 translation.
...
[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 19 out of 20.
[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 0.999992 rotation (cosine) and 0.000005 translation.
[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: 0.020320 / 0.020285.
[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 20 out of 20.
Transformation is:
0.997115 -0.067143 -0.035445 -2.211335
0.065973 0.997270 -0.033202 -0.393120
0.037577 0.030768 0.998821 -0.570432
0.000000 0.000000 0.000000 1.000000
ICP has converged, score is 0.0202461
ICP transformation 20 : cloud_icp -> cloud_in
Rotation matrix :
| 0.997 0.000 0.000 |
R = | 1.874 -0.008 0.000 |
| -0.000 36893488147419103232.000 0.000 |
Translation vector :
t = < 0.000, 512.000, 0.000 >
I've try several different hyper-parameters but facing everytime irrelevant values or/and unalignment of the 2 point clouds. Also I'm putting the 2 PC very close to each other in the beginning so that the algorithm should 'better handle the minimisation problem'.
Hyper-parameters
int n_iterations = 50;
pcl::IterativeClosestPoint<PointT, PointT> icp;
icp.setInputSource(cloud_tr);
icp.setInputTarget(scan_cloud);
icp.setMaxCorrespondenceDistance(0.05); // 1e-5
icp.setMaximumIterations(n_iterations);
icp.setTransformationEpsilon(1e-8);
icp.setEuclideanFitnessEpsilon(0.1);
icp.setRANSACOutlierRejectionThreshold (0.01);
icp.setUseReciprocalCorrespondences (false);
icp.align(*cloud_icp);
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
// Define PointCloudT and PointT for clarity
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
int iterations = 30;
void print4x4Matrix (const Eigen::Matrix4d & matrix)
{
printf ("Rotation matrix :\n");
printf (" | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2));
printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2));
printf (" | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2));
printf ("Translation vector :\n");
printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3));
}
PointCloudT::Ptr do_ICP(const PointCloudT::Ptr& scan_cloud, const PointCloudT::Ptr& cad_cloud, Eigen::Matrix4d& transformation_matrix, const bool do_visu)
{
// The point clouds we will be using
PointCloudT::Ptr cloud_tr (new PointCloudT); // Transformed point cloud
PointCloudT::Ptr cloud_icp (new PointCloudT); // ICP output point cloud
// Display in terminal the transformation matrix
std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
print4x4Matrix (transformation_matrix);
// Executing the transformation
pcl::transformPointCloud (*cad_cloud, *cloud_tr, transformation_matrix);
int n_iterations = 50;
pcl::IterativeClosestPoint<PointT, PointT> icp;
icp.setInputSource(cloud_tr);
icp.setInputTarget(scan_cloud);
icp.setMaxCorrespondenceDistance(0.05); // 1e-5
icp.setMaximumIterations(n_iterations);
icp.setTransformationEpsilon(1e-8);
icp.setEuclideanFitnessEpsilon(0.1);
icp.setRANSACOutlierRejectionThreshold (0.01);
icp.setUseReciprocalCorrespondences (false);
icp.align(*cloud_icp);
if (icp.hasConverged ())
{
std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;
std::cout << "\nICP transformation " << icp.getMaximumIterations () << " : cloud_icp -> cloud_in" << std::endl;
auto final_transformation_matrix = icp.getFinalTransformation ().cast<double>();
print4x4Matrix (final_transformation_matrix);
}
else
{
PCL_ERROR ("\nICP has not converged.\n");
return (nullptr);
}
if (do_visu)
{
pcl::visualization::PCLVisualizer viewer("ICP Demo");
// Créer deux viewports
int v1(0), v2(1);
viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);
// Couleurs pour les nuages
pcl::visualization::PointCloudColorHandlerCustom<PointT> scan_color(scan_cloud, 255, 255, 255); // Blanc
pcl::visualization::PointCloudColorHandlerCustom<PointT> cad_color(cloud_tr, 20, 180, 20); // Vert
pcl::visualization::PointCloudColorHandlerCustom<PointT> icp_color(cloud_icp, 180, 20, 20); // Rouge
// Ajouter les nuages au viewport 1
viewer.addPointCloud(scan_cloud, scan_color, "scan_cloud_v1", v1);
viewer.addPointCloud(cloud_tr, cad_color, "cad_cloud_v1", v1);
// Ajouter les nuages au viewport 2
viewer.addPointCloud(scan_cloud, scan_color, "scan_cloud_v2", v2);
viewer.addPointCloud(cloud_icp, icp_color, "icp_cloud_v2", v2);
// Ajouter des textes explicatifs
viewer.addText("Blanc: Nuage de scan\nVert: Nuage CAD transformé", 10, 15, 16, 1.0, 1.0, 1.0, "info_v1", v1);
viewer.addText("Blanc: Nuage de scan\nRouge: Nuage CAD aligné par ICP", 10, 15, 16, 1.0, 1.0, 1.0, "info_v2", v2);
// Configurer la caméra
viewer.setBackgroundColor(0, 0, 0, v1);
viewer.setBackgroundColor(0, 0, 0, v2);
viewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
// Lancer la visualisation
while (!viewer.wasStopped()) {
viewer.spin();
}
}
return cloud_icp;
}
int main ()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr scan_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cad_cloud (new pcl::PointCloud<pcl::PointXYZ>);
auto success_scan_ply = pcl::io::loadPLYFile ("path/to/your/target/cloud", *scan_cloud);
if (success_scan_ply == -1) {
PCL_ERROR("Couldn't read the .ply file\n");
return -1;
}
std::cout << "Loaded " << scan_cloud->width * scan_cloud->height << " data points from the .ply file." << std::endl;
auto sucess_cad_ply = pcl::io::loadPLYFile ("path/to/your/source/cloud", *cad_cloud);
if (sucess_cad_ply == -1) {
PCL_ERROR("Couldn't read the .ply file\n");
return -1;
}
std::cout << "Loaded " << cad_cloud->width * cad_cloud->height << " data points from the .ply file." << std::endl;
// Defining a rotation matrix and translation vector
Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();
// A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
// double theta = 0 ; // The angle of rotation in radians
// transformation_matrix (0, 0) = std::cos (theta);
// transformation_matrix (0, 1) = -sin (theta);
// transformation_matrix (1, 0) = sin (theta);
// transformation_matrix (1, 1) = std::cos (theta);
// // A translation on Z axis (0.4 meters)
// transformation_matrix (0, 3) = 0.0; // x
// transformation_matrix (1, 3) = 0.0; // y
// transformation_matrix (2, 3) = 2.0; // z
// Some Pre-processing
// 1 - Remove farest points
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PassThrough<pcl::PointXYZ> X_pass;
X_pass.setInputCloud (scan_cloud);
X_pass.setFilterFieldName ("x"); // Set the filter field to x-axis
X_pass.setFilterLimits (0.0, 1.2); // Set the threshold (adjust as needed)
X_pass.filter (*cloud_filtered);
pcl::PassThrough<pcl::PointXYZ> Z_pass;
Z_pass.setInputCloud (cloud_filtered);
Z_pass.setFilterFieldName ("z"); // Set the filter field to z-axis
Z_pass.setFilterLimits (0.0, 2.0); // Set the threshold (adjust as needed)
Z_pass.filter (*cloud_filtered);
// // 2 - Remove NaN points
std::vector<int> input_indices;
pcl::removeNaNFromPointCloud (*cloud_filtered, *cloud_filtered, input_indices);
std::cout << "Removed " << input_indices.size () << " NaN points from the input cloud." << std::endl;
// std::vector<int> target_indices;
// pcl::removeNaNFromPointCloud (*cad_cloud, *cad_cloud, target_indices);
// std::cout << "Removed " << target_indices.size () << " NaN points from the target cloud." << std::endl;
// 3 - Remove outliers
pcl::StatisticalOutlierRemoval<PointT> sor;
sor.setInputCloud(cloud_filtered);
sor.setMeanK(50); // Number of neighbors to analyze
sor.setStddevMulThresh(1.0); // Threshold for outlier removal
sor.filter(*cloud_filtered);
// 4 - Downsampling
pcl::VoxelGrid<PointT> voxel_filter;
voxel_filter.setInputCloud(cloud_filtered);
voxel_filter.setLeafSize(0.01f, 0.01f, 0.01f); // Set voxel size to 1 cm
voxel_filter.filter(*cloud_filtered);
std::cout << "Downsampled cloud size: " << cloud_filtered->width * cloud_filtered->height << std::endl;
auto cloud_icp = do_ICP(cloud_filtered, cad_cloud, transformation_matrix, true);
}
Please change path to some pointclouds (I cannot share mines). If it works for you, so the culprit might be how I create my pointclouds (enough points, to many points, ...).
Best !
Since my last comment helped you find a solution, here it is again as a proper answer:
auto in combination with Eigen can cause problems, see https://eigen.tuxfamily.org/dox/TopicPitfalls.html . auto final_transformation_matrix = icp.getFinalTransformation ().cast<double>(); should be changed to Eigen::Matrix4d final_transformation_matrix = icp.getFinalTransformation ().cast<double>();pcl::registration::FPCSInitialAlignment, pcl::registration::KFPCSInitialAlignment, pcl::SampleConsensusInitialAlignment, pcl::SampleConsensusPrerejective, pcl::PPFRegistration (see https://pointclouds.org/documentation/group__registration.html). What you did (estimating transformation on hand-picked points) is actually also implemented in PCL as pcl_manual_registration (https://github.com/PointCloudLibrary/pcl/tree/master/apps/src/manual_registration)