SLAM十四讲 Sophus库模板类的使用问题

在非模板类中，库是利用.c加.h的方式实现的，而在模板类库中是集合在一个.hpp中实现的，因此需要把包含文件改成以下形式：

#include "sophus/so3.hpp"
#include "sophus/se3.hpp"

模板类之所以叫模板类是因为其在申明一个新的实例时，需要指定数据类型，因此，我们只需要在每一个李代数定义的地方，将

Sophus::SO3 SO3_v

换成：

Sophus::SO3<double> SO3_v

你可以定义一个新的数据类型指向非模板类，例如：

using SO3Type = Sophus::SO3<double>;
using SE3Type = Sophus::SE3<double>;
 SO3Type SO3_v;

改完模板类之后大体上就不影响使用了，还有要注意的就是这个版本库的新特性。例如这个版本不再支持对李代数的直接输出：

cout<<"SO(3) from quaternion :"<<SO3_q<<endl;

看了一下源代码，这个版本的李代数是利用四元数为基本储存格式的，你要是想要和高博书中一样直接输出旋转向量，需要加个log()变化：

cout<<"SO(3) from quaternion :"<<SO3_q.log().transpose()<<endl;

同理，SE3也可以以log()的方法输出。。

需要注意的是，SE3用log()后输出的是李代数，对应的矩阵(采用hat()函数)是生成矩阵，而不是变换矩阵，这也就是为什么大家看到的平移部分的分量是（0.785  -0.785  0）而不是（1  0  0）的原因。

#include <iostream>
#include <cmath>
#include <Eigen/Core>

#include "sophus/so3.hpp"
#include "sophus/se3.hpp"
using namespace std;


int main() {
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2,Eigen::Vector3d(0,0,1)).toRotationMatrix();
    Sophus::SO3<double> SO3_R(R);
    Eigen::Quaterniond q(R);
    Sophus::SO3d SO3_Q(q);
//    Sophus::SO3d SO3_V(0,0,M_PI/2);
    cout<<SO3_R.log().transpose()<<"\n"<<endl;
    cout<<SO3_Q.log()<<endl;

    Eigen::Vector3d so3=SO3_R.log();//获得李代数
    //向量到反对称矩阵
    cout<<"so3 hat= \n"<<Sophus::SO3d::hat(so3)<<endl;
    //反对称矩阵到向量
    std::cout <<"so3 vee =\n"<<Sophus::SO3d::vee(Sophus::SO3d::hat(so3))<< std::endl;
    //向量扰动模型
    Eigen::Vector3d update_so3(1e-4,0,0);
    Sophus::SO3d SO3_update = Sophus::SO3d::exp(update_so3)*SO3_R;
    cout<<"SO3 update=\n"<<SO3_update.matrix()<<endl;
    cout<<"********************"<<endl;
    Eigen::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;
    //se3为六维向量
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout<<"se3="<<se3.transpose()<<endl;

    cout<<"se3 hat =\n"<<Sophus::SE3d::hat(se3)<<endl;
    cout<<"se3 vee =\n"<<Sophus::SE3d::vee(Sophus::SE3d::hat(se3))<<endl;

    Vector6d se3_updat;
    se3_updat.setZero();
    se3_updat(0,0)=1e-4f;
    Sophus::SE3d SE3_update = Sophus::SE3d::exp(se3_updat)*SE3_Rt;
    cout<<"SE3 update =\n"<<SE3_update.matrix()<<endl;

    return 0;
}

原文链接：https://blog.csdn.net/LeGe675797663/article/details/79602565