SLAM算法与工程实践——SLAM基本库的安装与使用(4):Sophus库

2023-12-10 11:20

本文主要是介绍SLAM算法与工程实践——SLAM基本库的安装与使用(4):Sophus库,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

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++ typeLie group nameDescriptionminimal representation#DoFMatrix representationcompact internal manifold representation#params
Rotation2<T>Special Orthogonal Group in 2D, SO(2)rotations in 2d, also called Circle Group, or just “angle”rotation angle12x2 matrixunit complex number2
Rotation3<T>Special Orthogonal Group in 3D, SO(3)rotations in 3d, 3D orientationsrotation vector33x3 matrixunit quaternion number4
Isometry2<T>Special Euclidean Group in 2D, SE(3)rotations and translations in 2D, also called 2D rigid body transformations, 2d poses, plane isometriestangent vector of SE(2)33x3 matrixunit complex number + translation vector2+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 isometriestangent vector of R x SE(3)64x4 matrixunit quaternion number + translation vector4+3 = 7
RxSo2<T>Direct product of SO(3) and scalar matrix, R x SO(2)scaling and rotations in 2Dtangent vector of R x SO(2)32x2 matrixnon-zero complex number2
RxSo3<T>Direct product of SO(3) and scalar matrix R x SO(3)scaling and rotations in 3Dtangent vector of R x SO(3)43x3 matrixnon-zero quaternion number4
Similarity2<T>Similarity Group in 2D, Sim(2)scaling, rotations and translation in 2Dtangent vector of Sim(2)43x3 matrixnon-zero complex number+ translation vector2+2 = 4
Similarity3<T>Similarity Group in 3D, Sim(3)scaling, rotations and translation in 3Dtangent vector of Sim(3)44x4 matrixnon-zero complex number+ translation vector4+3 = 7
Cartesian2<T>2D Euclidean Vector Space, R^2all vector spaces are trivial Lie groups, also called 2d translation group, the translation part of SE(2)2-vector23x3 matrix2-vector2
Cartesian3<T>3D Euclidean Vector Space, R^3all vector spaces are trivial Lie groups, also called 3d translation group, the translation part of SE(2)3-vector34x4 matrix3-vector3

基本用法

包含头文件

#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轴平移1Sophus::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 anyof the following names:SophusConfig.cmakesophus-config.cmakeAdd 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 beeninstalled.
Call Stack (most recent call first):CMakeLists.txt:4 (find_package)

原因是使用了ROS,在find_package时,要将 Sophus 分开写

# 这样写会报错
find_package(catkin REQUIRED COMPONENTSroscpppcl_rosstd_msgsSophus REQUIRED
)# 这样写不会报这次提到的错误
find_package(Sophus REQUIRED)find_package(catkin REQUIRED COMPONENTSroscpppcl_rosstd_msgs
)
错误2

参考:

fatal error: sophus/so3.h: No such file or directory

提示找不到 "sophus/so3.h 头文件

在这里插入图片描述

由于 sophus 安装到了 /usr/local/include/sophus 文件夹中,打开后显示文件如下

在这里插入图片描述

只有 so3.hppse3.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 list6 | 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 arguments27 |     Eigen::Matrix3d a_hat = Sophus::SO3::hat(a);|                                     ^~~

解决办法:

SO3:: 中的定义是模板类,要声明变量类型

在这里插入图片描述

Sophus::SO3<double> 要加上 double 类型声明

Eigen::Matrix3d a_hat = Sophus::SO3<double>::hat(a);// 或者使用 SO3dEigen::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)

这篇关于SLAM算法与工程实践——SLAM基本库的安装与使用(4):Sophus库的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/476967

相关文章

Python依赖库的几种离线安装方法总结

《Python依赖库的几种离线安装方法总结》:本文主要介绍如何在Python中使用pip工具进行依赖库的安装和管理,包括如何导出和导入依赖包列表、如何下载和安装单个或多个库包及其依赖,以及如何指定... 目录前言一、如何copy一个python环境二、如何下载一个包及其依赖并安装三、如何导出requirem

Tomcat的下载安装与使用教程

《Tomcat的下载安装与使用教程》本文介绍了Tomcat的下载、安装和使用方法,包括在本机和云服务器上部署Tomcat的过程,以及解决启动失败问题的方法... 目录Tomcat的下载安装与使用Tomcat的下载与安装Tomcat在本机运行使用Tomcat在php云服务器上的使用总结Tomcat的下载安装与

Windows环境下安装达梦数据库的完整步骤

《Windows环境下安装达梦数据库的完整步骤》达梦数据库的安装大致分为Windows和Linux版本,本文将以dm8企业版Windows_64位环境为例,为大家介绍一下达梦数据库的具体安装步骤吧... 目录环境介绍1 下载解压安装包2 根据安装手册安装2.1 选择语言 时区2.2 安装向导2.3 接受协议

Nginx实现高并发的项目实践

《Nginx实现高并发的项目实践》本文主要介绍了Nginx实现高并发的项目实践,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友们下面随着小编来一起学习学习吧... 目录使用最新稳定版本的Nginx合理配置工作进程(workers)配置工作进程连接数(worker_co

Python使用PIL库将PNG图片转换为ICO图标的示例代码

《Python使用PIL库将PNG图片转换为ICO图标的示例代码》在软件开发和网站设计中,ICO图标是一种常用的图像格式,特别适用于应用程序图标、网页收藏夹图标等场景,本文将介绍如何使用Python的... 目录引言准备工作代码解析实践操作结果展示结语引言在软件开发和网站设计中,ICO图标是一种常用的图像

使用Java发送邮件到QQ邮箱的完整指南

《使用Java发送邮件到QQ邮箱的完整指南》在现代软件开发中,邮件发送功能是一个常见的需求,无论是用户注册验证、密码重置,还是系统通知,邮件都是一种重要的通信方式,本文将详细介绍如何使用Java编写程... 目录引言1. 准备工作1.1 获取QQ邮箱的SMTP授权码1.2 添加JavaMail依赖2. 实现

MyBatis与其使用方法示例详解

《MyBatis与其使用方法示例详解》MyBatis是一个支持自定义SQL的持久层框架,通过XML文件实现SQL配置和数据映射,简化了JDBC代码的编写,本文给大家介绍MyBatis与其使用方法讲解,... 目录ORM缺优分析MyBATisMyBatis的工作流程MyBatis的基本使用环境准备MyBati

IDEA与JDK、Maven安装配置完整步骤解析

《IDEA与JDK、Maven安装配置完整步骤解析》:本文主要介绍如何安装和配置IDE(IntelliJIDEA),包括IDE的安装步骤、JDK的下载与配置、Maven的安装与配置,以及如何在I... 目录1. IDE安装步骤2.配置操作步骤3. JDK配置下载JDK配置JDK环境变量4. Maven配置下

使用Python开发一个图像标注与OCR识别工具

《使用Python开发一个图像标注与OCR识别工具》:本文主要介绍一个使用Python开发的工具,允许用户在图像上进行矩形标注,使用OCR对标注区域进行文本识别,并将结果保存为Excel文件,感兴... 目录项目简介1. 图像加载与显示2. 矩形标注3. OCR识别4. 标注的保存与加载5. 裁剪与重置图像

使用Python实现表格字段智能去重

《使用Python实现表格字段智能去重》在数据分析和处理过程中,数据清洗是一个至关重要的步骤,其中字段去重是一个常见且关键的任务,下面我们看看如何使用Python进行表格字段智能去重吧... 目录一、引言二、数据重复问题的常见场景与影响三、python在数据清洗中的优势四、基于Python的表格字段智能去重