SLAM 之数学基础篇

Sophus库的使用及李群与李代数

本例程使用模板类Sophus库（从旋转向量构造SO(3)有些问题，有待继续学习），和上一篇博客一样，使用CMake编译：

cd build
cmake ..
make

CMakeList.txt源码如下：

cmake_minimum_required(VERSION 2.8)

project (Sophus)

include_directories("/usr/include/eigen3")
include_directories("/usr/include/Sophus")

find_package(Sophus REQUIRED)

add_executable(sophus src/sophus.cpp)

target_link_libraries(sophus Sophus::Sophus)

sophus.cpp源码如下（在Sophus模板类中需要定义数据类型）：

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

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

int main (int argc, char** argv)
{
    /*********************************SO(3)操作***********************************************/

    //沿Z轴转90度的旋转矩阵
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();

    //旋转矩阵构造SO(3)
    Sophus::SO3<double> SO3_R(R);

    //旋转向量构造SO(3)
    //Sophus::SO3<double> SO3_v(0.0, 0.0, M_PI/2);

    //四元数构造SO(3)
    Eigen::Quaterniond q(R);
    Sophus::SO3<double> SO3_q(q);

    //使用对数获得李代数so3
    Eigen::Vector3d so3 = SO3_R.log();

    //输出SO(3)以so(3)形式输出
    cout << "SO(3) from Matrix: " << SO3_R.log() << endl;
    //cout << "SO(3) from Vector: " << SO3_v.log() << endl;
    cout << "SO(3) from Quaternion: " << SO3_q.log() << endl;

    //hat 为向量的反对称矩阵
    cout << "so3 hat = " <<  Sophus::SO3d::hat(so3) << endl;

    //vee 为反对称到向量
    cout << "so3 hat vee = " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;

    //增加扰动模型
    Eigen::Vector3d update_so3(1e-4, 0, 0); 
    Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3)*SO3_R;
    cout << " SO3 updated = " << SO3_updated.log() << endl;

    /*********************************SE(3)操作**************************************************/

    //沿X轴平移1
    Eigen::Vector3d t(1, 0, 0);

    //从R，t构造SE(3)
    Sophus::SE3d SE3_Rt(R, t);

    //从q，t构造SE(3)
    Sophus::SE3d SE3_qt(q, t);

    //输出SE(3)以se(3)六维向量形式输出
    typedef Eigen::Matrix<double, 6, 1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    Vector6d seq3 = SE3_qt.log();
    cout << "SE3 from R,t = " << endl << se3 << endl;
    cout << "SE3 from q,t = " << endl << seq3 << endl;

    //hat 为向量的反对称矩阵
    cout << "se3 hat = " << Sophus::SE3d::hat(se3) << endl;

    //vee 为反对称矩阵到向量
    cout << "se3 hat vee = " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;

    //增加扰动模型
    Vector6d update_se3;
    update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3)*SE3_Rt;
    cout << "SE3 updated = " << SE3_updated.matrix() << endl;

    return 0;
}