SLAM算法与工程实践系列文章
下面是SLAM算法与工程实践系列文章的总链接，本人发表这个系列的文章链接均收录于此
SLAM算法与工程实践系列文章链接
下面是专栏地址：
SLAM算法与工程实践系列专栏
文章目录
- SLAM算法与工程实践系列文章
- SLAM算法与工程实践系列文章链接
- SLAM算法与工程实践系列专栏
- 前言
- SLAM算法与工程实践——SLAM基本库的安装与使用（4）：Sophus库
- Sophus
- 安装
- 使用
- Lie Gropus
- 基本用法
- 出现的错误
- 错误1
- 错误2
- 错误3
- 错误3
- 错误4
- 错误5
前言
这个系列的文章是分享SLAM相关技术算法的学习和工程实践
SLAM算法与工程实践——SLAM基本库的安装与使用（4）：Sophus库
Sophus
参考：
Sophus库：对SO(3)和SE(3)的构建及扰动模型搭建
【一起读书】视觉SLAM十四讲 第4讲（下）这期干货满满！补充了很多Cmake进阶语法 分析了Sophus库相比于其他库的特殊之处 Pangolin配置见论区
Sophus库安装和使用
【7天搞定视觉SLAM】番外3——李代数库Sophus的使用
Sophus库官网链接：https://github.com/strasdat/Sophus
官网文档：
https://strasdat.github.io/Sophus/latest/
https://github.com/strasdat/Sophus/tree/fix-core-logging-performance-regression#lie-groups
https://farm-ng.github.io/docs/namespacesophus.html
Sophus库支持本章主要讨论的 SO(3) 和 SE(3)，此外，还含有二维运动SO(2),SE(2)及相似变换Sim(3)的内容。
它是直接在Eigen基础上开发的，我们不需要安装额外的依赖库。可以直接从GitHub上获取Sophus,在本书的代码目录slambook/3 rdparty下也提供了Sophus源代码。
https://github.com/gaoxiang12/slambook
由于历史原因，Sophus早期版本只提供了双精度的李群/李代数类。后续版本改写成了模板类。
模板类的Sophus中可以使用不同精度的李群/李代数，但同时增加了使用难度。在本书中，我们使用带模板的Sophus库。本书的3 rdparty中提供的Sophus是模板版本，它应该在你下载本书代码的时候就已经复制下来了。
安装
参考：
SLAM第四讲实践：李代数Sophus库的安装和使用
SLAM十四讲ch4代码调整
查看自己Eigen的版本号，因为Sophus依赖于Eigen，所以如果版本过低需要卸载重装（最低2.91.0）
cat /usr/include/eigen3/Eigen/src/Core/util/Macros.h | more
我的版本为3.3.7， 满足要求
Sophus相关库链接如下：
（1）Eigen3.3.7版本：https://gitlab.com/libeigen/eigen/-/releases
（3）fmt（这个很重要，没有就会一直报错）：https://github.com/fmtlib/fmt
（2）Sophus：https://github.com/strasdat/Sophus
安装fmt
git clone https://github.com/fmtlib/fmt.git
cd fmt
mkdir build
cd build
cmake ..
make
sudo make install
如果不安装，在编译 Sophus 时会提示没有安装 fmt 库
安装完成
Sophus包的原作者不是高翔，你就载github上打开slambook2master包，点到sopuhs里去，找到原作者，直接从他那下载zip
因为高翔也是调用别人的包,所以下载后解压出来是空的，直接点到原始作者发布sophus包的地方,再下载就行了
进入原作者的github下载 Sophus
git clone https://github.com/strasdat/Sophus.git
进入Spohus下载的文件夹
mkdir build
cd build
cmake ..
make
sudo make install
安装 fmt 后再编译就不会报错了
安装
都安装好后，源码中的 #include 就没有红线了
我这里使用的是slambook2的代码
有关于
同样因为时模板类的Sophus，源码中的所有Sophus::SO3都需要声明为Sophus::SO3d 或者
Sophus::SO3.这个问题好像不存在，貌似只存在于slambook1
如果你是slambook1的代码，请去源码修改
编译运行slambook2 中ch4的代码后，得到输出的误差和下图
记得修改 groundtruth.txt 和 estimated.txt 的路径
RMSE = 2.20728
Framebuffer with requested attributes not available. Using available framebuffer. You may see visual artifacts.
运行 useSophus 文件如下所示
使用
Lie Gropus
| c++ type | Lie group name | Description | minimal representation | #DoF | Matrix representation | compact internal manifold representation | #params |
|---|---|---|---|---|---|---|---|
Rotation2<T> | Special Orthogonal Group in 2D, SO(2) | rotations in 2d, also called Circle Group, or just “angle” | rotation angle | 1 | 2x2 matrix | unit complex number | 2 |
Rotation3<T> | Special Orthogonal Group in 3D, SO(3) | rotations in 3d, 3D orientations | rotation vector | 3 | 3x3 matrix | unit quaternion number | 4 |
Isometry2<T> | Special Euclidean Group in 2D, SE(3) | rotations and translations in 2D, also called 2D rigid body transformations, 2d poses, plane isometries | tangent vector of SE(2) | 3 | 3x3 matrix | unit complex number + translation vector | 2+2 = 4 |
Isometry3<T> | Special Euclidean Group in 3D, SE(3) | rotations and translations in 3D, also called rigid body transformations,6 DoF poses, Euclidean isometries | tangent vector of R x SE(3) | 6 | 4x4 matrix | unit quaternion number + translation vector | 4+3 = 7 |
RxSo2<T> | Direct product of SO(3) and scalar matrix, R x SO(2) | scaling and rotations in 2D | tangent vector of R x SO(2) | 3 | 2x2 matrix | non-zero complex number | 2 |
RxSo3<T> | Direct product of SO(3) and scalar matrix R x SO(3) | scaling and rotations in 3D | tangent vector of R x SO(3) | 4 | 3x3 matrix | non-zero quaternion number | 4 |
Similarity2<T> | Similarity Group in 2D, Sim(2) | scaling, rotations and translation in 2D | tangent vector of Sim(2) | 4 | 3x3 matrix | non-zero complex number+ translation vector | 2+2 = 4 |
Similarity3<T> | Similarity Group in 3D, Sim(3) | scaling, rotations and translation in 3D | tangent vector of Sim(3) | 4 | 4x4 matrix | non-zero complex number+ translation vector | 4+3 = 7 |
Cartesian2<T> | 2D Euclidean Vector Space, R^2 | all vector spaces are trivial Lie groups, also called 2d translation group, the translation part of SE(2) | 2-vector | 2 | 3x3 matrix | 2-vector | 2 |
Cartesian3<T> | 3D Euclidean Vector Space, R^3 | all vector spaces are trivial Lie groups, also called 3d translation group, the translation part of SE(2) | 3-vector | 3 | 4x4 matrix | 3-vector | 3 |
基本用法
包含头文件
#include "sophus/se3.hpp"
关于se3的官网文档：https://farm-ng.github.io/docs/namespacesophus_1_1se3.html
李群旋转表示
// 沿Z轴转90度的旋转矩阵
Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix(); //注意， AngleAxisd()中的旋转轴必须写成单位向量，如果不是单位向量，要先除以其模
// 或者四元数
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;
SO3上还是表示矩阵，我们初始化的矩阵需要 .matrix() 方法输出矩阵
结果如下
这里R也可以表示旋转，为什么需要转换成SO3的形式？ 因为SO3有很多其他的方法可以调用，如常用的 hat() 和 vee()
转换成李代数
// 使用对数映射获得它的李代数,使用SO3.log()可以实现大SO3->小so3的转换。
Vector3d so3 = SO3_R.log();
cout << "so3 = " << so3.transpose() << endl;
// 李代数so3 -> 李群SO3,使用SO3d::exp()可以实现小so3->大SO3的转换：
Sophus::SO3d SO3_so3 = Sophus::SO3d::exp(so3);
std::cout << "李代数so3 -> 李群SO3\n" << SO3_so3.matrix() <<std::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;
结果如下：
这里 s o 3 = ( 0 , 0 , 1.5708 ) so3=(0,0,1.5708) so3=(0,0,1.5708) 表示的是旋转向量，即
M_PI / 2 * Vector3d(0, 0, 1)
注意，这里so3->SO3 不是简单的直接代入指数函数，而是指数映射！数值计算的话需要泰勒展开来进行计算
增量扰动
// 增量扰动模型的更新
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;
结果如下
这个操作是定义了一个 θ n \theta n θn，然后将其作为 Δ R \Delta R ΔR，再与 R R R 相乘
对 S E ( 3 ) SE(3) SE(3) 的操作
// 对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;
使用总结
出现的错误
错误1
报错说找不到Sophus
-- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
-- Could NOT find Sophus (missing: Sophus_DIR)
-- Could not find the required component 'Sophus'. The following CMake error indicates that you either need to install the package with the same name or change your environment so that it can be found.
CMake Error at /opt/ros/noetic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
Could not find a package configuration file provided by "Sophus" with any
of the following names:
SophusConfig.cmake
sophus-config.cmake
Add the installation prefix of "Sophus" to CMAKE_PREFIX_PATH or set
"Sophus_DIR" to a directory containing one of the above files. If "Sophus"
provides a separate development package or SDK, be sure it has been
installed.
Call Stack (most recent call first):
CMakeLists.txt:4 (find_package)
原因是使用了ROS，在find_package时，要将 Sophus 分开写
# 这样写会报错
find_package(catkin REQUIRED COMPONENTS
roscpp
pcl_ros
std_msgs
Sophus REQUIRED
)
# 这样写不会报这次提到的错误
find_package(Sophus REQUIRED)
find_package(catkin REQUIRED COMPONENTS
roscpp
pcl_ros
std_msgs
)
错误2
参考：
fatal error: sophus/so3.h: No such file or directory
提示找不到 "sophus/so3.h 头文件
由于 sophus 安装到了 /usr/local/include/sophus 文件夹中，打开后显示文件如下
只有 so3.hpp 和 se3.hpp，没有 so3.h
将代码由 #include "sophus/so3.h" 改为
#include "sophus/so3.hpp"
即可
错误3
报错 没有参数列表的模板名称 “Sophus:：SO3”
error: invalid use of template-name ‘Sophus::SO3’ without an argument list
6 | Sophus::SO3 rot = Sophus::SO3(Eigen::Matrix3d::Identity());
| ^~~~~~
报错行代码为
Sophus::SO3 rot = Sophus::SO3(Eigen::Matrix3d::Identity());
要使用类 Sophus::SO3d ，将其改为
Sophus::SO3d rot = Sophus::SO3d(Eigen::Matrix3d::Identity());
即可
错误3
在求 单位向量a 的反对称矩阵时，报如下错误
error: ‘template<class Scalar_, int Options> class Sophus::SO3’ used without template arguments
27 | Eigen::Matrix3d a_hat = Sophus::SO3::hat(a);
| ^~~
解决办法：
SO3:: 中的定义是模板类，要声明变量类型
在 Sophus::SO3<double> 要加上 double 类型声明
Eigen::Matrix3d a_hat = Sophus::SO3<double>::hat(a);
// 或者使用 SO3d
Eigen::Matrix3d a_hat = Sophus::SO3d::hat(a);
错误4
将两个 Eigen::Vector3d 的向量相加，然后转换成SO3对象，加上上面红框的代码后，报如下错误
BCH_verify.cpp:(.text._ZN3fmt3v1019basic_memory_bufferIcLm500ESaIcEE4growEm[_ZN3fmt3v1019basic_memory_bufferIcLm500ESaIcEE4growEm]+0xf4): undefined reference to `fmt::v10::detail::assert_fail(char const*, int, char const*)'
collect2: error: ld returned 1 exit status
解决办法：
参考：
undefined reference to `vtable for fmt::v7::format_error‘
没有链接fmt库，链接上fmt 库即可。
target_link_libraries(BCH_verify ${Sophus_LIBRARIES} fmt)
错误5
参考：
使用cmake进行编译安装时指定安装位置
找不到 fmt
找到 sudo make install 时的目录
设置对应的路径
set(fmt_DIR /path)
