Instalação e uso da biblioteca Sophus

1. Introdução

A biblioteca Sophus é uma biblioteca C++ melhor para grupos de Lie e álgebras de Lie. Ela suporta SO(3), so(3), SE(3) e se(3) muito bem. A biblioteca Sophus é desenvolvida com base no Eigen e herda as classes definidas na biblioteca Eigen. Portanto, ao usar as classes na biblioteca Eigen, tanto o namespace Eigen quanto o namespace Sophus podem ser usados. Devido a razões históricas, a antiga biblioteca Sophus era uma classe sem modelo e só podia fornecer precisão dupla. Mais tarde, uma classe de modelo foi reescrita para suportar diferentes precisões, mas também aumentou a dificuldade de uso.

Resumindo, a biblioteca atual do Sophus tem duas versões: a classe sem modelo inicial e a classe de modelo atual.Este artigo apresentará como instalar essas duas versões. caminho de origem do github

2. Instalação do Sophus sem modelo

A biblioteca dependente do Sophus não modelo é Eigen, a versão é 3.3.X, a biblioteca Eigen precisa ser instalada com antecedência e a instalação pode se referir a
(1) Baixar o arquivo de origem (Eigen 3.3.X precisa ser instalado primeiro)

git clone https://github.com/strasdat/Sophus.git                  // 下载的最新版是模板类的
cd Sophus
git checkout a621ff    // 切换为非模板类的历史版本

(2) Instale o Sophus

cd Sophus
mkdir build
cd build
cmake ..  
make
sudo make install

Os arquivos de cabeçalho serão instalados /usr/local/include/sophusem

(3) Uso de Sophus sem modelo
Em classes sem modelo, a biblioteca é implementada .cadicionando o arquivo de cabeçalho de importação:.h

//引用非模板类库
#include “sophus/so3.h”
#include “sophus/se3.h”

Quando o cmake é compilado, a gravação do arquivo CMakeLists.txt

# 非模板类库
cmake_minimum_required( VERSION 2.8 )
project( useSophus )
set(CMAKE_CXX_STANDARD 11)

find_package( Sophus REQUIRED )
include_directories( ${Sophus_INCLUDE_DIRS} )
add_executable( useSophus useSophus.cpp )
# 由于非模板类版本是有库文件的，因此需要链接
target_link_libraries( useSophus ${Sophus_LIBRARIUES} )

3. Instalação do Sophus da classe de modelo

As bibliotecas dependentes da classe de modelo Sophus são Eigen (versão 3.3.X) e fmt. A biblioteca Eigen e a biblioteca fmt precisam ser instaladas com antecedência. A instalação da biblioteca Eigen pode se referir a isso

(1) Instale a biblioteca fmt

git clone https://github.com/fmtlib/fmt.git
cd fmt
# 默认安装是安装为静态库文件，以后有库文件依赖fmt库的话只能生成静态库，不能为共享库，因此推荐安装为共享库方式
# 如果要安装为共享库文件，就需要在CMakeLists.txt文件中添加：add_compile_options(-fPIC)
mkdir build
cd build
cmake ..
make
sudo make install

(2) Baixe os arquivos de origem do Sophus

git clone https://github.com/strasdat/Sophus.git

(3) Instale o Sophus

cd Sophus
mkdir build
cd build
cmake ..  
make
sudo make install

Os arquivos de cabeçalho serão instalados /usr/local/include/sophusem

(4) O uso da classe de modelo Sophus
A biblioteca de classe de modelo é implementada em uma coleção .hpp, portanto, o link do arquivo de biblioteca Sophus não é necessário, mas Sophus também depende da biblioteca fmt e o
arquivo de cabeçalho precisa ser introduzido para o link do arquivo da biblioteca fmt:

//引用模板类库
#include “sophus/so3.hpp”
#include “sophus/se3.hpp”

Quando o cmake é compilado, a gravação do arquivo CMakeLists.txt

# 模板类（依赖fmt库）
cmake_minimum_required(VERSION 3.0)
project(learn_Sophus)
set(CMAKE_CXX_STANDARD 11)

