经典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: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