这个算是ICP算法中的一个关键步骤,单独拿出来看一下。

算法流程如下:

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。

具体公式我就不贴图了,可以参考这篇“ICP算法在点云配准中的应用”论文的3.1节。

处理效果如下:

原始点集:

其中蓝点为原始点集,红点为旋转平移后的点集。

配准后点集:

计算得到的旋转平移矩阵,通过对蓝点集进行转换得到绿点集,比较红点集与绿点集是否基本一致。

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

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

打开微信扫一扫哦!

matlab练习程序(对应点集配准的四元数法)的更多相关文章

  1. matlab练习程序(点集配准的SVD法)

    上一篇博客中我们使用了四元数法计算ICP. 本篇我们使用SVD计算ICP. 下面是<视觉slam十四讲>中的计算方法: 计算步骤如下: 我们看到,只要求出了两组点之间的旋转,平移是非常容易 ...

  2. matlab练习程序(SUSAN检测)

    matlab练习程序(SUSAN检测) SUSAN算子既可以检测角点也可以检测边缘,不过角点似乎比不过harris,边缘似乎比不过Canny.不过思想还是有点意思的. 主要思想就是:首先做一个和原图像 ...

  3. (转)matlab练习程序(HOG方向梯度直方图)

    matlab练习程序(HOG方向梯度直方图)http://www.cnblogs.com/tiandsp/archive/2013/05/24/3097503.html HOG(Histogram o ...

  4. 点集配准技术(ICP、RPM、KC、CPD)

    在计算机视觉和模式识别中,点集配准技术是查找将两个点集对齐的空间变换过程.寻找这种变换的目的主要包括:1.将多个数据集合并为一个全局统一的模型:2.将未知的数据集映射到已知的数据集上以识别其特征或估计 ...

  5. matlab练习程序(旋转矩阵、欧拉角、四元数互转)

    欧拉角转旋转矩阵公式: 旋转矩阵转欧拉角公式: 旋转矩阵转四元数公式,其中1+r11+r22+r33>0: 四元数转旋转矩阵公式,q0^2+q1^2+q2^2+q3^2=1: 欧拉角转四元数公式 ...

  6. matlab练习程序(求向量间的旋转矩阵与四元数)

    问题是这样,如果我们知道两个向量v1和v2,计算从v1转到v2的旋转矩阵和四元数,由于旋转矩阵和四元数可以互转,所以我们先计算四元数. 我们可以认为v1绕着向量u旋转θ​角度到v2,u垂直于v1-v2 ...

  7. matlab示例程序--Motion-Based Multiple Object Tracking--卡尔曼多目标跟踪程序--解读

    静止背景下的卡尔曼多目标跟踪 最近学习了一下多目标跟踪,看了看MathWorks的关于Motion-Based Multiple Object Tracking的Documention. 官网链接:h ...

  8. matlab练习程序(透视投影,把lena贴到billboard上)

    本练习程序是受到了这个老外博文的启发,感觉挺有意思,就尝试了一下.他用的是opencv,我这里用的是matlab. 过去写过透视投影,当时是用来做倾斜校正的,这次同样用到了透视投影,不过更有意思,是将 ...

  9. matlab练习程序(多圆交点)

    最近总是对计算几何方面的程序比较感兴趣. 多圆求交点,要先对圆两两求交点. 有交点的圆分为相切圆和相交圆. 相切圆求法: 1.根据两圆心求直线 2.求公共弦直线方程 3.求两直线交点即两圆切点. 相交 ...

随机推荐

  1. [Swift]LeetCode222. 完全二叉树的节点个数 | Count Complete Tree Nodes

    Given a complete binary tree, count the number of nodes. Note: Definition of a complete binary tree ...

  2. [Swift]LeetCode649. Dota2 参议院 | Dota2 Senate

    In the world of Dota2, there are two parties: the Radiantand the Dire. The Dota2 senate consists of ...

  3. [Swift]LeetCode747. 至少是其他数字两倍的最大数 | Largest Number At Least Twice of Others

    In a given integer array nums, there is always exactly one largest element. Find whether the largest ...

  4. [Swift]LeetCode783. 二叉搜索树结点最小距离 | Minimum Distance Between BST Nodes

    Given a Binary Search Tree (BST) with the root node root, return the minimum difference between the ...

  5. Ubuntu12.04中的虚拟机安装Ubuntu16.04,并实现远程控制16.04

    目录 1.设置网络连接 1.1 VMware虚拟机网络地址设置 1.2 设置虚拟机内Ubuntu16.04的网络模式 未开机状态下 开机状态下 1.3 查看虚拟机中Ubuntu16.04的IP 2.进 ...

  6. 性能调优之Transformation

    优化之Aggregator组件 优化之Custom组件 优化之Joiner组件 优化之Lookup组件 优化之Normalizer组件 优化之Sequence Generator组件 优化之Sorte ...

  7. 面试题:合并2个有序数组(leetcode88)

    给定两个有序整数数组 nums1 和 nums2,将 nums2 合并到 nums1 中,使得 num1 成为一个有序数组. 说明: 初始化 nums1 和 nums2 的元素数量分别为 m 和 n. ...

  8. spring boot - 整合jpa

    <dependency> <groupId>org.springframework.boot</groupId> <artifactId>spring- ...

  9. Java连接数据库之MySQL

    工具: eclipse MySQL Navicat for MySQL MySQL 连接驱动:mysql-connector-java-5.0.4-bin.jar SQL 代码 CREATE TABL ...

  10. Git中用vim打开、修改、保存文件(转)

    一.vim 有两种工作模式: 1.命令模式:接受.执行 vim操作命令的模式,打开文件后的默认模式: 2.编辑模式:对打开的文件内容进行 增.删.改 操作的模式: 3.在编辑模式下按下ESC键,回退到 ...