Sophus介绍

1.sophus介绍

李群和李代数是slam的优化中必不可少的工具，虽然Eigen提供了几何模块，没有提供李代数的支持，一个较好的李代数库是由Sophus库，它提供了S0(3)，SE(3),。它是直接在Eigen的基础上开发的。所以不需要安装额外的依赖库，关于Eigen的使用，可以参考Eigen的介绍

2.安装

git clone https://github.com/strasdat/Sophus.git
cd Sophus/
mkdir build
cd build
cmake ..
make
sudo make install

3.使用方法

sophus提供了李群S0S,SE3，及李群和李代数之间的指数和对数映射，增量扰动模型的更新

3.1、so3

SO3的初始化

Matrix3d R=AngleAxisd(M_PI/2,Vector3d(0,0,1).toRotationMatrix();
//或者四元数
Quateriond q(R);
//李群的构造
Sophus::SO3d SO3_R(R);
Sophus::SO3d SO3_q(q);
//二者完全等价
cout<<"SO(3) from matrix: \n"<<SO3_R.matrix()<<endl;
cout<<"SO(3) from quaternion: \n"<<SO3_Q.matrix()<<endl;

so3的初始化：对数映射获得李代数

Vector3d so3=SO3_R.log();
cout<<"so3 = "<<so3.transpose();<<endl;

反对称变换

//hat为向量到反对称矩阵
Eigen::Matrix3d so3_matrix=Sophus::SO3d::hat(so3);
cout<<"so3 hat =\n"<<so3_matrix<<endl;
//相对的，vee为反对称矩阵到向量
cout<<"so3 hat vee = "<<Sophus::SO3d::vee(so3_matrix).transpose()<<endl;

增量扰动的更新

Vector3d update_so3(1e-4,0,0);
Sophus::SO3d SO3_updated=Sophus::SO3d::exp(update_so3)*SO3_R;
cout<<"SO3 update =\n"<<SO3_updated.matrix()<<endl;

3.2、SE3

对SE3的操作大同小异

		Vector3d t(1,0,0);
    Sophus::SE3d SE3_RT(R,t);
    Sophus::SE3d SE3_QT(q,t);
    cout<<"SE3 from R,t = \n"<<SE3_RT.matrix()<<endl;
    cout<<"SE3 from Q,t = \n"<<SE3_QT.matrix()<<endl;
    //李代数是一个六维向量，方便起见，起别名
    typedef Eigen::Matrix<double,6,1>Vector6d;
    Vector6d se3=SE3_RT.log();
    cout<<"se3 = "<<se3.transpose()<<endl;
    //观察输出，平移在前，旋转在后
    //同理，有hat和vee两个算符
    cout<<"se3 hat ="<<Sophus::SE3d::hat(se3)<<endl;
    cout<<"se3 hat vee = "<<Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose()<<endl;
    //最后演示更新
    Vector6d update_se3;
    update_se3.setZero();
    update_se3(0,0)=1e-4;
    Sophus::SE3d SE3_updated=Sophus::SE3d::exp(update_se3)*SE3_RT;
    cout<<"SE3 updated = \n"<<SE3_updated.matrix()<<endl;