Sophus库使用笔记-Toy模板网

这篇具有很好参考价值的文章主要介绍了Sophus库使用笔记。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

1. 李群李代数简介

sophus,# C++开源库,线性代数,矩阵

2. Sophus库简介

Eigen库是一个开源的C++线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。但是Eigen库提供了集合模块，但没有提供李代数的支持。一个较好的李群和李代数的库是Sophus库，它很好的支持了SO(3),so(3),SE(3)和se(3)。Sophus库是基于Eigen基础上开发的，继承了Eigen库中的定义的各个类。因此在使用Eigen库中的类时，既可以使用Eigen命名空间，也可以使用Sophus命名空间。

using Transformation = Matrix<Scalar, N, N>;
using Point = Vector3<Scalar>;
using HomogeneousPoint = Vector4<Scalar>;
using Line = ParametrizedLine3<Scalar>;
using Hyperplane = Hyperplane3<Scalar>;
using Tangent = Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;

此外，为了方便说明SE(4)和se(4)，Sophus库还typedef了Vector4d、Matrix4d、Vector6d和Matrix6d等，即：

Sophus::Vector4d
Sophus::Matrix4d
Sophus::Vector6d
Sophus::Matrix6d

3. 安装方式

git clone https://github.com/strasdat/Sophus.git
cd Sophus
mkdir build 
cd build
cmake ..
make -j

4. CMake编写

CMAKE_MINIMUM_REQUIRED( VERSION 2.8)
PROJECT(useSophus)
FIND_PACKAGE(Sophus REQUIRED)
INCLUDE_DIRECTORIES(${Sophus_INCLUDE_DIRS})
add_executable(useSophus useSophus.cpp)
TARGET_LINK_LIBRARIES(useSophus ${Sophus_LIBRARIES})

5. 头文件

#include <Eigen/Core> //导入eigen库的核心组件
#include <Eigen/Geometry> //导入eigen库的几何组件

#include "sophus/so2.hpp" //导入sophus库的so2头文件
#include "sophus/se2.hpp" //导入sophus库的se2头文件
#include "sophus/sim2.hpp" //导入sophus库的sim2头文件
 
#include "sophus/so3.hpp" //导入sophus库的so3头文件
#include "sophus/se3.hpp" //导入sophus库的se3头文件
#include "sophus/sim3.hpp" //导入sophus库的sim3头文件

6. SO3，so3，SE3和se3初始化以及相互转换关系

sophus,# C++开源库,线性代数,矩阵

7. 李代数so3

Sophus::Vector3d //因为so(3)仅仅只是一个普通的3维向量

8. 李群SO3的构造函数

SO3();
SO3(const SO3& other);
explicit SO3(const Matrix3d& _R); // 使用旋转矩阵初始化
explicit SO3(const Quaterniond& unit_quaternion); // 使用四元数初始化
SO3(double rot_x, double rot_y, double rot_z); // 使用轴角初始化

9. SO3与so3的相互转换

Eigen::Vector3d so3_V1 = SO3_V1.log();//so(3) // 在Sophus(Eigen)中用vector3d表示,使用对数映射获得李群对应的李代数

Sophus::SO3 SO3_V2 = Sophus::SO3::exp(so3_V1); //使用指数映射将李代数转化为李群

Eigen::Matrix3d M_so3_V1 = Sophus::SO3::hat(so3_V1);//hat为向量到其对应的反对称矩阵

Eigen::Vector3d V_M = Sophus::SO3::vee(M_so3_V1);//vee为反对称矩阵对应的向量

10. SO3增量扰动模型

Eigen::Vector3d update_so3(1e-4,0,0);//假设更新量为这么多
Eigen::Matrix3d update_matrix=Sophus::SO3::exp(update_so3).matrix();//将李群转换为旋转矩阵
cout<<"SO3 update Matrix=\n"<<update_matrix<<endl<<endl;

Sophus::SO3 SO3_updated=Sophus::SO3::exp(update_so3)*SO3_R;
cout<<"SO3 updated = \n"<<SO3_updated<<endl;

Eigen::Matrix3d SO3_updated_matrix=SO3_updated.matrix();//将李群转换为旋转矩阵
cout<<"SO3 updated Matrix = \n"<<SO3_updated_matrix<<endl<<endl;

11. 李代数se3

Sophus::Vector6d //因为se(3)仅仅只是一个普通的6维向量

12. 李群SE3的构造函数

SE3();
SE3(const SO3& so3, const Vector3d& translation);
SE3(const Matrix3d& rotation_matrix, const Vector3d& translation);
SE3(const Quaterniond& unit_quaternion, const Vector3d& translation_);
SE3(const SE3& other);

13. SE3与se3的相互转换

Sophus::Vector6d se3_Rt=SE_Rt.log(); //se(3)在Sophus中用Vector6d表示,使用对数映射获得李群对应的李代数

Sophus::SE3 SE3_Rt2=Sophus::SE3::exp(se3_Rt); //使用指数映射将李代数转化为李群

Sophus::Matrix4d M_se3_Rt=Sophus::SE3::hat(se3_Rt);

Sophus::Vector6d V_M_se3=Sophus::SE3::vee(M_se3_Rt);

14. SE3增量扰动模型

Sophus::Vector6d update_se3=Sophus::Vector6d::Zero();
update_se3(0)=1e-4d;

cout<<"update_se3\n"<<update_se3.transpose()<<endl<<endl;

Eigen::Matrix4d update_matrix2=Sophus::SE3::exp(update_se3).matrix();//将李群转换为旋转矩阵
cout<<"update matrix=\n"<<update_matrix2<<endl<<endl;

Sophus::SE3 SE3_updated=Sophus::SE3::exp(update_se3)*SE3_Rt2;
cout<<"SE3 updated=\n"<<SE3_updated<<endl<<endl;

Eigen::Matrix4d SE3_updated_matrix=SE3_updated.matrix();//将李群转换为旋转矩阵
cout<<"SE3 updated Matrix=\n"<<SE3_updated_matrix<<endl<<endl;

15. SO3, SE3和se3的输出说明

尽管SO3对应于矩阵群，但是SO3在使用cout时是以so3形式输出的，输出的是一个3维向量；

SE3在使用cout输出时输出的是一个6维向量，其中前3维为对应的so3的值，后3维为实际的平移向量t；

se3在使用cout输出时输出的也是一个6维向量，但是其前3维为平移值ρρ（注意此时的ρρ与SE3输出的t是不同的，t=Jρρ,其中J是雅克比矩阵），后3维为其对应的so3.

16. 插值

#include "sophus/interpolate.hpp"

Sophus::SE3f c = Sophus::interpolate(a, b, 0.5);

17. 转化为Eigen

Sophus::SE3f mTrl;
Eigen::Matrix3f mRrl = mTrl.rotationMatrix();
Eigen::Vector3d mtrl = mTrl.translation();

参考文献

Sophus库的安装和使用教程（转载）_山城盛夏的博客-程序员ITS401_sophus库 - 程序员ITS401

Sophus源码逐行解读 ( 结合视觉SLAM十四讲的概念 ) - 掘金文章来源地址https://www.toymoban.com/news/detail-599519.html

到了这里，关于Sophus库使用笔记的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！

Sophus库使用笔记