圖像配准是圖像處理研究領域中的一個典型問題和技術難點，其目的在於比較或融合針對同一對象在不同條件下獲取的圖像，例如圖像會來自不同的采集設備，取自不同的時間，不同的拍攝視角等等，有時也需要用到針對不同對象的圖像配准問題。具體地說，對於一組圖像數據集中的兩幅圖像，通過尋找一種空間變換把一幅圖像映射到另一幅圖像，使得兩圖中對應於空間同一位置的點一一對應起來，從而達到信息融合的目的。 一個經典的應用是場景的重建，比如說一張茶幾上擺了很多杯具，用深度攝像機進行場景的掃描，通常不可能通過一次采集就將場景中的物體全部掃描完成，只能是獲取場景不同角度的點雲，然后將這些點雲融合在一起，獲得一個完整的場景。
ICP（Iterative Closest Point迭代最近點）算法是一種點集對點集配准方法。如下圖所示，PR（紅色點雲）和RB（藍色點雲）是兩個點集，該算法就是計算怎么把PB平移旋轉，使PB和PR盡量重疊。
用數學語言描述如下，即ICP算法的實質是基於最小二乘法的最優匹配，它重復進行“確定對應關系的點集→計算最優剛體變換”的過程，直到某個表示正確匹配的收斂准則得到滿足。
ICP算法基本思想：
如果知道正確的點對應，那么兩個點集之間的相對變換（旋轉、平移）就可以求得封閉解。
首先計算兩個點集X和P的質心，分別為μx和μp
然后在兩個點集中分別減去對應的質心（Subtract the corresponding center of mass from every point in the two point sets before calculating the transformation）
目標函數E(R,t)的優化是ICP算法的最后一個階段。在求得目標函數后，采用什么樣的方法來使其收斂到最小，也是一個比較重要的問題。求解方法有基於奇異值分解的方法、四元數方法等。
如果初始點“足夠近”，可以保證收斂性
ICP算法優點：
- 可以獲得非常精確的配准效果
- 不必對處理的點集進行分割和特征提取
- 在較好的初值情況下，可以得到很好的算法收斂性
ICP算法的不足之處：
- 在搜索對應點的過程中，計算量非常大，這是傳統ICP算法的瓶頸
- 標准ICP算法中尋找對應點時，認為歐氏距離最近的點就是對應點。這種假設有不合理之處，會產生一定數量的錯誤對應點
針對標准ICP算法的不足之處，許多研究者提出ICP算法的各種改進版本，主要涉及如下所示的6個方面。
標准ICP算法中，選用點集中所有的點來計算對應點，通常用於配准的點集元素數量都是非常巨大的，通過這些點集來計算，所消耗的時間很長。在后來的研究中，提出了各種方法來選擇配准元素，這些方法的主要目的都是為了減小點集元素的數目，即如何選用最少的點來表征原始點集的全部特征信息。在點集選取時可以：1.選取所有點；2.均勻采樣（Uniform sub-sampling ）；3.隨機采樣（Random sampling）；4.按特征采樣（Feature based Sampling ）；5.法向空間均勻采樣（Normal-space sampling），如下圖所示，法向采樣保證了法向上的連續性（Ensure that samples have normals distributed as uniformly as possible）
基於特征的采樣使用一些具有明顯特征的點集來進行配准，大量減少了對應點的數目。
點集匹配上有：最近鄰點（Closet Point）
法方向最近鄰點（Normal Shooting）
投影法（Projection）
根據之前算法的描述，下面使用Python來實現基本的ICP算法（代碼參考了這里）：
import numpy as np 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 R: 3x3 rotation matrix t: 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_A BB = B - centroid_B # rotation matrix W = np.dot(BB.T, AA) U, s, VT = np.linalg.svd(W) R = np.dot(U, VT) # special reflection case if np.linalg.det(R) < 0: VT[2,:] *= -1 R = 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] = R T[0:3, 3] = t return T, R, t 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 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 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 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 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 distances, indices = nearest_neighbor(src[0:3,:].T, dst[0:3,:].T) # compute the transformation between the current source and nearest destination points 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 src = np.dot(T, src) # check error mean_error = np.sum(distances) / distances.size if abs(prev_error-mean_error) < tolerance: break prev_error = mean_error # calculcate final tranformation 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 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) np.set_printoptions(precision=3,suppress=True) print T
上面代碼創建一個源點集A（在0-100的整數范圍內隨機生成20個3維數據點），然后將A繞Z軸旋轉45°並沿XYZ軸分別移動一小段距離，得到點集B。結果如下，可以看出ICP算法正確的計算出了變換矩陣。
需要注意幾點：
1.首先需要明確公式里的變換是T（P→X）， 即通過旋轉和平移把點集P變換到X。我們這里求的變換是T（A→B），要搞清楚對應關系。
2.本例只用了20個點進行測試，ICP算法在求最近鄰點的過程中需要計算20×20次距離並比較大小。如果點的數目巨大，那算法的效率將非常低。
3.兩個點集的初始相對位置對算法的收斂性有一定影響，最好在“足夠近”的條件下進行ICP配准。
參考：
Iterative Closest Point (ICP) and other matching algorithms
http://www.mrpt.org/Iterative_Closest_Point_%28ICP%29_and_other_matching_algorithms
PCL學習筆記二：Registration (ICP算法)
http://www.voidcn.com/blog/u010696366/article/p-3712120.html