find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
add_executable(learn_Sophus sophus_1.cpp)
# 需要链接fmt的动态库文件
target_link_libraries(learn_Sophus fmt::fmt)
# 或者也可以改为
# target_link_libraries(learn_Sophus Sophus::Sophus)

4. Instalação do Sophus da classe de modelo que não depende da biblioteca fmt

Em circunstâncias normais, o Sophus da classe de modelo dependerá do fmt. A biblioteca fmt é usada para implementar a função de saída formatada de texto de E/S e realizar a impressão do arquivo de log no Sophus. Portanto, remover a biblioteca fmt não afetará nosso uso normal da biblioteca Sophus. E a própria biblioteca Sophus é puramente composta de arquivos hpp como a biblioteca Eigen, e não requer um link para o arquivo da biblioteca. Agora há uma dependência adicional na biblioteca fmt, que destrói a beleza simplificada do Biblioteca Sophus.
No github, o autor disse que especificar "-DUSE_BASIC_LOGGING=ON" ao compilar o código-fonte no cmake pode instalar a biblioteca que não depende do fmt, a saber:

cd Sophus
mkdir build
cd build
cmake ../ -DUSE_BASIC_LOGGING=ON
make
sudo make install

Mas acontece que ainda existem arquivos individuais (como common.hpp) no arquivo hpp compilado que usam a biblioteca fmt. De acordo com a pergunta no github , encontrei uma solução:
adicione antes de incluir \#define SOPHUS_USE_BASIC_LOGGING. Observe que deve ser antes de incluir, caso contrário, ainda depende de Para a biblioteca fmt:

// C++自己的程序中：
// 需要在include sophus库头文件前添加该宏定义
#define SOPHUS_USE_BASIC_LOGGING
#include <iostream>
#include <sophus/so3.hpp>
#include <sophus/se3.hpp>

Neste ponto, o arquivo CMakeLists.txt é:

cmake_minimum_required(VERSION 3.0)
project(learn_Sophus)
set(CMAKE_CXX_STANDARD 11)

find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
# 此时不需要库文件的链接了
add_executable(learn_Sophus sophus_1.cpp)

5. Uso básico da biblioteca Sophus

O exemplo a seguir é o uso da biblioteca Sophus da classe modelo, e a classe não modelo é basicamente semelhante, exceto que o tipo de dados mudou um pouco.

/*
Eigen库是一个开源的C++线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。
但是Eigen库提供了集合模块，但没有提供李代数的支持。一个较好的李群和李代数的库是Sophus库，它很好的支持了
SO(3),so(3),SE(3)和se(3)。Sophus库是基于Eigen基础上开发的，继承了Eigen库中的定义的各个类。因此在
使用Eigen库中的类时，既可以使用Eigen命名空间，也可以使用Sophus命名空间:
    Eigen：：Matrix3d和Sophus：：Matrix3d
    Eigen：：Vector3d和Sophus：：Vector3d
此外，为了方便说明SE(4)和se(4)，Sophus库还typedef了Vector4d、Matrix4d、Vector6d和Matrix6d等，即：
    Sophus：：Vector4d
    Sophus：：Matrix4d
    Sophus：：Vector6d
    Sophus：：Matrix6d
*/

// 添加该宏定义可以使sophus库不依赖fmt库，必需得在include<sophus>前添加
// #define SOPHUS_USE_BASIC_LOGGING
#include <iostream>
#include <sophus/so3.hpp>
#include <sophus/se3.hpp>
#include <cmath>
#include <Eigen/Dense>

void func_1();
void func_2();

int main()
{
    
    
    // func_1();
    func_2();
    return 0;
}

