Sophus线性代数库学习大全

目录

1、Sophus安装方式

2、Sophus库简介

2.1 Sophus库简介

2.2 Eigen库简介      

3、Sophus的使用教程

3.1 Sophus库头文件

3.2 CMakeLists.txt文件 

3.3 Sophus库的各种形式的表示如下

3.4 SO3的构造函数为

3.5 SE3的构造函数为

3.6 SO3,SE3和se3的输出说明

4、高翔《视觉SLAM十四讲》程序注解

（1）CMakeLists.txt

（2）UseSophus.cpp

5 参考文献

        Eigen库是一个开源的C++线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能（ Eigen库线性代数库学习大全）。但是Eigen库提供了集合模块，但没有提供李代数的支持。一个较好的李群和李代数的库是Sophus库，它很好的支持了SO(3),so(3),SE(3)和se(3)。

1、Sophus安装方式

        git clone https://github.com/strasdat/Sophus.git
        cd Sophus
        git checkout a621ff
        mkdir build 
        cd build
        cmake ..
        make

2、Sophus库简介

2.1 Sophus库简介

        Sophus库是基于Eigen基础上开发的，继承了Eigen库中的定义的各个类。因此在使用Eigen库中的类时，既可以使用Eigen命名空间，也可以使用Sophus命名空间，如下所示。

Eigen::Matrix3d
Eigen::Vector3d
Sophus::Matrix3d
Sophus::Vector3d

        Eigen库的参数和类型的表达方式如下，具体关于Eigen库的知识点请参考链接 （ Eigen库线性代数库学习大全）。

2.2 Eigen库简介      

（1）Eigen库矩阵模板函数 

        Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类,矩阵模板函数共包含六个参数.

template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>

         参数说明：

• typename _Scalar：矩阵元素的类型；
• int _Rows：矩阵的行数；
• int _Cols：矩阵的列数；
•  int _Options：矩阵是行主序（RowMajor）或列主序（ColMajor），默认是列主序；
• int _MaxRows：矩阵的最大行数；
• int _MaxCols：矩阵的最大列数；

Eigen::Matrix<int, 3, 4> mat1;              //  3x4 的 int 类型的矩阵 mat1
Eigen::Matrix<double, 3, Dynamic> mat2;     //  3x? 的 double 类型的矩阵 mat2
Eigen::Matrix<float, Dynamic, 4> mat3;      //  ?x4 的 float 类型的矩阵 mat3
Eigen::Matrix<long, Dynamic, Dynamic> mat4; //  ?x? 的 long 类型的矩阵 mat4

        动态矩阵：在 Eigen 中可以用 Dynamic 表示行或者列数未知，所以在定义一个矩阵时并不能确定矩阵的大小，只有在运行时才可以确定大小，然后进行动态分配。

        静态矩阵：在定义时便明确给定了行数以及列数，在编译时就可以分配好内存。

（2）Eigen库类型

        在 EigenEigen 中 typedef 了很多矩阵的类型，通常命名为 Matrix前缀加一个长度为 1∼4 的字符串 S 的命名——MatrixS。

        其中 S 可以用来判断该矩阵类型，数字 n 表示 n ∗ n，n 的范围是2∼4，字母 d、f、i、cd、f、i、c 表示 double、float、int、complexdouble、float、int、complex，另外 X 表示行或者列数未知的矩阵。

typedef Matrix<std::complex<double>, 2, 2> Eigen::Matrix2cd;            //  2x2 的 cd 类型的矩阵
typedef Matrix<double, 2, 2> Eigen::Matrix2d;                           //  2x2 的 d 类型的矩阵
typedef Matrix<std::complex<double>, 2, Dynamic> Eigen::Matrix2Xcd;     //  2x? 的 cd 类型的矩阵
typedef Matrix<std::complex<float>, Dynamic, 2> Eigen::MatrixX2cf;      //  ?x2 的 cf 类型的矩阵
typedef Matrix<std::complex<double>, Dynamic, Dynamic> Eigen::MatrixXcd;//  ?x? 的 cd 类型的矩阵
typedef Matrix<int, Dynamic, Dynamic> Eigen::MatrixXi;                  //  ?x? 的 i 类型的矩阵

        同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix，

//例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量
Vector3d v_3d;
// 这是一样的
Matrix<float, 3, 1> vd_3d;
// Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
Matrix3d matrix_33 = Matrix3d::Zero(); //初始化为零

3、Sophus的使用教程

3.1 Sophus库头文件

#include "sophus/se3.hpp"

3.2 CMakeLists.txt文件 

// cmake_minimum_required( VERSION 2.8 )
// project( useSophus )
// include_directories( "/usr/local/include/eigen3/" )

