Sophus源码逐行解读 ( 结合视觉SLAM十四讲的概念 )

作者： 边城量子 ( shihezichen@live.cn )

简介

本文介绍了sophus库的部分源码实现，包括sophus 命名空间中的基本的数据类型定义、 so3.hpp、se3.hpp 中的类成员函数的逐行讲解。

本文会对对代码涉及的SLAM概念、C++语法、代码逻辑均会结合源码进行针对性解读。目的是通过解读和结合 视觉SLAM十四讲 相关章节、相关概念进行对照，对前几章节学习过的概念进行复习和检阅。

欢迎留言和讨论。

基础阅读

在开始阅读本文源码之前，可先通过如下文章了解 Sophus 库的基本安装和基本使用:

• Ubuntu18.04下Sophus库的安装（非模板库模式）
• Ubuntu18.04下Sophus库的安装（模板库模式）
• Sophus库编程基础
• Sophus库用到一些C++语言特性:
  • C++中 enable_if 的用法( 以Sophus库为例 )
  • c++中 explicit (以Sophus为例)
  • C++中 宏用do{} while(false)的原因( 以Sophus 为例)
  • C++中 template cast ( 以Sophus库为例 )
  • C++ typename和constexpr（以Sophus库为例）
  • C++ traits ( 以Sophus库为例 )
• Lie Group Foundations ： 对李群、李代数的相关信息的总结。基本概念、映射关系、伴随、Jacobian等重要的性质和公式

基本概念

映射关系

• $\ ^\wedge: \mathbb R^m \longrightarrow \mathfrak m$ : 把正切向量 $\boldsymbol \tau$ 映射到李代数中的元素 $\boldsymbol \tau^\wedge$ , 即: 向量$\longrightarrow$反对称矩阵

• $\ ^\vee: \mathfrak m \longrightarrow \mathbb R^m$ : $\ ^\wedge$ 的逆运算, 把李代数中的元素 $\boldsymbol \tau^\wedge$ 映射到正切向量 $\boldsymbol \tau$, 即: 反对称矩阵$\longrightarrow$ 向量

• $\exp (\boldsymbol \tau^\wedge): \mathbb R^n \longrightarrow \mathcal M$ : 把正切向量 $\boldsymbol \tau$ 映射到李代数后, 然后再映射为李群中的元素 $\mathcal X$

• $\log(\mathcal X)^\vee: \mathcal M \longrightarrow \mathbb R^m$ : $\exp$ 的逆运算, 把李群中的元素 $\mathcal X$ 映射到李代数中的元素后, 再映射为正切向量 $\boldsymbol \tau$ 其中 $\mathbb R^m$ 代表m维向量空间， $\mathfrak m$ 代表李代数，$\mathcal M$ 代表李群

关于李群基础知识的更进一步的详细信息请参考帖子： Lie Group Foundations

Sophus库的数据结构

在Sophus库中是如何表示李群、李代数这些基本概念的呢？

• 旋转矩阵用来描述三维空间刚体运动
• 李群 SO(3) : 3x3旋转矩阵
• 李代数 $\mathfrak {so}(3)$ : 3x1三维向量, 直接使用了正切向量来代表
• 李群 SE(3) : 4x4变换矩阵
• 李代数 $\mathfrak {se}(3)$ : 6x1六维向量, 平移在前, 旋转在后

关键文件

Sophus库中的sophus目录:

average.hpp
common.hpp
example_ensure_handler.cpp
geometry.hpp
interpolate.hpp
interpolate_details.hpp
num_diff.hpp
rotation_matrix.hpp
rxso2.hpp
rxso3.hpp
se2.hpp
se3.hpp
sim2.hpp
sim3.hpp
sim_details.hpp
so2.hpp
so3.hpp
spline.hpp
test_macros.hpp
types.hpp
velocities.hpp

下面将重点关注 so3.hpp、se3.hpp的源码实现。在这之前，需要先了解 types.hpp 中的数据结构定义和类型声明。

sophus/types.hpp

• 作用： 定义通用数据类型别名

• 特点： 采用与Eigen类似的方法， 定义出Sophus自己的 Vector， Vector2，Vector2f, Matrix， Matrix3, Matrix3d 等等预置类型。

• Vector

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

  Sophus使用了 Eigen::Matrix 来定义自己的 Vector 变量，即大小为 M x 1，元素类型为Scalar的矩阵。 回顾Eigen::Matrix的定义：

  Matrix<typename Scalar, int RowsAtCompileTime, int ColsAtCompileTime>
  /**
  参数说明：
  Matrix定义，有6个模板参数，主要使用前三个参数，剩下的有默认值。
     Scalar: 元素类型
     RowsAtCompileTime：矩阵行数
     ColsAtCompileTime: 矩阵列数
  */
  
  // Eigen使用 typedef 定义一些预定义类型，如：
  typedef Matrix<float, 4, 4> Matrix4f;
  

• Vector2, Vector2f, Vector2d

  template <class Scalar, int Options = 0>
  using Vector2 = Vector<Scalar, 2, Options>;
  using Vector2f = Vector2<float>;
  using Vector2d = Vector2<double>;
  

  • Sophus 使用 Vector 来定义 Vector2， 即把Vector的行数固化为2。

  • 有了Vector2后， 通过模板参数类型特化为 float或double，可以特化定义出Vector2f,Vector2d数据类型。

  • 更高维度的Vector， 如Vector3, Vector4, Vector6, Vector7，使用上面相同的思路。

• Matrix, Matrix2, Matrix2f

  template <class Scalar, int M, int N>
  using Matrix = Eigen::Matrix<Scalar, M, N>;
  
  template <class Scalar>
  using Matrix2 = Matrix<Scalar, 2, 2>;
  using Matrix2f = Matrix2<float>;
  using Matrix2d = Matrix2<double>;
  

  使用相同的思路， 定义Matrix， Matrix2， Matrix2f，以及更高维度的矩阵 Matrix6，Matrix7

sophus/so2.hpp

• 作用： 定义so2中的数据类型 SO2d, SO2f

• SO2d, SO2f

  namespace Sophus {
    template <class Scalar_, int Options = 0>
    class SO2;
    using SO2d = SO2<double>;
    using SO2f = SO2<float>;
  }  // namespace Sophus
  

  可以看出， Sophus是先通过模板方式定义出SO2，然后再对其中元素类型Scalar_进行特化后定义除了SO2d， SO2f so2.hpp 中也大量定义了其他类型, 使用的手法和 Vector、Matrix 等类型的定义类似，就不再赘述.

sophus/so3.hpp & se3.hpp

1. Sophus 新增定义

文件开始是 Sophus 和 Eigen 命名空间中的类型定义或声明.

比如声明了 Sophus 命名空间中 SO3、SO3d、SO3f 数据类型; 在Eigen:internal命名空间中补充了一些 traits 的定义, 这些traits是对 Eigen 库既有traits的补充, 比如补充了 Sophus::SO3 类型、Map<Sophus::SO3类型。

sophus.so3.hpp:

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

namespace Eigen {
namespace internal {

template <class Scalar_, int Options_>
struct traits<Sophus::SO3<Scalar_, Options_>> {
  static constexpr int Options = Options_;
  using Scalar = Scalar_;
  using QuaternionType = Eigen::Quaternion<Scalar, Options>;
};

template <class Scalar_, int Options_>
struct traits<Map<Sophus::SO3<Scalar_>, Options_>>
    : traits<Sophus::SO3<Scalar_, Options_>> {
  static constexpr int Options = Options_;
  using Scalar = Scalar_;
  using QuaternionType = Map<Eigen::Quaternion<Scalar>, Options>;
};

template <class Scalar_, int Options_>
struct traits<Map<Sophus::SO3<Scalar_> const, Options_>>
    : traits<Sophus::SO3<Scalar_, Options_> const> {
  static constexpr int Options = Options_;
  using Scalar = Scalar_;
  using QuaternionType = Map<Eigen::Quaternion<Scalar> const, Options>;
};
}  // namespace internal
}  // namespace Eigen

• C++ traits是做什么的？ Sophus新增了哪些 traits？

  可查看相关帖子： C++ traits ( 以Sophus库为例 ) ： juejin.cn/post/707562…

• Sophus新增了哪些 traits ？

  template <class Scalar_, int Options_> 
  struct traits<Sophus::SO3<Scalar_, Options_>> { 
    static constexpr int Options = Options_; 
    using Scalar = Scalar_; 
    using QuaternionType = Eigen::Quaternion<Scalar, Options>; };
  

  • 在Eigen::internal 命名空间中,增加了一个新的traits.其中的Sophus::SO3<Scalar_, Options_>, 表明这个 traits 是针对哪个数据类型的特化.

  • 在这个traits类里面, 又定义了两个新的数据类型, 分别是Scalar, QuaternionType.

  • 在有了如上定义之后, 以后如果要使用到这两个新定义的数据类型, 则可以采用如下代码形式:

    template<typename Derived>
    class T {
    public:
        using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
        using QuaternionType = typename Eigen::internal::traits<Derived>::QuaternionType;
        //...
    };
    

    定义新的数据类型Scalar, QuaternionType, 即把traits中的类型暴露出来并别名化的过程.

  • 当出现以上代码时, Scalar也并不一定会指向上面为SO3所定义的traits. 因为Eigen库中为旋转向量、旋转矩阵等等都定义了对应的traits, 还是要看Derived是什么类型. 在Sophus库中, 其中的模板参数Derrived往往会被特化为Sophus::SO3类型, 这时候编译器就会使用上面traits的定义了.

2. SO3Base 基本数据类型

• 代码注释
  • $SO3$ 是一个矩阵的乘法群, 它主要用于在3维空间的旋转.
  • 它满足群的定义, 即满足封闭性/幺元/结合律等;
  • 它还是一个正交群, 具有正交的性质,如 $R \times R^\top=I$ ,以及行列式值为1.
  • 它不满足交换律,即一般情况下 $R_1 \times R_2 \neq R_2 \times R_1$ , 这就意味着先围绕 $x$ 轴旋转再围绕 $y$ 轴旋转, 并不一定等于先围绕 $y$ 轴旋转再围绕 $x$ 轴旋转.

sophus/so3.hpp:

template <class Derived>
class SO3Base {
 public:
  static constexpr int Options = Eigen::internal::traits<Derived>::Options;
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  using QuaternionType =
      typename Eigen::internal::traits<Derived>::QuaternionType;
  using QuaternionTemporaryType = Eigen::Quaternion<Scalar, Options>;

  
  static int constexpr DoF = 3;
  static int constexpr num_parameters = 4;
  static int constexpr N = 3;
  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>;

• C++ typename 和 constexpr 是干什么的？

  可查看相关帖子：C++ typename和constexpr（以Sophus库为例）：juejin.cn/post/707562…

• 基本的类成员

  static int constexpr DoF = 3;
  static int constexpr num_parameters = 4;
  static int constexpr N = 3;
  

  • DoF： 代表正交群的自由度数(Degree of Freedom)
  • num_parameters: 在Sophus::SOBase类内部实现中, 是使用四元数来表示旋转的, 这里的num_parameters 类成员表示的就是内部实现的数据类型所使用的参数个数(四元数使用了4个变量).
  • N: N=3代表的是SO3变换是 3x3 的矩阵

• 重要的类成员
  接下来的代码, 则是针对 SO3 的情况, 重新特化定义了一些类型，如下：

  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>;
  

  • Transformation: 3x3 的变换矩阵, 其含义是由单位旋转向量通过符号$\ ^\wedge$ 得到的反对称矩阵，具体可查看 hat() 函数

  • Point : 3x1的空间点

  • HomogeneousPoint: 4x1的齐次表示空间点

  • Tangent : 3x1的切向量，也就是对应的李代数

  • Adjoint: 3x3的伴随矩阵，是通过 adj() 函数返回的数据类型

3. SO3Base::adj()

so3.hpp :

• SO3Base::adj() 函数

  SOPHUS_FUNC Adjoint Adj() const { return matrix(); }
  

  矩阵A的伴随变换, 返回的是一个 3x3 的矩阵, 代表伴随变换后的矩阵

• 伴随的定义

  先来看 视觉SLAM十四讲 中对伴随性质的描述, 参见书中第4章的习题5,6中的定义：

  证明:

  $$\boldsymbol { (Rp)^\wedge = Rp^\wedge R^\top =Rp^\wedge R^{-1} } \tag{习题5}$$
  $$\boldsymbol R \exp(\boldsymbol p^\wedge) \boldsymbol R^\top = \exp((\boldsymbol {Rp} )^\wedge) \tag{习题6}$$

  这个式子被称为 $SO(3)$ 上的伴随性质. 同样的，在 $SE(3)$ 上亦有伴随性质：

  $$T \exp(\boldsymbol \xi^\wedge) T^{-1} = \exp((Ad(T) \boldsymbol \xi )^\wedge)$$

  备注

  从上面的 $SO(3)$ 的公式无法显式的看出 $R$ 的伴随，这是因为对于 $R \in SO(3)$ , 其伴随矩阵就是自己.

• 伴随的本质

  相比视觉SLAM十四讲， 文献 A micro Lie theory for state estimation in robotics 中的伴随定义更能体现伴随的本质。

  • 对李代数的伴随变换

    如下定义所示， 假设已知李群元素 $T$, 则它的对其李代数的伴随变换被定义为如下形式(即从李代数到李代数的线性映射）：

    $$Ad(T)： \mathfrak m \longrightarrow \mathfrak m; \quad \boldsymbol \xi_{T}^\wedge \longmapsto Ad(T) \boldsymbol \xi_{T}^\wedge \triangleq T \boldsymbol \xi_{T}^\wedge T^{-1}$$

    从上述定义可以看出，上述伴随含义是： 一个非李群幺元 $\boldsymbol \epsilon$ 处的李代数元素 $\boldsymbol \xi^\wedge_T$， 被映射为李群幺元对应的李代数元素 $T \boldsymbol \xi_{T}^\wedge T^{-1}$。

    我们继续。

  • 对切向量的伴随变换

    此外， 我们还知道李代数元素可以通过 $\ ^\vee$ 运算符得到正切向量。

    因此, 在上述变换两边都施加 $\ ^\vee$ 操作符，则可以得到一个从正切向量到正切向量的变换，重新定义为 $\mathbf {Ad}(T)$（由于是对向量的变换，此处应为矩阵）, 如下：

    $$\mathbf {Ad}(T): \mathbb R^m \longrightarrow \mathbb R^m;\quad \boldsymbol \xi_{T} \longmapsto \mathbf {Ad}(T) \boldsymbol \xi_{T} = (T \boldsymbol \xi_{T}^\wedge T^{-1} )^\vee$$

    此时，已经比较接近其本质了。

    这个伴随变换的含义就是: 一个不是李群幺元处的正切空间里的向量 $\boldsymbol \xi_{T}$, 被映射到了李群幺元处的正切空间里的向量 $(T \boldsymbol \xi^\wedge_{T} T^{-1} )^\vee$。

    这个对向量进行变换作用的矩阵 $\mathbf {Ad}(T)$ ，则被称为 $T$ 的伴随变换矩阵。

  • 等价

    上面的式子和视觉SLAM十四讲中的式子是等价的, 如下：对上面的式子两边先取 $^\wedge$ 操作， 再取 $\exp()$, 即可变为形式一样。：

    $$\begin{aligned} \mathbf {Ad}(T) \boldsymbol \xi_{T} &= (T \boldsymbol \xi_{T}^\wedge T^{-1} )^\vee \\ \ \\ \exp ( \mathbf {Ad}(T) \boldsymbol \xi^\wedge_{T} ) &= \exp( T \boldsymbol \xi_{T}^\wedge T^{-1} ) = T \exp( \boldsymbol \xi^\wedge_T) T^{-1} \end{aligned}$$

• 伴随的性质

  $\mathbf {Ad}(T^{-1}) = \mathbf {Ad}^{-1}(T)$

  $\mathbf {Ad}(T_1 T_2) = \mathbf {Ad}(T_1) \mathbf {Ad}(T_2)$

  • 伴随的逆变换，根据其其定义就是把李群幺元处正切空间里的向量，映射到李群其他元素处的正切空间里的向量
  • 上面的式子，等号左边往往比右边计算起来更快。
  • 对李代数做伴随变换和对切向量做伴随变换是等价的，但往往后者更容易。

• 结语

  在李群中有很多元素，每个元素都对应有正切空间，这些正切空间中的向量，可以通过伴随这种线性变换，被映射到李群幺元处的正切空间中。

• 参考阅读

  这里的伴随是指李群, 并不是Eigen库中的伴随adjoint()的含义, 那个伴随是共轭+转置的联合作用的意思: 实数域中, 实数的共轭是自己, 所以只有转置在起作用.

  对李群de 其他更详细的信息可阅读帖子 Lie Group Foundations 和文献： A micro Lie theory for state estimation in robotics

• adj() 源码实现

  下面通过源码方式来跟踪 adj()函数内部实现。

  • adj() 内部调用了 SO3Base::matrix() 函数

    SOPHUS_FUNC Transformation matrix() const {
        return unit_quaternion().toRotationMatrix();
    }
    

    通过代码, 可以看到, 这里是一个单位四元数到一个旋转矩阵的变换, 所以返回的单位四元数转换而来的旋转矩阵.

  • matrix() 内部调用了 SO3Base::unit_quaternion() 函数

    SOPHUS_FUNC QuaternionType const& unit_quaternion() const {
       return static_cast<Derived const*>(this)->unit_quaternion();
    }
    

    其中unit_quaternion()函数实现比较简单, 就是调用派生类的的同名函数完成数据转换, 返回一个四元数:

  • matrix() 内部调用了Eigen库的 QuaternionBase::toRotationMatrix() 函数

    四元数->旋转矩阵的函数toRotationMatrix()是调用Eigen库的函数完成的, 可以稍微看一下Eigen库中 Geometry 中的实现片段：

      template<class Derived>
      EIGEN_DEVICE_FUNC inline 
      typename QuaternionBase<Derived>::Matrix3
      QuaternionBase<Derived>::toRotationMatrix(void) const
      {        
        Matrix3 res;
    
        const Scalar tx  = Scalar(2)*this->x();
        const Scalar ty  = Scalar(2)*this->y();
        const Scalar tz  = Scalar(2)*this->z();
        const Scalar twx = tx*this->w();
        const Scalar twy = ty*this->w();
        const Scalar twz = tz*this->w();
        const Scalar txx = tx*this->x();
        const Scalar txy = ty*this->x();
        const Scalar txz = tz*this->x();
        const Scalar tyy = ty*this->y();
        const Scalar tyz = tz*this->y();
        const Scalar tzz = tz*this->z();
    
        res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
        res.coeffRef(0,1) = txy-twz;
        res.coeffRef(0,2) = txz+twy;
        res.coeffRef(1,0) = txy+twz;
        res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
        res.coeffRef(1,2) = tyz-twx;
        res.coeffRef(2,0) = txz-twy;
        res.coeffRef(2,1) = tyz+twx;
        res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
    
        return res;
      }
    

    整个这个实现, 其实就是 视觉SLAM十四讲 第3讲中的公式 (3.35 )的代码实现, 十四讲书中的上下文如下:

    设四元数 $\mathbf q=q_0 + q_1 \mathbf i + q_2 \mathbf j + q_3 \mathbf k$, 对应的旋转矩阵 $R$ 为:

    $$\mathbf R = \left [ \begin{matrix} 1-2q_2^2 -2q_3^2 & 2q_1q_2 + 2q_0q_3 & 2q_1q_3 - 2q_0q_2 \\ 2q_1q_2 - 2q_0q_3 & 1-2q_1^2 -2q_3^2 & 2q_2q_3 + 2q_0q_1 \\ 2q_1q_3 + 2q_0q_2 & 2q_2q_3 - 2q_0q_1 & 1-2q_1^2 -2q_2^2 \end{matrix} \right ] \tag{3.35}$$

• 结语

  至此, adj()函数分析完毕. 随SO3 而言, adj()函数返回的就是内部单位四元数所对应的旋转矩阵. 但对SE3, adj()函数返回又的是什么呢?

4. SE3Base::adj() 类比

作为和 SO(3) 的对比, 下面也看看Sophus库中的SE3Base::adj() 函数的实现。

sophus/se3.hpp:

• SE3Base::adj() 函数

  SOPHUS_FUNC Adjoint Adj() const {
    Sophus::Matrix3<Scalar> const R = so3().matrix();
    Adjoint res;
    res.block(0, 0, 3, 3) = R;
    res.block(3, 3, 3, 3) = R;
    res.block(0, 3, 3, 3) = SO3<Scalar>::hat(translation()) * R;
    res.block(3, 0, 3, 3) = Matrix3<Scalar>::Zero(3, 3);
    return res;
  }

• 自由度与Adjoint

  • 自由度 在SO3Base中， DoF常数定义为3； 在SE3Base 中, DoF常数定义为6: 旋转3个自由度 + 平移3个自由度;
  • Adjoint 在SE3Base 中, Adjoint 数据类型被定义为6x6 的矩阵;
    在Sim3Base中, Adjoint 数据类型被定义为7x7 的矩阵;

  矩阵和其伴随的自由度，可结合 视觉SLAM十四讲 中第3讲的表3-1进行理解:

  $$\begin{array}{|c|c|c|c|} \hline {变换名称} & {矩阵形式} & {自由度} & {不变性质} \\ \hline \\ {欧式变换} & \left [\begin{matrix} \mathbf R & \mathbf t \\ \mathbf 0^\top & 1 \end{matrix} \right ] & 6 & 长度、夹角、体积 \\ \\ \hline \\ {相似变换} & \left [\begin{matrix} s \mathbf R & \mathbf t \\ \mathbf 0^\top & 1 \end{matrix} \right ] & 7 & 体积比 \\ \\ \hline \\ {放射变换} & \left [\begin{matrix} \mathbf A & \mathbf t \\ \mathbf 0^\top & 1 \end{matrix} \right ] & 12 & 平行性、体积比 \\ \\ \hline \\ {射影变换} & \left [\begin{matrix} \mathbf A & \mathbf t \\ \mathbf 0^\top & \mathbf v \end{matrix} \right ] & 15 & 接触平面的相交和相切 \\ \\ \hline \end{array}$$

• SE(3) Adj定义
  回顾一下 视觉SLAM十四讲 中的第4讲的公式(4.48)和(4.49), 书中内容如下:

  在$SE(3)$ 上亦有伴随性质:

  $$\mathbf T \exp(\mathbf \xi^\wedge) \mathbf T^{-1} = exp((\mathrm Ad(\mathbf T) \mathbf \xi)^\wedge ) \tag{4.48}$$

  其中:

  $$\mathrm {Ad}(\mathbf T) = \left [ \begin{matrix} \mathbf R & \mathbf t^\wedge \mathbf R \\ \mathbf 0 & \mathbf R \end{matrix} \right ] \tag{4.49}$$

  Adjoint res;
  res.block(0, 0, 3, 3) = R;
  res.block(3, 3, 3, 3) = R;
  res.block(0, 3, 3, 3) = SO3<Scalar>::hat(translation()) * R;
  res.block(3, 0, 3, 3) = Matrix3<Scalar>::Zero(3, 3);
  

  通过对比代码和公式, 可以发现: SE3Base::adj() 代码实现完全遵循上述公式 (4.49)： 左上角是 $R$, 右下角为 $R$, 右上角是 $t^\wedge R$, 左下角为 $\mathbf c 0$。

5. SO3Base::angleX()、angleY()、angleZ()

这几个函数返回的是旋转矩阵分别绕 x,y,z 轴的角度(这里是假设另外两个轴不旋转)的角度.

  template <class S = Scalar>
  SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleX() const {
    Sophus::Matrix3<Scalar> R = matrix();
    Sophus::Matrix2<Scalar> Rx = R.template block<2, 2>(1, 1);
    return SO2<Scalar>(makeRotationMatrix(Rx)).log();
  }
  
  template <class S = Scalar>
  SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleY() const {
    Sophus::Matrix3<Scalar> R = matrix();
    Sophus::Matrix2<Scalar> Ry;
    // clang-format off
    Ry <<
      R(0, 0), R(2, 0),
      R(0, 2), R(2, 2);
    // clang-format on
    return SO2<Scalar>(makeRotationMatrix(Ry)).log();
  }
  
  template <class S = Scalar>
  SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleZ() const {
    Sophus::Matrix3<Scalar> R = matrix();
    Sophus::Matrix2<Scalar> Rz = R.template block<2, 2>(0, 0);
    return SO2<Scalar>(makeRotationMatrix(Rz)).log();
  }

代码返回的是绕某个轴的旋转角度, 返回类型和SO3构造时传入的数据类型一样, 比如浮点或双精度；

• 绕坐标轴的旋转矩阵

  例如，绕z轴旋转 $\theta$ 角的旋转矩阵如下:

  $$R_z(\theta) = \left [ \begin{matrix} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{matrix} \right ] \\ \ \\ R_x(\theta) = \left [ \begin{matrix} 1 & 0 & 0 \\ 0 & \cos \theta & 0 \\ 0 & \sin \theta & \cos \theta \end{matrix} \right ] \\ \ \\ R_y(\theta) = \left [ \begin{matrix} \cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta \end{matrix} \right ]$$

• 代码逻辑

  以代码中计算绕 x 轴旋转的代码 angleX() 为例：

     template <class S = Scalar>
     SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S>
     angleX() const {
       Sophus::Matrix3<Scalar> R = matrix();
       Sophus::Matrix2<Scalar> Rx = R.template block<2, 2>(1, 1);
       return SO2<Scalar>(makeRotationMatrix(Rx)).log();
     }
  

  它就是按照上述公式中的 $R_x(\theta)$ 的定义方式, 把3x3 矩阵R的右下角部分的2x2的矩阵块(block)取出来, 然后转为2x2旋转矩阵, 转为SO2后, 使用log()函数得到

  在$SO3$ 中, 李群$SO3$通过对数映射log()得到的李代数$so3$是一个3x1的向量. 在SO(2)中, 对数映射log() 得到的是旋转角度.

• 补充说明

  需注意的是：代码中调用的函数makeRotationMatrix() 是把一个任意的2x2或更大的的方针变为一个最接近的正交矩阵且保证其行列式为正值。这件事情并不是显而易见的, 其实现用到了SVD分解和一些技巧, 有兴趣的可查看具体源码实现。

6. SO3Base::cast()

cast()函数就是把字面的意思, 把本类中各种数据类型中的模板参数的Scalar的数据类型转成一个新的数据类型, 比如整型转为浮点型.

sophus/so3.hpp:

  /// Returns copy of instance casted to NewScalarType.
  template <class NewScalarType>
  SOPHUS_FUNC SO3<NewScalarType> cast() const {
    return SO3<NewScalarType>(unit_quaternion().template cast<NewScalarType>());
  }

通过 unit_quaternion() 函数获取到内部的单位四元数, 然后通过 template cast转换为新类型

• C++ template cast 是做什么的？

  可参见帖子：C++中 template cast ( 以Sophus库为例 )：juejin.cn/post/707563…

7. SO3Base::data()

函数作用主要是返回内部的单位四元数.

so3.hpp:

 SOPHUS_FUNC Scalar* data() {
    return unit_quaternion_nonconst().coeffs().data();
  }

  /// Const version of data() above.
  ///
  SOPHUS_FUNC Scalar const* data() const {
    return unit_quaternion().coeffs().data();
  }

这部分就是返回内部数据, 没有特别的地方。 需要提醒的是：返回的四元数3个虚部在前，第4个是实部.

8. SO3Base::Dx_this_mul_exp_x_at_0()

sophus/so3.hpp:

/// Returns derivative of  this * SO3::exp(x)  wrt. x at x=0.
SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
      const {
    Matrix<Scalar, num_parameters, DoF> J;
    Eigen::Quaternion<Scalar> const q = unit_quaternion();
    Scalar const c0 = Scalar(0.5) * q.w();
    Scalar const c1 = Scalar(0.5) * q.z();
    Scalar const c2 = -c1;
    Scalar const c3 = Scalar(0.5) * q.y();
    Scalar const c4 = Scalar(0.5) * q.x();
    Scalar const c5 = -c4;
    Scalar const c6 = -c3;
    J(0, 0) = c0;
    J(0, 1) = c2;
    J(0, 2) = c3;
    J(1, 0) = c1;
    J(1, 1) = c0;
    J(1, 2) = c5;
    J(2, 0) = c6;
    J(2, 1) = c4;
    J(2, 2) = c0;
    J(3, 0) = c5;
    J(3, 1) = c6;
    J(3, 2) = c2;

    return J;
  }

此处的雅可比，返回的是一个4x3的矩阵, 即 num_parameters 为4, DoF 为3. 这里求的是是 SO3对象(即四元数)相对于(wrt)so3向量（即注释中的x）的雅可比。

没有搞清楚这里的计算原则。个人理解: 首先，其功能应该主要是为了配合 ceres 库的求导；其次，由于这里仅仅Base类中的定义，实际上调用的应该都是继承类的对应函数。

9. SO3Base::params()

so3.hpp

  SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
    return unit_quaternion().coeffs();
  }

没有特别的，返回类内部单位四元数的参数值： q.imag[0],q.imag[1],q.imag[2],q.real

10. SO3Base::inverse()

返回类内部单位四元数的逆。 so3.hpp

  SOPHUS_FUNC SO3<Scalar> inverse() const {
    return SO3<Scalar>(unit_quaternion().conjugate());
  }

• 四元数的逆

  可联系视觉SLAM十四讲中的第3讲对四元数的性质的相关讲解, 书中公式(3.29)附近， 四元数的共轭、模长、逆的定义如下：

  $$共轭 ：\boldsymbol q^{\star} = s_a - x_a \boldsymbol i - y_a \boldsymbol j - z_a \boldsymbol k = \left [ s_a, - \boldsymbol v_a \right ] \tag{3.25}$$
  $$模长：\Vert \boldsymbol q \Vert = \sqrt{\displaystyle s^2 + x^2 + y^2 + z^2} \tag{3.27}$$
  $$逆 ：\boldsymbol q^{-1} = \dfrac{\boldsymbol q^{\star}} { \Vert \boldsymbol q \Vert } \tag{3.29}$$

• 代码实现

  由定义可知， 如果q是单位四元数，则模为1，则其逆和共轭相同。这也是代码实现中使用共轭函数conjugate()求逆的原因。

11. SE3Base::inverse() 类比

在上面看完SO3的逆之后， 也来看看SE3这块的代码时怎样的（在se3.hpp中）。

sophue/se3.hpp

  SOPHUS_FUNC SE3<Scalar> inverse() const {
    SO3<Scalar> invR = so3().inverse();
    return SE3<Scalar>(invR, invR * (translation() * Scalar(-1)));
  }

返回的是SE3的逆。

• 李群SE(3)定义

  先回顾一下"视觉SLAM十四讲"中第四讲的对李群SE(3)的定义，见书中公式（4.2）前后，如下：

  $$SO(3) = \{ \boldsymbol R \in \mathbb R^{3 \times 3} \ \vert \ \boldsymbol {RR}^\top = \boldsymbol I, \det(\boldsymbol R) = 1 \} \tag{4.1}$$
  $$SE(3) = \left \{ \boldsymbol T = \left [ \begin{matrix} \boldsymbol R & \boldsymbol t \\ \boldsymbol 0^\top & \boldsymbol 1 \end{matrix} \right ] \in \mathbb R^{4 \times 4} \ \vert \ \boldsymbol R \in SO(3), \boldsymbol t \in \mathbb R^3 \right \} \tag{4.2}$$

• SE(3)的逆

  于是，我们根据SE(3)定义来求它的逆：

  $$\begin{aligned} \left [ \begin{matrix} \boldsymbol R & \boldsymbol t \\ 0^\top & \boldsymbol 1 \end{matrix} \right ] \left [ \begin{matrix} \boldsymbol a & \boldsymbol b \\ \boldsymbol c & \boldsymbol d \end{matrix} \right ] &= \boldsymbol I \\ \left [ \begin{matrix} \boldsymbol {Ra+tc} & \boldsymbol {Rb+td} \\ \boldsymbol c & \boldsymbol d \end{matrix} \right ] &= \left [ \begin{matrix} \boldsymbol 1 & \boldsymbol 0 \\ \boldsymbol 0 & \boldsymbol 1 \end{matrix} \right ] \\ \therefore \boldsymbol { c =0, \quad d=0 } \\ \left [ \begin{matrix} \boldsymbol {Ra} & \boldsymbol {Rb+t} \\ \boldsymbol 0 & \boldsymbol 1 \end{matrix} \right ] &= \left [ \begin{matrix} \boldsymbol 1 & \boldsymbol 0 \\ \boldsymbol 0 & \boldsymbol 1 \end{matrix} \right ] \\ \therefore \begin{cases} \boldsymbol {Ra} = \boldsymbol 1 \\ \boldsymbol {Rb+t=0} \end{cases} \\ \therefore \begin{cases} \boldsymbol a = \boldsymbol R^{-1} \\ \boldsymbol b = \boldsymbol R^{-1} \boldsymbol {(-t)} \end{cases} \end{aligned}$$

  所以SE3逆矩阵如下， 这其实是一个新的SE3：

  $$\left [ \begin{matrix} \boldsymbol R^{-1} & \boldsymbol R^{-1} (- \boldsymbol t) \\ \boldsymbol 0^\top & \boldsymbol 1 \end{matrix} \right ]$$

• 代码分析

  然后再来看代码实现，发现代码遵循的也是上述定义对逆矩阵的定义, 重新贴一下代码:

      SO3<Scalar> invR = so3().inverse();
      return SE3<Scalar>(invR, invR * (translation() * Scalar(-1)));
  

  代码中，通过SE3的构造函数， 传入其所需的 $R^{-1}$ 和 $R^{-1}t$ 两个参数构造新的 SE(3).

12. SO3Base::log() & SO3Base::logAndTheta()

计算的是计算李群SO3所对应的李代数向量so3。 其中 log() 调用了 logAndTheta()，返回代表李代数的向量 $J$。

sophus/so3.hpp:

SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; }

