SLAM十四讲_第四讲(ch4)_Sophus的基本使用以及轨迹误差（包含代码详细注释，包含绘制估计轨迹和真实轨迹）

一、Sophus的基本使用

注：在编译前，应该装好Sophus库，该库要用到一些依赖，可自行去csdn查找
在github的slambook2(by_gaoxiang)上给出了相关的第三方库

该程序主要包含简单的李群李代数的的定义以及转换
源码如下：

//
//该程序包含Sophus库的基本使用
//包括 李群（SO3、SE3）  、 李代数（so3、 se3）  以及相应的转换
#include<iostream>
#include<cmath>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include"sophus/se3.hpp"
using namespace std;
using namespace Eigen;
int main(int argc, char** argv)
{
   
   
    cout<<"**********************************下面是SO3、 so3(旋转) ***********************************"<<endl;
    Matrix3d R;
    AngleAxisd  rotation_vector(M_PI/2,Vector3d(0,0,1));//旋转向量，绕z轴旋转90°
    R=rotation_vector.toRotationMatrix();//得到旋转矩阵R
    cout<< "旋转矩阵 R = " <<endl<<R<<endl<<endl;
    Quaterniond q(R);//得到四元数
    cout<< "四元数 q = " <<endl<<q.coeffs()<<endl<<endl;
    cout<<" *********************************李群*************************** "<<endl;
    Sophus::SO3d SO3_R(R);//李群是矩阵
    Sophus::SO3d SO3_q(q);
    cout<<"SO3 from rotation matrix : \n"<< SO3_R.matrix() <<endl;
    cout<<"SO3 from Quaternion : \n"<< SO3_q.matrix() <<endl;
    cout<<" *********************************利用对数映射得到李代数*************************** "<<endl;
    Vector3d so3=SO3_R.log();//李代数是三维向量
    cout<<"so3 = "<< so3.transpose()<<endl;
    cout<<" *****利用hat()函数，可以将向量变到反对称矩阵，利用vee()函数，可以将反对称矩阵变回向量****** "<<endl;
    cout<<"so3 hat= \n"<< Sophus::SO3d::hat(so3)<<endl;
    cout<<"so3 hat vee = "<< Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose()<<endl;
    cout<<" *********************************增量扰动模型的更新*************************** "<<endl;
    Vector3d so3_update(1e-4,0,0);//用李代数表示更新，李代数可以实现加减
    Sophus::SO3d SO3_update =Sophus::SO3d::exp(so3_update)*SO3_R;//先用李代数得到更新量，再把更新量变为李群，左乘原来的李群就得到更新了
    cout<<"SO3 update = \n"<< SO3_update.matrix() <<endl;
    cout<<"**********************************下面是SE3、 se3(将旋转和平移放在一起，也就是整个的变换) ***********************************"<<endl;
    //和上面SO3 类似
    Vector3d t(1,0,0);//translation
    Sophus::SE3d SE3_Rt(R,t);//李群SE3_Rt由旋转矩阵R和平移t构成
    Sophus::SE3d SE3_qt(q,t);//李群SE3_Rt由四元数q和平移t构成
    cout<<"SE3 from rotation matrix and t : \n"<< SE3_Rt.matrix() <<endl;
    cout<<"SO3 from Quaternion and t : \n"<< SE3_qt.matrix()<<endl;
    cout<<" *********************************利用对数映射得到李代数*************************** "<<endl;
    //李代数se3是六维向量，前三维表示平移，后三维表示旋转
    //利用typedef定义一个Vector6d
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3=SE3_Rt.log();
    cout<<" se3 = "<<se3.transpose()<<endl;
    cout<<" *****利用hat()函数，可以将向量变到反对称矩阵，利用vee()函数，可以将反对称矩阵变回向量****** "<<endl;
    cout<<"se3 hat= \n"<< Sophus::SE3d::hat(se3)<<endl;
    cout<<"so3 hat vee = "<< Sophus