關于目前ICP算法的理解和總結
基本概念之配準
有兩個點集,source和target,target不變,source經過旋轉(Rotation)和平移(Translation)甚至加上尺度(Scale)變換,使得變換后的source點集盡量和target點集重合,這個變換的過程就叫點集配準。
兩個點集的對應,輸出通常是一個4×4剛性變換矩陣:代表旋轉和平移,它應用于源數據集,結果是完全與目標數據集匹配。下圖是“雙對應”算法中一次迭代的步驟:
image.png
對兩個數據源a,b匹配運算步驟如下:
- 從其中一個數據源a出發,分析其最能代表兩個數據源場景共同點的關鍵點k
- 在每個關鍵點ki處,算出一個特征描述子fi
- 從這組特征描述子{fi}和他們在a和b中的XYZ坐標位置,基于fi和xyz的相似度,找出一組對應
- 由于實際數據源是有噪的,所以不是所有的對應都有效,這就需要一步一步排除對匹配起負作用的對應
從剩下的較好對應中,估計出一個變換
icp優化思路
首先icp是一步一步迭代得到較好的結果,解的過程其實是一個優化問題,并不能達到絕對正解。這個過程中求兩個點云之間變換矩陣是最重要的,PCL里是用奇異值分解SVD實現的
從上面的icp流程中我們可以看到的可以用不同的算法對1、2、3這三個步驟進行優化
關于PLICP調研和優化方案,后續給出
目前我們的代碼中的重要參數如下,需要我們設置的參數為加粗部分
inline void inline void setSearchMethodTarget(const KdTreePtr &tree) kdtree
加速搜索,還有一個Target的函數,用法與之一致。
inline void setInputSource (constPointCloudSourceConstPtr &cloud)
需要匹配的點云。
inline void setInputTarget (constPointCloudTargetConstPtr &cloud)
基準點云,也就是從Source到Target的匹配。
inline void setMaxCorrespondenceDistance (doubledistance_threshold)
忽略在此距離之外的點,如果兩個點云距離較大,這個值要設的大一些(PCL默認距離單位是m)。目前我們的launch文件中mcpd設置的大小為0.05
inline void setTransformationEpsilon (doubleepsilon)
第2個約束,這個值一般設為1e-6或者更小。(目前在launch文件中并沒有設置,代碼1e-6,這個值可以更?。?/h4>
inline void setEuclideanFitnessEpsilon (doubleepsilon)
第3個約束,前后兩次迭代誤差的差值。(目前在launch文件中并沒有設置,代碼1e-6,太小,可優化)
inline void setMaximumIterations (intnr_iterations)
第1個約束,迭代次數,幾十上百都可能出現。目前在launch文件中maxIterations為250
inline void align (PointCloudSource &output)
inline void setEuclideanFitnessEpsilon (doubleepsilon)
inline void setMaximumIterations (intnr_iterations)
inline void align (PointCloudSource &output)
輸出配準后點云。
inline Matrix4 getFinalTransformation ()
獲取最終的轉換矩陣。
bool ScanMatch::icp(const pcl::PointCloud<pcl::PointXYZ>::Ptr &scan_cloud,
const pcl::PointCloud<pcl::PointXYZ>::Ptr &map_cloud, tf2::Transform &pose_corrections_out) {
pose_corrections_out = tf2::Transform::getIdentity();
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> reg;
//第2個約束,這個值一般設為1e-6或者更小
reg.setTransformationEpsilon(1e-6);
//第3個約束,前后兩次迭代誤差的差值
reg.setEuclideanFitnessEpsilon(1e-6);
//忽略在此距離之外的點,如果兩個點云距離較大,這個值要設的大一些
reg.setMaxCorrespondenceDistance(0.02);//0.02
// 第1個約束,迭代次數,幾十上百都可能出現
reg.setMaximumIterations(maxIterations_);
//進行RANSAC迭代
reg.setRANSACIterations(maxIterations_);
//剔除錯誤估計,可用 RANSAC 算法,或減少數量
reg.setRANSACOutlierRejectionThreshold(0.05);
//設置查找近鄰時是否雙向搜索
reg.setUseReciprocalCorrespondences(true);
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr target_pointcloud_search_method(
new pcl::search::KdTree<pcl::PointXYZ>());
target_pointcloud_search_method->setInputCloud(map_cloud);
reg.setInputTarget(map_cloud);
reg.setSearchMethodTarget(target_pointcloud_search_method, true);
}
pcl::search::KdTree<pcl::PointXYZ>::Ptr reference_pointcloud_search_method(new pcl::search::KdTree<pcl::PointXYZ>());
reference_pointcloud_search_method->setInputCloud(map_cloud);
{
pcl::search::KdTmree<pcl::PointXYZ>::Ptr source_pointcloud_search_method(new pcl::search::KdTree<pcl::PointXYZ>());
source_pointcloud_search_method->setInputCloud(scan_cloud);
reg.setSearchMethodSource(source_pointcloud_search_method, true);
reg.setInputSource(scan_cloud);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr unused(new pcl::PointCloud<pcl::PointXYZ>());
//調用該函數開配準
reg.align(*unused);
const Eigen::Matrix4f &transf = reg.getFinalTransformation();
tf::Transform tf;
matrixAsTransfrom(transf, tf);
if (reg.hasConverged()) {
tfToTF2(tf, pose_corrections_out);
}
return false;
}