SOPHUS_FUNC TangentAndTheta logAndTheta() const {
    TangentAndTheta J;
    using std::abs;
    using std::atan2;
    using std::sqrt;
    Scalar squared_n = unit_quaternion().vec().squaredNorm();
    Scalar w = unit_quaternion().w();

    Scalar two_atan_nbyw_by_n;

    if (squared_n <
        Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {
      // If quaternion is normalized and n=0, then w should be 1;
      // w=0 should never happen here!
      SOPHUS_ENSURE(abs(w) >= Constants<Scalar>::epsilon(),
                    "Quaternion ({}) should be normalized!",
                    unit_quaternion().coeffs().transpose());
      Scalar squared_w = w * w;
      two_atan_nbyw_by_n =
          Scalar(2) / w - Scalar(2.0 / 3.0) * (squared_n) / (w * squared_w);
      J.theta = Scalar(2) * squared_n / w;
    } else {
      Scalar n = sqrt(squared_n);

      Scalar atan_nbyw = (w < Scalar(0)) ? atan2(-n, -w) : atan2(n, w);
      two_atan_nbyw_by_n = Scalar(2) * atan_nbyw / n;
      J.theta = two_atan_nbyw_by_n * n;
    }

    J.tangent = two_atan_nbyw_by_n * unit_quaternion().vec();
    return J;
  }

• 李群、李代数映射关系

  根据视觉SLAM十四讲中第四讲的介绍，我们知道两者是对数/指数映射关系，见书中公式(4.13):

  $$\boldsymbol {so(3)} = \{ \boldsymbol \phi \in \mathbb R^3, \boldsymbol \Phi = \boldsymbol \phi^\wedge \in \mathbb R^{3 \times 3} \} \tag{4.13}$$

  在Sophus库中， 李群SO3是用一个类成员变量四元数来表示，而李代数 $so(3)$ 是三维向量，每个向量都可对应到一个反对称矩阵， 它和$SO(3)$的关系指数映射：$\boldsymbol R = \exp (\boldsymbol \phi ^\wedge )$

• TangentAndTheta 数据类型

  在理解代码的计算逻辑前，先了解涉及的重要数据结构类型 TangentAndTheta， 它定义的变量为 J，代表李代数向量，是函数的返回值， 见代码：

      TangentAndTheta J;
  

  进一步查看 TangentAndTheta 类型的定义，如下：

    struct TangentAndTheta {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  
        Tangent tangent;
        Scalar theta;
    };
  

  可以看出这个结构体有两个成员变量tangent和theta， 它们分别代表的是什么含义呢？

• tangent变量

  tangent 的含义就是李群SO3通过log()映射得到的李代数so3。如本文前面数据类型那块展示所示，z这个变量的类型是 Tangent, 在SO3Base类定义为3x1的向量，在SE3Base类中定义为6x1的向量。

  其次，这个tangent 变量对应的其实是旋转向量 $\theta \boldsymbol n$中的旋转轴 $\boldsymbol n$ (单位向量)，$\theta$ 代表的是旋转角大小。这一结论在下面会详细讲解由来。

  类型Tangent 是3x1的变量，见如下代码：

  static int constexpr DoF = 3;
  ...
  using Tangent = Vector<Scalar, DoF>;  
  

  我们把Vector的类型代码也贴出来：

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

• theta变量

  theta 的含义就是上面讲到的旋转向量$\theta \boldsymbol n$中的 $\theta$， 代表旋转角度大小。

• 旋转向量与李代数

  上面提到李代数 J 变量对应的是旋转向量的旋转轴。为什么这么说？

  此结论在视觉SLAM十四讲第3讲中的旋转角和欧拉角章节的相关描述中给了以上，大概在公式(3.14)附近。

  一起回顾一下 视觉SLAM十四讲 是如何来安排章节论证旋转向量就是李代数的：

  • Step1: 第3.3章节建立旋转向量与旋转矩阵关系

    在书中第三讲的3.3章节首次出现了旋转向量概念，定义如上就是 $\theta \boldsymbol n$, 然后又说了一下旋转向量 $\theta \boldsymbol n$ 和旋转矩阵 $\boldsymbol R$ 的转换关系符合罗德里格斯公式：

    $$\boldsymbol R=\cos \theta \boldsymbol I + (1-\cos \theta) \boldsymbol{nn^\top} + \sin \theta \boldsymbol {n^\wedge}$$

    这让我们初步建立了旋转向量和旋转矩阵的关系, 就是罗德里格斯公式。

  • Step2: 4.1 章节说明旋转矩阵与李代数 $\boldsymbol \phi$ 的指数关系

    在书中4.1.1小节，通过解一个微分方程

    $$\boldsymbol R(t) \boldsymbol R(t)^\top = \boldsymbol I \tag{4.5}$$

    最终得到了

    $$\boldsymbol R(t) = \exp (\boldsymbol \phi^\wedge t) \tag{4.10}$$

    这里可以知道，旋转矩阵是可以近似的用一个称为 $\boldsymbol \phi$ 的变量的反对称矩阵的指数形式表示的，而且书中把这个变量 $\boldsymbol \phi$ 定义为切空间上的李代数向量；

  • Step3： 4.2章节说明李代数和旋转矩阵的关系也是罗德里格斯公式

    在4.2章节，则是从 $\exp (\phi^\wedge)$ 如何计算入手， 讨论它的泰勒展开表达，最后得到了它的表达式也是罗德里格斯公式：

    $$\begin{aligned} \boldsymbol R &= \exp(\boldsymbol \phi^\wedge) = \exp(\theta \boldsymbol n^\wedge) \\ &= \cos \theta \boldsymbol I + (1-\cos \theta) \boldsymbol{nn^\top} + \sin \theta \boldsymbol {n^\wedge} \tag{4.22} \end{aligned}$$

  • 结论

    由于旋转矩阵 $\boldsymbol R$ 和李代数 $\boldsymbol \phi$ 以及旋转向量 $\theta \boldsymbol n$ 的关系都是罗德里格斯公式， 这就说明了, 第三章提到的旋转向量 $\theta \boldsymbol n$ 第四章提到的李代数向量 $\boldsymbol \phi = \theta \boldsymbol a$ 是相同的概念。

• 变量含义

  Scalar squared_n = unit_quaternion().vec().squaredNorm();
  
  Scalar w = unit_quaternion().w();
  
  Scalar two_atan_nbyw_by_n;
  

  下面为了便于讨论和对照公式，我们把SO3中的类成员四元数记为 $[w, \boldsymbol v ]$, 其中 $w$ 代表实部， $\boldsymbol v$ 代表虚部。

  • squared_n变量 : 代表四元数的向量部分（虚部）的范数, 它是个标量，即上述四元数记法$[w, \boldsymbol v ]$中$\boldsymbol v$的范数 $\Vert \boldsymbol v\Vert$.

  • w 变量: 代表四元数的实部部分，即上述四元数记法$[w, \boldsymbol v ]$中的的 $w$.

  • two_atan_nbyw_by_n 变量:
    变量名中的 n 字母的含义就是指squared_n变量，变量名中的w 字母代表是$w$变量。 变量名中 nbyw, 代表含义就是虚部范数除以实部， 即 $\Vert \boldsymbol v \Vert / w$ 的意思；

    变量名在前面又加了 atan_ ，表示对计算结果求 反正切 atan()的意思；

    变量名在后面又加了 _byn,同理，就是结果再除以 $\Vert \boldsymbol v \Vert$ ;

    变量名在前面又加了two_表示2倍的意思。

    综上所述，整个变量two_atan_nbyw_by_n 从命名的字面含义解释, 代表的是如下这个公式：

    $$2 \ \dfrac{ atan( \Vert \boldsymbol v \Vert / w ) }{\Vert \boldsymbol v \Vert} \tag{*}$$

    稍后我们后面会解释这个式子的来源。

• 代码逻辑: if分支部分
  在以上变量含义清楚后，再来看代码中 if 分支的逻辑：

  if (squared_n <
      Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {
      SOPHUS_ENSURE(abs(w) >= Constants<Scalar>::epsilon(),
                  "Quaternion ({}) should be normalized!",
                  unit_quaternion().coeffs().transpose());
      Scalar squared_w = w * w;
      two_atan_nbyw_by_n =
        Scalar(2) / w - Scalar(2.0 / 3.0) * (squared_n) / (w * squared_w);
      J.theta = Scalar(2) * squared_n / w;    
  }else{
      ...
  }    
  

  以上代码计算变量 two_atan_nbyw_by_n 时的逻辑如下所示：

  $$2.0/w - 2.0/3.0 * \Vert \boldsymbol v \Vert / (w*w*w)$$

  代码中出现了一些系数，比如2.0， 3.0等，应该是 atan的泰勒展开得到：

  $$2 \ atan(\Vert \boldsymbol v \Vert / w) \approx 2 \ (\Vert \boldsymbol v \Vert / w) - 2.0/3.0 \ ( \Vert \boldsymbol v \Vert / w)^3$$

  此处代码逻辑引用到了一篇文献： Integrating Generic Sensor Fusion Algorithms with Sound State Representation through Encapsulation of Manifolds, 作者是 C. Hertzberg 等。

  文献中的公式(31)如下：

  $$\overline{log} \left [ \begin{matrix} w \\ \boldsymbol v \end{matrix} \right ] = \begin{cases} 0 & \boldsymbol v = 0 \\ \\ \dfrac{atan(\Vert \boldsymbol v \Vert / w) }{ \Vert \boldsymbol v \Vert } \boldsymbol v & \boldsymbol v \ne 0, w \ne 0 \\ \\ \pm \dfrac{\pi/2}{ \Vert \boldsymbol v \Vert } & w \ne 0 \tag{31} \end{cases}$$

  有关公式详细推导信息可参见文献描述。此处可以对公式做一个简单的理解：

  分子上的 atan(...) 通过反正切求出的就是旋转角度 $\theta$， 而 $\dfrac{\boldsymbol v}{\Vert \boldsymbol v \Vert}$ 得到的是一个单位向量，就是我们描述旋转向量 $\theta \boldsymbol n$ 中的$\boldsymbol n$。

  代码中最终计算的角度乘以2，是因为使用四元数反正切计算得到的角度是真正旋转角的一半，这一点在视觉SLAM十四讲 中也有提及，即转了一半的感觉。

  备注：

  这里能省略泰勒展开式中 $\Vert \boldsymbol v \Vert$ 高阶项的前提是它的值很小，多项式运算来近似了反三角函数运算，以提升代码执行效率。

• 代码逻辑: else分支部分

      ...
    } else {
        Scalar n = sqrt(squared_n);
  
        // w < 0 ==> cos(theta/2) < 0 ==> theta > pi
        //
        // By convention, the condition |theta| < pi is imposed by wrapping theta
        // to pi; The wrap operation can be folded inside evaluation of atan2
        //
        // theta - pi = atan(sin(theta - pi), cos(theta - pi))
        //            = atan(-sin(theta), -cos(theta))
        //
        Scalar atan_nbyw = (w < Scalar(0)) ? atan2(-n, -w) : atan2(n, w);
        two_atan_nbyw_by_n = Scalar(2) * atan_nbyw / n;
        J.theta = two_atan_nbyw_by_n * n;
    } 
  

  这里的计算就直接使用了反正切计算库函数atan2。其他部分依然是套用上面的文献中所给出的公式。只是为了让 $\theta$ 落在 $(- \pi, \pi)$ 区间做了一些处理。

13. SE3Base::log()类比

同样的，也同时来看看 SE3 中对应的log() 函数 ，它定义在 se3.hpp 中，看它和SO3的log函数有什么不同。

它的含义是，已知 SE3李群， 求对应的李代数 se3 这个 6x1向量.

sophus/se3.hpp:

    SOPHUS_FUNC Tangent log() const {
        // For the derivation of the logarithm of SE(3), see
        // J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices
        // and logarithms of orthogonal matrices", IJRA 2002.
        // https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf
        // (Sec. 6., pp. 8)
        using std::abs;
        using std::cos;
        using std::sin;
        Tangent upsilon_omega;
        auto omega_and_theta = so3().logAndTheta();
        Scalar theta = omega_and_theta.theta;
        Vector3<Scalar> const& omega = omega_and_theta.tangent;
        upsilon_omega.template tail<3>() = omega;
        Matrix3<Scalar> V_inv = SO3<Scalar>::leftJacobianInverse(omega, theta);
        upsilon_omega.template head<3>() = V_inv * translation();
        return upsilon_omega;
    }

• 李代数se(3)定义

  先结合视觉SLAM十四讲中的第四讲的李代数 se(3)的定义来理解什么是se3, 在公式(4.15)左右：

  $$\begin{aligned} \mathcal {se}(3) &= & \left \{ \boldsymbol \xi= \left [ \begin{matrix} \boldsymbol \rho \\ \boldsymbol \phi \end{matrix} \right ] \in \mathbb R^6, \ \boldsymbol \rho \in \mathbb R^3, \ \boldsymbol \phi \in \mathcal {so}(3) , \ \xi^\wedge = \left [ \begin{matrix} \boldsymbol \phi^\wedge & \boldsymbol \rho \\ \boldsymbol 0^\top & 1 \end{matrix} \right ] \in \mathbb R^{4 \times 4 } \right \} \end{aligned}$$

  由定义可知， 一个 se3 李代数是一个代表平移的 $\boldsymbol \rho$ 和一个代表旋转的 so3 李代数 $\boldsymbol \phi$ 组成的。上述中的 $\xi^\wedge$ 中的符号 $^\wedge$ 表示把一个 6 x 1的向量映射为一个 4 x 4 的矩阵的变换。

• se(3)指数映射定义

  再来看视觉SLAM十四讲中对 se(3)李代数的指数映射的定义， 在书中公式(4.25) 左右， se(3)做指数映射后即为SE3, 以下面就是SE(3)的定义：

  $$\exp(\boldsymbol \xi^\wedge) \triangleq \left [ \begin{matrix} \boldsymbol R & \boldsymbol{J \rho} \\ \boldsymbol 0^\top & \boldsymbol 1 \end{matrix} \right ] = \boldsymbol T \tag{4.25}$$

  矩阵左上角部分 $\boldsymbol R$ 就是SO(3)李群中的元素， 它的值为 $\displaystyle \sum_{n=0}^{\infin} \dfrac{1}{n!} (\boldsymbol \phi^\wedge)^n$, 由se(3)中的表示旋转的$\boldsymbol \phi$计算可得。

  矩阵右上角的 $\boldsymbol J$ 的定义在书中如下(设旋转向量 $\boldsymbol \phi=\theta \boldsymbol a$)，$\boldsymbol J$ 可以由 $\boldsymbol \phi = \theta \boldsymbol a$ 通过下式计算得到，这部分也被称为SE(3) 的平移部分（注意和真实的平移 $\boldsymbol \rho$ 相差了一个 $\boldsymbol J$）：

  $$\boldsymbol J = \dfrac {\sin \theta}{\theta} \boldsymbol I + (1 - \dfrac{\sin \theta}{\theta}) \boldsymbol {aa}^\top + \dfrac{1 - \cos \theta}{\theta} \boldsymbol a^\wedge \tag{4.26}$$

• SE3数据结构

  在 Sophus 库中，SE3类的代码如下， 可知其也是遵循上述定义的。SE3类有两个成员变量: 表示旋转的 $so3\_ = \boldsymbol R$ 和表示平移的 $translation\_ = \boldsymbol {J \rho}$。

   protected:
      SO3Member  so3_;
      TranslationMember  translation_;
  

  为便于描述，下面简记translation_ 为 $\boldsymbol t = \boldsymbol {J \rho}$, 简记so_为 $\boldsymbol R$。

• 问题本质

  了解了基本的类的数据结构中成员变量定义和其真实含义之后，再来看SE3的log()实现就比较清楚了。问题本质就是 : 已知SE3类的的 $\boldsymbol R$ 和 $\boldsymbol t = \boldsymbol {J \rho}$， 根据上述公式(4.15)、(4.25)、(4.26)求 $\boldsymbol \phi$ 和 $\boldsymbol \rho$。

• 如何计算 $\boldsymbol \phi$
  通过上面的文字分析可知， $\boldsymbol R$ 只和 $\boldsymbol \phi$ 有关，而且SO3的对数映射函数log()上面刚刚分析过， 因此可以通过对 $\boldsymbol R$ 调用SO3的log()来求得其所对应的李代数向量 $\boldsymbol \phi$. 对应的代码就是：

  auto omega_and_theta = so3().logAndTheta();
  Scalar theta = omega_and_theta.theta;
  

• 如何计算 $\boldsymbol \rho$

  通过上面的文字分析可知，通过 $\boldsymbol \phi = \theta \boldsymbol a$ 可以得到 $\boldsymbol J$ , 而 $\boldsymbol t$ 已知， 通过解方程组 $\boldsymbol t = \boldsymbol {J \rho} \longrightarrow \boldsymbol \rho = \boldsymbol J^{-1} \boldsymbol t$ 可得到 $\boldsymbol \rho$. 对应的代码就是：

  Matrix3<Scalar> V_inv = SO3<Scalar>::leftJacobianInverse(omega, theta);
  upsilon_omega.template head<3>() = V_inv * translation();
  

• 总结

  一起来再看一下Sophus库中对SE3求log()的代码逻辑：

    Tangent upsilon_omega;
    auto omega_and_theta = so3().logAndTheta();
    Scalar theta = omega_and_theta.theta;
    Vector3<Scalar> const& omega = omega_and_theta.tangent;
    upsilon_omega.template tail<3>() = omega;
    Matrix3<Scalar> V_inv = SO3<Scalar>::leftJacobianInverse(omega, theta);
    upsilon_omega.template head<3>() = V_inv * translation();
  

  代码解释：

  • so3().logAndTheta() 这句就是 SO3 的 log() 对数映射函数，前一小节刚刚介绍过；
  • omega_and_theta.tangent 变量即为上述公式中的 $\boldsymbol \phi$；
  • Matrix3<Scalar> V_inv = SO3<Scalar>::leftJacobianInverse(omega, theta) 这句求的就是 $\boldsymbol J^{-1}$
  • upsilon_omega.template head<3>() = V_inv * translation(); 等号右边就是上述公式上的 $\boldsymbol {J^{-1} t}$
  • 其中的代码 SO3<Scalar>::leftJacobianInverse() 此处暂不展开，在本文下面章节讲 雅可比和逆雅可比时还会专门讲解。

  至此，Sophus 库计算SE3对应的se3 李代数的函数log() 代码逻辑和原理就分析清楚了。

  备注:

  这里的计算引用到了一篇文献， Computing exponentials of skew symmetric matrices and logarithms of orthogonal matrices，里面详细推导了雅可比计算过程， 通过 $\boldsymbol \phi = \theta \boldsymbol a$ 计算 $\boldsymbol J$ 的公式(4.26) 的由来，有兴趣可以深入了解。

14. SO3Base::normalize()

四元数的归一化函数。

sophus/so3.hpp：

  SOPHUS_FUNC void normalize() {
    Scalar length = unit_quaternion_nonconst().norm();
    SOPHUS_ENSURE(length >= Constants<Scalar>::epsilon(),
                  "Quaternion ({}) should not be close to zero!",
                  unit_quaternion_nonconst().coeffs().transpose());
    unit_quaternion_nonconst().coeffs() /= length;
  }

这个是SO3归一化函数，即四元数的实部、虚部都除以整个四元数的范数进行归一化。

此外，在SE3中的归一化其实和SO3是一样的，即SE它只对旋转部分做了归一化。所以下面就不类比 SE3 的 normalize() 函数了.

15. SO3Base::matrix()

返回四元数对应的旋转矩阵。

sophus/so3.hpp:

  /// Returns 3x3 matrix representation of the instance.
  ///
  /// For SO(3), the matrix representation is an orthogonal matrix ``R`` with
  /// ``det(R)=1``, thus the so-called "rotation matrix".
  ///
  SOPHUS_FUNC Transformation matrix() const {
    return unit_quaternion().toRotationMatrix();
  }

返回的就是四元数所对应的3x3的旋转矩阵。

16. SE3Base::matrix()类比

看完了SO3， 同样的再来看SE3的matrix()函数是怎么实现的。

sophus/se3.hpp:

  SOPHUS_FUNC Transformation matrix() const {
    Transformation homogenious_matrix;
    homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4();
    homogenious_matrix.row(3) =
        Matrix<Scalar, 1, 4>(Scalar(0), Scalar(0), Scalar(0), Scalar(1));
    return homogenious_matrix;
  }

返回的就是一个4x4的矩阵，注释中说得也比较清楚， 就是返回如下形式的矩阵：

这个就是前面讲SE3的log时已经详细提到的 SE(3) 的定义，也是 视觉sLAM十四讲 中的定义，这里就不在赘述。 注意这里返回的都是齐次的。它也是有非齐次返回值的函数，叫matrix3x4()，逻辑基本一致，有兴趣可以去了解。

17. SO3Base::operator*()

重载了*乘法运算符, 便于两个 SO3 对象相乘。

sophus/so3.hpp:

  /// Group multiplication, which is rotation concatenation.
  ///
  template <typename OtherDerived>
  SOPHUS_FUNC SO3Product<OtherDerived> operator*(
      SO3Base<OtherDerived> const& other) const {
    using QuaternionProductType =
        typename SO3Product<OtherDerived>::QuaternionType;
    const QuaternionType& a = unit_quaternion();
    const typename OtherDerived::QuaternionType& b = other.unit_quaternion();
    /// NOTE: We cannot use Eigen's Quaternion multiplication because it always
    /// returns a Quaternion of the same Scalar as this object, so it is not
    /// able to multiple Jets and doubles correctly.
    return SO3Product<OtherDerived>(QuaternionProductType(
        a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
        a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
        a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
        a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()));
  }

这个运算符重载后，SO3对象和另外的SO3对象就可以直接使用星号*相乘。我们知道Sophus库中对李群SO3定义为是一个四元数，因此两个对象相乘之后代表做了两次旋转。

• 四元数相乘

  这里代码并没有太多原理， 就是把四元数写为其最原始的形式，然后相乘，最后得到新的四元数的实部和虚部。

  可对照视觉SLAM十四讲第三讲中的公式(3.23) 来理解这里的代码实现：

  $$\begin{aligned} \boldsymbol{q}_{a} \boldsymbol{q}_{b}=& s_{a} s_{b}-x_{a} x_{b}-y_{a} y_{b}-z_{a} z_{b} \\ &+\left(s_{a} x_{b}+x_{a} s_{b}+y_{a} z_{b}-z_{a} y_{b}\right) i \\ &+\left(s_{a} y_{b}-x_{a} z_{b}+y_{a} s_{b}+z_{a} x_{b}\right) j \\ &+\left(s_{a} z_{b}+x_{a} y_{b}-y_{a} x_{b}+z_{a} s_{b}\right) k \end{aligned} \tag{3.24}$$

• 备注

  代码的注释部分也解释了为什么不直接用Eigen库的四元数乘法来相乘。是因为 Eigen 库出于执行效率的考虑，所有的类型的数据类型都是明确的，比如double、float、int等。

  当两个不同数据类型的四元数相乘时，它会按第一个四元数的数据类型返回新的四元数，而这里会按照第二个四元数的数据类型返回新的四元数（当然， 两个四元数的数据类型一致时，使用两种方式均可）。

18. SE3Base::operator*()类比

同样的， 来看看Sophus 库中 SE3 的乘法运算符重载是怎样实现的.

sophus/se3.hpp:

  /// Group multiplication, which is rotation concatenation.
  ///
  template <typename OtherDerived>
  SOPHUS_FUNC SE3Product<OtherDerived> operator*(
      SE3Base<OtherDerived> const& other) const {
    return SE3Product<OtherDerived>(
        so3() * other.so3(), translation() + so3() * other.translation());
  }

• SE3相乘 这段比较容易理解，根据前面讨论SE3的 log函数时的结论，我们可以知道，两个SE3相乘是如下形式：

  $$\left [ \begin{matrix} \boldsymbol R_1 & \boldsymbol t_1 \\ \boldsymbol 0^\top & \boldsymbol 1 \end{matrix} \right ] \left [ \begin{matrix} \boldsymbol R_2 & \boldsymbol t_2 \\ \boldsymbol 0^\top & \boldsymbol 1 \end{matrix} \right ] = \left [ \begin{matrix} \boldsymbol {R_1 R_2} & \boldsymbol { R_1 t_2 + t_1 } \\ \boldsymbol 0^\top & \boldsymbol 1 \end{matrix} \right ]$$

  上述代码表达的就是公式所示的含义：

  通过在构造函数参数中传入代表左上角的 $R_1 R_2$ 和代表右上角的 $R_1t_2 + t1$，构造了一个新的SE3对象作为乘法运算的返回值。

19. SO3::LeftJacobian()、SO3::leftJacobianInverse()

这里求解是是SO3的左雅可比$J_l$ 和逆

sophus/so3.hpp:

  /// Returns the left Jacobian on lie group. See 1st entry in rightmost column
  /// in: http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
  ///
  /// A precomputed `theta` can be optionally passed in
  ///
  /// Warning: Not to be confused with Dx_exp_x(), which is derivative of the
  ///          internal quaternion representation of SO3 wrt the tangent vector
  ///
  SOPHUS_FUNC static Sophus::Matrix<Scalar, DoF, DoF> leftJacobian(
      Tangent const& omega, optional<Scalar> const& theta_o = nullopt) {
    using std::cos;
    using std::sin;
    using std::sqrt;

    Scalar const theta_sq = theta_o ? *theta_o * *theta_o : omega.squaredNorm();
    Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
    Matrix3<Scalar> const Omega_sq = Omega * Omega;
    Matrix3<Scalar> V;

    if (theta_sq <
        Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {
      V = Matrix3<Scalar>::Identity() + Scalar(0.5) * Omega;
    } else {
      Scalar theta = theta_o ? *theta_o : sqrt(theta_sq);
      V = Matrix3<Scalar>::Identity() +
          (Scalar(1) - cos(theta)) / theta_sq * Omega +
          (theta - sin(theta)) / (theta_sq * theta) * Omega_sq;
    }
    return V;
  }

• 变量定义

  • 入参 omega: 代表旋转向量 $\boldsymbol \phi = \theta \boldsymbol n$
  • 入参 theta_o: 代表旋转的角度大小 $\theta$
  • 变量 V: 待返回的雅可比矩阵
  • 变量 theta_sq：旋转角的平方 $\theta ^2$
  • 变量 theta : 也是代表旋转的角度大小 -- 用于入参没有传入 theta_o 参数时，则通过计算旋转向量 $\boldsymbol \phi$ 的2范数得到
  • 变量 Omega: 代表由上面旋转向量生成的反对称矩阵 $\boldsymbol \phi ^\wedge$
  • 变量 Omega_sq: 反对称矩阵的平方 $(\boldsymbol \phi^\wedge)^2$

• 代码中给出所引用到的公式出处： asrl.utias.utoronto.ca/~tdb/bib/ba…

• 代码实现：if 分支

  if (theta_sq <
      Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {
    V = Matrix3<Scalar>::Identity() + Scalar(0.5) * Omega;
  } 
  

  整个 if分支 计算过程是照 pdf 中列出的公式实现，先来看 pdf 中的公式定义：

  $$\begin{aligned} \mathbf{J_l} &= \int_{0}^{1} \mathbf{C}^{\alpha} d \alpha \equiv \sum_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\phi^{\wedge}\right)^{n} \\ & \equiv \frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a a}^{T}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge} \\ & \approx \boldsymbol{I}+\frac{1}{2} \boldsymbol \phi^{\wedge} \\ \mathbf{J_l}^{-1} &\approx \boldsymbol I - \dfrac{1}{2} \boldsymbol \phi^{\wedge} \\ \mathbf J_l ( -\boldsymbol \phi ) &= \exp(\boldsymbol \phi^\wedge) \mathbf J_r(\boldsymbol \phi) \end{aligned} \\$$

  这个公式也是视觉SLAM十四讲中第四讲中的公式(4.26). 代码中的 omega 就是公式中的 $\boldsymbol \phi$.

  这个pdf只有两页，它就是 state estimation for robotics 这本书的中间的大横页，大概在书中公式(7.194)附近。

  当旋转角度 theta 很小时, 就可以使用上述公式计算近似值， 代码中变量 Omega 就是公式中的 $\phi^\wedge$：

