經典ICP算法的問題

最近可能要用三維點雲實現一個三維場景重建的功能，從經典的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:range
    sourcePoint(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:rangeS
    minDistance = getDistance(sourcePoint(i,:),targetPoint(1,:));
    aimPoint(i,:) = targetPoint(1,:);
    for j = 1:1:rangeT
        distance = getDistance(sourcePoint(i,:),targetPoint(j,:));
        if distance < minDistance
            minDistance = distance;
            aimPoint(i,:) = targetPoint(j,:);
        end
    end
end
end

%%%旋轉四元數
function [quaternion] = getRevolveQuaternion( sourcePoint , targetPoint )
    %%% 協方差
    pp = sourcePoint' * targetPoint;
    range = size(sourcePoint,1);
    pp = pp / range;
    
    %%% 反對稱矩陣
    dissymmetryMatrix = pp - pp' ;
    
    %%% 列向量delta
    delta = [dissymmetryMatrix(2,3) ; dissymmetryMatrix(3,1) ; dissymmetryMatrix(1,2)];
    
    %%%對稱矩陣Q
    Q = [ 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:range
    point(i,:) = point(i,:) - centre;    
end
end

function [point] = counterCentreTransform(point,centre)
range = size(point,1);
for i = 1:1:range
    point(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來驗證算法能不能實現點集的擬合。

 

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

 

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