码迷,mamicode.com
首页 > 其他好文 > 详细

VIO学习之第一章编程练习

时间:2019-10-18 11:05:46      阅读:136      评论:0      收藏:0      [点我收藏+]

标签:family   com   div   注意   ade   orm   sop   表示   htm   

请编程验证对于小量w = [0.01, 0.02, 0.03]T,通过李代数和四元数两种更新方法得到的结果接近。

这段代码本身不难,但是可能对于初学者,对Eigen库和Sophus库不太熟悉,这里我把两个库的官方使用说明网站贴上来

Eigen库编程文档

Sophus库编程文档

有了这两文档,使用Eigen和Sophus不会再有编程上的问题,

还有一点值得注意的是:我们都是使用单位四元数表示旋转,这在下面的代码里也有对应,就是

q_new.normalize();

感兴趣的同学可以尝试把这行代码注释掉,看看结果如何。

#include <Eigen/Dense>
#include <iostream>
#include "sophus/so3.hpp"

using namespace std;
using namespace Eigen;

int main(){
    
    Eigen::Matrix3dR = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(1,1,1)).toRotationMatrix();
    
    Quaterniond q_raw(R);

    Quaterniond q_w(1.0,0.005,0.01,0.015);
    
    Quaterniond q_new = q_raw*q_w;
    
    q_new.normalize();
    
    Matrix3d R_new_q = q_new.toRotationMatrix();
    
    cout<<"R is "<< R_new_q << "by q"<<endl;

    Vector3d so3_w(0.01,0.02,0.03);
    
    Sophus::SO3 SO3_w = Sophus::SO3::exp(so3_w);

    Sophus::SO3 SO3_R_raw(R);

    Sophus::SO3 SO3_R_new_lie_l = SO3_R_raw * SO3_w;
    
    cout<<"R is "<< SO3_R_new_lie_l.matrix() << "by Lie"<<endl;
    
}
下面是结果,可以看到,通过李代数和四元数的更新两个旋转矩阵确实相差不大
技术图片

 

VIO学习之第一章编程练习

标签:family   com   div   注意   ade   orm   sop   表示   htm   

原文地址:https://www.cnblogs.com/jxLuTech/p/11696975.html

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!