zoukankan      html  css  js  c++  java
  • matlab练习程序(对应点集配准的四元数法)

    这个算是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学习最常见的问题,每天进步一点点,业精于勤荒于嬉

     打开微信扫一扫哦!

  • 相关阅读:
    JS站点
    1011 World Cup Betting (20分)
    1007 Maximum Subsequence Sum (25分)(动态规划DP)
    1006 Sign In and Sign Out (25分)
    1005 Spell It Right (20分)
    1004 Counting Leaves (30分)(DFS)
    1003 Emergency (25分)(Dijkstra算法)
    1002 A+B for Polynomials (25分)
    1001 A+B Format (20分)
    canvas
  • 原文地址:https://www.cnblogs.com/52geek/p/10127499.html
Copyright © 2011-2022 走看看