算法流程如下：

1.首先得到同名点集P和X。

2.计算P和X的均值up和ux。

3.由P和X构造协方差矩阵sigma。

4.由协方差矩阵sigma构造4*4对称矩阵Q。

5.计算Q的特征值与特征向量。其中Q最大特征值对应的特征向量即为最佳旋转向量q。

6.通过四元数q得到旋转矩阵R。

7.根据R计算最佳平移向量qr。

clear all;
close all;
clc;%生成原始点集
X=[];Y=[];Z=[];
for i=-180:2:180for j=-90:2:90x = i * pi / 180.0;y = j * pi / 180.0;X =[X,cos(y) * cos(x)];Y =[Y,sin(y) * cos(x)];Z =[Z,sin(x)];end
end
P=[X(1:3000)' Y(1:3000)' Z(1:3000)'];%生成变换后点集
i=0.5;j=0.3;k=0.7;
Rx=[1 0 0;0 cos(i) -sin(i); 0 sin(i) cos(i)];
Ry=[cos(j) 0 sin(j);0 1 0;-sin(j) 0 cos(j)];
Rz=[cos(k) -sin(k) 0;sin(k) cos(k) 0;0 0 1];
R=Rx*Ry*Rz;
X=P*R + [0.2,0.3,0.4];plot3(P(:,1),P(:,2),P(:,3),'b.');
hold on;
plot3(X(:,1),X(:,2),X(:,3),'r.');%计算点集均值
up = mean(P);
ux = mean(X);P1=P-up;
X1=X-ux;%计算点集协方差
sigma=P1'*X1/(length(X1));
sigma_mi = sigma - sigma';
M=sigma+sigma'-trace(sigma)*[1,0,0;0,1,0;0,0,1];%由协方差构造4*4对称矩阵
Q=[trace(sigma) sigma_mi(2,3) sigma_mi(3,1) sigma_mi(1,2);sigma_mi(2,3) M(1,1) M(1,2) M(1,3);sigma_mi(3,1) M(2,1) M(2,2) M(2,3);sigma_mi(1,2) M(3,1) M(3,2) M(3,3)];%计算特征值与特征向量
[x,y] = eig(Q);
e = diag(y);%计算最大特征值对应的特征向量
lamda=max(e);
for i=1:length(Q)if lamda==e(i)break;end
end
q=x(:,i);q0=q(1);q1=q(2);q2=q(3);q3=q(4);%由四元数构造旋转矩阵
RR=[q0^2+q1^2-q2^2-q3^2 ,2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2);2*(q1*q2+q0*q3), q0^2-q1^2+q2^2-q3^2, 2*(q2*q3-q0*q1);2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), q0^2-q1^2-q2^2+q3^2];%计算平移向量
qr=ux-up*RR';%验证旋转矩阵与平移向量正确性
Pre = P*RR'+qr;figure;
plot3(P(:,1),P(:,2),P(:,3),'b.');
hold on;
plot3(X(:,1),X(:,2),X(:,3),'r.');plot3(Pre(:,1),Pre(:,2),Pre(:,3),'go');

参考

https://www.bbsmax.com/A/GBJrN6pRJ0/
https://wenku.baidu.com/view/859d2071f46527d3240ce073.html?sxts=1544685305315

对应点集配准的四元数法（ICP算法中的一个关键步骤）相关推荐

matlab点集配准,matlab练习程序（对应点集配准的四元数法）
clear all; close all; clc; %生成原始点集 X=[];Y=[];Z=[]; for i=-180:2:180 for j=-90:2:90 x = i * pi / 180. ...
在决策树类相关算法中，一个接点的基尼系数通常是大于还是小于他的父节点？是总是大于还是总是小于？
在决策树类相关算法中,一个接点的基尼系数通常是大于还是小于他的父节点?是总是大于还是总是小于? 下图为笔者构建的树模型并可视化之后的效果图: 可视化决策树的代码如下: 具体细节可以参考: 构建决策树模 ...
如何在matlab中表达点集,matlab练习程序（点集配准的SVD法）
上一篇博客中我们使用了四元数法计算ICP. 本篇我们使用SVD计算ICP. 下面是<视觉slam十四讲>中的计算方法: 计算步骤如下: 我们看到,只要求出了两组点之间的旋转,平移是非常容易 ...
ML之HMM：HMM算法相关论文、关键步骤、测试代码配图集合
ML之HMM:HMM算法相关论文.算法过程.关键步骤.测试代码配图集合目录 HMM算法相关论文 HMM算法算法过程 HMM算法关键步骤 HMM算法测试代码 HMM算法相关论文部分内容因为版本升级丢 ...
在医学图像分析中使用ICP算法进行点云配准
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达本文转载自「计算机视觉工坊」,该公众号重点在于介绍深度学习.智能驾驶等领域,一个小众的公众号. 论文标 ...
绝对不可错过的图形学算法！迭代最近点算法——ICP算法
图像配准是图像处理研究领域中的一个典型问题和技术难点,其目的在于比较或融合针对同一对象在不同条件下获取的图像,例如图像会来自不同的采集设备,取自不同的时间,不同的拍摄视角等等,有时也需要用到针对不同对 ...
论文翻译—ICP算法论文（节选1~4章）
标题:A Method for Registration of 3-D Shapes 作者: J. Besl, Neil D. McKay 目录: Ⅰ Introduction Ⅱ Literatur ...
VTK修炼之道58：图形基本操作进阶_点云配准技术（迭代最近点ICP算法）
1.Iterative Closest Points算法点云数据配准最经典的方法是迭代最近点算法(Iterative Closest Points,ICP).ICP算法是一个迭代的过程,每次迭代中对 ...
三维点集拟合：平面拟合、RANSAC、ICP算法
ACM算法分类:http://www.kuqin.com/algorithm/20080229/4071.html;CSDN容易吞图,不过编辑器里面图片还是显示的..... 一. 拟合一个平面 ...
点集配准技术（ICP、RPM、KC、CPD）
在计算机视觉和模式识别中,点集配准技术是查找将两个点集对齐的空间变换过程.寻找这种变换的目的主要包括:1.将多个数据集合并为一个全局统一的模型:2.将未知的数据集映射到已知的数据集上以识别其特征或估计 ...

对应点集配准的四元数法（ICP算法中的一个关键步骤）

参考

对应点集配准的四元数法（ICP算法中的一个关键步骤）相关推荐

最新文章

热门文章