SLAM算法与工程实践——SLAM基本库的安装与使用（4）：Sophus库

yangtsejin

已于 2024-03-25 17:34:05 修改

阅读量2.2k

点赞数 41

分类专栏： SLAM算法与工程实践系列文章标签：算法 ubuntu 机器人计算机视觉

于 2023-12-10 01:29:11 首次发布

本文链接：https://blog.csdn.net/qq_37648371/article/details/134765488

版权

SLAM算法与工程实践系列专栏收录该内容

23 篇文章 11 订阅

订阅专栏

SLAM算法与工程实践系列文章

下面是SLAM算法与工程实践系列文章的总链接，本人发表这个系列的文章链接均收录于此

SLAM算法与工程实践系列文章链接

下面是专栏地址：

SLAM算法与工程实践系列专栏

前言

这个系列的文章是分享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 文件如下所示

在这里插入图片描述

出现的错误

错误1

在编译Sophus时报如下错误

sophus/so2.cpp:32:26: error: lvalue required as left operand of assignment
   32 |   unit_complex_.real() = 1.;
      |                          ^~
/home/hwkan/jin0325_ws/src/S-FAST_LIO-master/3dparty/Sophus/sophus/so2.cpp:33:26: error: lvalue required as left operand of assignment
   33 |   unit_complex_.imag() = 0.;

在这里插入图片描述

报错位置在 sophus/so2.cpp 中，找到对应位置

SO2::SO2()
{
  unit_complex_.real() = 1.;
  unit_complex_.imag() = 0.;
}

在这里插入图片描述

将其改为

SO2::SO2()
{
  unit_complex_.real(1.);
  unit_complex_.imag(0.);
}

在这里插入图片描述

这样就可以正常编译了

错误2

在编译算法时显示Sophus的链接错误

/usr/bin/ld: /home/hwkan/jin0325_ws/src/S-FAST_LIO-master/include/esekfom.hpp:218: undefined reference to `Sophus::SO3::log() const'
/usr/bin/ld: CMakeFiles/fastlio_mapping.dir/src/laserMapping.cpp.o: in function `void set_posestamp<geometry_msgs::PoseWithCovariance_<std::allocator<void> > >(geometry_msgs::PoseWithCovariance_<std::allocator<void> >&)':
/home/hwkan/jin0325_ws/src/S-FAST_LIO-master/src/laserMapping.cpp:489: undefined reference to `Sophus::SO3::matrix() const'
/usr/bin/ld: CMakeFiles/fastlio_mapping.dir/src/laserMapping.cpp.o: in function `void set_posestamp<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> >&)':
/home/hwkan/jin0325_ws/src/S-FAST_LIO-master/src/laserMapping.cpp:489: undefined reference to `Sophus::SO3::matrix() const'
/usr/bin/ld: CMakeFiles/fastlio_mapping.dir/src/laserMapping.cpp.o: in function `esekfom::esekf::h_share_model(esekfom::dyn_share_datastruct&, boost::shared_ptr<pcl::PointCloud<pcl::PointXYZINormal> >&, KD_TREE<pcl::PointXYZINormal>&, std::vector<std::vector<pcl::PointXYZINormal, Eigen::aligned_allocator<pcl::PointXYZINormal> >, std::allocator<std::vector<pcl::PointXYZINormal, Eigen::aligned_allocator<pcl::PointXYZINormal> > > >&, bool) [clone ._omp_fn.0]':

在这里插入图片描述

在CMakeLists.txt 里面加上

set(Sophus_LIBRARIES libSophus.so)

在这里插入图片描述

如果找不到 libSophus.so, 在 make install sophus 后用 sudo ldconfig 更新下

make install
sudo ldconfig

使用

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;

结果如下：

在这里插入图片描述

这里 $so 3 = (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;

结果如下

在这里插入图片描述

这个操作是定义了一个 $\theta n$ ，然后将其作为 $\Delta R$ ，再与 $R$ 相乘

对 $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)

yangtsejin

关注

41
点赞
踩
27

收藏

觉得还不错? 一键收藏
0
评论
SLAM算法与工程实践——SLAM基本库的安装与使用（4）：Sophus库

Sophus库
复制链接

扫一扫

专栏目录

SLAM算法与工程实践——SLAM基本库的安装与使用（4）：Sophus库

SLAM算法与工程实践系列文章

SLAM算法与工程实践系列文章链接

SLAM算法与工程实践系列专栏

文章目录

前言

SLAM算法与工程实践——SLAM基本库的安装与使用（4）：Sophus库

Sophus

安装

出现的错误

错误1

错误2

使用

Lie Gropus

基本用法

出现的错误

错误1

错误2

错误3

错误3

错误4

错误5