MTK数学库源码剖析：SO(3)与SE(3)的李群-李代数转换精讲

📘 深入理解流形上的微积分，从基础数学到工业级实现

---

📌 引言

在激光SLAM、视觉惯性里程计(VIO)以及卫星导航等领域，状态估计是核心问题。我们通常要估计机器人的姿态（旋转）和位姿（旋转+平移），这些量生活在流形上——它们对加法不封闭，不能直接用欧氏空间的微积分工具。

为了解决这一难题，李群与李代数理论应运而生。通过将流形上的元素映射到其切空间（李代数），我们可以把非线性优化问题转化为线性最小二乘问题，从而借助高斯-牛顿、LM等经典算法高效求解。

MTK（Manifold Toolkit） 是一个轻量级的 C++ 头文件库，提供了流形运算的基础数学工具。本文将结合其核心头文件 mtkmath.hpp，详细剖析 $SO(3)$ 与 $SE(3)$ 的李群-李代数转换，并深入解析代码实现中的数值稳定技巧。

---

一、为什么需要李群和李代数？

1.1 旋转矩阵的"加法"困境

三维旋转矩阵 $R\in SO(3)$ 满足 $R{R}^{\top }=I$，行列式为 $1$。

两个旋转矩阵相乘仍然是旋转矩阵（群乘法），但两个旋转矩阵相加一般不再是旋转矩阵。

然而，在优化算法中，我们需要对变量进行"加增量"操作（即 $x\leftarrow x+\Delta x$），并求目标函数对增量的导数。如果直接在矩阵空间上进行加法，会破坏约束，导致结果不合法。

1.2 切空间的魔力

李代数 $\mathfrak{so}(3)$ 是三维向量空间，它与 $SO(3)$ 在单位元处的切空间同构。旋转向量（轴角）就是 $\mathfrak{so}(3)$ 的元素：

• 方向 → 旋转轴
• 模长 → 旋转角度

在切空间中，我们可以自由做加减法、求梯度，然后通过指数映射将切空间中的增量"卷回"到流形上，保证结果始终处于流形内。

同理，$SE(3)$ 的切空间 $\mathfrak{se}(3)$ 是六维向量，涵盖了旋转和平移的增量。

💡 一句话总结：李代数负责"微积分"，李群负责"几何约束"。二者的桥梁就是指数映射（$\exp$）和对数映射（$\log$）。

---

二、SO(3) 与 so(3) 的转换细节

2.1 定义与表示

李群：

$$SO(3)=\left\{R\in {\mathbb{R}}^{3\times 3}|R{R}^{\top }=I,\det (R)=1\right\}$$

• 维数 = 3，由绕三个轴的旋转角度参数化。

李代数：

$$\mathfrak{so}(3)=\left\{\varphi \in {\mathbb{R}}^3\right\}$$

通常记 $\varphi =\theta \mathbf{u}$，其中 $\mathbf{u}$ 是单位轴，$\theta$ 是旋转角度。其反对称矩阵表示为：

$${\varphi }^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]$$

2.2 指数映射（so(3) → SO(3)）

这就是著名的罗德里格斯公式（Rodrigues’ Formula）：

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

其中 $\theta =\Vert \varphi \Vert$。

⚠️ 当 $\theta$ 接近 0 时，直接用该公式会面临除零风险，故需要泰勒展开：

$$\exp ({\varphi }^{\wedge })\approx I+{\varphi }^{\wedge }+\frac{1}{2}({\varphi }^{\wedge }{)}^2+\cdots$$

💻 代码对应：

• hat(v) 生成 ${\varphi }^{\wedge }$；
• cos_sinc_sqrt(x2) 输入 ${\theta }^2$，返回 first = cosθ 和 second = sinθ/θ（即 $\text{sinc}$）；
• A_matrix(v) 计算的是公式中 $\frac{1-\cos \theta }{{\theta }^2}$ 和 $\frac{\theta -\sin \theta }{{\theta }^3}$ 的组合系数。

2.3 对数映射（SO(3) → so(3)）

给定旋转矩阵 $R$，提取旋转向量 $\varphi$：

$$\theta =\arccos \left(\frac{\text{tr}(R)-1}{2}\right),\varphi =\frac{\theta }{2\sin \theta }\left[\begin{array}{c}{R}_{32}-R_{23}\\ R_{13}-R_{31}\\ R_{21}-R_{12}\end{array}\right]$$

（当 $\theta$ 极小时，使用泰勒展开避免除零。）

💻 代码对应： mtkmath.hpp 中的 log() 函数模板提供了泛型支持。对于纯旋转，w 是标量部分（单位四元数的实部），vec 是向量部分（虚部），它利用 atan2 提取出角度和轴。

---

三、SE(3) 与 se(3) 的转换细节

3.1 定义与表示

李群：

$$SE(3)=\left\{T=\left[\begin{array}{cc}R& t\\ 0^{\top }& 1\end{array}\right]|R\in SO(3),t\in {\mathbb{R}}^3\right\}$$

• 维数 = 6。

李代数：

$$\mathfrak{se}(3)=\left\{\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]\in {\mathbb{R}}^6\right\}$$

其中 $\rho$ 是平移分量，$\varphi$ 是旋转分量。

⚠️ 注意：这里的平移 $\rho$ 不是最终的平移量 $t$，而是经过雅可比矩阵缩放后的结果。

其 $4\times 4$ 矩阵表示：

$${\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^{\top }& 0\end{array}\right]$$

3.2 指数映射（se(3) → SE(3)）

$$\exp ({\xi }^{\wedge })=\left[\begin{array}{cc}\exp ({\varphi }^{\wedge })& J_l(\varphi )\rho \\ 0^{\top }& 1\end{array}\right]$$

• 旋转部分 $R$：直接由 $\varphi$ 通过 $SO(3)$ 指数映射得到。
• 平移部分 $t$：由左雅可比矩阵 $J_l$ 作用于 $\rho$ 得到：

$$t=J_l(\varphi )\cdot \rho$$

其中左雅可比矩阵的闭式解为：

$$J_l(\varphi )=I+\frac{1-\cos \theta }{{\theta }^2}{\varphi }^{\wedge }+\frac{\theta -\sin \theta }{{\theta }^3}({\varphi }^{\wedge }{)}^2$$

💻 代码对应：

• A_matrix(v) 函数计算的就是左雅可比矩阵 $J_l$；
• exp() 模板函数使用 cos_sinc_sqrt 计算出 $\cos \theta$ 和 $\text{sinc}\theta$，然后组合出旋转矩阵 $R$ 和平移向量 $t$。

3.3 对数映射（SE(3) → se(3)）

已知变换矩阵 $T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$：

• 旋转部分 $\varphi$：直接由 $R$ 通过 $SO(3)$ 对数映射得到。
• 平移部分 $\rho$：由左雅可比矩阵的逆 $J_l^{-1}$ 作用于 $t$ 得到：

$$\rho =J_l^{-1}(\varphi )\cdot t$$

💻 代码对应：

• A_inv(v) 函数计算的就是 $J_l^{-1}$；
• A_inv_trans(v) 则是其转置，在协方差传播中用于调整雅可比矩阵的变换方向。

---

四、mtkmath.hpp 源码精讲

该头文件位于 MTK 库的 src/mtkmath.hpp，包含若干模板函数，提供了流形运算所需的原子操作。下面逐一剖析关键函数。

4.1 辅助模板：

template<class Manifold>
struct traits {
    typedef typename Manifold::scalar scalar;
    enum { DOF = Manifold::DOF };
    typedef vect<DOF, scalar> vectorized_type;
    typedef Eigen::Matrix<scalar, DOF, DOF> matrix_type;
};

作用：从流形类中萃取标量类型、自由度、向量化类型和协方差矩阵类型。这是一种典型的类型特征（type traits） 模式，使算法可以统一处理不同的流形（如 SO3、SE3、S2）。

📌 注意：对于基本类型 float 和 double，提供了特化版本，继承自 Scalar<float> 等（Scalar 可能是一个简单封装，定义了 DOF=1 等）。

4.2 cos_sinc_sqrt —— 数值稳定的 cos(θ) 与 sinc(θ)

template<class scalar>
std::pair<scalar, scalar> cos_sinc_sqrt(const scalar &x2) {
    using std::sqrt; using std::cos; using std::sin;
    static scalar const taylor_0_bound = boost::math::tools::epsilon<scalar>();
    static scalar const taylor_2_bound = sqrt(taylor_0_bound);
    static scalar const taylor_n_bound = sqrt(taylor_2_bound);
    assert(x2 >= 0);
    if (x2 >= taylor_n_bound) {
        scalar x = sqrt(x2);
        return std::make_pair(cos(x), sin(x)/x);
    }
    // 泰勒展开（角度极小）
    static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.};
    scalar cosi = 1., sinc = 1;
    scalar term = -1/2. * x2;
    for (int i=0; i<3; ++i) {
        cosi += term;
        term *= inv[2*i];
        sinc += term;
        term *= -inv[2*i+1] * x2;
    }
    return std::make_pair(cosi, sinc);
}

