點(diǎn)云數(shù)據(jù)

點(diǎn)云數(shù)據(jù)通常表示為N行，至少3列的矩陣，其中N表示點(diǎn)的數(shù)量，每一行代表一個(gè)點(diǎn)。通常3列分別是點(diǎn)在空間中（x，y，z）的坐標(biāo)。如果點(diǎn)云數(shù)據(jù)有除空間中坐標(biāo)外的附加信息，如來(lái)自LIDAR傳感器的點(diǎn)云數(shù)據(jù)，那么它可能具有每個(gè)點(diǎn)的附加值，例如“反射率”，其是在該位置中障礙物反射多少激光光束的量度。 在這種情況下，點(diǎn)云數(shù)據(jù)可能是N×4陣列。

三維點(diǎn)云配準(zhǔn)

點(diǎn)云的配準(zhǔn)過(guò)程，就是求兩個(gè)點(diǎn)云之間的一個(gè)旋轉(zhuǎn)平移矩陣，源點(diǎn)云通過(guò)旋轉(zhuǎn)平移矩陣后，變換成和目標(biāo)點(diǎn)云矩陣相同的矩陣。

可以表示為以下方程：

其中，pt，ps分別是目標(biāo)點(diǎn)云和源點(diǎn)云中的一對(duì)對(duì)應(yīng)點(diǎn)。
R，T就是我們要求的旋轉(zhuǎn)平移矩陣。
但是，我們并不知道兩個(gè)點(diǎn)云中，點(diǎn)與點(diǎn)之間的對(duì)應(yīng)關(guān)系，這也是配準(zhǔn)的核心問題。

三維點(diǎn)云配準(zhǔn)分為粗配準(zhǔn)與精配準(zhǔn)兩步。
粗配準(zhǔn)就是在完全不清楚兩個(gè)點(diǎn)云的相對(duì)位置關(guān)系的情況下，找到一個(gè)這兩個(gè)點(diǎn)云近似的旋轉(zhuǎn)平移矩陣（不一定很精確，但是已經(jīng)大概是對(duì)的了）。
精配準(zhǔn)就是在已知一個(gè)旋轉(zhuǎn)平移矩陣的初值的情況下（這個(gè)初值大概已經(jīng)是正確的了），進(jìn)一步計(jì)算得到更加精確的旋轉(zhuǎn)平移矩陣。

配準(zhǔn)的評(píng)價(jià)標(biāo)準(zhǔn)：比較通用的是LCP(Largetst Common Pointset)（最大公共點(diǎn)集）。給定兩個(gè)點(diǎn)集P，Q，找到一個(gè)變換T，使得變換后的T（P）與Q的重疊度最大。在變換后的P內(nèi)任意一點(diǎn)，如果在容差范圍內(nèi)有另外一個(gè)Q的點(diǎn)，則認(rèn)為該點(diǎn)是重合點(diǎn)。重合點(diǎn)占所有點(diǎn)數(shù)量的比例就是重疊度。

（一）粗配準(zhǔn)

（1）暴力配準(zhǔn)

粗配準(zhǔn)中，最簡(jiǎn)單粗暴的一個(gè)方法就是暴力配準(zhǔn)。找到一個(gè)剛體變換需要3對(duì)對(duì)應(yīng)點(diǎn)，即在點(diǎn)集P中選取3個(gè)點(diǎn)，如果我們能找到它們?cè)邳c(diǎn)集Q中的對(duì)應(yīng)點(diǎn)，即能根據(jù)它們的坐標(biāo)求出旋轉(zhuǎn)平移矩陣。（具體怎么求：pass）

暴力配準(zhǔn)步驟：