# 为使用 sophus，您需要使用find_package命令找到它
find_package( Sophus REQUIRED )
include_directories( ${Sophus_INCLUDE_DIRS} )
add_executable( useSophus useSophus.cpp )
target_link_libraries( useSophus ${Sophus_LIBRARIES} fmt)

 

3.3 Sophus库的各种形式的表示如下

// 李代数so(3)
Sophus::Vector3d     //so(3)仅仅只是一个普通的3维向量
// 李代数se(3)
Sophus::Vector6d     //se(3)仅仅只是一个普通的6维向量

3.4 SO3的构造函数为

SO3 ();
SO3 (const SO3 & other);
explicit SO3 (const Matrix3d & _R);
explicit SO3 (const Quaterniond & unit_quaternion);
SO3 (double rot_x, double rot_y, double rot_z);

3.5 SE3的构造函数为

SE3 ();
SE3 (const SO3 & so3,const Vector3d & translation);
SE3 (const Matrix3d & rotation_matrix,const Vector3d & translation);
SE3 (const Quaterniond & unit_quaternion,const Vector3d & translation_);
SE3 (const SE3 & other);

3.6 SO3,SE3和se3的输出说明

• 尽管SO3对应于矩阵群，但是SO3在使用cout时是以so3形式输出的，输出的是一个3维向量； 
• SE3在使用cout输出时输出的是一个6维向量，其中前3维为对应的so3的值，后3维为实际的平移向量t；
• se3在使用cout输出时输出的也是一个6维向量，但是其前3维为平移值ρρ（注意此时的ρρ与SE3输出的t是不同的，t=Jρρ,其中J是雅克比矩阵），后3维为其对应的so3.

4、高翔《视觉SLAM十四讲》程序注解

（1）CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( useSophus )
 
# 为使用 sophus，您需要使用find_package命令找到它
find_package( Sophus REQUIRED )
#set(Sophus_LIBRARIES libSophus.so)
 
include_directories( ${Sophus_INCLUDE_DIRS} )
include_directories( "/usr/local/include/eigen3/" )
 
add_executable( useSophus useSophus.cpp )
target_link_libraries( useSophus ${Sophus_LIBRARIES} fmt)

（2）UseSophus.cpp

#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

/// 本程序演示sophus的基本用法
/// 建立顺序：旋转矩阵 > 李群SO3 > 李代数so3   
/// 建立顺序看文章示意图
int main(int argc, char **argv) {
  // Eigen::AngleAxisd(),声明了Eigen名称空间，所以省略Eigen
  // M_PI/2：旋转90度；
  // M_PI/4：旋转45度；    
  // 沿Z轴转90度的旋转矩阵
  Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix(); //旋转矩阵建立
  // 或者四元数
  Quaterniond q(R);
  Sophus::SO3d SO3_R(R);              // Sophus::SO3d可以直接从旋转矩阵构造
  Sophus::SO3d SO3_q(q);              // 也可以通过四元数构造
  // 二者是等价的
  cout << "SO(3) from matrix:n" << SO3_R.matrix() << endl;
  cout << "SO(3) from quaternion:n" << SO3_q.matrix() << endl;
  cout << "they are equal" << endl;

  // 使用对数映射获得它的李代数
  Vector3d so3 = SO3_R.log();
  cout << "so3 = " << so3.transpose() << endl;
  // hat 为向量到反对称矩阵
  cout << "so3 hat=n" << Sophus::SO3d::hat(so3) << endl;
  // 相对的，vee为反对称到向量
  cout << "so3 hat vee= " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;

  // 增量扰动模型的更新
  Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
  Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
  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);           // 从R,t构造SE(3)
  Sophus::SE3d SE3_qt(q, t);            // 从q,t构造SE(3)
  cout << "SE3 from R,t= n" << SE3_Rt.matrix() << endl;
  cout << "SE3 from q,t= n" << SE3_qt.matrix() << endl;
  // 李代数se(3) 是一个六维向量，方便起见先typedef一下
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  Vector6d se3 = SE3_Rt.log();
  cout << "se3 = " << se3.transpose() << endl;
  // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
  // 同样的，有hat和vee两个算符
  cout << "se3 hat = n" << Sophus::SE3d::hat(se3) << endl;
  cout << "se3 hat vee = " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;

  // 最后，演示一下更新
  Vector6d update_se3; //更新量
  update_se3.setZero();
  update_se3(0, 0) = 1e-4;
  Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
  cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;

  return 0;
}

5 参考文献

（1）博客：Sophus库的安装和使用教程_Dongdong Bai的博客-CSDN博客_sophus

（2）高翔《视觉SLAM十四讲》

最后

以上就是务实棉花糖为你收集整理的Sophus线性代数库学习大全的全部内容，希望文章能够帮你解决Sophus线性代数库学习大全所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。