阈值设定：taylor_n_bound ≈ sqrt(sqrt(eps))，对于 double 约为 1e-4。当 ${\theta }^2<10^{-8}$（即 $\theta <10^{-4}$）时，使用泰勒级数展开计算 $\cos \theta$ 和 $\text{sinc}\theta$。

泰勒展开推导：

$$\cos \theta =1-\frac{{\theta }^2}{2!}+\frac{{\theta }^4}{4!}-\frac{{\theta }^6}{6!}+\cdots$$

$$\text{sinc}(\theta )=\frac{\sin \theta }{\theta }=1-\frac{{\theta }^2}{3!}+\frac{{\theta }^4}{5!}-\frac{{\theta }^6}{7!}+\cdots$$

代码中利用递推关系，每次迭代同时累加两项，减少了乘除次数。

工程意义：该函数返回的 $(\cos \theta ,\sin \theta /\theta )$ 是计算旋转矩阵和雅可比矩阵的基石。小角度下的直接计算会导致灾难性抵消（$\sin \theta /\theta$ 趋近于 1，但 $1-{\theta }^2/6+\cdots$ 精度更高），因此这种分段策略是工业级代码的标配。

4.3 hat —— 反对称矩阵生成器

template<typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> hat(const Base& v) {
    Eigen::Matrix<typename Base::scalar, 3, 3> res;
    res << 0, -v[2], v[1],
           v[2], 0, -v[0],
           -v[1], v[0], 0;
    return res;
}

将三维向量映射到 $\mathfrak{so}(3)$ 的矩阵表示。这是所有后续公式的基石。

4.4 A_matrix —— 左雅可比矩阵 $J_l$

template<typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> A_matrix(const Base & v) {
    double squaredNorm = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
    double norm = std::sqrt(squaredNorm);
    if (norm < MTK::tolerance<typename Base::scalar>()) {
        return Eigen::Matrix<...>::Identity();
    } else {
        return I + (1 - cos(norm))/squaredNorm * hat(v) +
               (1 - sin(norm)/norm)/squaredNorm * hat(v) * hat(v);
    }
}

公式：

$$J_l=I+\frac{1-\cos \theta }{{\theta }^2}{v}^{\wedge }+\frac{\theta -\sin \theta }{{\theta }^3}(v^{\wedge }{)}^2$$

注意代码中第三项系数写成了 (1 - sin(norm)/norm) / squaredNorm，这与 $(\theta -\sin \theta )/{\theta }^3$ 等价。当 norm 小于容差时，直接返回单位矩阵，因为此时高阶项可忽略。

4.5 A_inv 与 A_inv_trans —— 左雅可比矩阵的逆及其转置

template<typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> A_inv(const Base& v) {
    // ...
    if (v.norm() > MTK::tolerance<...>()) {
        res = I - 0.5*hat(v) + (1 - v.norm()*cos(v.norm()/2) / (2*sin(v.norm()/2)))
                                * hat(v)*hat(v) / v.squaredNorm();
    } else { res = I; }
    return res;
}

该公式来源于 $J_l^{-1}$ 的闭式解，在推导中利用了 $\cot (\theta /2)$ 的级数展开。A_inv_trans 仅符号不同，对应 $J_l^{-\top }$，用于协方差传播时的变换方向。

4.6 exp —— 指数映射模板

template<class scalar, int n>
scalar exp(vectview<scalar, n> result, vectview<const scalar, n> vec, const scalar& scale = 1) {
    scalar norm2 = vec.squaredNorm();
    std::pair<scalar, scalar> cos_sinc = cos_sinc_sqrt(scale*scale * norm2);
    scalar mult = cos_sinc.second * scale;
    result = mult * vec;
    return cos_sinc.first;
}

此函数用于将切向量 vec 映射到流形上。对于单位四元数（对应 $SO(3)$），n=3 表示虚部，实部由返回的 $\cos \theta$ 给出。参数 scale 用于调整单位（比如角速度乘以时间步长）。返回 $\cos \theta$，便于调用者构建完整的流形元素（如四元数的标量部分）。

4.7 log —— 对数映射模板

