手写VIO第一讲习题:第二题, 更新方式验证

#include<iostream>
#include<cmath>
#include"/usr/include/eigen3/Eigen/Core"
//包含几何模块
#include"/usr/include/eigen3/Eigen/Geometry"
#include "/usr/local/include/sophus/so3.h"
using namespace std;

int main(int argc,char **argv)
{

    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(1,1,1)).toRotationMatrix();
    Sophus::SO3 SO3_R(R);
    Sophus::SO3 SO3_q(R);
    cout<<"SO(3) from matrix: "<<SO3_R<<endl;
    cout<<"SO(3) from quaterniond: "<<SO3_q<<endl;

    Eigen::Quaterniond q(R);
    Eigen::Vector3d w(0.01,0.02,0.03);//小量w
    Eigen::Matrix3d rotation_matrix3;

    //扰动更新
    Sophus::SO3 SO3_updated = SO3_R*Sophus::SO3::exp(w);
    cout<<"扰动更新结果: "<<SO3_updated<<endl;

    //四元数更新
    Eigen::Quaterniond q_update(1,0.005,0.01,0.015);
    q_update.normalize();//归一化
    Eigen::Quaterniond q_updated;
    q_updated = q*q_update;
    Sophus::SO3 SO3_qupdated(q_updated);
    cout << "四元数更新结果:" << SO3_qupdated;
    return 0;
}