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();//最終的變換矩陣