template<class scalar, int n>
void log(vectview<scalar, n> result,
         const scalar &w, const vectview<const scalar, n> vec,
         const scalar &scale, bool plus_minus_periodicity){
    scalar nv = vec.norm();
    if(nv < tolerance<scalar>()) {
        if(!plus_minus_periodicity && w < 0) {
            // 找最大分量，处理负实部情况
            int i; nv = vec.cwiseAbs().maxCoeff(&i);
            result = scale * std::atan2(nv, w) * vect<n, scalar>::Unit(i);
            return;
        }
        nv = tolerance<scalar>();
    }
    scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w));
    result = s * vec;
}

该函数是 exp 的逆，用于从流形元素 $(w,vec)$ 提取切向量。当 nv（虚部范数）很小时，需要特殊处理。若 plus_minus_periodicity 为真，则认为 $(w,vec)$ 与 $(-w,-vec)$ 等价（如单位四元数），此时使用 atan(nv/w) 的单值分支；否则使用 atan2(nv, w) 处理负实部。当 w < 0 且不允许周期性时，通过取绝对值最大分量确定方向，避免歧义。

4.8 其他辅助函数

• tolerance<scalar>()：返回 float 为 1e-5，double 为 1e-11，用于判断小量。
• normalize：角度归一化，已被注释（未使用）。
• S2_w_expw_：球面 $S^2$ 流形的指数映射偏导，用于重力向量等二自由度状态。

---

五、工程应用与注意事项

5.1 在 SLAM 中的应用

• IMU 预积分：利用 exp 和 log 处理旋转增量的累加与误差传播。
• 误差状态卡尔曼滤波（ESKF）：需要 A_matrix 和 A_inv 来更新协方差矩阵。
• 图优化：在 Ceres 或 g2o 中，需要定义流形上的增量加法操作（boxplus），其底层即调用指数映射。

5.2 数值稳定性要点

• 始终使用 cos_sinc_sqrt 而非直接计算 $\sin \theta /\theta$，特别是在角度极小的情况。
• 在雅可比矩阵计算中，当 norm 小于容差时，直接返回单位阵可避免除零。
• 对数映射中的小量处理（如 plus_minus_periodicity）保证了结果的唯一性和平滑性。

5.3 性能优化提示

• 代码中注释了使用霍纳方案和 SSE2 并行计算的优化思路，未来可进一步加速。
• 对于固定类型 float/double，模板特化（如 tolerance）避免了运行时分支。

---

六、直观理解总结（物理意义）

映射方向	公式	物理意义	代码核心函数
代数 → 群（Exp）	$R=\exp ({\varphi }^{\wedge })$	给定绕某轴转多少角度，求出最终姿态。积分（累加增量）	A_matrix, hat, cos_sinc_sqrt
群 → 代数（Log）	$\varphi =\log (R{)}^{\vee }$	给定两个姿态，求出它们之间的旋转差向量。求差（做减法）	A_inv, log
代数 → 群（Exp）	$T=\exp ({\xi }^{\wedge })$	给定角速度+线速度（体速），求经过该速度作用后的位姿。	exp 模板（内部调 cos_sinc_sqrt）
群 → 代数（Log）	$\xi =\log (T{)}^{\vee }$	给定两个位姿，求出它们之间的六维扰动（误差向量）。	log 模板（内部调 A_inv）

---

七、总结

MTK 的 mtkmath.hpp 虽然代码精简，却浓缩了流形微积分的核心算法。通过深入理解 cos_sinc_sqrt、hat、A_matrix、A_inv 等函数，我们不仅掌握了 $SO(3)$ 与 $SE(3)$ 的转换细节，更学会了如何在实际工程中处理数值不稳定性问题。

在机器人学中，李代数（向量）用来做"加减"和"求导"（因为它是线性空间），李群（矩阵）用来做"复合"和"变换"（因为它保证了几何约束）。mtkmath.hpp 提供的 hat、A_matrix、A_inv 以及 cos_sinc_sqrt，正是在底层为这两个空间的自由转换提供了数值稳定且高效的数学支撑。

如果没有这些转换，所有的优化算法（如 Ceres、g2o 或 ESKF）都无法对旋转和位姿进行正确的求导。在自动驾驶、机器人导航等领域，正确理解和实现这些底层工具，是构建高精度状态估计系统的基石。希望本文能帮助读者扫清数学障碍，更加自信地阅读和修改此类底层代码。

延伸阅读：

• Hertzberg et al., “Integrating generic sensor fusion algorithms with sound state representations through encapsulation of manifolds”, 2011.
• Barfoot, “State Estimation for Robotics”, Cambridge University Press.
• Solà et al., “A Micro Lie Theory for State Estimation in Robotics”, 2018.