VIO学习之第一章编程练习

请编程验证对于小量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;
    
}
下面是结果，可以看到，通过李代数和四元数的更新两个旋转矩阵确实相差不大