首页 > 代码库 > 困境：经典ICP算法的一些问题
困境：经典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 endendend%%%旋转四元数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; endendfunction [point] = counterCentreTransform(point,centre)range = size(point,1);for i = 1:1:range point(i,:) = point(i,:) + centre; endend%%% 计算两点距离的平方,即欧拉距离和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来验证算法能不能实现点集的拟合。
试验了几次之后，发现无法收敛：
问题应该出在旋转四元数和旋转矩阵求解上，这块是一直没能理解透彻的部分。
声明：以上内容来自用户投稿及互联网公开渠道收集整理发布，本网站不拥有所有权，未作人工编辑处理，也不承担相关法律责任，若内容有误或涉及侵权可进行投诉： 投诉/举报 工作人员会在5个工作日内联系你，一经查实，本站将立刻删除涉嫌侵权内容。