matlab点集配准,matlab练习程序（对应点集配准的四元数法）

clear all;

close all;

clc;

%生成原始点集

X=[];Y=[];Z=[];

for i=-180:2:180

for j=-90:2:90

x = 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

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

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‘);

关注公众号： MATLAB基于模型的设计 (ID：xaxymaker) ，每天推送MATLAB学习最常见的问题，每天进步一点点，业精于勤荒于嬉。

打开微信扫一扫哦！

matlab点集配准,matlab练习程序（对应点集配准的四元数法）相关推荐

如何在matlab中表达点集,matlab练习程序（点集配准的SVD法）
上一篇博客中我们使用了四元数法计算ICP. 本篇我们使用SVD计算ICP. 下面是<视觉slam十四讲>中的计算方法: 计算步骤如下: 我们看到,只要求出了两组点之间的旋转,平移是非常容易 ...
ICP配准MATLAB实现
ICP配准MATLAB实现本文介绍了ICP算法及其MATLAB实现目录 ICP配准MATLAB实现一.ICP算法 1.算法简介 2. 算法局限性二.代码实现 1.平移旋转计算 2.ICP算法代 ...
图像平移配准matlab,(MATLAB应用图像处理)第6章MATLAB图像配准.ppt
第6章 MATLAB图像配准 6.1 图像配准概述 6.1.1 图像配准定义图像配准是对从不同传感器.不同时间.不同视点所获得的两幅或多幅图像进行最佳匹配,以达到空间匹配和叠加目的的处理过程.本质上 ...
matlab gui优化,matlabgui优化程序
的优化工具箱提供了各种优化函数,这些优化函数可以通过在命令行输入相应的函数名加以调用;此外为了使用方便,MATLAB 还提供了图形界面的优化工具 (GUI Optimization ...... ...
matlab中提供滤波器的种类有,求MATLAB巴特沃思低通滤波器程序
公告: 为响应国家净网行动,部分内容已经删除,感谢读者理解. 话题:求MATLAB巴特沃思低通滤波器程序.回答:击响应不变法函数 [bz,az]=impinvar(b,a,Fs) [bz,az]=im ...
如何将MATLAB程序发布为独立的不依赖MATLAB环境可执行的程序包（基于Matlab R2015b版）
如何将MATLAB程序发布为独立的不依赖MATLAB环境可执行的程序包(基于MatlabR2015b版) 关键字:MatlabR2015b,可独立运行,可脱离Matlab环境,m文件,Matlab程序 ...
matlab的exec程序,C++调用Matlab画图的一段程序
劳动节闲来无事,写了一天程序,just for fun. 看,这是C++调用Matlab画图的一段程序.暂时不想多解释了,有兴趣的话,看看下面的代码吧. 以下几段代码由上到下,越来越旧.最上面的是最新 ...
matlab 万年历,matlab时钟万年历 matlab时钟万年历（附word说明文档）：程序主要联合开发网 - pudn.com...
matlab时钟万年历所属分类:图形图像处理开发工具:matlab 文件大小:54KB 下载次数:6 上传日期:2018-08-24 21:54:23 上传者:小小葡萄123 说明: mat ...
matlab实现简单的交互式程序
matlab实现简单的交互式程序 x = input('请输入圆的半径='); y = pi*x^2; m = pi*x*2; fprintf('圆的面积为%f,圆的周长为%f\n',y,m);
PV_Characteristic：基于MATLAB/Simulink的光伏特性程序
PV_Characteristic:基于MATLAB/Simulink的光伏特性程序,改程序说明了太阳辐射强度.光伏电池温度.理想因子.光伏模块的串并联电阻如何影响光伏的输出特性曲线和输出功率 ID: ...

matlab点集配准,matlab练习程序（对应点集配准的四元数法）

matlab点集配准,matlab练习程序（对应点集配准的四元数法）相关推荐

最新文章

热门文章