Instalación y uso de la biblioteca de Sophus

1. Introducción

La biblioteca de Sophus es una mejor biblioteca de C++ para grupos de Lie y álgebras de Lie. Admite SO(3), so(3), SE(3) y se(3) muy bien. La biblioteca de Sophus se desarrolla en base a Eigen y hereda las clases definidas en la biblioteca de Eigen. Por lo tanto, al usar las clases en la biblioteca Eigen, se pueden usar tanto el espacio de nombres Eigen como el espacio de nombres Sophus. Debido a razones históricas, la primera biblioteca de Sophus no era una clase de plantilla y solo podía proporcionar precisión doble. Más tarde, se reescribió una clase de plantilla para admitir diferentes precisiones, pero también aumentó la dificultad de uso.

En resumen, la biblioteca actual de Sophus tiene dos versiones: la primera clase sin plantilla y la clase con plantilla actual.Este artículo le explicará cómo instalar estas dos versiones. ruta de origen de github

2. Instalación de Sophus sin plantilla

La biblioteca dependiente de Sophus sin plantilla es Eigen, la versión es 3.3.X, la biblioteca Eigen debe instalarse con anticipación y la instalación puede consultar
(1) Descargar el archivo fuente (Eigen 3.3.X debe instalarse primero)

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

(2) Instalar Sophus

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

Los archivos de encabezado se instalarán /usr/local/include/sophusen

(3) Uso de Sophus sin plantilla
En las clases sin plantilla, la biblioteca se implementa .cagregando el archivo de encabezado de importación:.h

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

Cuando se compila cmake, la escritura del archivo 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. Instalación de Sophus de clase de plantilla

Las bibliotecas dependientes de la clase de plantilla Sophus son Eigen (versión 3.3.X) y fmt. La biblioteca Eigen y la biblioteca fmt deben instalarse con anticipación. La instalación de la biblioteca Eigen puede consultar esto

(1) Instalar la 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) Descargar archivos fuente de Sophus

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

(3) Instalar Sophus

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

Los archivos de encabezado se instalarán /usr/local/include/sophusen

(4) Uso de la clase de plantilla Sophus
La biblioteca de clases de plantilla se implementa en una colección .hpp, por lo que no se requiere el enlace del archivo de biblioteca de Sophus, pero Sophus también depende de la biblioteca fmt, y el
archivo de encabezado debe introducirse para la enlace del archivo de la biblioteca fmt:

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

Cuando se compila cmake, la escritura del archivo 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. Instalación de Sophus de clase de plantilla que no depende de la biblioteca fmt

En circunstancias normales, Sophus de la clase de plantilla se basará en fmt. La biblioteca fmt se utiliza para implementar la función de salida con formato de texto de E/S y realiza la impresión de archivos de registro en Sophus. Por lo tanto, eliminar la biblioteca fmt no afectará nuestro uso normal. de la biblioteca de Sophus. Y la biblioteca de Sophus en sí está compuesta exclusivamente de archivos hpp como la biblioteca Eigen, y no requiere el enlace del archivo de la biblioteca. Ahora hay una dependencia adicional de la biblioteca fmt, que destruye la belleza simplificada de la Biblioteca Sophus.
En github, el autor dijo que especificar "-DUSE_BASIC_LOGGING=ON" al compilar el código fuente en cmake puede instalar la biblioteca que no depende de fmt, a saber:

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

Pero resulta que todavía hay archivos individuales (como common.hpp) en el archivo hpp compilado que usa la biblioteca fmt. De acuerdo con la pregunta en github , encontré una solución:
agréguelo antes de incluir \#define SOPHUS_USE_BASIC_LOGGING, preste atención a esto debe estar antes de incluir, de lo contrario, todavía depende de Para la biblioteca fmt:

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

En este punto, el archivo CMakeLists.txt es:

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 de la biblioteca de Sophus

El siguiente ejemplo es el uso de la biblioteca Sophus de la clase de plantilla, y la clase sin plantilla es básicamente similar, excepto que el tipo de datos ha cambiado un poco.

/*
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;

}

El uso se resume de la siguiente manera: