成都市网站设,网站换模板,做网站托管,强的网站建设最近可能要用三维点云实现一个三维场景重建的功能#xff0c;从经典的ICP算法开始#xff0c;啃了一些文档#xff0c;对其原理也是一知半解。 迭代最近点算法综述 大致参考了这份文档之后#xff0c;照流程用MATLAB实现了一个简单的ICP算法#xff0c;首先是发现这份文档…最近可能要用三维点云实现一个三维场景重建的功能从经典的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 为了看到迭代过程这段代码每次只是进行一次迭代但是实际情况下需要不断迭代直到两点集的方差收敛达到拟合要求。 用随机数生成了一个含一百个点的点集A,并对A进行一次随机的空间变化得到B这样A,B是完全可以拟合的两个点集 点集A: 点集B: 用A,B来验证算法能不能实现点集的拟合。 试验了几次之后发现无法收敛 问题应该出在旋转四元数和旋转矩阵求解上这块是一直没能理解透彻的部分。 转载于:https://www.cnblogs.com/moranBlogs/p/3798257.html