• 隨機(jī)在點(diǎn)集P中選取3個(gè)點(diǎn)，隨機(jī)在點(diǎn)集Q中選取3個(gè)點(diǎn)。
• 計(jì)算旋轉(zhuǎn)平移矩陣T
• 計(jì)算 ||T（P）- Q||<$\delta$的點(diǎn)的個(gè)數(shù)$k_i$（點(diǎn)集P經(jīng)過(guò)旋轉(zhuǎn)平移矩陣后，點(diǎn)集中與點(diǎn)集Q的點(diǎn)的距離小于給定的閾值$\delta$的點(diǎn)的個(gè)數(shù)）
• 遍歷所有的可能性，選取最高的$k_i$作為最后的結(jié)果

暴力配準(zhǔn)的缺點(diǎn)：時(shí)間復(fù)雜度高。

假設(shè)點(diǎn)集P的大小為n，點(diǎn)集Q的大小為m，分別隨機(jī)選取3個(gè)點(diǎn)組成關(guān)聯(lián)對(duì)，則在選取3個(gè)點(diǎn)時(shí)就分別有$A_n^3$和$A_m^3$種可能，點(diǎn)集P中選取的任意一種可能，均需遍歷點(diǎn)集Q中的所有可能。因此，完成整個(gè)算法共需遍歷$A_n^3\times A_m^3=n(n?1)(n?2)m(m?1)(m?2)$種可能。因此，暴力配準(zhǔn)的時(shí)間復(fù)雜度為$O(n^3{m}^3)$。（當(dāng)n，m足夠大時(shí)，-1,-2等都失去了意義）

（2）4PCS（4-Points Congruent Sets）（4點(diǎn)全等集）

4PCS的核心原理在于，在剛性變換中，同一平面上的4個(gè)點(diǎn)，在經(jīng)過(guò)剛性變換后，其某些比率是穩(wěn)定不變的。

如圖，{a，b，c，d}四點(diǎn)共面，其中兩條直線的交點(diǎn)為e。在剛性變換中，r1，r2是穩(wěn)定不變的。

4PCS步驟：

• 在點(diǎn)集P中選取共面四點(diǎn)對(duì)作為base B={a，b，c，d}

• 計(jì)算比率$r_1$，$r_2$

• 點(diǎn)集Q（n個(gè)點(diǎn)）中，每?jī)蓚€(gè)點(diǎn)組成一對(duì)（$C_n^2$），對(duì)于每一對(duì)點(diǎn)$q_1$，$q_2$，計(jì)算他們的中間點(diǎn)
  $$e_1=q_1+r_1(q_2?q_1)$$$$e_2=q_1+r_2(q_2?q_1)$$
  
  如圖，每一對(duì)點(diǎn)都可以計(jì)算出兩個(gè)中間點(diǎn)$e_1,e_2$

• 若點(diǎn)集Q中，任意兩對(duì)點(diǎn)，一對(duì)由$r_1$計(jì)算得到的中間點(diǎn)$e_1$和另一對(duì)由$r_2$計(jì)算得到的中間點(diǎn)$e_2$的距離在允許范圍內(nèi)，那么可以認(rèn)為這兩對(duì)點(diǎn)可能是base B={a，b，c，d}的仿射對(duì)應(yīng)點(diǎn)。
  
  如圖，此時(shí)可認(rèn)為點(diǎn)集{$q_1$，$q_2$，$q_3$，$q_4$}是base B={a，b，c，d}的仿射對(duì)應(yīng)點(diǎn)

• 可能存在k個(gè)這樣的點(diǎn)集，這k個(gè)點(diǎn)集再作如下處理：
  （1）根據(jù)每個(gè)點(diǎn)集與base B，計(jì)算出旋轉(zhuǎn)平移矩陣T
  （2）計(jì)算 ||T（P）- Q||<$\delta$的點(diǎn)的個(gè)數(shù)$k_i$（點(diǎn)集P經(jīng)過(guò)旋轉(zhuǎn)平移矩陣后，點(diǎn)集T（P）與點(diǎn)集Q，距離小于給定的閾值$\delta$的點(diǎn)的個(gè)數(shù)）
  （3）遍歷所有k個(gè)點(diǎn)集，選取最高的$k_i$作為最后的結(jié)果

