文章目錄

• 前言
• 一、ICP算法基礎(chǔ)
• • 1.1 提取待匹配點對
  • 1.2 計算旋轉(zhuǎn)平移矩陣
  • 1.3 計算變換后的點和目標(biāo)點之間的偏差
• 二、ICP算法變種
• • 2.1 PLICP
  • 2.2 PointToPlane ICP
  • 2.3 NICP
  • 2.4 LM_ICP
• 三、程序示例
• • 1. 傳統(tǒng)方法
  • 2. PointToPlane ICP
• 總結(jié)

---

前言

ICP（Iterative Closest Point，最近鄰點迭代）是應(yīng)用最廣泛的三維點云配準(zhǔn)算法之一，網(wǎng)上講ICP算法原理的非常多，這里列舉幾個個人覺得講的比較好的。
三維點云配準(zhǔn) – ICP 算法原理及推導(dǎo)
ICP（迭代最近點）算法
PCL學(xué)習(xí)筆記二：Registration (ICP算法)

原始的ICP算法的問題在于點對之間只分析距離之間的關(guān)系從而引起迭代次數(shù)高，最終導(dǎo)致的計算時間過長，所以很多學(xué)者提出了各種各樣的改進(jìn)算法，如：PLICP，PointToPlane ICP，NICP，LM_ICP算法等。
本文對各種ICP算法的原理以及其簡單的應(yīng)用進(jìn)行分析。

---

一、ICP算法基礎(chǔ)

ICP算法的基本邏輯是先通過對應(yīng)關(guān)系估計、關(guān)鍵點檢測等方法找到源點云和目標(biāo)點云的待匹配點對，然后計算旋轉(zhuǎn)和平移矩陣，對source點云進(jìn)行旋轉(zhuǎn)平移到target點云上，根據(jù)設(shè)置的閾值進(jìn)行判斷，如果不滿足閾值要求就重復(fù)以上過程繼續(xù)計算，最終達(dá)到最大迭代次數(shù)或者滿足設(shè)定的均方差閾值才能停止迭代。
可以用一個公式表示：

1.1 提取待匹配點對

首先按需要進(jìn)行blob。
然后進(jìn)行Correspondences estimation（對應(yīng)關(guān)系估計），得到共同部分的點云。

/** \brief A CorrespondenceEstimation object, used to estimate correspondences between the source and the target cloud. */CorrespondenceEstimationPtr correspondence_estimation_;/** \brief The minimum number of correspondences that the algorithm needs before attempting to estimate the * transformation. The default value is 3.*/int min_number_correspondences_;correspondence_estimation_->setInputTarget (target_);if (correspondence_estimation_->requiresTargetNormals ())correspondence_estimation_->setTargetNormals (target_blob);

具體源碼自行查看，下面列出Correspondences estimation的計算代碼,里面包含了兩種計算方法，分別為determineCorrespondences和determineReciprocalCorrespondences 。
determineCorrespondences會計算所有點的對應(yīng)關(guān)系。
determineReciprocalCorrespondences計算兩個點云共同部分的對應(yīng)關(guān)系。

最后進(jìn)行一個CorrespondenceRejector，去除錯誤的估計。

for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i){registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];if (rej->requiresTargetPoints ())rej->setTargetPoints (target_blob);if (rej->requiresTargetNormals () && target_has_normals_)rej->setTargetNormals (target_blob);}

1.2 計算旋轉(zhuǎn)平移矩陣

我們默認(rèn)變換為剛性變換，通過空間中兩點間的變換關(guān)系計算R和T矩陣。假定第一次計算的矩陣為$R_0$和$T_0$。PCL中ICP的初始矩陣為

Eigen::Vector4f pt (0.0f, 0.0f, 0.0f, 1.0f), pt_t; Eigen::Matrix4f tr = transform.template cast<float> ()

也就是：
$?\begin{array}{cccc}1& & & 0\\ & 1& & 0\\ & & 1& 0\\ & & & 1\end{array}?$
公式如下：
$p_i$=$R_0$*$q_i$+$T_0$
其中：

T = $\left[\begin{array}{ccc}tx& ty& tz\end{array}\right]$
具體的計算采用奇異值分解的方法，具體計算過程前言部分推薦的文章有寫。

1.3 計算變換后的點和目標(biāo)點之間的偏差

對點集p進(jìn)行變換，計算變換后的點集$p_1$和q的距離值。
Distance=$\sum _{i=1}^n||p_1$-q ||${}^2$
Distance和設(shè)定的閾值進(jìn)行對比，如果大于，則跳到第一步重新開始循環(huán)迭代，如果達(dá)到最大迭代次數(shù)還沒有滿足閾值要求，也會停止迭代，保留最近一次的迭代結(jié)果。
PCL中還有一個收斂條件設(shè)置setTransformationEpsilon，意思是上一個變換矩陣和當(dāng)前變換矩陣的差異值，如果差異值小于閾值，也認(rèn)為達(dá)到收斂。

PCL迭代部分的代碼如下：

// Estimate the transformtransformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);// Transform the datatransformCloud (*input_transformed, *input_transformed, transformation_);// Obtain the final transformationfinal_transformation_ = transformation_ * final_transformation_;++nr_iterations_;

二、ICP算法變種

因為計算點對間的最小距離的方法過于耗時和低效，所以出現(xiàn)了很多加速方法，例如先提取點云特征，然后進(jìn)行特征間的匹配，可以極大減少匹配時間；或者對計算源點云中點到目標(biāo)點云對應(yīng)點線或者面的最小距離，可以降低。

2.1 PLICP

PLICP計算當(dāng)前幀中的點到參考幀中最近鄰兩點連線的最小距離值，在slam中應(yīng)用較多，可以或者更小的迭代次數(shù)和更高的精度。
原理可以參考論文：
A. Censi, “An ICP variant using a point-to-line metric,” 2008 IEEE International Conference on Robotics and Automation, Pasadena, CA, 2008, pp. 19-25, doi: 10.1109/ROBOT.2008.4543181

2.2 PointToPlane ICP

計算Source點云中點到目標(biāo)點云對應(yīng)點形成的面的最小距離值。

這里ni為qi的法線,minE為最小歐式距離。

2.3 NICP

NICP與傳統(tǒng)ICP的不同之處在于NICP會多考慮曲率和法線的方向一致問題，如果對應(yīng)點的曲率和法線方向超過設(shè)定閾值，不會建立對應(yīng)點的匹配。所以在計算的時候需要計算點云的法線信息，然后進(jìn)行匹配時需要額外對對應(yīng)點對的法線和曲率限定閾值。

2.4 LM_ICP

LM_ICP在計算最小距離的時候采用LM模型來進(jìn)行，算法原理可以查看論文：
結(jié)合Kinect的雙目視覺場景三維重建。

三、程序示例

1. 傳統(tǒng)方法

傳統(tǒng)方法計算全部點云的對應(yīng)關(guān)系導(dǎo)致計算時間非常長。

icp.setInputSource(source); icp.setInputTarget(target); icp.setTransformationEpsilon(1e-8); icp.setMaxCorrespondenceDistance(1); //添加一個最大距離的閾值，超過最大距離的點不作為對應(yīng)點。 icp.setEuclideanFitnessEpsilon(0.00005); icp.setMaximumIterations(150); icp.setUseReciprocalCorrespondences(true);

迭代1次：

迭代134次：

2. PointToPlane ICP

PointToPlane ICP的核心類是IterativeClosestPointWithNormals，默認(rèn)情況下，此實現(xiàn)使用傳統(tǒng)的點對面目標(biāo)，并使用目標(biāo)點云的法線計算點對面距離。
另外在計算法線的時候采用OpenMP來進(jìn)行多線程加速。

pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal>ptoplane_icp;ptoplane_icp.setInputSource(source_with_normals); ptoplane_icp.setInputTarget(target_with_normals); ptoplane_icp.setTransformationEpsilon(1e-8); ptoplane_icp.setMaxCorrespondenceDistance(1); ptoplane_icp.setEuclideanFitnessEpsilon(0.0001); ptoplane_icp.setMaximumIterations(150);

可見只進(jìn)行了7次迭代，用時1.6s.

---

總結(jié)

ICP算法功能強大，算法種類也很多，在實際使用時需要根據(jù)實際應(yīng)用場景開發(fā)適合的ICP自適應(yīng)算法。

總結(jié)

以上是生活随笔為你收集整理的基于PCL的ICP及其变种算法实现的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。