视觉slam中Sophus库的基本使用

一、Sophus库简介：

Eigen库是一个开源的C++线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。但是Eigen库提供了集合模块，但没有提供李代数的支持。一个较好的李群和李代数的库是Sophus库，它很好的支持了SO(3),so(3),SE(3)和se(3)。Sophus库是基于Eigen基础上开发的，继承了Eigen库中的定义的各个类。因此在使用Eigen库中的类时，既可以使用Eigen命名空间，也可以使用Sophus命名空间。

Eigen：：Matrix3d和Sophus：：Matrix3d
Eigen：：Vector3d和Sophus：：Vector3d

此外，为了方便说明SE(4)和se(4)，Sophus库还typedef了Vector4d、Matrix4d、Vector6d和Matrix6d等，即：

Sophus：：Vector4d
Sophus：：Matrix4d
Sophus：：Vector6d
Sophus：：Matrix6d

二、Sophus的基本使用

笔者使用的是带模板的Sophus库，本库使用起来也很简单。其中李代数下的SO3的类为

Sophus::SO3d ,可以直接使用Eigen中的旋转矩阵和四元数进行初始化。

     AngleAxisd roation_vector(M_PI/2,Vector3d(0,0,1));
    Eigen::Matrix3d R=roation_vector.toRotationMatrix();

    Quaterniond q(R);
    Sophus::SO3d so3_R(R);
    Sophus::SO3d so3_Q(q);

了解李群李代数的都知道SO3上还是表示矩阵，我们初始化的矩阵如何进行输出？需要.matrix()输出矩阵。

    cout<<"this from matrix : \n"<<so3_R.matrix()<<endl;
    cout<<"this from quater : \n"<<so3_Q.matrix()<<endl;

1、使用对数映射转换为so3

SO3.log()就可以将SO3上的元素对数表示到so3中，其中so3的定义依然是Vector3d,其实就是Eigen中的类。

    Vector3d so3=so3_Q.log();
    cout<<"so3 = \n"<<so3.transpose()<<endl;

 2、so3->SO3

使用SO3d::hat()可以实现so3->SO3的转换。

    //将so3转换到SO3中
    cout<<"so3 hat= \n"<<Sophus::SO3d::hat(so3)<<end

 使用so3d::vee可以实现SO3->so3的转换：

    //SO3转换到so3中
    cout<<"SO3 vee=\n"<<Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose()<<endl

 3、增加扰动

 学过slam十四讲的应该知道，旋转矩阵不满足加法的封闭，这样求导就很麻烦。所以引了BCH近似公式来近似加法。Sophus中提供exp用来进行计算：

    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;

学完so3和SO3接下的SE3就很简单了是大同小异的，所以直接贴代码很容易就可以看懂。

4、SE3和se3

      //se3和SE3
    Vector3d t(1,0,0);
    Sophus::SE3d SE3_Rt(R,t);
    Sophus::SE3d SE3_qt(q,t);

    cout<<"this from matrix : \n"<<SE3_Rt.matrix()<<endl;
    cout<<"this from quater : \n"<<SE3_qt.matrix()<<endl;

    //因为se3为6维的向量，而Eigen中没有6为vecor6d,所以自己定义一个
    typedef Eigen::Matrix<double,6,1>  Vector6d;
    Vector6d se3=SE3_Rt.log();
    cout<<"se3= \n"<<se3.transpose()<<endl;     //平移在前而旋转在后

    //同样使用exp和vee
     //将se3转换到SE3中
    cout<<"se3 hat= \n"<<Sophus::SE3d::hat(se3)<<endl;

    //SE3转换到se3中
    cout<<"SE3 vee=\n"<<Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose()<<endl;

    //同样加入扰动
    Vector6d update_se3;
    update_se3.setZero();
    update_se3(1,0)=1e-4d;
    cout<<"update_se3 is \n"<<update_se3.transpose()<<endl;
    Sophus::SE3d SE3_updated=Sophus::SE3d::exp(update_se3)*SE3_Rt;
    cout<<" SE3 update is \n"<<SE3_updated.matrix()<<endl;

以下为so3到SO3转换和SE3到se3的转换图：