最近可能要用三維點(diǎn)云實(shí)現(xiàn)一個(gè)三維場(chǎng)景重建的功能，從經(jīng)典的ICP算法開始，啃了一些文檔，對(duì)其原理也是一知半解。

?

迭代最近點(diǎn)算法綜述

大致參考了這份文檔之后，照流程用MATLAB實(shí)現(xiàn)了一個(gè)簡(jiǎn)單的ICP算法，首先是發(fā)現(xiàn)這份文檔中一個(gè)明顯的錯(cuò)誤，

公式6

?

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

?

Matlab代碼如下：

%%% ICP迭代最近點(diǎn)算法function [sourcePoint,aimPoint,distance] = ICPiterator( sourcePoint , targetPoint ) %%% 獲得匹配點(diǎn)集，重心 aimPoint = getAimPoint(sourcePoint,targetPoint); sourcePointCentre = getCentre(sourcePoint); aimPointCentre = getCentre(aimPoint); %%% 平移矩陣 T = getTranslation(aimPointCentre,sourcePointCentre); %%% 中心化 midSourcePoint = centreTransform(sourcePoint, sourcePointCentre); midAimPoint = centreTransform(aimPoint, aimPointCentre); %%%旋轉(zhuǎn)四元數(shù) quaternion = getRevolveQuaternion(midSourcePoint,midAimPoint); %%%旋轉(zhuǎn)矩陣 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%%% 點(diǎn)對(duì)搜索匹配，得到匹配點(diǎn)集 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%%%旋轉(zhuǎn)四元數(shù) function [quaternion] = getRevolveQuaternion( sourcePoint , targetPoint )%%% 協(xié)方差pp = sourcePoint' * targetPoint; range = size(sourcePoint,1); pp = pp / range; %%% 反對(duì)稱矩陣dissymmetryMatrix = pp - pp' ; %%% 列向量deltadelta = [dissymmetryMatrix(2,3) ; dissymmetryMatrix(3,1) ; dissymmetryMatrix(1,2)]; %%%對(duì)稱矩陣QQ = [ trace(pp) delta' ; delta pp + pp' - trace(pp)*eye(3) ]; %%%最大特征值，對(duì)應(yīng)特征向量即為旋轉(zhuǎn)四元數(shù)maxEigenvalues = max(eig(Q)); quaternion = null(Q - maxEigenvalues*eye(length(Q))); end%%% 旋轉(zhuǎn)矩陣 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%%% 點(diǎn)集重心 function [centre] = getCentre( point )range = length(point); centre = sum(point)/range; end%%% 獲取平移矩陣 function [T] = getTranslation( aimPointCentre , sourcePointCentre )T = aimPointCentre - sourcePointCentre; end%%% 點(diǎn)集中心化 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%%% 計(jì)算兩點(diǎn)距離的平方,即歐拉距離和 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

?

為了看到迭代過(guò)程，這段代碼每次只是進(jìn)行一次迭代，但是實(shí)際情況下需要不斷迭代，直到兩點(diǎn)集的方差收斂，達(dá)到擬合要求。

?

用隨機(jī)數(shù)生成了一個(gè)含一百個(gè)點(diǎn)的點(diǎn)集A,并對(duì)A進(jìn)行一次隨機(jī)的空間變化，得到B，這樣A,B是完全可以擬合的兩個(gè)點(diǎn)集；

?

點(diǎn)集A:

?

點(diǎn)集B:

?

用A,B來(lái)驗(yàn)證算法能不能實(shí)現(xiàn)點(diǎn)集的擬合。

?

試驗(yàn)了幾次之后，發(fā)現(xiàn)無(wú)法收斂：

?

問(wèn)題應(yīng)該出在旋轉(zhuǎn)四元數(shù)和旋轉(zhuǎn)矩陣求解上，這塊是一直沒(méi)能理解透徹的部分。

?

轉(zhuǎn)載于:https://www.cnblogs.com/moranBlogs/p/3798257.html

總結(jié)

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

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。