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

#include
#include
#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: "<