void func_1()
{
    
    
    // 李群SO3和旋转矩阵R和李代数so3

    // 1.旋转矩阵R <-> 李群SO3
    // 沿Z轴旋转90度的旋转矩阵
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2,Eigen::Vector3d::UnitZ()).toRotationMatrix();
    // Sophus模板库和eigen一样选择精度，如SO3d、SO3f、SE3d、SE3f
    // 旋转矩阵R -> 李群SO3
    Sophus::SO3d SO3_R(R);  // 构造函数参数可以是旋转矩阵
    // 旋转矩阵R <- 李群SO3
    Eigen::Matrix3d R_SO3 = SO3_R.matrix();
    std::cout << SO3_R.matrix() <<std::endl; // 输出时需要将SO3转换为矩阵形式

    // 2.四元数q -> 李群SO3
    Eigen::Quaterniond q(R);
    Sophus::SO3d SO3_q(q);  // 构造函数参数可以是四元数
    std::cout << SO3_q.matrix() <<std::endl; 

    // 3. 李群SO3 <-> 李代数so3
    // 李群SO3 -> 李代数so3
    Eigen::Vector3d so3 =  SO3_R.log();
    std::cout << so3.transpose() <<std::endl;
    // 李群SO3 <- 李代数so3
    Sophus::SO3d SO3_so3 = Sophus::SO3d::exp(so3);
    std::cout << SO3_so3.matrix() <<std::endl; 

    // 4.李代数so3 <-> 三维反对称矩阵R_v
    // 李代数so3 -> 三维反对称矩阵R_v
    Eigen::Matrix3d R_v = Sophus::SO3d::hat(so3);
    std::cout << R_v <<std::endl; 
    // 李代数so3 <- 三维反对称矩阵R_v
    Eigen::Vector3d so3_Rv = Sophus::SO3d::vee(R_v);
    std::cout << so3_Rv.transpose() <<std::endl;

    // 5.增加扰动
    Eigen::Vector3d update_so3(1e-4,0,0);//增加扰动
    Sophus::SO3d SO3_updated=Sophus::SO3d::exp(update_so3)*SO3_R;
    std::cout<<SO3_updated.matrix()<<std::endl;


}

void func_2()
{
    
    
    // 李群SE3和变换矩阵T和李代数se3

    // 1.(旋转矩阵R,平移向量t) <-> 李群SE3
    Eigen::Vector3d t(1,0,0);
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2,Eigen::Vector3d::UnitZ()).toRotationMatrix();
    // (旋转矩阵R,平移向量t) -> 李群SE3
    Sophus::SE3d SE3_Rt(R,t);
    std::cout<<SE3_Rt.matrix()<<std::endl;  // 矩阵的组成是[R,t;0,1]
    // (旋转矩阵R,平移向量t) <- 李群SE3
    Eigen::Matrix3d R_ = SE3_Rt.matrix().block<3,3>(0,0);
    Eigen::Vector3d t_ = SE3_Rt.matrix().block<3,1>(0,3);
    std::cout<<R_<<std::endl;
    std::cout<<t_<<std::endl;

    // 2.(四元数q,平移向量t) -> 李群SE3
    Eigen::Quaterniond q(R);
    Sophus::SE3d SE3_qt(q,t);
    std::cout<<SE3_qt.matrix()<<std::endl;

    // 3.李群SE3 <-> 李代数se3
    // 李群SE3 -> 李代数se3
    Sophus::Vector6d se3 = SE3_Rt.log(); //Eigen库中没有预先定义6维向量，可以使用Sophus库中定义的
    std::cout<<se3.transpose()<<std::endl;  //平移在前而旋转在后,这里的平移并不是真正空间上的平移
    // 李群SE3 <- 李代数se3
    Sophus::SE3d SE3_se3 = Sophus::SE3d::exp(se3);
    std::cout<<SE3_se3.matrix()<<std::endl;

    // 4.李代数se3 <-> 反对称矩阵R_v
    // 李代数se3 -> 反对称矩阵R_v
    Sophus::Matrix4d R_v = Sophus::SE3d::hat(se3);
    std::cout<<R_v<<std::endl;
    // 李代数se3 <- 反对称矩阵R_v
    Sophus::Vector6d se3_Rv = Sophus::SE3d::vee(R_v);
    std::cout<<se3_Rv.transpose()<<std::endl;

    // 5.增加扰动
    Sophus::Vector6d update_se3;
    update_se3.setZero();
    update_se3(1,0)=1e-4;
    Sophus::SE3d SE3_updated=Sophus::SE3d::exp(update_se3)*SE3_Rt;
    std::cout<<SE3_updated.matrix()<<std::endl;

}

O uso é resumido da seguinte forma: