Sophus库：对SO(3)和SE(3)的构建及扰动模型搭建

本文链接：https://blog.youkuaiyun.com/weixin_45929038/article/details/122867599

这篇博客介绍了如何利用Sophus库在C++中处理SO(3)和SE(3)的几何变换，包括旋转和平移。通过旋转矩阵和四元数创建SO(3)和SE(3)对象，进行对数映射、指数映射、向量与反对称矩阵的转换，并展示了增量更新的方法。此外，还提到了Sophus库的API使用及配置选项。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

简介

使用Eigen库中Geometry模块能够实现对 $S O (3)$ 、 $S E (3)$ 的构建，而使用Sophus库能够进行李代数的求导。

Sophus库通常用于 2D和 3D的几何问题（即计算机视觉、机器人应用领域的使用），库中实现了特殊正交群 $S O (2)$ 和 $S O (3)$ 来表示2D和3D中的旋转，以及特殊的欧几里得群 $S E (2)$ 和 $S E (3)$ 来表示2D和3D中的刚体变换。

Sophus库提供了C++和Python两种版本的API，本文记录带模板的C++版本使用。

Sophus的GitHub地址：https://github.com/strasdat/Sophus

Sophus的API文档地址：https 😕/strasdat.github.io/Sophus/

Sophus库基于Eigen 3.3.X 和（可选）fmt开发，在配置 Sophus 时，可以通过将“-DUSE_BASIC_LOGGING=ON”传递给 cmake 来消除 fmt 依赖。

使用方式

此处，博主正在学习高博的《视觉SLAM十四讲》，对书上例程进行学习：

$S O (3)$ 相关

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/so3.hpp"

using namespace std;
using namespace Eigen;

int main()
{
    //  定义旋转矩阵（绕Z轴旋转90°）
    Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
    //  定义四元数
    Quaterniond q(R);
    //  从旋转矩阵进行构建SO(3)
    Sophus::SO3<double> SO3_R(R);
    //  从四元数进行构建SO(3)
    Sophus::SO3d SO3_q(q);
    //  输出李群内容
    cout << "SO(3)\n" << SO3_R.matrix() << endl;
    //  对数映射，获取李代数
    Vector3d so3 = SO3_R.log();
    cout << "so(3)\n" << so3.transpose() << endl;
    //  从向量到反对称矩阵
    Matrix3d hat = Sophus::SO3d::hat(so3);
    cout << "so(3) hat \n" << hat << endl;
    //  从反对称矩阵到向量
    cout << "so3 hat vee \n" << Sophus::SO3d::vee(hat).transpose() << endl;
    
    /*增量扰动模型(左乘)的更新*/
    //  设置更新量
    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;
    
    return 0;
}

$S E (3)$ 相关

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

int main()
{
    //  定义旋转矩阵（绕Z轴旋转90°）
    Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
    //  定义四元数
    Quaterniond q(R);
    //  定义平移矢量（沿X轴平移1）
    Vector3d t(1, 0, 0);
    //  从旋转矩阵进行构建SE(3)
    Sophus::SE3d SE3_Rt(R,t);
    //  从四元数进行构建SE(3)
    Sophus::SE3d SE3_qt(q,t);
    //  输出李群内容
    cout << "SE(3)\n" << SE3_Rt.matrix() << endl;
    //  定义六维变量TypeDef
    typedef Matrix<double, 6, 1> Vector6d;
    //  对数映射，获取李代数
    Vector6d se3 = SE3_Rt.log();
    cout << "se3\n" << se3.transpose() << endl;
    //  Sophus库中，se(3)向量中平移在前，旋转在后
    //  从向量到反对称矩阵
    Matrix<double, 6, 6> hat = Sophus::SE3d::hat(se3);
    cout << "se3 hat \n" << hat << endl;
    //  从反对称矩阵到向量
    cout << "se3 hat vee \n" << Sophus::SE3d::vee(hat).transpose() << endl;
    
    /*增量扰动模型(左乘)的更新*/
    //  设置更新量
    Vector3d update_se3;
    //  置零初始化
    update_se3.setZero();
    update_se3(0,0)=1e-4d;
    //  指数映射，获取更新量李群
    Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
    cout << "SE3 updated \n" << SE3_updated.matrix() << endl;
    
    return 0;
}

总结

李群	SO(3)	SE(3)
旋转矩阵构建	`Sophus::SO3d SO3(R)`	`Sophus::SE3d SE3(R,t)`
四元数构建	`Sophus::SO3d SO3(q)`	`Sophus::SO3d SO3(q,t)`
输出	`SO3.matrix()`	`SE3.matrix()`
对数映射	`Vector3d so3=SO3.log()`	`Vecotr6d se3=SE3.log()`
指数映射	`SO3d::exp(so3)`	`SE3d::exp(se3)`
向量到反对称矩阵	`SO3d::hat(so3)`	`SE3d::hat(se3)`
反对称矩阵到向量	`SO3d::vee(hat)`	`SE3d::vee(hat)`