经典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来验证算法能不能实现点集的拟合。

试验了几次之后，发现无法收敛：

问题应该出在旋转四元数和旋转矩阵求解上，这块是一直没能理解透彻的部分。