Slam十四讲 ch4 useSophus.cpp

CmakeLists.txt 

cmake_minimum_required(VERSION 3.0)
project(useSophus)

# 为使用 sophus，需要使用find_package命令找到它
find_package(Sophus REQUIRED)

# Eigen
include_directories("/usr/include/eigen3")
add_executable(useSophus test.cpp)

target_link_libraries(useSophus Sophus::Sophus)

test.cpp

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry> //几何运算
#include <Eigen/Dense> //稠密函数计算，包括core
#include <cmath> //数学计算的库函数


#include <sophus/se3.hpp> //三元李代数的库



using namespace std;
using namespace Eigen;
using namespace Sophus;

 int main(int argc, char const *argv[])
{
    //Matrix3d R = AngleAxis(M_1_PI/3,Vector3d(0,1,0));直接这样写会说没有重载,AngleAxis后面有d
    Matrix3d R = AngleAxisd(M_1_PI/3,Vector3d(0,1,0)).toRotationMatrix();//需要进行重载变化，对了记得加括号
    Quaterniond q(R);//将旋转矩阵转换为四元数
    
    //Sophus::SO3d SO3_R(R) ;
    SO3d SO3_R(R) ;//这个旋转矩阵直接转换成李代数
    SO3d SO3_q(q) ;//四元数转换为李代数

    cout << "旋转矩阵转换的李代数为:\n" << SO3_R.matrix() <<endl;
    cout << "四元数转换的李代数为:\n" << SO3_q.matrix() <<endl;
    cout << "他们是等价的！" << endl;
    //这里我们使用对数映射以获得李代数
    Vector3d so3 = SO3_R.log();
    //我们输出看看
    cout << "指数映射之后的李代数是:" << so3.transpose() << endl; // 0  0.106103        0
    //这里的hat为向量到反对称矩阵，直接输出看看
    cout << "[1,2,3]的反对称矩阵为:\n" << Sophus::SO3d::hat(Vector3d(1,2,3)) << endl;//这里我们就直接输出了
    cout << "李代数的反对称矩阵为:\n" << Sophus::SO3d::hat(so3)<< endl;
    //vee为反对称到向量
    cout << "vee：\n" << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;//转置一下

    //增量扰动模型的更新
    Vector3d update_so3(1e-3,0,0);//假设我们的量只有这么多，是一个微小量
    Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3)*SO3_R;
    cout << "exp方法就是将一个向量转换为一个旋转矩阵:\n"<<Sophus::SO3d::exp(update_so3).matrix() << endl;
    cout << "SO3_updated = \n" << SO3_updated.matrix() <<endl ;
    
    
    cout << "**************************************************" << endl;
    //对SE(3)操作大同小异同
    Vector3d t(1,0,0);   //沿着x数轴平移1
    Sophus::SE3d SE3_Rt(R,t); //旋转矩阵
    Sophus::SE3d SE3_qt(q,t); //四元数
    cout << "SE3 from R is \n" << SE3_Rt.matrix() << endl; 
    cout << "SE3 from q is \n" << SE3_qt.matrix() << endl; 
    cout << "they are equal!" << endl;
    //李代数是一个六维向量，先typdef一下
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3 = SE3_Rt.log();//指数映射
    cout << "变换矩阵对应的李代数是" << se3.transpose()<< endl;//平移在前，旋转在后
    
    cout << "se3 hat:\n  " << Sophus::SE3d::hat(se3) << endl; //是一个4*4的矩阵，矩阵
    cout << "se3 hat vee : \n" <<   Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;

    //演示更新
    Vector6d update_se3;
    update_se3.setZero();//设置为零
    update_se3(0,0) = 1e-4d; //
    cout << "扰动量李代数为" <<update_se3.transpose() << endl ;
    Sophus::SE3d  SE3_updated = Sophus::SE3d::exp(update_se3)*SE3_Rt;
    cout << "变换之后的矩阵为\n" << SE3_updated.matrix() <<endl;




    

    return 0;
}

 输出结果：

转矩阵转换的李代数为:
 0.994376         0  0.105904
        0         1         0
-0.105904         0  0.994376
四元数转换的李代数为:
 0.994376         0  0.105904
        0         1         0
-0.105904         0  0.994376
他们是等价的！
指数映射之后的李代数是:       0 0.106103        0
[1,2,3]的反对称矩阵为:
 0 -3  2
 3  0 -1
-2  1  0
李代数的反对称矩阵为:
        0        -0  0.106103
        0         0        -0
-0.106103         0         0
vee：
       0 0.106103        0
exp方法就是将一个向量转换为一个旋转矩阵:
     1      0      0
     0      1 -0.001
     0  0.001      1
SO3_updated = 
    0.994376            0     0.105904
 0.000105904            1 -0.000994376
   -0.105904        0.001     0.994376
**************************************************
SE3 from R is 
 0.994376         0  0.105904         1
        0         1         0         0
-0.105904         0  0.994376         0
        0         0         0         1
SE3 from q is 
 0.994376         0  0.105904         1
        0         1         0         0
-0.105904         0  0.994376         0
        0         0         0         1
they are equal!
变换矩阵对应的李代数是 0.999062         0 0.0530516         0  0.106103         0
se3 hat:
          0        -0  0.106103  0.999062
        0         0        -0         0
-0.106103         0         0 0.0530516
        0         0         0         0
se3 hat vee : 
 0.999062         0 0.0530516         0  0.106103         0
0.0001      0      0      0      0      0
变换之后的矩阵为
 0.994376         0  0.105904    1.0001
        0         1         0         0
-0.105904         0  0.994376         0
        0         0         0         1