Use the PCL library
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <ros/ros.h>
int main(int argc, char **argv)
{<!-- -->
ros::init(argc, argv, "iterative_closest_point");
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the point cloud data
cloud_in->width = 500;
cloud_in->height = 1;
cloud_in->is_dense = false;
cloud_in->points.resize(cloud_in->width * cloud_in->height);
for (size_t i = 0; i < cloud_in->points. size(); + + i)
{<!-- -->
cloud_in->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
cloud_in->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
cloud_in->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
}
std::cout << "Saved " << cloud_in->points. size() << " data points to input:"
<< std::endl;
// for (size_t i = 0; i < cloud_in->points. size(); + + i)
// std::cout << " " << cloud_in->points[i].x << " " << cloud_in->points[i].y << " " << cloud_in- >points[i].z << std::endl;
*cloud_out = *cloud_in;
// std::cout << "size:" << cloud_out->points. size() << std::endl;
for (size_t i = 0; i < cloud_in->points. size(); + + i)
{<!-- -->
cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
cloud_out->points[i].y = cloud_in->points[i].y + 0.7f;
cloud_out->points[i].z = cloud_in->points[i].z + 0.7f;
}
// std::cout << "Transformed " << cloud_in->points. size() << " data points:"
// << std::endl;
// for (size_t i = 0; i < cloud_out->points. size(); + + i)
// std::cout << " " << cloud_out->points[i].x << " " << cloud_out->points[i].y << " " << cloud_out- >points[i].z << std::endl;
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputCloud(cloud_in);
icp.setInputTarget(cloud_out);
icp.setMaxCorrespondenceDistance(100); // The maximum distance threshold between two corresponding points in the target. If the distance is greater than this threshold, the points will be ignored during the alignment
icp.setMaximumIterations(1000); //The number of iterations has reached the maximum number of iterations imposed by the user
// icp.setTransformationEpsilon(1e-6); //The epsilon (difference) between the previous transformation and the current estimated transformation (i.e. two pose transformations) is less than the value imposed by the user
// icp.setEuclideanFitnessEpsilon(1e-6); //The sum of Euclidean squared errors is less than the user-defined threshold
// icp.setRANSACIterations(0); // Set the number of RANSAC runs
// icp.setInputCloud(cloud_in);
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;
return (0);
}
Does not use the PCL library
#include <iostream>
#include <ros/ros.h>
#include <eigen3/Eigen/Dense>
#include <vector>
using namespace std;
using namespace Eigen;
#define PI (double)3.141592653589793
// distance of two points (x1, y1, z1) and (x2, y2, z2)
float getDistanceOfTwoPoints(float x1, float y1, float z1, float x2, float y2, float z2)
{<!-- -->
return sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2) + pow(z1 - z2, 2));
}
// get closer point of [x,y,z] in {[sourcePoints_x, sourcePoints_y,sourcePoints_z]}
void getClosestPoint(float x, float y, float z, const vector<Vector3f> & amp; sourcePoints, float & amp; find_x, float & amp; find_y, float & amp; find_z)
{<!-- -->
double dist_min = 99999;
for (int i = 0; i < sourcePoints. size(); i ++ )
{<!-- -->
double dist = getDistanceOfTwoPoints(x, y, z, sourcePoints[i](0), sourcePoints[i](1), sourcePoints[i](2));
if (dist < dist_min)
{<!-- -->
dist_min = dist;
find_x = sourcePoints[i](0);
find_y = sourcePoints[i](1);
find_z = sourcePoints[i](2);
}
}
}
int getClosestPointID(float x, float y, float z, const vector<Vector3f> & sourcePoints)
{<!-- -->
int id = 0;
double dist_min = 99999;
for (int i = 0; i < sourcePoints. size(); i ++ )
{<!-- -->
double dist = getDistanceOfTwoPoints(x, y, z, sourcePoints[i](0),sourcePoints[i](1),sourcePoints[i](2));
if (dist < dist_min)
{<!-- -->
dist_min = dist;
id = i;
}
}
return id;
}
// get all points distance of {sourcePoints_x, sourcePoints_y, sourcePoints_z} and {[targetPoints_x,targetPoints_y,sourcePoints_z]} as loss
float getLossShapeMatch(const vector<Vector3f> & amp; sourcePoints, const vector<Vector3f> & amp; targetPoints)
{<!-- -->
float loss_sum = 0;
for (int i = 0; i < targetPoints. size(); i ++ )
{<!-- -->
float closest_x, closest_y, closest_z;
getClosestPoint(targetPoints[i](0), targetPoints[i](1), targetPoints[i](2), sourcePoints,closest_x, closest_y, closest_z);
float loss = getDistanceOfTwoPoints(targetPoints[i](0), targetPoints[i](1), targetPoints[i](2),closest_x, closest_y,closest_z);
loss_sum = loss_sum + loss;
}
return loss_sum;
}
//
/* Point cloud registration algorithm – kabsch
* One-to-one correspondence between vertices in the two point clouds P and P'
* Find the rotation and translation transformation matrix
* pts1 is the original point cloud pts2 is the point cloud to be matched
*/
void KABSCH(const vector<Vector3f> & amp;pts1, const vector<Vector3f> & amp;pts2)
{<!-- -->
// Calculate centroid coordinates
Vector3f p1(0,0,0);
Vector3f p2(0,0,0); // center of mass
int N = pts1. size();
for (int i = 0; i < N; i ++ )
{<!-- -->
p1 + = pts1[i];
p2 + = pts2[i];
}
p1 = Vector3f((p1) / N);
p2 = Vector3f((p2) / N);
// Calculate the centroid coordinates of each point
std::vector<Vector3f> q1(N), q2(N); // remove the center
for (int i = 0; i < N; i ++ )
{<!-- -->
q1[i] = pts1[i] - p1;
q2[i] = pts2[i] - p2;
}
// compute q1*q2^T sum
Eigen::Matrix3f W = Eigen::Matrix3f::Zero();
for (int i = 0; i < N; i ++ )
{<!-- -->
W + = Eigen::Vector3f(q1[i](0), q1[i](1), q1[i](2)) * Eigen::Vector3f(q2[i](0), q2[i] (1), q2[i](2)).transpose();
}
// SVD on W
Eigen::JacobiSVD<Eigen::Matrix3f> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f U = svd.matrixU();
Eigen::Matrix3f V = svd.matrixV();
Eigen::Matrix3f R_12 = U * (V.transpose());
Eigen::Vector3f t_12 = Eigen::Vector3f(p1(0), p1(1), p1(2)) - R_12 * Eigen::Vector3f(p2(0), p2(1), p2(2));
// verify
Eigen::AngleAxisf R_21;
R_21.fromRotationMatrix(R_12.transpose());
std::cout<<"R_12.transpose(): "<<std::endl;
std::cout<<R_12.transpose()<<std::endl;
cout << "aix: " << R_21. axis(). transpose() << endl;
cout << "angle: " << R_21. angle() * 180 / PI << endl;
cout << "t: " << (-R_12.transpose() * t_12).transpose() << endl;
}
/*
* ICP algorithm
* source_pcls original point cloud target_pcls transformed point cloud
*/
void ICP(const vector<Vector3f> & amp; source_pcls, const vector<Vector3f> & amp; target_pcls)
{<!-- -->
int iter_times = 5000;
double loss_now;
double loss_improve = 1000;
double loss_before = getLossShapeMatch(target_pcls,source_pcls) / source_pcls.size();
int source_pcls_size = source_pcls. size();
int target_pcls_size = target_pcls. size();
Vector3f source_pcls_c(0,0,0);
Vector3f target_pcls_c(0,0,0);
// Calculate centroid coordinates
for (size_t i = 0; i < source_pcls_size; i ++ )
{<!-- -->
source_pcls_c + = source_pcls[i];
}
source_pcls_c = Vector3f((source_pcls_c) / source_pcls_size);
for (size_t i = 0; i < target_pcls_size; i ++ )
{<!-- -->
target_pcls_c + = target_pcls[i];
}
target_pcls_c = Vector3f((target_pcls_c) / target_pcls_size);
// calculate centroid coordinates
vector<Vector3f> q1(source_pcls_size), q2(target_pcls_size); // remove the center
q1 = source_pcls;
// for (size_t i = 0; i < source_pcls_size; i ++ )
// {<!-- -->
// q1[i] = source_pcls[i] - source_pcls_c;
// }
for (size_t i = 0; i < target_pcls_size; i ++ )
{<!-- -->
q2[i] = target_pcls[i] - target_pcls_c;
}
Eigen::Matrix3f R0, R1;
R0. setIdentity();
R1. setIdentity();
Eigen::Vector3f t0;
std::vector<Vector3f> q0 ; //Defined as calculation error
int num = 0;
float loss_st = 9999; //The distance loss from the original point cloud to the target point cloud
while (num < iter_times & amp; & amp; /* loss_st>0.5 */loss_improve > 0.04)
{<!-- -->
Vector3f q1_c(0,0,0);
// Calculate centroid coordinates
for (size_t i = 0; i < q1. size(); i ++ )
{<!-- -->
q1_c + = q1[i];
}
q1_c = Vector3f((q1_c) / q1. size());
// calculate centroid coordinates
for (size_t i = 0; i < q1. size(); i ++ )
{<!-- -->
q1[i] = q1[i] - q1_c;
}
// compute q1*q2^T sum
Eigen::Matrix3f W = Eigen::Matrix3f::Zero();
// Find the closest point corresponding to q1 in q2
for (size_t i = 0; i < source_pcls_size; i ++ )
{<!-- -->
int id = getClosestPointID(q1[i](0), q1[i](1), q1[i](2),q2);
W + = Eigen::Vector3f(q1[i](0), q1[i](1), q1[i](2)) * Eigen::Vector3f(q2[id](0), q2[id] (1), q2[id](2)).transpose();
}
// SVD on W
Eigen::JacobiSVD<Eigen::Matrix3f> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f U = svd.matrixU();
Eigen::Matrix3f V = svd.matrixV();
Eigen::Matrix3f R_12 = U * (V.transpose());
Eigen::Vector3f t_12 = Eigen::Vector3f(q1_c(0), q1_c(1), q1_c(2)) - R_12 * Eigen::Vector3f(target_pcls_c(0), target_pcls_c(1), target_pcls_c(2));
Eigen::Vector3f t_21 = (-R_12.transpose() * t_12).transpose();
Eigen::Matrix3f R_21 = R_12.transpose();
R0 = R0*R_21; // The final rotation transformation matrix from the original point cloud to the target point cloud
R1 = R1*R_12;
Eigen::Vector3f t1 = Eigen::Vector3f(source_pcls_c(0), source_pcls_c(1), source_pcls_c(2)) - R1 * Eigen::Vector3f(target_pcls_c(0), target_pcls_c(1), target_pcls_c(2));
t0 = (-R1.transpose() * t1).transpose();
// Update the transformed point cloud vector for the next cycle
for (size_t i = 0; i < source_pcls_size; i ++ )
{<!-- -->
Eigen::Vector3f p = R_21 * q1[i] + t_21;
q1[i] = p;
}
q0. clear();
for (size_t i = 0; i < source_pcls_size; i ++ )
{<!-- -->
Eigen::Vector3f p = R0 * source_pcls[i] + t0;
q0.push_back(p);
}
loss_st = getLossShapeMatch(target_pcls, q0) / q0.size();
num++;
float loss_now = getLossShapeMatch(q2, q1) / q1. size();
loss_improve = abs(loss_before - loss_now); // When the difference between the previous transformation matrix and the current transformation matrix is less than the threshold, it is considered to have converged,
// std::cout<<"itertime: "<<iter_times<<", loss now: "<<loss_now<<", improve: "<<loss_improve<<std::endl;
loss_before = loss_now;
}
std::cout<<"------ICP RESULT-------"<<std::endl;
std::cout << "num: " << num << std::endl;
std::cout << "R0: " << std::endl<< R0 << std::endl;
std::cout << "t0: "<< t0.transpose() <<std::endl;
// std::cout << "R_12.transpose(): " << std::endl;
// std::cout << R_12.transpose() << std::endl;
// std::cout << "t: " << (-R_12.transpose() * t_12).transpose() << std::endl;
std::cout << "loss_improve: " << loss_improve << std::endl;
std::cout << "loss_st: " << loss_st <<std::endl;
// for (size_t i = 0; i < q0.size(); i ++ )
// {<!-- -->
// std::cout<<"q0 "<<i<<" "<<q0[i].transpose()<<std::endl;
// }
// for (size_t i = 0; i < target_pcls. size(); i ++ )
// {<!-- -->
// std::cout<<"target_pcls "<<i<<" "<<target_pcls[i].transpose()<<std::endl;
// }
}