【SLAM】Sophus库的超详细解析

在视觉SLAM中，李群李代数是描述位姿比较常用的一种表达形式。但是，在Eigen中并不提供对它的支持，一个较好的李群和李代数的库是Sophus库，它很好的支持了SO3、so3、SE3、se3。

Sophus简介

代码仓库：https://github.com/strasdat/Sophus。

本文所对照的源码版本是2023.9.7日期，因此和网上其他的一些教程版本有些出入，但是总体相差不大。总体来说，差别在SO3的double构造函数、SO3的<<运算符重写都取消了等。

Sophus库是基于Eigen基础上开发的，继承了Eigen库中的定义的各个类。因此在使用Eigen库中的类时，既可以使用Eigen命名空间，也可以使用Sophus命名空间。比如：

Eigen::Matrix3d 和 Sophus::Matrix3d
Eigen::Vector3d 和 Sophus::Vector3d

此外，为了方便说明SE4和se4，Sophus库还typedef了Vector4d、Matrix4d、Vector6d和Matrix6d等，即：

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

Sophus使用教程

李群李代数的描述形式

Sophus库中李群李代数的描述形式如下：

// 李群SO3
namespace Sophus {
template <class Scalar_, int Options = 0>
class SO3;
using SO3d = SO3<double>;
using SO3f = SO3<float>;
}

// 李群SE3
namespace Sophus {
template <class Scalar_, int Options = 0>
class SE3;
using SE3d = SE3<double>;
using SE3f = SE3<float>;
}

template <class Scalar, int M, int Options = 0>
using Vector = Eigen::Matrix<Scalar, M, 1, Options>;

// 李代数so3
namespace Sophus {
template <class Scalar, int Options = 0>
using Vector3 = Vector<Scalar, 3, Options>;
using Vector3f = Vector3<float>;
using Vector3d = Vector3<double>;
}

//李代数se3
namespace Sophus {
template <class Scalar>
using Vector6 = Vector<Scalar, 6>;
using Vector6f = Vector6<float>;
using Vector6d = Vector6<double>;
}

可以看出：对于李群，Sophus直接定义了相应的类；而对于李代数，Sophus直接使用Eigen::Matrix<>定义了一个向量表示。

其中，so3的3个值对应的就是旋转，se3的前3个值对应的就是平移，后3个值对应的就是旋转。但是，se3平移的3个值都不直接等于平移，都需要经过一些转换才行，如何转换，在下文将会讲述到。

那么，李群类的构造函数有哪些呢？

// 默认构造函数将单位四元数初始化为单位旋转
SOPHUS_FUNC SO3();

// 拷贝构造函数
SOPHUS_FUNC SO3(SO3 const& other);

// 从SO3Base的其他子类进行拷贝构造
template <class OtherDerived>
SOPHUS_FUNC SO3(SO3Base<OtherDerived> const& other);

// 基于旋转矩阵的构造函数
// 前提条件：旋转矩阵需要正交
static int constexpr N = 3;
using Transformation = Matrix<Scalar, N, N>;
SOPHUS_FUNC SO3(Transformation const& R);

// 基于四元数的构造函数
// 前提条件：四元数不得接近零
template <class D>
SOPHUS_FUNC explicit SO3(Eigen::QuaternionBase<D> const& quat);


// 默认构造函数
SOPHUS_FUNC SE3();

// 拷贝构造函数
SOPHUS_FUNC SE3(SE3 const& other) = default;

// 从SE3Base的其他子类进行拷贝构造
template <class OtherDerived>
SOPHUS_FUNC SE3(SE3Base<OtherDerived> const& other);

// 基于SO3、平移向量的的构造函数
template <class OtherDerived, class D>
SOPHUS_FUNC SE3(SO3Base<OtherDerived> const& so3,
                Eigen::MatrixBase<D> const& translation);

// 基于旋转矩阵、平移向量的构造函数
// 前提条件：旋转矩阵需要正交
SOPHUS_FUNC SE3(Matrix3<Scalar> const& rotation_matrix, Point const& translation);

// 基于四元数、平移向量的构造函数
// 前提条件：四元数不得接近零
SOPHUS_FUNC SE3(Eigen::Quaternion<Scalar> const& quaternion,
                Point const& translation);

// 基于4*4矩阵的构造函数
// 前提条件：左上3*3子矩阵需要正交。最后一行必须是（0,0,0,1）
SOPHUS_FUNC explicit SE3(Matrix4<Scalar> const& T);

可以看到，除了最基本的默认构造函数和拷贝构造函数之外，李群类还支持一下的构造函数：

SO(3)还支持旋转矩阵类，四元数类，不支持旋转向量（轴角）
SE(3)还支持旋转矩阵类+平移向量，四元数类+平移向量，4*4的变换矩阵类

需要注意的是，网上的一些版本会有类似于一下的构造函数：

Sophus::SO3f SO3_v(0, 0, M_PI/2);  // 这里注意，不是旋转向量的三个坐标值，有点像欧拉角构造

但是，从本文的Sophus版本上看，已经取消了该构造函数。

李群李代数的转换关系

下图展示了李群李代数的相互转换关系：

在这里插入图片描述

有一点需要注意：

在视觉SLAM十四讲中提到，李群李代数的转换关系 $SO3=\exp{(so3^{\wedge})}$ 。也就是说，so3实际上就是由所谓的旋转向量组成的空间，而指数映射即罗德里格斯公式。 $\exp$ 运算里面的参数应该是一个反对称矩阵，而不是一个向量。但是从图中看，so3直接通过SO3::exp()方法转换成SO3。

这是因为，在SO3::exp()内部已经经过了反对称运算的操作。具体可以参考文章：Sophus::SO3d::exp() 方法中的参数为什么是一个向量？。

还有一点需要注意：

对于SO3而言，对应的so3经过反对称和 $\exp$ 计算就得到了；即， $so3=\phi$ ：

$\exp(\phi^{\wedge})=R=SO3$

对于SE3而言，对应的so3后三个元素经过反对称和 $\exp$ 计算就得到SE3的左上3 * 3 的旋转部分，so3的前三个元素左乘一个 $J$ 才能得到SE3的右上3 * 1的平移部分。这个 $J$ 实际上就是BCH公式的 $J_l$ 的值；即， $se3=\xi=\begin{bmatrix}\rho \\ \phi \end{bmatrix}$ ：

$\exp(\xi^{\wedge})=\begin{bmatrix}R&J\rho\\0&1\end{bmatrix}=T$

这部分可以参考文章：sophus库根据se3的exp函数求解平移向量。

从代码上，先看李群初始化和相互转换部分：

// 先定义轴角、旋转矩阵、四元数、变换矩阵
Eigen::AngleAxisd A(M_PI / 2, Eigen::Vector3d(0, 0, 1));     // 旋转轴：(0,0,1)，角度：180度
Eigen::Matrix3d R = A.matrix();
Eigen::Quaterniond Q(A);
Eigen::Vector3d t(1, 0, 0);
Eigen::Isometry3d T= Eigen::Isometry3d::Identity();          // 虽然称为3d，实质上是4＊4的矩阵
T.rotate(R);
T.pretranslate(t);

// 初始化部分
Sophus::SO3d SO3_R(R);       // 从旋转矩阵初始化
Sophus::SO3d SO3_Q(Q);       // 从四元数初始化
// Sophus::SO3d SO3_A(A);       // 从轴角初始化，错误！！！！

Sophus::SE3d SE3_R(R, t);            // 从旋转矩阵初始化
Sophus::SE3d SE3_Q(Q, t);            // 从四元数初始化
Sophus::SE3d SE3_SO3(SO3_R, t);      // 从SO3初始化
Sophus::SE3d SE3_T(T.matrix());      // 从变换矩阵初始化

// 相互转化
Sophus::SO3d SO3_SE3 = SE3_R.so3();				// 从SE3转换到SO3
Eigen::Vector3d t_SE3 = SE3_R.translation(); 	// 从SE3转换到t
Eigen::Matrix3d R_SE3 = SE3_R.rotationMatrix();	// 从SE3转换到R
Eigen::Matrix4d T_SE3 = SE3_R.matrix();			// 从SE3转换到T

从代码上，再看李群李代数的转换部分：

Eigen::Vector3d so3(0, 0, M_PI / 2);            // so3在Eigen中用Vector3d表示
Sophus::SO3d SO3_so3 = Sophus::SO3d::exp(so3);
so3 = SO3_so3.log();

Eigen::Vector6d se3(0, 0 , 1, 0, 0, M_PI / 2);  // se3在Eigen中用Vector6d表示
Sophus::SE3d SE3_se3 = Sophus::SE3d::exp(se3);
se3 = SE3_se3.log();

在扰动模型中：

Eigen::AngleAxisd A(M_PI / 2, Eigen::Vector3d(0, 0, 1));
Eigen::Vector3d t(1, 0, 0);

Sophus::SO3d SO3_R(A.matrix());
Eigen::Vector3d update_so3(1e-4, 0, 0);                 // 假设更新量为这么多
Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;

Sophus::SE3d SE3_R(A.matrix(), t);
Sophus::Vector6d update_se3(1e-4, 0, 0, 0, 0, 0);       // 假设更新量为这么多
Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_R;

注意，这里仅仅是指原李群经过一个小的扰动更新成新的李群，并不涉及到BCH公式，或者求导的操作。

从代码上，再看李代数与反对称矩阵的转换部分：

Eigen::Vector3d so3(0, 0, M_PI / 2);
Eigen::Matrix3d so3_hat = Sophus::SO3d::hat(so3);
so3 = Sophus::SO3d::vee(so3_hat);

Eigen::Vector6d se3(0, 0 , 1, 0, 0, M_PI / 2);
Eigen::Matrix6d se3_hat = Sophus::SE3d::hat(se3);
se3 = Sophus::SE3d::vee(se3_hat);

其他代码

在李群李代数中，有两个公式比较常见，一个是 $\exp$ 的罗德里格斯公式，一个是BCH公式，这里先简单回顾一下。

求解 $\exp(so3^{\wedge})$ ，那么（将 $so 3$ 写成 $\theta a$ ，其中， $\theta$ 为模长， $a$ 是一个长度为1的方向向量）：

$\exp(\theta a^{\wedge})=I+(1-cos\theta)a^{\wedge}a^{\wedge}+sin\theta a^{\wedge}$

BCH公式 $\exp((\phi_1+\phi_2)^{\wedge})=\exp(\phi_1)^{\wedge}\exp((J_r(\phi_1)\phi_2)^{\wedge})$ ，那么（将 $\phi_1$ 写成 $\theta a$ ，其中， $\theta$ 为模长， $a$ 是一个长度为1的方向向量）：

$J_{r}(\theta a)=I+(1-\frac{sin\theta}{\theta})a^{\wedge}a^{\wedge}+\frac{cos\theta - 1}{\theta}a^{\wedge}$

ORB-SLAM3中IMU预积分中有一个函数，就实现了Exp和BCH的右雅可比矩阵的函数，代码为：

const float eps = 1e-4;

IntegratedRotation::IntegratedRotation(const float &x, const float &y, const float &z) {
  const float d2 = x * x + y * y + z * z;
  const float d = sqrt(d2);

  Eigen::Vector3f v;
  v << x, y, z;
  Eigen::Matrix3f W = Sophus::SO3f::hat(v);
  if (d < eps) {
    // 小值的Exp近似计算
    deltaR = Eigen::Matrix3f::Identity() + W;
    // 小值的右雅可比，近似为I
    rightJ = Eigen::Matrix3f::Identity();
  } else {
    // Exp 罗德里格斯公式
    deltaR = Eigen::Matrix3f::Identity() + W * sin(d) / d + W * W * (1.0f - cos(d)) / d2;
    // SO3的BCH公式，右雅可比
    rightJ = Eigen::Matrix3f::Identity() - W * (1.0f - cos(d)) / d2 + W * W * (d - sin(d)) / (d2 * d);
  }
}