这个算是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]LeetCode272. 最近的二分搜索树的值 II $ Closest Binary Search Tree Value II

    Given a non-empty binary search tree and a target value, find k values in the BST that are closest t ...

  2. [Swift]LeetCode933. 最近的请求次数 | Number of Recent Calls

    Write a class RecentCounter to count recent requests. It has only one method: ping(int t), where t r ...

  3. Jmeter-阶梯场景设置

    接上一篇[Jmeter-常用线程组设置及场景运行时间计算] Jmeter复杂场景设计,依赖插件jp@gc - Stepping Thread Group (deprecated)和jp@gc - Ul ...

  4. spring boot - 整合jpa多对对关系保存和查询示例

    pojo: package com.example.zs.springDataJpa; import org.hibernate.annotations.Proxy; import javax.per ...

  5. Scala安装教程

    首先去Java官网下载Java的安装包 jdk-8u121-windows-x64.exe 再去Scala官网下载Scala的安装包 Scala2.12.1 安装Java: 配置Java环境变量(系统 ...

  6. linux安装字体方法

    1.查看系统中文字体 #fc-list :lang=zh 2.如果提示commont not fount 说明为安装fontconfig 3.安装fontconfig #yum -y install ...

  7. jenkins修改数据存放路径

    如果是用tomcat做容器的话,则在./bin/catalina.sh文件添加以下语句即可:export JENKINS_HOME="存放路径(需存在)" 例如:

  8. 『高次同余方程 Baby Step Giant Step算法』

    高次同余方程 一般来说,高次同余方程分\(a^x \equiv b(mod\ p)\)和\(x^a \equiv b(mod\ p)\)两种,其中后者的难度较大,本片博客仅将介绍第一类方程的解决方法. ...

  9. RabbitMQ学习笔记(三) 发布与订阅

    发布与订阅 在我们使用手机发送消息的时候,即可以选择给单个手机号码发送消息,也可以选择多个手机号码,群发消息. 前面学习工作队列的时候,我们使用的场景是一个消息只能被一个消费者程序实例接收并处理,但是 ...

  10. plsql中的procedure和function编程

    procedure:存储过程 eg:   create or replace procedure p   --这儿是和plsql的区别         is         cursor c      ...