点云配准（三） 传统点云配准算法概述
一.點云配準介紹
1.點云配準的概念
???????? 圖像配準是圖像處理研究領域中的一個典型問題和技術難點，其目的在于比較或融合針對同一對象在不同條件下獲取的圖像，例如圖像會來自不同的采集設備，取自不同的時間，不同的拍攝視角等等，有時也需要用到針對不同對象的圖像配準問題。具體地說，對于一組圖像數據集中的兩幅圖像，通過尋找一種空間變換把一幅圖像映射到另一幅圖像，使得兩圖中對應于空間同一位置的點一一對應起來，從而達到信息融合的目的。 一個經典的應用是場景的重建，比如說一張茶幾上擺了很多杯具，用深度攝像機進行場景的掃描，通常不可能通過一次采集就將場景中的物體全部掃描完成，只能是獲取場景不同角度的點云，然后將這些點云融合在一起，獲得一個完整的場景。
? ? ? ? 簡單點說，點云配準（Point Cloud Registration）指的是輸入兩幅點云?(source) 和?(target) ，輸出一個變換使得和的重合程度盡可能高。或者說，對于兩個不同視角下的坐標系，比如世界坐標系和相機坐標系，我們需要求出一個變換使得兩個坐標系變換到統一視角下。我們這里只考慮剛性變換，即變換只包括旋轉、平移。
2.點云配準的過程
? ? ? ? 目前，傳統的主流點云配準技術主要包括粗配準和精配準兩個階段。粗配準階段的目的是，對于任意初始狀態的兩片點云，使得兩片點云大致對齊，給旋轉矩陣R和平移向量T提供初值。而精配準是在粗配準的基礎上，進行更精確、更細化的配準。總而言之，點云配準的過程就是矩陣變換的過程。
二.點云粗配準算法
????????點云粗配準又被稱為點云初始配準，旨在對任意初始位置的兩片點云進行粗略的配準，使其大致對齊，從而為點云的精配準提供良好的初始位置。點云粗配準算法主要分為兩大類，分別是基于全局搜索思想的配準方法和基于幾何特征描述的配準方法。
1.基于全局搜索思想的配準方法
???????? 基于全局搜索思想的配準方法通常從源數據中隨機地選擇幾個點（通常是三個），并根據對目標數據的窮舉搜索從目標數據中找到對應的點，計算所有可能的變換矩陣，通過投票的方式或者選取誤差函數最小的方式確定最優變換。這種通過考慮所有可能的對應關系，可以得到較好的配準效果，但往往會產生很大的計算負荷。其中最常用的算法框架是RANSAC（隨機采樣一致性）算法。
1.1 RANSAC點云配準算法
????????RANSAC 算法最早是在數學/統計學領域提出，它是一種利用隨機采集的樣本來準確擬合出整體數學模型參數的方法。它的主要思想是從給定的樣本集中隨機選取一些樣本并估計一個數學模型，將樣本中的其余樣本帶入該數學模型中驗證，如果有足夠多的樣本誤差在給定范圍內，則該數學模型最優，否則繼續循環該步驟。
? ? ? ? 后來，RANSAC算法被引入三維點云配準領域，產生了基于RANSAC思想的RANSAC點云配準算法，其算法過程主要如下：
? ? ? ? ?其本質就是不斷的對源點云進行隨機樣本采樣并求出對應的變換模型，接著對每一次隨機變換模型進行測試，并不斷循環該過程直到選出最優的變換模型作為最終結果。根據大數定律，可知道在進行大量重復采樣實驗的情況之下，隨機模擬可以近似地將正確結果求解出來。 當然，RANSAC配準算法也存在著有限次隨機性帶來的不穩定配準和計算量大等弊端。
1.2 4PCS算法
? ? ? ? 4PCS算法全稱為 4-Points Congruent Sets 即 4點全等集配準算法。該算法也是基于RANSAC算法框架，對兩片點云的初始姿態不做約束，針對搜索對應點的策略進行了優化，將基本的三組對應點擴展到了四組具有一定約束性的對應點集，大大增加了算法的魯棒性，提高了算法的搜索效率，算法的時間復雜度約為。該算法的核心思想就是利用剛體變換中的幾何不變性（向量/線段比例、點間歐幾里得距離），根據剛性變換后交點所占線段比例不變以及點之間的歐幾里得距離不變的特性，在目標點云中盡可能尋找４個近似共面點（近似全等四點集）與之對應，從而利用最小二乘法計算得到變換矩陣，基于RANSAC算法框架迭代選取多組基，根據最大公共點集（LCP）的評價準則進行比較得到最優變換。
（1）全等四點集
? ? ? ? 在4PCS算法中，我們將局部配準點云由三個點擴展為四個點，并且這四個點具有一定的附加約束，如果能夠在目標點云中也找到相應的近似滿足約束的四點集，我們就將這兩個對應四點集稱為全等四點集，用于求解點云變換。相比傳統的RANSAC配準算法中完全隨機采樣的方式，通過全等四點集的應用，一方面算法減少了計算量，提高了效率，使得全局搜索更有目標性；另一方面算法使用帶有約束的局部四點配準，準確性和魯棒性更高。四點集的選擇和約束標準如下：
- 首先在源點云中隨機選擇三個點，要求這三點所構成的三角形面積盡量的大（三點確定一個平面，向量叉積可以計算面積），但是三點間的距離不能超過一定的閾值上限，該上限由兩片點云的給定重疊率 f 確定。因為三點之間距離越大，配準的魯棒性越高；但距離過大，三點均在兩點云的重疊區域之外了，配準效果又不好。因此需要在滿足上限的基礎之上，盡可能保證大的三級形面積。若沒有給定點云重疊率f，則可以進行f=1.0,0.75,0.5...重疊率遞減測試，選擇最優變換。
- 確定三點后，源點云四點集中第四點的選擇方式為：遍歷源點云中所有的點，對每一個點進行計算驗證選擇最優的第四點。第四點需要與其他三點組成的平面盡可能的共面（即不強制要求四點共面，但第四點到其他三點平面的距離盡可能小），并且第四點與其他三點的距離也要滿足距離閾值范圍（不能太近不能太遠）。
- 源點云中的四點集選擇完成后，就可以計算其四點構成的兩線段交點的變換不變比，根據不變比在目標點云中遍歷搜索對應的滿足該約束所有四點集用于配準計算，這就是（近似）全等四點集。
（2）4PCS算法流程
? ? ? ? ?在了解了全等四點集這一核心概念之后，我們來介紹一下基于全等四點集尋找對應點的4PCS的算法步驟如下：
- STEP1：在源點云P中，使用上述的四點集的選擇方法隨機選擇一個四點集B={b1,b2,b3,b4}，其中(b1,b2)確定線段1，(b3,b4)確定線段2。接著去計算不變量d1=|b1-b2|，d2=|b3-b4|，不變比r1=|b1-e|/|b1-b2|，r2=|b3-e|/|b3-b4|。注意因為四點不一定共面，兩條線段也不一定相交，所以可以使用連接兩個線段的最近點的中心點作為“交點”。
- STEP2：在目標點云Q中，遍歷所有的點對，篩選滿足第一個約束（線段長度在d1或d2誤差范圍內）的點對集合R1，R2。其表示為：
- STEP3：遍歷點對集合R1中的所有點對元素，計算其線段上滿足不變比r1的目標交點，然后將所有計算結果e存入搜索樹ANN Tree（近似最鄰近搜索樹，最常見的是K-D Tree算法），并構建相應的映射
- STEP4：遍歷點對集合R2中的所有點對元素，計算其線段上滿足不變比r2的目標交點，并構建相應的映射。然后遍歷所有的點，在之前構建的ANN Tree中搜索可接受誤差范圍內的重合點，若可找到則說明能在Q中找到一個對應的近似全等四點集。最終求得所有的近似全等四點集
- STEP5：遍歷所有的近似全等四點集，對每一個，通過最小二乘法計算其與B的對應變換矩陣。然后使用該變換矩陣對源點云P進行變換得到P'，統計P'與Q中的最大公共點集（LCP），記max(LCP)的變換矩陣為本次迭代的最優變換矩陣T并保留。
- STEP6：不斷迭代這個過程，記錄最優的變換。迭代結束后得到的變換矩陣即為最優變換矩陣。原論文算法框架如下：
注意：該算法的實現過程中還使用了一些增強方法。比如在上述不變量的基礎上，添加了對應點的法向量計算，只有滿足線段長度近似相等且端點法向量夾角也近似相等的前提下，才認為是一對對應線段/向量，進一步加強搜索條件，減少了搜索數量。
（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.基于幾何特征描述的配準方法
待補。
2.1 FPFH算法
待補。
三.點云精配準算法
????????經過粗配準之后，兩片點云的重疊部分已經可以大致對齊，但精度還遠遠達不到我們的要求。精配準算法就是在粗配準的基礎上再進行進一步的配準，提升配準的精度。目前精配準算法中主流的是ICP（Iterative Closest Point，迭代最近點）算法。
1.ICP算法
????????ICP算法要求待配準的兩片點云具有較好的初始位置（初始變換R和T），即要求兩片點云大致對齊。其基本思想是選取兩片點云中距離最近的點作為對應點，通過所有對應點對求解旋轉和平移變換矩陣，并通過不斷迭代的方式使兩片點云之間的誤差越來越小，直至滿足我們提前設定的閾值要求或迭代次數。 ICP算法的算法框架如下：
? ? ? ? ?可以看到，整個ICP算法迭代可以拆解為兩個核心子問題，即尋找匹配最近點和計算最優變換。接下來我們將對這兩個核心問題分別進行說明。
1.1 ICP算法核心說明
（1）尋找匹配最近點
????????利用初始變換? 、或上一次迭代得到的變換，對初始點云P進行變換，得到一個臨時的變換點云P'。然后用這個點云P'和目標點云Q進行匹配，找出源點云中每一個點在目標點云中的最近鄰點作為對應點，為接下來的計算最優變換做準備。常見的最鄰近點匹配方法有：
- 暴力循環法：對兩點云分別進行雙重循環，遍歷匹配每一個點對，計算其歐氏距離，選擇距離最近的作為該點的對應點并保存。該方法的復雜度為
- ANN(Approximate Nearest Neighbor) 搜索：使用近似最近鄰搜索算法，將點插入搜索樹結構上，最常見的搜索樹結構算法是K-D Tree，加速搜索匹配過程，該方法的復雜度為。我們主要使用該KD Tree算法進行查找加速。
（2）計算最優變換
? ? ? ? 在找到最近匹配對應點之后，我們需要計算使得兩片對應點云配準的最優變換參數R和T。假設分別表示源點云和目標點云，則我們的目標優化函數為最小化變換后對應點之間的距離，即 ：
? ? ? ? ?在這里我們將使用SVD奇異值分解來計算最優變換參數，下面給出最終計算公式結果。關于該定理的證明可以參考我之前的博客或文章：?https://zhuanlan.zhihu.com/p/104735380
1.2 ICP算法代碼實現
import numpy as np #計算最優變換參數R、T（SVD奇異值分解） def best_fit_transform(A, B):'''Calculates the least-squares best-fit transform between corresponding 3D points A->BInput:A: Nx3 numpy array of corresponding 3D pointsB: Nx3 numpy array of corresponding 3D pointsReturns:T: 4x4 homogeneous transformation matrix 齊次坐標R: 3x3 rotation matrixt: 3x1 column vector'''assert len(A) == len(B)# translate points to their centroids# 求點云質心，并變換坐標，消除平移的影響centroid_A = np.mean(A, axis=0)centroid_B = np.mean(B, axis=0)AA = A - centroid_ABB = B - centroid_B# rotation matrix# 變換后的坐標對應點相乘得到WW = np.dot(BB.T, AA)U, s, VT = np.linalg.svd(W) #對W進行SVD分解，得到矩陣R = np.dot(U, VT) #最優旋轉矩陣=UV^T# special reflection case# 反射特殊情況考慮if np.linalg.det(R) < 0:VT[2,:] *= -1R = np.dot(U, VT)# translation# 最優平移t = centroid_B.T - np.dot(R,centroid_A.T)# homogeneous transformation# 構造齊次坐標T = np.identity(4)T[0:3, 0:3] = RT[0:3, 3] = treturn T, R, t#尋找最近匹配點（暴力二重循環法），可以使用NearestNeighbors替換搜索 def nearest_neighbor(src, dst):'''Find the nearest (Euclidean) neighbor in dst for each point in srcInput:src: Nx3 array of pointsdst: Nx3 array of pointsOutput:distances: Euclidean distances (errors) of the nearest neighborindecies: 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.inffor j, d in enumerate(dst):dist = np.linalg.norm(s-d)if dist < min_dist:min_dist = distindecies[i] = jdistances[i] = dist return distances, indecies#ICP算法迭代 def icp(A, B, init_pose=None, max_iterations=50, tolerance=0.001):'''The Iterative Closest Point methodInput:A: Nx3 numpy array of source 3D points 原點云B: Nx3 numpy array of destination 3D point 目標點云init_pose: 4x4 homogeneous transformation 初始變換參數max_iterations: exit algorithm after max_iterations 最大迭代次數tolerance: convergence criteria 收斂誤差Output:T: final homogeneous transformation 齊次坐標變換矩陣distances: Euclidean distances (errors) of the nearest neighbor 誤差'''# make points homogeneous, copy them so as to maintain the originalssrc = 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# 點云初始變換if init_pose is not None:src = np.dot(init_pose, src)prev_error = 0for i in range(max_iterations):# find the nearest neighbours between the current source and destination points#1.找最近匹配點對應distances, indices = nearest_neighbor(src[0:3,:].T, dst[0:3,:].T)# compute the transformation between the current source and nearest destination points#2.計算最優變換參數（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.更新變換點云src = np.dot(T, src)# check error#4.檢查誤差是否收斂 MSELossmean_error = np.sum(distances) / distances.sizeif abs(prev_error-mean_error) < tolerance:breakprev_error = mean_error# calculcate final tranformation#5.迭代結束或誤差收斂后，計算最終的變換參數（初始A->當前src，因為變換T沒有迭代累計）T,_,_ = best_fit_transform(A, src[0:3,:].T)return T, distancesif __name__ == "__main__":A = np.random.randint(0,101,(20,3)) # 20 points for test 隨機源點云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 #隨即擾動->目標點云T, distances = icp(A, B) #ICP算法計算得到最優參數np.set_printoptions(precision=3,suppress=True)print T總結
以上是生活随笔為你收集整理的点云配准（三） 传统点云配准算法概述的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 项目管理之成熟度模型
- 下一篇： JeecgBoot框架学习