Sophus库安装和使用

1.前言

Sophus库是一个较好的李群和李代数的C++库，它很好的支持了SO(3),so(3),SE(3)和se(3)。Sophus库是基于Eigen基础上开发的，继承了Eigen库中的定义的各个类。因此在使用Eigen库中的类时，既可以使用Eigen命名空间，也可以使用Sophus命名空间。由于历史原因，早期的Sophus库是非模板类，只能提供双精度，后来又改写了一个模板类的，支持不同精度但也增加了使用难度。

总之，现在的Sophus库有两个版本：早期的非模板类和现在的模板类，本文会介绍如何安装这两个版本。github源码路径

2.非模板类Sophus安装

非模板类Sophus的依赖库是Eigen，版本为3.3.X，需提前安装好Eigen库，安装可参考
（1）下载源文件（需要先安装Eigen 3.3.X）

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

（2）安装Sophus

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

头文件会安装到/usr/local/include/sophus下

（3）非模板Sophus的使用
在非模板类中，库是利用.c加.h的方式实现的
引入头文件：

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

cmake编译时，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.模板类Sophus安装

模板类Sophus的依赖库是Eigen(版本为3.3.X)和fmt，需提前安装好Eigen库和fmt库，Eigen库的安装可参考这个

（1）安装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）下载Sophus源文件

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

（3）安装Sophus

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

头文件会安装到/usr/local/include/sophus下

（4）模板类Sophus的使用
模板类库中是集合在一个.hpp中实现的，因此不需要Sophus库文件的链接，但是Sophus中还依赖于fmt库，需要对fmt库文件链接
引入头文件：

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

cmake编译时，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.不依赖fmt库的模板类Sophus安装

正常情况下，模板类的Sophus会依赖fmt，fmt库是用于实现I/O文本格式化输出的功能，在Sophus中实现日志文件打印，因此去除掉fmt库也不影响我们正常使用Sophus库，且Sophus库本身和Eigen库一样纯用hpp文件构成，本身不需要库文件的链接，现在多了个fmt库的依赖感觉破坏了Sophus库的简化美感。
在github上，作者说在cmake编译源码时指定"-DUSE_BASIC_LOGGING=ON"可以安装不依赖于fmt库，即：

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

但结果发现编译的hpp文件中还是存在个别文件(如commont.hpp)用到了fmt库，根据github上的提问，找到了一种解决方法：
在include前添加\#define SOPHUS_USE_BASIC_LOGGING，注意必需是include前，否则还是依赖于fmt库：

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

此时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.Sophus库的基本使用

下面的例子是模板类的Sophus库使用，非模板类基本类似，就是数据类型变了点。

/*
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前添加
// #define SOPHUS_USE_BASIC_LOGGING
#include 
#include 
#include 
#include 
#include 

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;

}

使用总结如下：