ICP算法簡介
根據點雲數據所包含的空間信息，可以直接利用點雲數據進行配准。主流算法為最近迭代算法（ICP，Iterative Closest Point）,該算法是根據點雲數據首先構造局部幾何特征，然后再根據局部幾何特征進行點雲數據重定位。
一、 ICP原理
假設兩個點雲數據集合P和G，要通過P轉換到G（假設兩組點雲存在局部幾何特征相似的部分），可以通過P叉乘四元矩陣進行旋轉平移變換到G，或者SVD法將P轉換到G位置，總體思想都是需要一個4x4的旋轉平移矩陣。對於每次旋轉平移變換后計算P的所有（采樣）點到G對應（最近）點的距離，用最小二乘法（求方差）求出最小二乘誤差，看是否在要求的范圍內，如果最小二乘誤差小於設定的值，（或迭代次數達到上限，或每次重新迭代后最小二乘誤差總在一個很小的范圍內不再發生變化），則計算結束，否則繼續進行迭代。
粗配准優化方法：主成分分析法
精配准優化方法：基於正交投影的ICp算法改進
二、粗配准優化
PCA是一種有效的檢測數據集簡化分析方法，用於減少數據集的維數，同時保持數據集對方差貢獻最大特征，對於點集P(x1,x2,…,xn)，其中，xi是n維數據，均值和協方差矩陣分別為：
$\bar x = \frac{{\mathop \sum \nolimits_{i = 1}^n {x_i}}}{n}$
${\rm{cov}} = \frac{{\mathop \sum \nolimits_{i = 1}^n \left( {{x_i} - \bar x} \right){{\left( {{x_i} - \bar x} \right)}^T}}}{n}$
協方差矩陣cov的特征向量，即為點集P的主軸。對於三維點雲數據，以均值為坐標系的原點，PCA求得的三個特征向量對應XYZ軸，建立點雲的參考坐標系。
由於PCA反應了數據集對方差貢獻的最大特征，相似度大的兩片點雲，只要把其參考坐標系調整到一致，即可達到初始配准的目的。由於可能出現坐標軸的兩個方向相差180o的情況，需要建立最小包圍盒來測試兩片點雲是否調整重合，通過坐標變換可以將數據點雲包圍盒變換到模型點雲參考坐標系中，使兩包圍盒的空間位置大體一致。計算包圍盒的重合體積，如果大於某一設定的容差，則兩片點雲大致重合，如果小於設定容差，則反轉數據點雲參考坐標系坐標軸再次嘗試。通過坐標系調整后，即可達到點雲的初始配准目的，為下一步的精確配准提供較好的初值。
三、精配准優化
通過初始配准，兩片點雲大致重合但是誤差精度還遠遠達不到實際應用中的精度要求。為了使用點雲間的誤差達到最小，還需要對其進行精確配准。ICP算法是最常用的數據精確配准方法，算法在每次迭代的過程中，對數據點雲的每一點，在模型點雲中尋找歐氏距離最近點作為對應點，通過這組對應點使目標函數s2最小化：
${{\rm{s}}^2} = {\rm{min}}\mathop \sum \limits_{i = 1}^N {\left\| {{Q_i} - \left( {R{P_i} + t} \right)} \right\|^2}$
來得到最優的四維轉換矩陣（包含平移和旋轉），將四維變換矩陣作用到點雲數據上，得到新的數據點雲帶入下次迭代過程。但是ICP算法具有比較明顯的缺陷，要求數據點雲上的每一點在模型點雲上都要找到對應點，為了尋找對應點，算法需要遍歷模型點雲上的每一點，配准速度很慢，且容易陷入局部最優解。
由於ICP算法的時間消耗主要花費在計算對應點集上，如果能降低此步驟的時間消耗，即可以提高ICP算法效率，具體計算請參考博客點雲對齊。