---

前言

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

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

---

一、ICP算法基礎

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

1.1 提取待匹配點對

首先按需要進行blob。
然后進行Correspondences estimation（對應關系估計），得到共同部分的點云。

/** \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會計算所有點的對應關系。
determineReciprocalCorrespondences計算兩個點云共同部分的對應關系。

最后進行一個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 計算旋轉平移矩陣

我們默認變換為剛性變換，通過空間中兩點間的變換關系計算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 計算變換后的點和目標點之間的偏差

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

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算法變種

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

2.1 PLICP

PLICP計算當前幀中的點到參考幀中最近鄰兩點連線的最小距離值，在slam中應用較多，可以或者更小的迭代次數和更高的精度。
原理可以參考論文：
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點云中點到目標點云對應點形成的面的最小距離值。

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

2.3 NICP

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

2.4 LM_ICP

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

三、程序示例

1. 傳統方法

傳統方法計算全部點云的對應關系導致計算時間非常長。

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

迭代1次：

迭代134次：

2. PointToPlane ICP

PointToPlane ICP的核心類是IterativeClosestPointWithNormals，默認情況下，此實現使用傳統的點對面目標，并使用目標點云的法線計算點對面距離。
另外在計算法線的時候采用OpenMP來進行多線程加速。

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); 

可見只進行了7次迭代，用時1.6s.

---

總結

ICP算法功能強大，算法種類也很多，在實際使用時需要根據實際應用場景開發適合的ICP自適應算法。