文章目錄

• 前言
• 一、ICP算法基礎(chǔ)
• • 1.1 提取待匹配點(diǎn)對(duì)
  • 1.2 計(jì)算旋轉(zhuǎn)平移矩陣
  • 1.3 計(jì)算變換后的點(diǎn)和目標(biāo)點(diǎn)之間的偏差
• 二、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，最近鄰點(diǎn)迭代）是應(yīng)用最廣泛的三維點(diǎn)云配準(zhǔn)算法之一，網(wǎng)上講ICP算法原理的非常多，這里列舉幾個(gè)個(gè)人覺得講的比較好的。
三維點(diǎn)云配準(zhǔn) – ICP 算法原理及推導(dǎo)
ICP（迭代最近點(diǎn)）算法
PCL學(xué)習(xí)筆記二：Registration (ICP算法)

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

---

一、ICP算法基礎(chǔ)

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

1.1 提取待匹配點(diǎn)對(duì)

首先按需要進(jìn)行blob。
然后進(jìn)行Correspondences estimation（對(duì)應(yīng)關(guān)系估計(jì)），得到共同部分的點(diǎ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的計(jì)算代碼,里面包含了兩種計(jì)算方法，分別為determineCorrespondences和determineReciprocalCorrespondences 。
determineCorrespondences會(huì)計(jì)算所有點(diǎn)的對(duì)應(yīng)關(guān)系。
determineReciprocalCorrespondences計(jì)算兩個(gè)點(diǎn)云共同部分的對(duì)應(yīng)關(guān)系。

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

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 計(jì)算旋轉(zhuǎn)平移矩陣

我們默認(rèn)變換為剛性變換，通過空間中兩點(diǎn)間的變換關(guān)系計(jì)算R和T矩陣。假定第一次計(jì)算的矩陣為$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]$
具體的計(jì)算采用奇異值分解的方法，具體計(jì)算過程前言部分推薦的文章有寫。

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

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

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

2.1 PLICP

PLICP計(jì)算當(dāng)前幀中的點(diǎn)到參考幀中最近鄰兩點(diǎn)連線的最小距離值，在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

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

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

2.3 NICP

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

2.4 LM_ICP

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

三、程序示例

1. 傳統(tǒng)方法

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

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

迭代1次：

迭代134次：

2. PointToPlane ICP

PointToPlane ICP的核心類是IterativeClosestPointWithNormals，默認(rèn)情況下，此實(shí)現(xiàn)使用傳統(tǒng)的點(diǎn)對(duì)面目標(biāo)，并使用目標(biāo)點(diǎn)云的法線計(jì)算點(diǎn)對(duì)面距離。
另外在計(jì)算法線的時(shí)候采用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次迭代，用時(shí)1.6s.

---

總結(jié)

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

總結(jié)

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

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