• 代码实现：else 分支

  } else {
    Scalar theta = theta_o ? *theta_o : sqrt(theta_sq);
    V = Matrix3<Scalar>::Identity() +
        (Scalar(1) - cos(theta)) / theta_sq * Omega +
        (theta - sin(theta)) / (theta_sq * theta) * Omega_sq;
  }
  

  先来看代码，把代码的实现逻辑还原后， 可知其为如下公式：

  $$J_l = \boldsymbol I + \dfrac{( 1 - cos \theta)}{\theta} \boldsymbol a^\wedge + \dfrac{\theta - sin \theta}{\theta^3} (\boldsymbol {a^\wedge})^2$$

  而我们再次回顾 state estimation for robotics 这本书中间的大横页公式汇总或公式(7.37)， 或者视觉SLAM十四讲公式(4.26), 我们发现它们列出的 $J_l$ 是如下公式：

  $$J_l = \dfrac{\sin \theta}{\theta} \boldsymbol I + \dfrac{( 1 - cos \theta)}{\theta} \boldsymbol a^\wedge + \dfrac{\theta - sin \theta}{\theta} \boldsymbol {aa^T}$$

  可见此处代码实现和公式不同。 那代码实现的依据是什么呢？代码中没有给出解释。

  经过查证资料，发现此处代码实现很可能是引用如下文献： Stochastic Models, Information Theory, and Lie Groups, Vol. 2， 在文献公式(10.86)处给出了SO(3)的左右Jacobian; 在文献 quaternion kinematics for the error-state kalman filter 中公式(183,184)也给出了类似的公式（引用形式），形式如下：

  $$J_l(\boldsymbol x) = \boldsymbol I + \dfrac{1 - \cos \theta}{ \theta^2} \boldsymbol x^\wedge + \dfrac{ \theta - \sin \theta}{\theta^3} \boldsymbol (\boldsymbol x^\wedge)^2 \tag{10.86}$$
  $$J_l^{-1}(\boldsymbol x) = \boldsymbol I - \dfrac{1}{2} \boldsymbol x^\wedge + \left( \dfrac{1}{\theta^2} - \dfrac{ 1 + \cos \theta }{ 2 \theta \sin \theta } \right ) ( \boldsymbol x^\wedge )^2 \tag{10.86}$$
  $$J_r(\boldsymbol x) = \boldsymbol I - \dfrac{1 - \cos \theta}{\theta^2} \boldsymbol x^\wedge + \dfrac{\theta - \sin \theta }{\theta^3} \boldsymbol (\boldsymbol x^\wedge)^2 \tag{10.86}$$
  $$J_r^{-1}(\boldsymbol x) = \boldsymbol I + \dfrac{1}{2} \boldsymbol x^\wedge + \left( \dfrac{1}{\theta^2} - \dfrac{ 1 + \cos \theta }{ 2 \theta \sin \theta } \right ) ( \boldsymbol x^\wedge )^2 \tag{10.86}$$

  通过对比可以发现： 代码实现逻辑和上述中的 $J_l$ 公式定义完全一致。至此，左雅可比求解函数逻辑基本搞清楚了。

  Sophus代码实现选择了不同来源文献的原因，以后弄清楚了再来刷新被章节（备注：Stochastic Models, Information Theory, and Lie Groups, Vol. 2 是91年左右发表的文献）。