因此，4PCS算法的時(shí)間復(fù)雜度包括兩部分，點(diǎn)集Q（n個(gè)點(diǎn)）中任意選取兩個(gè)點(diǎn)組成一對(duì)，計(jì)算$e_1,e_2$，需作$A_n^2=n(n?1)次$，在找出的$k$個(gè)可能是base B的仿射對(duì)應(yīng)點(diǎn)的四點(diǎn)集中，遍歷檢查，獲得最好的旋轉(zhuǎn)平移矩陣T。$O(n^2+k)$

（二）精配準(zhǔn)
精配準(zhǔn)的模式基本已固定為使用ICP算法（Iterative Closest Point）（最近點(diǎn)迭代法）及其各種變種。
ICP算法的步驟：

• （1）ICP算法的核心是最小化一個(gè)目標(biāo)函數(shù)：$$f(R,T)=\frac{1}{N_p}\sum _{i=1}^{N_P}|p_t^i?R?p_s^i?T{|}^2$$
  $p_t^i，p_s^i$是一對(duì)對(duì)應(yīng)點(diǎn)，總共有$N_p$對(duì)對(duì)應(yīng)點(diǎn)，這個(gè)目標(biāo)函數(shù)實(shí)際上是找到旋轉(zhuǎn)平移矩陣R，T，使得所有對(duì)應(yīng)點(diǎn)之間的歐式距離的平方和最小。

• （2）尋找對(duì)應(yīng)點(diǎn)。起初并不知道有哪些對(duì)應(yīng)點(diǎn)，因此，在有粗配準(zhǔn)獲得的旋轉(zhuǎn)平移矩陣的初值下，用此旋轉(zhuǎn)平移矩陣對(duì)源點(diǎn)云進(jìn)行變換，得到一個(gè)變換后的點(diǎn)云，將這個(gè)變換后的點(diǎn)云與目標(biāo)點(diǎn)云進(jìn)行比較，只要兩個(gè)點(diǎn)云中存在距離小于一定閾值（ICP算法的一個(gè)參數(shù)）的點(diǎn)，就認(rèn)為這兩個(gè)點(diǎn)是對(duì)應(yīng)點(diǎn)。（最鄰近點(diǎn)）

• （3）R，T優(yōu)化。有了對(duì)應(yīng)點(diǎn)后，根據(jù)目標(biāo)函數(shù)優(yōu)化R，T。

• （4）迭代。優(yōu)化得到了新的R，T，導(dǎo)致變換后的點(diǎn)云的一些點(diǎn)的位置發(fā)生了變化，一些最近鄰點(diǎn)對(duì)也相應(yīng)地發(fā)生了變化，回到步驟（2）中重新尋找對(duì)應(yīng)點(diǎn)。迭代（2）（3）步驟，直到滿足迭代終止條件，如R，T的變化量小于一定值，或目標(biāo)函數(shù)的變化小于一定值，或最近鄰點(diǎn)對(duì)不再改變等（ICP算法的另一個(gè)參數(shù)）

• （5）迭代終止后，最終獲得精配準(zhǔn)的R，T

ICP算法的參數(shù)：
（1）距離閾值。兩個(gè)點(diǎn)云中距離小于距離閾值的點(diǎn)，才算作對(duì)應(yīng)點(diǎn)。
（2）迭代的終止條件。

至此，完成點(diǎn)云數(shù)據(jù)的配準(zhǔn)。

參考文獻(xiàn)：
[1] Aiger D, Mitra N J, Cohen-Or D. 4-points congruent sets for robust pairwise surface registration[J]. Acm Transactions on Graphics, 2008, 27(3):85.
[2]https://blog.csdn.net/u011600592/article/details/70258097

總結(jié)

以上是生活随笔為你收集整理的三维点云配准的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

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