最近可能要用三維點云實現一個三維場景重建的功能，從經典的ICP算法開始，啃了一些文檔，對其原理也是一知半解。

?

迭代最近點算法綜述

大致參考了這份文檔之后，照流程用MATLAB實現了一個簡單的ICP算法，首先是發現這份文檔中一個明顯的錯誤，

公式6

?

求兩個點集的協方差，其中(Pi-p)和(Qi-p')分別求兩個點集的各點與重心的差，都是（3*1）向量，這是不能相乘的，根據后文推斷，此物的結果應為（3*3）矩陣，所以我大(zuo)膽(si)的改為（Pi-p)' * (Qi-p')，做一次嘗試。

?

Matlab代碼如下：

%%% ICP迭代最近點算法function [sourcePoint,aimPoint,distance] = ICPiterator( sourcePoint , targetPoint ) %%% 獲得匹配點集，重心 aimPoint = getAimPoint(sourcePoint,targetPoint); sourcePointCentre = getCentre(sourcePoint); aimPointCentre = getCentre(aimPoint); %%% 平移矩陣 T = getTranslation(aimPointCentre,sourcePointCentre); %%% 中心化 midSourcePoint = centreTransform(sourcePoint, sourcePointCentre); midAimPoint = centreTransform(aimPoint, aimPointCentre); %%%旋轉四元數 quaternion = getRevolveQuaternion(midSourcePoint,midAimPoint); %%%旋轉矩陣 revolveMatrix = getRevolveMatrix(quaternion); %%%變換sourcePoint = midSourcePoint * revolveMatrix; sourcePoint = counterCentreTransform(sourcePoint,sourcePointCentre); range = length(sourcePoint); for i = 1:1:rangesourcePoint(i,:) = sourcePoint(i,:) + T; end%%%閾值判定，歐拉距離和 distance = getDistance(sourcePoint,aimPoint); end%%% 點對搜索匹配，得到匹配點集 function [aimPoint] = getAimPoint( sourcePoint , targetPoint ) rangeS = length(sourcePoint ); rangeT = length(targetPoint); aimPoint = zeros(rangeS,3); for i = 1:1:rangeSminDistance = getDistance(sourcePoint(i,:),targetPoint(1,:)); aimPoint(i,:) = targetPoint(1,:); for j = 1:1:rangeTdistance = getDistance(sourcePoint(i,:),targetPoint(j,:)); if distance < minDistanceminDistance = distance; aimPoint(i,:) = targetPoint(j,:); endend end end%%%旋轉四元數 function [quaternion] = getRevolveQuaternion( sourcePoint , targetPoint )%%% 協方差pp = sourcePoint' * targetPoint; range = size(sourcePoint,1); pp = pp / range; %%% 反對稱矩陣dissymmetryMatrix = pp - pp' ; %%% 列向量deltadelta = [dissymmetryMatrix(2,3) ; dissymmetryMatrix(3,1) ; dissymmetryMatrix(1,2)]; %%%對稱矩陣QQ = [ trace(pp) delta' ; delta pp + pp' - trace(pp)*eye(3) ]; %%%最大特征值，對應特征向量即為旋轉四元數maxEigenvalues = max(eig(Q)); quaternion = null(Q - maxEigenvalues*eye(length(Q))); end%%% 旋轉矩陣 function [revolveMatrix] = getRevolveMatrix(quaternion)revolveMatrix = [ quaternion(1,1)^2 + quaternion(2,1)^2 - quaternion(3,1)^2 - quaternion(4,1)^2 2 * (quaternion(2,1)*quaternion(3,1) - quaternion(1,1)*quaternion(4,1)) 2 * (quaternion(2,1)*quaternion(4,1) + quaternion(1,1)*quaternion(3,1));2 * (quaternion(2,1)*quaternion(3,1) + quaternion(1,1)*quaternion(4,1)) quaternion(1,1)^2 - quaternion(2,1)^2 + quaternion(3,1)^2 - quaternion(4,1)^2 2 * (quaternion(3,1)*quaternion(4,1) - quaternion(1,1)*quaternion(2,1)); 2 * (quaternion(2,1)*quaternion(4,1) - quaternion(1,1)*quaternion(3,1)) 2 * (quaternion(3,1)*quaternion(4,1) + quaternion(1,1)*quaternion(2,1)) quaternion(1,1)^2 - quaternion(2,1)^2 - quaternion(3,1)^2 + quaternion(4,1)^2 ]; end%%% 點集重心 function [centre] = getCentre( point )range = length(point); centre = sum(point)/range; end%%% 獲取平移矩陣 function [T] = getTranslation( aimPointCentre , sourcePointCentre )T = aimPointCentre - sourcePointCentre; end%%% 點集中心化 function [point] = centreTransform(point,centre) range = size(point,1); for i = 1:1:rangepoint(i,:) = point(i,:) - centre; end endfunction [point] = counterCentreTransform(point,centre) range = size(point,1); for i = 1:1:rangepoint(i,:) = point(i,:) + centre; end end%%% 計算兩點距離的平方,即歐拉距離和 function [distance] = getDistance(point1,point2)distance = (point1(1,1) - point2(1,1))^2 + (point1(1,2) - point2(1,2))^2 + (point1(1,3) - point2(1,3))^2; end

?

為了看到迭代過程，這段代碼每次只是進行一次迭代，但是實際情況下需要不斷迭代，直到兩點集的方差收斂，達到擬合要求。

?

用隨機數生成了一個含一百個點的點集A,并對A進行一次隨機的空間變化，得到B，這樣A,B是完全可以擬合的兩個點集；

?

點集A:

?

點集B:

?

用A,B來驗證算法能不能實現點集的擬合。

?

試驗了幾次之后，發現無法收斂：

?

問題應該出在旋轉四元數和旋轉矩陣求解上，這塊是一直沒能理解透徹的部分。

?

轉載于:https://www.cnblogs.com/moranBlogs/p/3798257.html

總結

以上是生活随笔為你收集整理的经典ICP算法的问题的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。