20. SE3::LeftJacobian()、SE3::leftJacobianInverse 类比

同样的， 作为类比，也来看看 SE3 的 LeftJacobian() 函数的代码逻辑。

sophus/se3.hpp:

  SOPHUS_FUNC static Sophus::Matrix<Scalar, DoF, DoF> leftJacobian(
      Tangent const& upsilon_omega) {
    Vector3<Scalar> const upsilon = upsilon_omega.template head<3>();
    Vector3<Scalar> const omega = upsilon_omega.template tail<3>();
    Matrix3<Scalar> const J = SO3<Scalar>::leftJacobian(omega);
    Matrix3<Scalar> Q = jacobianUpperRightBlock(upsilon, omega);
    Matrix6<Scalar> U;
    U << J, Q, Matrix3<Scalar>::Zero(), J;
    return U;
  }

通过代码阅读，发现代码逻辑表达的是一个如下形式的矩阵 U:

其中 $J$ 代表的是其中旋转部分 $\phi$ 所生成的李群SO(3)的雅可比， 在上面章节刚刚遇到过。而Q有一个比较复杂的计算过程，封装在函数 jacobianUpperRightBlock() 中。

整个这段代码的依据又是什么呢？

在文献 Stochastic Models, Information Theory, and Lie Groups, Vol. 2 的公式(10.95)和state estimation for robotics 的公式(7.85) , 两个文献均给出了 $J_l$ 的计算公式以及右上角 $Q$ 块的计算公式。 此处以后者为例列出公式如下：

其中

对比代码可以发现，代码逻辑的确是按照以上公式定义来的。 jacobianUpperRightBlock()函数比较复杂，其代码中的变量的含义注释如下：

  Scalar const theta = sqrt(theta_sq);
  Scalar const i_theta = Scalar(1) / theta;
  Scalar const i_theta_sq = i_theta * i_theta;
  Scalar const i_theta_po4 = i_theta_sq * i_theta_sq;
  Scalar const st = sin(theta);
  Scalar const ct = cos(theta);
  Scalar const c1 = i_theta_sq - st * i_theta_sq * i_theta;
  Scalar const c2 = k1By2 * i_theta_sq + ct * i_theta_po4 - i_theta_po4;
  Scalar const c3 = i_theta_po4 + k1By2 * ct * i_theta_po4 -
                    Scalar(1.5) * st * i_theta * i_theta_po4;
  Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
  Matrix3<Scalar> const OmegaUpsilon = Omega * Upsilon;
  Matrix3<Scalar> const OmegaUpsilonOmega = OmegaUpsilon * Omega;

