Sophus(李群、李代数库)

Love The Way You Lie 2022-03-15 12:14 261阅读 0赞

### 文章目录 ###

*   *  介绍
     *  代码实例
     *  基本应用
     *   *  SO3
         *  SE3
         *  SIM3

## 介绍 ##

*  下载：

1.  模板类：  
    `git clone https://github.com/strasdat/Sophus.git`
2.  非模板类：

git clone https://github.com/strasdat/Sophus.git
    cd Sophus
    git checkout a621ff

*  安装: cmake工程，

mkdir build && cd build
    cmake ..
    make -j 8
    sudo make install

--------------------

## 代码实例 ##

#include <iostream>
    using namespace std;
    
    
    #include <Eigen/Core>
    #include <Eigen/Geometry>
    
    
    #include "../../source/Sophus/sophus/so3.hpp"
    #include "../../source/Sophus/sophus/se3.hpp"
    
    
    int main()
    { 
        Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
        Eigen::Quaterniond q(R);
    
     // --------------------------利群的初始化--------------------------
        Sophus::SO3d SO3d_R(R);
        //Sophus::SO3d SO3d_v(0.0, 0.0, M_PI/2); 非模板类提供这样的构造，模板类没有了
        Sophus::SO3d SO3d_q(q);
    
    
        cout << "SO(3) from matrix " << SO3d_R.matrix() << endl;
        //cout << "SO(3) from vector " << SO3d_v.matrix() << endl;
        cout << "SO(3) from q" << SO3d_q.matrix() << endl;
    
    
        
        //----------------------李群到李代数----------------------------
        Eigen::Vector3d so3 = SO3d_R.log(); // 获取李群的李代数
        cout << "so(3) hat = " << endl;
        cout << Sophus::SO3d::hat(so3) << endl; //输出李代数的反对称矩阵
        cout << "so(3) hat vee = " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;// vee 为反对称 到向量
        
    
        // --------------增量扰动模型的跟新-------------------------------
        Eigen::Vector3d update_so3(1e-4, 0, 0); //新的增量
        Sophus::SO3d SO3_Update = Sophus::SO3d::exp(update_so3)*SO3d_R ;  //左乘模型
        cout << "SO3d updated = "<< endl;
        cout  << SO3_Update.log().transpose() << endl;
    
    
    
    
        // *******************************************************************************
        cout << "**********************************我是萌萌的分割线************************" << endl;
        Eigen::Vector3d t(1, 0, 0);// t 平移部分，沿着x轴平移1
        Sophus::SE3d SE3_Rt(R, t);   // 从R ,t 构造SE3
        Sophus::SE3d SE3_qt(q,t);  //从 q, t构造SE3
        cout << "SE3 = " << endl << SE3_Rt.matrix() << endl; // 变换矩阵T
        
    
    
        typedef Eigen::Matrix<double, 6, 1> Vector6d;
        Vector6d se3 = SE3_Rt.log();
        cout << "se3 = " << endl;
        cout << se3.transpose() << endl;  // 输出李代数se3
    
    
    
        cout << "se3 hat = " << endl << Sophus::SE3d::hat(se3) << endl;
        cout << "se3 hat vee = " << endl << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)) << 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 << "SE3d updated = " << endl << SE3_updated.matrix() << endl;  // 变换矩阵T的形式
        return 0;
    }

CMakeLists.txt

cmake_minimum_required(VERSION 3.10)
    
    
    project(sophus)
    
    find_package(Sophus REQUIRED)
    include_directories(${ Sophus_INCLUDE_DIRS})
    include_directories("/usr/include/eigen3")
    
    
    add_executable(sophus main.cpp)
    target_link_libraries(sophus ${ Sophus_LIBRARIES})

## 基本应用 ##

### SO3 ###

*   构造

Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();
        Eigen::Quaterniond q(R);
    
    	// 1. 默认无参构造
    	Sophus::SO3d so3d_I;   // 通过四元数(1, 0, 0, 0) 构造的，等于没有旋转
    
    	// 2. 通过旋转矩阵构造
        Sophus::SO3d so3d_R(R);  
    
    	// 3. 通过四元数构造()
        Sophus::SO3d so3d_q(q);

*  常用的函数

1.  `matrix()` ：将群转成矩阵的形式
2.  `angleX()`：得到Ｘ轴的旋转，弧度值，对应也有Y,Z
3.  `static hat()` : from vector o skew-symmetric matrix, 静态函数，以类名来调用
4.  `static vee()`: from skew-symmetric matrix to vector
5.  `unit_quaternion()` 得到单位旋转四元数
6.  `static lieBracket()` ：计算两向量的李括号的结果

--------------------

### SE3 ###

*   构造

Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();
        Eigen::Quaterniond q(R);
        Eigen::Vector3d t(1,2,3);
    
    	//1. 从旋转矩阵和平移来构造
        Sophus::SE3d se3_Rt(R, t);　　// 
        
        //2. 从四元数和平移来构造
        Sophus::SE3d se3_qt(q, t);　　
    
    
    	//3. 从4*4的矩阵来构造，左上角rotaion矩阵必须的正交且determinant=1,最后一行为(0,0,0,1)
    	Eigen::Matrix4d T;
        T << 1, 0, 0, 1,
    	 0, 1, 0, 2,
    	 0, 0, 1, 3,
    	 0, 0, 0, 1;
        Sophus::SE3d se3_4m(T);
    
    	//4. 从SO3 和　t　构造
    	Sophus::SO3d so3_R(R);
        Sophus::SE3d se3_Rt(so3_R, t);

*  常用的函数

1.  `matrix()` 将群转成矩阵的形式
2.  `rotaionMatrix()` : 取旋转矩阵
3.  `translation` 取平移部分
4.  `static hat()` ６维向量到４\*４矩阵的操作，左上角是向量的旋转部分对应的反对称矩阵，右上角是平移部分
5.  `static vee()`４\*４矩阵到６维向量的操作
6.  `angleX()`：得到Ｘ轴的旋转，弧度值，对应也有Y,Z

--------------------

### SIM3 ###

*   构造

在这里插入代码片