因为学校项目的原因,可能要实现一个三维场景重建的功能,然后从经典的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来验证算法能不能实现点集的拟合。
试验了几次之后,发现无法收敛:
问题应该出在旋转四元数和旋转矩阵求解上,这块是一直没能理解透彻的部分。
困境:经典ICP算法的一些问题
时间: 2024-10-11 23:08:09