SLAM中的ICP配准简介与使用

1.原理简介

给定两个点云集合：

求解R（旋转矩阵）和t（平移矩阵）：

讨论argmin

可以看到以上的E这个合集便是欧式变化的两要素
接下来求X Q两簇点云的平均位姿

算完后再进行一个平移

具体作用可参见二维，无非就是中心移到原点

虚线左边是未平移，右边是平移的
然后构造W

进行SVD分解 则有

2.使用方法

初始化部分

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>icp;//定义 初始化
icp.setInputSource(set_InputSource);   //设置输入的点云
icp.setInputTarget(set_InputTarget);    //目标点云
icp.setMaxCorrespondenceDistance(100);//关联距离
icp.setTransformationEpsilon(1e-10);//位姿收敛阈值
icp.setEuclideanFitnessEpsilon(0.001);//欧式距离收敛阈值
icp.setMaximumIterations(1000);//迭代次数
icp.align(*cloud_icp,initial_pose_matrix);//初始位姿

结果获取部分

transform_probability = icp.getFitnessScore();//max_range maximum allowable distance between a point and its correspondence in the target (default: double::max) 最大距离
result_pose_matrix = icp.getFinalTransformation();//最终的变换矩阵