经典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来验证算法能不能实现点集的拟合。
试验了几次之后，发现无法收敛：
问题应该出在旋转四元数和旋转矩阵求解上，这块是一直没能理解透彻的部分。
