一.點(diǎn)云配準(zhǔn)介紹
1.點(diǎn)云配準(zhǔn)的概念
???????? 圖像配準(zhǔn)是圖像處理研究領(lǐng)域中的一個(gè)典型問(wèn)題和技術(shù)難點(diǎn)，其目的在于比較或融合針對(duì)同一對(duì)象在不同條件下獲取的圖像，例如圖像會(huì)來(lái)自不同的采集設(shè)備，取自不同的時(shí)間，不同的拍攝視角等等，有時(shí)也需要用到針對(duì)不同對(duì)象的圖像配準(zhǔn)問(wèn)題。具體地說(shuō)，對(duì)于一組圖像數(shù)據(jù)集中的兩幅圖像，通過(guò)尋找一種空間變換把一幅圖像映射到另一幅圖像，使得兩圖中對(duì)應(yīng)于空間同一位置的點(diǎn)一一對(duì)應(yīng)起來(lái)，從而達(dá)到信息融合的目的。 一個(gè)經(jīng)典的應(yīng)用是場(chǎng)景的重建，比如說(shuō)一張茶幾上擺了很多杯具，用深度攝像機(jī)進(jìn)行場(chǎng)景的掃描，通常不可能通過(guò)一次采集就將場(chǎng)景中的物體全部掃描完成，只能是獲取場(chǎng)景不同角度的點(diǎn)云，然后將這些點(diǎn)云融合在一起，獲得一個(gè)完整的場(chǎng)景。
? ? ? ? 簡(jiǎn)單點(diǎn)說(shuō)，點(diǎn)云配準(zhǔn)（Point Cloud Registration）指的是輸入兩幅點(diǎn)云?(source) 和?(target) ，輸出一個(gè)變換使得和的重合程度盡可能高?；蛘哒f(shuō)，對(duì)于兩個(gè)不同視角下的坐標(biāo)系，比如世界坐標(biāo)系和相機(jī)坐標(biāo)系，我們需要求出一個(gè)變換使得兩個(gè)坐標(biāo)系變換到統(tǒng)一視角下。我們這里只考慮剛性變換，即變換只包括旋轉(zhuǎn)、平移。
2.點(diǎn)云配準(zhǔn)的過(guò)程
? ? ? ? 目前，傳統(tǒng)的主流點(diǎn)云配準(zhǔn)技術(shù)主要包括粗配準(zhǔn)和精配準(zhǔn)兩個(gè)階段。粗配準(zhǔn)階段的目的是，對(duì)于任意初始狀態(tài)的兩片點(diǎn)云，使得兩片點(diǎn)云大致對(duì)齊，給旋轉(zhuǎn)矩陣R和平移向量T提供初值。而精配準(zhǔn)是在粗配準(zhǔn)的基礎(chǔ)上，進(jìn)行更精確、更細(xì)化的配準(zhǔn)?？偠灾?，點(diǎn)云配準(zhǔn)的過(guò)程就是矩陣變換的過(guò)程。
二.點(diǎn)云粗配準(zhǔn)算法
????????點(diǎn)云粗配準(zhǔn)又被稱為點(diǎn)云初始配準(zhǔn)，旨在對(duì)任意初始位置的兩片點(diǎn)云進(jìn)行粗略的配準(zhǔn)，使其大致對(duì)齊，從而為點(diǎn)云的精配準(zhǔn)提供良好的初始位置。點(diǎn)云粗配準(zhǔn)算法主要分為兩大類，分別是基于全局搜索思想的配準(zhǔn)方法和基于幾何特征描述的配準(zhǔn)方法。
1.基于全局搜索思想的配準(zhǔn)方法
???????? 基于全局搜索思想的配準(zhǔn)方法通常從源數(shù)據(jù)中隨機(jī)地選擇幾個(gè)點(diǎn)（通常是三個(gè)），并根據(jù)對(duì)目標(biāo)數(shù)據(jù)的窮舉搜索從目標(biāo)數(shù)據(jù)中找到對(duì)應(yīng)的點(diǎn)，計(jì)算所有可能的變換矩陣，通過(guò)投票的方式或者選取誤差函數(shù)最小的方式確定最優(yōu)變換。這種通過(guò)考慮所有可能的對(duì)應(yīng)關(guān)系，可以得到較好的配準(zhǔn)效果，但往往會(huì)產(chǎn)生很大的計(jì)算負(fù)荷。其中最常用的算法框架是RANSAC（隨機(jī)采樣一致性）算法。
1.1 RANSAC點(diǎn)云配準(zhǔn)算法
????????RANSAC 算法最早是在數(shù)學(xué)/統(tǒng)計(jì)學(xué)領(lǐng)域提出，它是一種利用隨機(jī)采集的樣本來(lái)準(zhǔn)確擬合出整體數(shù)學(xué)模型參數(shù)的方法。它的主要思想是從給定的樣本集中隨機(jī)選取一些樣本并估計(jì)一個(gè)數(shù)學(xué)模型，將樣本中的其余樣本帶入該數(shù)學(xué)模型中驗(yàn)證，如果有足夠多的樣本誤差在給定范圍內(nèi)，則該數(shù)學(xué)模型最優(yōu)，否則繼續(xù)循環(huán)該步驟。
? ? ? ? 后來(lái)，RANSAC算法被引入三維點(diǎn)云配準(zhǔn)領(lǐng)域，產(chǎn)生了基于RANSAC思想的RANSAC點(diǎn)云配準(zhǔn)算法，其算法過(guò)程主要如下：
? ? ? ? ?其本質(zhì)就是不斷的對(duì)源點(diǎn)云進(jìn)行隨機(jī)樣本采樣并求出對(duì)應(yīng)的變換模型，接著對(duì)每一次隨機(jī)變換模型進(jìn)行測(cè)試，并不斷循環(huán)該過(guò)程直到選出最優(yōu)的變換模型作為最終結(jié)果。根據(jù)大數(shù)定律，可知道在進(jìn)行大量重復(fù)采樣實(shí)驗(yàn)的情況之下，隨機(jī)模擬可以近似地將正確結(jié)果求解出來(lái)。 當(dāng)然，RANSAC配準(zhǔn)算法也存在著有限次隨機(jī)性帶來(lái)的不穩(wěn)定配準(zhǔn)和計(jì)算量大等弊端。
1.2 4PCS算法
? ? ? ? 4PCS算法全稱為 4-Points Congruent Sets 即 4點(diǎn)全等集配準(zhǔn)算法。該算法也是基于RANSAC算法框架，對(duì)兩片點(diǎn)云的初始姿態(tài)不做約束，針對(duì)搜索對(duì)應(yīng)點(diǎn)的策略進(jìn)行了優(yōu)化，將基本的三組對(duì)應(yīng)點(diǎn)擴(kuò)展到了四組具有一定約束性的對(duì)應(yīng)點(diǎn)集，大大增加了算法的魯棒性，提高了算法的搜索效率，算法的時(shí)間復(fù)雜度約為。該算法的核心思想就是利用剛體變換中的幾何不變性（向量/線段比例、點(diǎn)間歐幾里得距離），根據(jù)剛性變換后交點(diǎn)所占線段比例不變以及點(diǎn)之間的歐幾里得距離不變的特性，在目標(biāo)點(diǎn)云中盡可能尋找４個(gè)近似共面點(diǎn)（近似全等四點(diǎn)集）與之對(duì)應(yīng)，從而利用最小二乘法計(jì)算得到變換矩陣，基于RANSAC算法框架迭代選取多組基，根據(jù)最大公共點(diǎn)集（LCP）的評(píng)價(jià)準(zhǔn)則進(jìn)行比較得到最優(yōu)變換。
（1）全等四點(diǎn)集
? ? ? ? 在4PCS算法中，我們將局部配準(zhǔn)點(diǎn)云由三個(gè)點(diǎn)擴(kuò)展為四個(gè)點(diǎn)，并且這四個(gè)點(diǎn)具有一定的附加約束，如果能夠在目標(biāo)點(diǎn)云中也找到相應(yīng)的近似滿足約束的四點(diǎn)集，我們就將這兩個(gè)對(duì)應(yīng)四點(diǎn)集稱為全等四點(diǎn)集，用于求解點(diǎn)云變換。相比傳統(tǒng)的RANSAC配準(zhǔn)算法中完全隨機(jī)采樣的方式，通過(guò)全等四點(diǎn)集的應(yīng)用，一方面算法減少了計(jì)算量，提高了效率，使得全局搜索更有目標(biāo)性；另一方面算法使用帶有約束的局部四點(diǎn)配準(zhǔn)，準(zhǔn)確性和魯棒性更高。四點(diǎn)集的選擇和約束標(biāo)準(zhǔn)如下：
- 首先在源點(diǎn)云中隨機(jī)選擇三個(gè)點(diǎn)，要求這三點(diǎn)所構(gòu)成的三角形面積盡量的大（三點(diǎn)確定一個(gè)平面，向量叉積可以計(jì)算面積），但是三點(diǎn)間的距離不能超過(guò)一定的閾值上限，該上限由兩片點(diǎn)云的給定重疊率 f 確定。因?yàn)槿c(diǎn)之間距離越大，配準(zhǔn)的魯棒性越高；但距離過(guò)大，三點(diǎn)均在兩點(diǎn)云的重疊區(qū)域之外了，配準(zhǔn)效果又不好。因此需要在滿足上限的基礎(chǔ)之上，盡可能保證大的三級(jí)形面積。若沒(méi)有給定點(diǎn)云重疊率f，則可以進(jìn)行f=1.0,0.75,0.5...重疊率遞減測(cè)試，選擇最優(yōu)變換。
- 確定三點(diǎn)后，源點(diǎn)云四點(diǎn)集中第四點(diǎn)的選擇方式為：遍歷源點(diǎn)云中所有的點(diǎn)，對(duì)每一個(gè)點(diǎn)進(jìn)行計(jì)算驗(yàn)證選擇最優(yōu)的第四點(diǎn)。第四點(diǎn)需要與其他三點(diǎn)組成的平面盡可能的共面（即不強(qiáng)制要求四點(diǎn)共面，但第四點(diǎn)到其他三點(diǎn)平面的距離盡可能?。⑶业谒狞c(diǎn)與其他三點(diǎn)的距離也要滿足距離閾值范圍（不能太近不能太遠(yuǎn)）。
- 源點(diǎn)云中的四點(diǎn)集選擇完成后，就可以計(jì)算其四點(diǎn)構(gòu)成的兩線段交點(diǎn)的變換不變比，根據(jù)不變比在目標(biāo)點(diǎn)云中遍歷搜索對(duì)應(yīng)的滿足該約束所有四點(diǎn)集用于配準(zhǔn)計(jì)算，這就是（近似）全等四點(diǎn)集。
（2）4PCS算法流程
? ? ? ? ?在了解了全等四點(diǎn)集這一核心概念之后，我們來(lái)介紹一下基于全等四點(diǎn)集尋找對(duì)應(yīng)點(diǎn)的4PCS的算法步驟如下：
- STEP1：在源點(diǎn)云P中，使用上述的四點(diǎn)集的選擇方法隨機(jī)選擇一個(gè)四點(diǎn)集B={b1,b2,b3,b4}，其中(b1,b2)確定線段1，(b3,b4)確定線段2。接著去計(jì)算不變量d1=|b1-b2|，d2=|b3-b4|，不變比r1=|b1-e|/|b1-b2|，r2=|b3-e|/|b3-b4|。注意因?yàn)樗狞c(diǎn)不一定共面，兩條線段也不一定相交，所以可以使用連接兩個(gè)線段的最近點(diǎn)的中心點(diǎn)作為“交點(diǎn)”。
- STEP2：在目標(biāo)點(diǎn)云Q中，遍歷所有的點(diǎn)對(duì)，篩選滿足第一個(gè)約束（線段長(zhǎng)度在d1或d2誤差范圍內(nèi)）的點(diǎn)對(duì)集合R1，R2。其表示為：
- STEP3：遍歷點(diǎn)對(duì)集合R1中的所有點(diǎn)對(duì)元素，計(jì)算其線段上滿足不變比r1的目標(biāo)交點(diǎn)，然后將所有計(jì)算結(jié)果e存入搜索樹ANN Tree（近似最鄰近搜索樹，最常見(jiàn)的是K-D Tree算法），并構(gòu)建相應(yīng)的映射
- STEP4：遍歷點(diǎn)對(duì)集合R2中的所有點(diǎn)對(duì)元素，計(jì)算其線段上滿足不變比r2的目標(biāo)交點(diǎn)，并構(gòu)建相應(yīng)的映射。然后遍歷所有的點(diǎn)，在之前構(gòu)建的ANN Tree中搜索可接受誤差范圍內(nèi)的重合點(diǎn)，若可找到則說(shuō)明能在Q中找到一個(gè)對(duì)應(yīng)的近似全等四點(diǎn)集。最終求得所有的近似全等四點(diǎn)集
- STEP5：遍歷所有的近似全等四點(diǎn)集，對(duì)每一個(gè)，通過(guò)最小二乘法計(jì)算其與B的對(duì)應(yīng)變換矩陣。然后使用該變換矩陣對(duì)源點(diǎn)云P進(jìn)行變換得到P'，統(tǒng)計(jì)P'與Q中的最大公共點(diǎn)集（LCP），記max(LCP)的變換矩陣為本次迭代的最優(yōu)變換矩陣T并保留。
- STEP6：不斷迭代這個(gè)過(guò)程，記錄最優(yōu)的變換。迭代結(jié)束后得到的變換矩陣即為最優(yōu)變換矩陣。原論文算法框架如下：
注意：該算法的實(shí)現(xiàn)過(guò)程中還使用了一些增強(qiáng)方法。比如在上述不變量的基礎(chǔ)上，添加了對(duì)應(yīng)點(diǎn)的法向量計(jì)算，只有滿足線段長(zhǎng)度近似相等且端點(diǎn)法向量夾角也近似相等的前提下，才認(rèn)為是一對(duì)對(duì)應(yīng)線段/向量，進(jìn)一步加強(qiáng)搜索條件，減少了搜索數(shù)量。
（3）原論文及代碼地址
- 論文地址：4-points Congruent Sets for Robust Surface Registration (stanford.edu)
- 代碼地址：http://vecg.cs.ucl.ac.uk/Projects/SmartGeometry/fpcs/paper_docs/4pcs_1.3.tar.gz
2.基于幾何特征描述的配準(zhǔn)方法
待補(bǔ)。
2.1 FPFH算法
待補(bǔ)。
三.點(diǎn)云精配準(zhǔn)算法
????????經(jīng)過(guò)粗配準(zhǔn)之后，兩片點(diǎn)云的重疊部分已經(jīng)可以大致對(duì)齊，但精度還遠(yuǎn)遠(yuǎn)達(dá)不到我們的要求。精配準(zhǔn)算法就是在粗配準(zhǔn)的基礎(chǔ)上再進(jìn)行進(jìn)一步的配準(zhǔn)，提升配準(zhǔn)的精度。目前精配準(zhǔn)算法中主流的是ICP（Iterative Closest Point，迭代最近點(diǎn)）算法。
1.ICP算法
????????ICP算法要求待配準(zhǔn)的兩片點(diǎn)云具有較好的初始位置（初始變換R和T），即要求兩片點(diǎn)云大致對(duì)齊。其基本思想是選取兩片點(diǎn)云中距離最近的點(diǎn)作為對(duì)應(yīng)點(diǎn)，通過(guò)所有對(duì)應(yīng)點(diǎn)對(duì)求解旋轉(zhuǎn)和平移變換矩陣，并通過(guò)不斷迭代的方式使兩片點(diǎn)云之間的誤差越來(lái)越小，直至滿足我們提前設(shè)定的閾值要求或迭代次數(shù)。 ICP算法的算法框架如下：
? ? ? ? ?可以看到，整個(gè)ICP算法迭代可以拆解為兩個(gè)核心子問(wèn)題，即尋找匹配最近點(diǎn)和計(jì)算最優(yōu)變換。接下來(lái)我們將對(duì)這兩個(gè)核心問(wèn)題分別進(jìn)行說(shuō)明。
1.1 ICP算法核心說(shuō)明
（1）尋找匹配最近點(diǎn)
????????利用初始變換? 、或上一次迭代得到的變換，對(duì)初始點(diǎn)云P進(jìn)行變換，得到一個(gè)臨時(shí)的變換點(diǎn)云P'。然后用這個(gè)點(diǎn)云P'和目標(biāo)點(diǎn)云Q進(jìn)行匹配，找出源點(diǎn)云中每一個(gè)點(diǎn)在目標(biāo)點(diǎn)云中的最近鄰點(diǎn)作為對(duì)應(yīng)點(diǎn)，為接下來(lái)的計(jì)算最優(yōu)變換做準(zhǔn)備。常見(jiàn)的最鄰近點(diǎn)匹配方法有：
- 暴力循環(huán)法：對(duì)兩點(diǎn)云分別進(jìn)行雙重循環(huán)，遍歷匹配每一個(gè)點(diǎn)對(duì)，計(jì)算其歐氏距離，選擇距離最近的作為該點(diǎn)的對(duì)應(yīng)點(diǎn)并保存。該方法的復(fù)雜度為
- ANN(Approximate Nearest Neighbor) 搜索：使用近似最近鄰搜索算法，將點(diǎn)插入搜索樹結(jié)構(gòu)上，最常見(jiàn)的搜索樹結(jié)構(gòu)算法是K-D Tree，加速搜索匹配過(guò)程，該方法的復(fù)雜度為。我們主要使用該KD Tree算法進(jìn)行查找加速。
（2）計(jì)算最優(yōu)變換
? ? ? ? 在找到最近匹配對(duì)應(yīng)點(diǎn)之后，我們需要計(jì)算使得兩片對(duì)應(yīng)點(diǎn)云配準(zhǔn)的最優(yōu)變換參數(shù)R和T。假設(shè)分別表示源點(diǎn)云和目標(biāo)點(diǎn)云，則我們的目標(biāo)優(yōu)化函數(shù)為最小化變換后對(duì)應(yīng)點(diǎn)之間的距離，即 ：
? ? ? ? ?在這里我們將使用SVD奇異值分解來(lái)計(jì)算最優(yōu)變換參數(shù)，下面給出最終計(jì)算公式結(jié)果。關(guān)于該定理的證明可以參考我之前的博客或文章：?https://zhuanlan.zhihu.com/p/104735380文章來(lái)源：http://www.zghlxwxcb.cn/news/detail-787296.html
文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-787296.html
1.2 ICP算法代碼實(shí)現(xiàn)
import numpy as np
#計(jì)算最優(yōu)變換參數(shù)R、T（SVD奇異值分解）
def best_fit_transform(A, B):
'''
Calculates the least-squares best-fit transform between corresponding 3D points A->B
Input:
A: Nx3 numpy array of corresponding 3D points
B: Nx3 numpy array of corresponding 3D points
Returns:
T: 4x4 homogeneous transformation matrix 齊次坐標(biāo)
R: 3x3 rotation matrix
t: 3x1 column vector
'''
assert len(A) == len(B)
# translate points to their centroids
# 求點(diǎn)云質(zhì)心，并變換坐標(biāo)，消除平移的影響
centroid_A = np.mean(A, axis=0)
centroid_B = np.mean(B, axis=0)
AA = A - centroid_A
BB = B - centroid_B
# rotation matrix
# 變換后的坐標(biāo)對(duì)應(yīng)點(diǎn)相乘得到W
W = np.dot(BB.T, AA)
U, s, VT = np.linalg.svd(W) #對(duì)W進(jìn)行SVD分解，得到矩陣
R = np.dot(U, VT) #最優(yōu)旋轉(zhuǎn)矩陣=UV^T
# special reflection case
# 反射特殊情況考慮
if np.linalg.det(R) < 0:
VT[2,:] *= -1
R = np.dot(U, VT)
# translation
# 最優(yōu)平移
t = centroid_B.T - np.dot(R,centroid_A.T)
# homogeneous transformation
# 構(gòu)造齊次坐標(biāo)
T = np.identity(4)
T[0:3, 0:3] = R
T[0:3, 3] = t
return T, R, t
#尋找最近匹配點(diǎn)（暴力二重循環(huán)法），可以使用NearestNeighbors替換搜索
def nearest_neighbor(src, dst):
'''
Find the nearest (Euclidean) neighbor in dst for each point in src
Input:
src: Nx3 array of points
dst: Nx3 array of points
Output:
distances: Euclidean distances (errors) of the nearest neighbor
indecies: dst indecies of the nearest neighbor
'''
indecies = np.zeros(src.shape[0], dtype=np.int)
distances = np.zeros(src.shape[0])
for i, s in enumerate(src):
min_dist = np.inf
for j, d in enumerate(dst):
dist = np.linalg.norm(s-d)
if dist < min_dist:
min_dist = dist
indecies[i] = j
distances[i] = dist
return distances, indecies
#ICP算法迭代
def icp(A, B, init_pose=None, max_iterations=50, tolerance=0.001):
'''
The Iterative Closest Point method
Input:
A: Nx3 numpy array of source 3D points 原點(diǎn)云
B: Nx3 numpy array of destination 3D point 目標(biāo)點(diǎn)云
init_pose: 4x4 homogeneous transformation 初始變換參數(shù)
max_iterations: exit algorithm after max_iterations 最大迭代次數(shù)
tolerance: convergence criteria 收斂誤差
Output:
T: final homogeneous transformation 齊次坐標(biāo)變換矩陣
distances: Euclidean distances (errors) of the nearest neighbor 誤差
'''
# make points homogeneous, copy them so as to maintain the originals
src = np.ones((4,A.shape[0]))
dst = np.ones((4,B.shape[0]))
src[0:3,:] = np.copy(A.T)
dst[0:3,:] = np.copy(B.T)
# apply the initial pose estimation
# 點(diǎn)云初始變換
if init_pose is not None:
src = np.dot(init_pose, src)
prev_error = 0
for i in range(max_iterations):
# find the nearest neighbours between the current source and destination points
#1.找最近匹配點(diǎn)對(duì)應(yīng)
distances, indices = nearest_neighbor(src[0:3,:].T, dst[0:3,:].T)
# compute the transformation between the current source and nearest destination points
#2.計(jì)算最優(yōu)變換參數(shù)（SVD）
T,_,_ = best_fit_transform(src[0:3,:].T, dst[0:3,indices].T)
# update the current source
# refer to "Introduction to Robotics" Chapter2 P28. Spatial description and transformations
#3.更新變換點(diǎn)云
src = np.dot(T, src)
# check error
#4.檢查誤差是否收斂 MSELoss
mean_error = np.sum(distances) / distances.size
if abs(prev_error-mean_error) < tolerance:
break
prev_error = mean_error
# calculcate final tranformation
#5.迭代結(jié)束或誤差收斂后，計(jì)算最終的變換參數(shù)（初始A->當(dāng)前src，因?yàn)樽儞QT沒(méi)有迭代累計(jì)）
T,_,_ = best_fit_transform(A, src[0:3,:].T)
return T, distances
if __name__ == "__main__":
A = np.random.randint(0,101,(20,3)) # 20 points for test 隨機(jī)源點(diǎn)云
rotz = lambda theta: np.array([[np.cos(theta),-np.sin(theta),0],
[np.sin(theta),np.cos(theta),0],
[0,0,1]])
trans = np.array([2.12,-0.2,1.3])
B = A.dot(rotz(np.pi/4).T) + trans #隨即擾動(dòng)->目標(biāo)點(diǎn)云
T, distances = icp(A, B) #ICP算法計(jì)算得到最優(yōu)參數(shù)
np.set_printoptions(precision=3,suppress=True)
print T到了這里，關(guān)于點(diǎn)云配準(zhǔn)（三） 傳統(tǒng)點(diǎn)云配準(zhǔn)算法概述的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
![[點(diǎn)云配準(zhǔn)]LCD(2D-3D特征配準(zhǔn)算法)例程align_point_cloud.py解析](https://imgs.yssmx.com/Uploads/2024/02/481623-1.png)