• 输入参数 upsilon: 即公式中的 $\boldsymbol \rho$，3x1向量， 也是se(3)李代数定义中的 $\ro$，视觉SLAM十四讲的公式(4.15)处定义, upsilon=$\rho$
• 输入参数 omega: 即公式中的 $\boldsymbol \phi$, 3x1向量, 就是se(3)李代数定义中的 $\phi$, omega=$\phi$。 公式中的 $\phi^\wedge$ 即为对它求反对称矩阵。
• 变量 theta： 即公式中的$\phi$(注意未加粗), 是个标量。 李代数也对应一个旋转向量 omega=$\omega = \theta \boldsymbol n$，代码中变量 \theta 即代表旋转角度，它一般是通过计算向量omega=$\phi$ 的模来获得。公式中的 $\sin \phi$ 即为对它求三角函数。
• 变量 i_theta, i_theta_sq, i_theta_po4: 即 $\dfrac{1}{\theta}, \dfrac{1}{\theta^2}, \dfrac{1}{\theta^4}$ 的变量表示
• 变量 st, ct: 即 $\sin \theta, \cos \theta$
• 变量 c1: 即公式中的 $(\dfrac{\phi - \sin \phi}{\phi^3})$, 注意这里使用的是标量$\phi$， 即角度$\theta$.
• 变量 c2: 即公式中的 $(\dfrac{\phi^2 + 2 \cos \phi -2}{2 \phi^4})$
• 变量 c3: 即公式中的 $\dfrac{2\phi - 3\sin \phi + \phi \cos \phi}{2\phi^5}$
• 变量 Omega: 即 $\boldsymbol \phi^\wedge$
• 变量 Upsilon: 即 $\boldsymbol \rho^\wedge$
• 变量 OmegaUpsilon: 即 $\boldsymbol {\phi^\wedge \rho^\wedge}$
• 变量 OmegaUpsilonOmega: 即 $\boldsymbol {\phi^\wedge \rho^\wedge \phi^\wedge }$

了解了变量含义之后，剩下的代码就是对着公式拼的过程了：

      Q = k1By2 * Upsilon +
          c1 * (OmegaUpsilon + Upsilon * Omega + OmegaUpsilonOmega) -
          c2 * (theta_sq * Upsilon + Scalar(2) * OmegaUpsilonOmega) +
          c3 * (OmegaUpsilonOmega * Omega + Omega * OmegaUpsilonOmega);

上述代码就是按公式把四部分按照公式拼起来的。

至此，SE(3) 的雅可比和雅可比逆也分析清楚了。

21. SO3::Dx_exp_x()

函数求解 $\exp(\boldsymbol x^\wedge)$ 对$\boldsymbol x$ 的导数。

sophus/so3.hpp

 SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
      Tangent const& omega) {
    using std::cos;
    using std::exp;
    using std::sin;
    using std::sqrt;
    Scalar const c0 = omega[0] * omega[0];
    Scalar const c1 = omega[1] * omega[1];
    Scalar const c2 = omega[2] * omega[2];
    Scalar const c3 = c0 + c1 + c2;

    if (c3 < Constants<Scalar>::epsilon()) {
      return Dx_exp_x_at_0();
    }

    Scalar const c4 = sqrt(c3);
    Scalar const c5 = 1.0 / c4;
    Scalar const c6 = 0.5 * c4;
    Scalar const c7 = sin(c6);
    Scalar const c8 = c5 * c7;
    Scalar const c9 = pow(c3, -3.0L / 2.0L);
    Scalar const c10 = c7 * c9;
    Scalar const c11 = Scalar(1.0) / c3;
    Scalar const c12 = cos(c6);
    Scalar const c13 = Scalar(0.5) * c11 * c12;
    Scalar const c14 = c7 * c9 * omega[0];
    Scalar const c15 = Scalar(0.5) * c11 * c12 * omega[0];
    Scalar const c16 = -c14 * omega[1] + c15 * omega[1];
    Scalar const c17 = -c14 * omega[2] + c15 * omega[2];
    Scalar const c18 = omega[1] * omega[2];
    Scalar const c19 = -c10 * c18 + c13 * c18;
    Scalar const c20 = Scalar(0.5) * c5 * c7;
    Sophus::Matrix<Scalar, num_parameters, DoF> J;
    J(0, 0) = -c0 * c10 + c0 * c13 + c8;
    J(0, 1) = c16;
    J(0, 2) = c17;
    J(1, 0) = c16;
    J(1, 1) = -c1 * c10 + c1 * c13 + c8;
    J(1, 2) = c19;
    J(2, 0) = c17;
    J(2, 1) = c19;
    J(2, 2) = -c10 * c2 + c13 * c2 + c8;
    J(3, 0) = -c20 * omega[0];
    J(3, 1) = -c20 * omega[1];
    J(3, 2) = -c20 * omega[2];
    return J;
}

• 李群导数定义:李代数求导

  此种导数的定义，就是 视觉SLAM十四讲 中提到的李代数求导。公式(4.40)左右。

  假设 $\boldsymbol C = \exp(\phi^\wedge) \in SO(3), \quad \mathbf v \in \mathbb R^3$ , 则 $\dfrac{\partial (\boldsymbol C \mathbf v) }{ \partial \boldsymbol \phi }$ 为对于 $\boldsymbol \phi=(\phi_1, \phi_2, \phi_3)$ 的导数。根据导数基本定义, 对某一个坐标系基 $\boldsymbol e_i$ 的导数为：

  $$\dfrac{\partial (\boldsymbol C \mathbf v)}{ \partial \boldsymbol e_i} = \lim_{h \rightarrow 0} \dfrac{\exp( (\boldsymbol \phi + h \boldsymbol e_i)^\wedge) \mathbf v - \exp(\boldsymbol \phi^\wedge) \mathbf v }{ h }$$

  使用 BCH 公式近似：

  $$\exp((\boldsymbol \phi + h \boldsymbol e_i)^\wedge) \approx \exp((\mathbf J_l h \boldsymbol e_i)^\wedge) \exp(\boldsymbol \phi^\wedge) \approx (\boldsymbol I + h(\mathbf J_l \boldsymbol e_i)^\wedge) \exp (\boldsymbol \phi^\wedge) = (\boldsymbol I + h(\mathbf J_l \boldsymbol e_i)^\wedge) \boldsymbol C$$

  根据上式代入到上式的导数定义，则导数为：

  $$\dfrac{\partial (\boldsymbol C \mathbf v) }{\partial \boldsymbol \phi_i} = (\mathbf J_l \boldsymbol e_i)^\wedge \mathbf C \mathbf v = (\mathbf C \mathbf v )^\wedge \mathbf J_l \boldsymbol e_i = (\exp(\boldsymbol \phi^\wedge) \mathbf v )^\wedge \mathbf J_l \boldsymbol e_i$$

  扩展到整个坐标系

  $$\dfrac{\partial (\boldsymbol C \mathbf v) }{\partial \boldsymbol \phi} = (\mathbf C \mathbf v )^\wedge \mathbf J_l = (\exp(\boldsymbol \phi^\wedge) \mathbf v)^\wedge \mathbf J_l$$

  可知有公式： $\dfrac{\partial (\boldsymbol C \mathbf v) }{\partial \boldsymbol \phi_i} = (\mathbf J_l \boldsymbol e_i)^\wedge \mathbf C \mathbf v$

• 代码逻辑

  按照上述公式分别计算矩阵的每个元素， 填写到 $\mathbf J(n_1,n_2)$ 中.

23. SO3::exp()、SO3::expAndTheta()

求解当前SO3做指数运算的结果，以及原SO3对应的theta. 即我们经常说的 $\exp (\boldsymbol \phi^\wedge)$, 返回一个新的SO3。

sophus/so3.hpp:

  SOPHUS_FUNC static SO3<Scalar> exp(Tangent const& omega) {
    Scalar theta;
    return expAndTheta(omega, &theta);
  }
  SOPHUS_FUNC static SO3<Scalar> expAndTheta(Tangent const& omega,
                                             Scalar* theta) {
    SOPHUS_ENSURE(theta != nullptr, "must not be nullptr.");
    using std::abs;
    using std::cos;
    using std::sin;
    using std::sqrt;
    Scalar theta_sq = omega.squaredNorm();

    Scalar imag_factor;
    Scalar real_factor;
    if (theta_sq <
        Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {
      *theta = Scalar(0);
      Scalar theta_po4 = theta_sq * theta_sq;
      imag_factor = Scalar(0.5) - Scalar(1.0 / 48.0) * theta_sq +
                    Scalar(1.0 / 3840.0) * theta_po4;
      real_factor = Scalar(1) - Scalar(1.0 / 8.0) * theta_sq +
                    Scalar(1.0 / 384.0) * theta_po4;
    } else {
      *theta = sqrt(theta_sq);
      Scalar half_theta = Scalar(0.5) * (*theta);
      Scalar sin_half_theta = sin(half_theta);
      imag_factor = sin_half_theta / (*theta);
      real_factor = cos(half_theta);
    }

    SO3 q;
    q.unit_quaternion_nonconst() =
        QuaternionMember(real_factor, imag_factor * omega.x(),
                         imag_factor * omega.y(), imag_factor * omega.z());
    SOPHUS_ENSURE(abs(q.unit_quaternion().squaredNorm() - Scalar(1)) <
                      Sophus::Constants<Scalar>::epsilon(),
                  "SO3::exp failed! omega: {}, real: {}, img: {}",
                  omega.transpose(), real_factor, imag_factor);
    return q;
  }

代码在角度很小时，主要是用到了泰勒展开近似运算。 在角度不是很小时，使用了四元数:

（未完待续）

附录： 相关文献

• A micro Lie theory for state estimation robotics. 前半部分详细介绍了李群、李代数基本概念，以及伴随，同时对李群微分四种形式均做了定义；

• Stochastic Models, Information Theory, and Lie Groups, Vol. 2 p40 定义了左右Jacobin;

• Integrating Generic Sensor Fusion Algorithms with Sound State Representation through Encapsulation of Manifolds， C. Hertzberg. 讲解SO3的对数函数计算公式

• State Estimation for Robotics. 书中公式(7.37)的大横页插页给出了SO(3)、SE(3)的近似求解公式，可用于雅可比的求解

• Computing exponentials of skew symmetric matrices and logarithms of orthogonal matrices ，详细推导了 $SE(3)$ 求 log() 时所用到的雅可比 $J$ 计算过程。文中还对扩展到了SO(n)、SE(n) n$\ge$4的情况进行了讨论，如类罗德里格斯公式。