使用PCL库
#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>);
// 填入点云数据
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); // 目标中两个对应点之间的最大距离阈值。如果距离大于此阈值,则在对齐过程中将忽略这些点
icp.setMaximumIterations(1000); //迭代次数已达到用户施加的最大迭代次数
// icp.setTransformationEpsilon(1e-6); //先前转换和当前估计转换(即两次位姿转换)之间的 epsilon(差异)小于用户施加的值
// icp.setEuclideanFitnessEpsilon(1e-6); //欧几里得平方误差的总和小于用户定义的阈值
// icp.setRANSACIterations(0); // 设置RANSAC运行次数
// 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);
}
不使用PCL库
#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 closet point of [x,y,z] in {[sourcePoints_x, sourcePoints_y,sourcePoints_z]}
void getClosestPoint(float x, float y,float z, const vector<Vector3f> &sourcePoints,float &find_x, float &find_y, float &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> &sourcePoints,const vector<Vector3f> &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;
}
//
/* 点云配准算法–kabsch
* 两个点云P和P’内各个顶点一一对应
* 求出旋转平移变换矩阵
* pts1 是原始点云 pts2是待匹配的点云
*/
void KABSCH(const vector<Vector3f> &pts1, const vector<Vector3f> &pts2)
{
// 计算质心坐标
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);
// 计算每个点的去质心坐标
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 求和
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));
// 验证
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 算法
* source_pcls 原始点云 target_pcls 变换后的点云
*/
void ICP(const vector<Vector3f> &source_pcls, const vector<Vector3f> &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);
// 计算质心坐标
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);
// 计算去质心坐标
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 ; //定义 为计算误差
int num = 0;
float loss_st = 9999; //原始点云到目标点云的距离损失
while (num < iter_times && /* loss_st>0.5 */loss_improve > 0.04)
{
Vector3f q1_c(0,0,0);
// 计算质心坐标
for (size_t i = 0; i < q1.size(); i++)
{
q1_c += q1[i];
}
q1_c = Vector3f((q1_c) / q1.size());
// 计算去质心坐标
for (size_t i = 0; i < q1.size(); i++)
{
q1[i] = q1[i] - q1_c;
}
// compute q1*q2^T 求和
Eigen::Matrix3f W = Eigen::Matrix3f::Zero();
// 在q2 中寻找 q1 对应的最近点
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; // 最终旋转变换矩阵 从原始点云到目标点云
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();
// 更新变换后的点云向量 下一次循环使用
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); // 前一个变换矩阵和当前变换矩阵的差异小于阈值时,就认为已经收敛了,
// 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;
// }
}