SO(3) 数学库源码深度剖析：从罗德里格斯公式到数值稳定性

aiccc

188人浏览 · 2026-09-02 18:24:09

aiccc · 2026-09-02 18:24:09 发布

在 SLAM 和机器人状态估计中，三维旋转是最基础也是最容易出错的数学对象。旋转矩阵虽然直观，但它的 9 个参数存在冗余约束，直接进行优化和求导极为困难。李群 $S O (3)$ 与李代数 $so(3)\mathfrak{so}(3)$ 的理论正是为解决这一问题而生。

本文将从一个真实的 SO3_MATH.h 源码出发，结合《视觉SLAM十四讲》中的理论知识，深入剖析：

反对称矩阵的两种实现方式
指数映射（罗德里格斯公式）的三种重载及其数学等价性
对数映射（重点）：从旋转矩阵提取旋转向量的完整数学推导与代码逐行对应
欧拉角提取与万向锁处理
数值稳定性的工程考量

无论你是 SLAM 算法工程师、机器人开发者，还是对三维几何计算感兴趣的学生，本文都将帮你彻底打通从数学公式到工业级代码的最后一公里。

一、文件概览

SO3_MATH.h 是一个轻量级的头文件库，仅依赖 Eigen，实现了三维旋转的核心运算：

功能模块	对应函数	数学本质
反对称矩阵生成	`skew_sym_mat` / 宏 `SKEW_SYM_MATRX`	`hat` 映射（ $so(3)\mathfrak{so}(3)$ 的矩阵表示）
指数映射（3个重载）	`Exp(ang), Exp(ang_vel, dt), Exp(v1,v2,v3)`	旋转向量 → 旋转矩阵（罗德里格斯公式）
对数映射	`Log(R)`	旋转矩阵 → 旋转向量
欧拉角提取	`RotMtoEuler(R)`	旋转矩阵 → Z-Y-X 欧拉角

二、反对称矩阵：SO(3) 运算的基石

2.1 宏定义版本

#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0

这个宏展开后生成 9 个元素，配合 Eigen 的逗号初始化语法构造反对称矩阵：

Eigen::Matrix<T, 3, 3> K;
K << SKEW_SYM_MATRX(r_axis);
// 展开为：
// K << 0.0, -v[2], v[1],
//      v[2], 0.0, -v[0],
//      -v[1], v[0], 0.0;

2.2 函数版本

template<typename T>
Eigen::Matrix<T, 3, 3> skew_sym_mat(const Eigen::Matrix<T, 3, 1> &v)
{
    Eigen::Matrix<T, 3, 3> skew_sym_mat;
    skew_sym_mat << 0.0, -v[2], v[1],
                    v[2], 0.0, -v[0],
                    -v[1], v[0], 0.0;
    return skew_sym_mat;
}

2.3 数学意义

对于向量 $v=[v_x, v_y, v_z]^T$ ，其反对称矩阵为：

$v^{\wedge} = \begin{bmatrix} 0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0 \end{bmatrix}$

这个矩阵与叉积等价： $v∧⋅w=v×wv^{\wedge} \cdot w = v \times w$ 。它是罗德里格斯公式、旋转矩阵微分、李代数运算的核心基石。

三、指数映射（Exp）：旋转向量 → 旋转矩阵

代码提供了 3 个重载版本，核心算法均为罗德里格斯公式。

3.1 版本一：右值引用版本

template<typename T>
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &&ang)
{
    T ang_norm = ang.norm();
    Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
    if (ang_norm > 0.0000001)
    {
        Eigen::Matrix<T, 3, 1> r_axis = ang / ang_norm;
        Eigen::Matrix<T, 3, 3> K;
        K << SKEW_SYM_MATRX(r_axis);
        return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K;
    }
    else
    {
        return Eye3;
    }
}

3.2 版本二：角速度 × 时间增量

template<typename T, typename Ts>
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &ang_vel, const Ts &dt)
{
    T ang_vel_norm = ang_vel.norm();
    T r_ang = ang_vel_norm * dt;  // 总旋转角度 = 角速度 × 时间
    // ... 后续与版本一相同
}

应用场景：IMU 预积分中，根据角速度和采样时间计算旋转增量。

3.3 版本三：三个标量分量

template<typename T>
Eigen::Matrix<T, 3, 3> Exp(const T &v1, const T &v2, const T &v3)
{
    T norm = sqrt(v1*v1 + v2*v2 + v3*v3);
    // ... 后续与版本一相同
}

3.4 罗德里格斯公式的三种等价形式

形式一：代码中的简化形式（使用单位轴 $u$ 和角度 $θ\theta$ ）

当 $ϕ=θ⋅u\phi = \theta \cdot u$ （ $u$ 为单位向量）时：

$\sin\theta \cdot u^{\wedge} + (1 - \cos\theta) \cdot (u^{\wedge})^2$

形式二：《视觉SLAM十四讲》中的通用形式（直接使用旋转向量 $ϕ\phi$ ）

$\frac{\sin\theta}{\theta} \cdot \phi^{\wedge} + \frac{1 - \cos\theta}{\theta^2} \cdot (\phi^{\wedge})^2$

其中 $θ=∥ϕ∥\theta = \lVert \phi \rVert$ 。

形式三：《十四讲》中的几何分解形式

利用恒等式 $(u∧)2=u⋅uT−I(u^{\wedge})^2 = u \cdot u^T - I$ ，形式一可变形为：

$\cos\theta \cdot I + (1 - \cos\theta) \cdot uu^T + \sin\theta \cdot u^{\wedge}$

三种形式的等价性证明

证明形式一 ↔ 形式二：

由 $ϕ=θ⋅u\phi = \theta \cdot u$ ，有 $ϕ∧=θ⋅u∧\phi^{\wedge} = \theta \cdot u^{\wedge}$ 。代入形式二：

$\begin{aligned} R &= I + \frac{\sin\theta}{\theta} \cdot (\theta u)^{\wedge} + \frac{1 - \cos\theta}{\theta^2} \cdot ((\theta u)^{\wedge})^2 \\ &= I + \sin\theta \cdot u^{\wedge} + (1 - \cos\theta) \cdot (u^{\wedge})^2 \end{aligned}$

证明形式一 ↔ 形式三：

利用恒等式 $(u∧)2=uuT−I(u^{\wedge})^2 = uu^T - I$ ：

$\begin{aligned} R &= I + \sin\theta \cdot u^{\wedge} + (1 - \cos\theta) \cdot (uu^T - I) \\ &= I + \sin\theta \cdot u^{\wedge} + (1 - \cos\theta) \cdot uu^T - (1 - \cos\theta) \cdot I \\ &= \cos\theta \cdot I + (1 - \cos\theta) \cdot uu^T + \sin\theta \cdot u^{\wedge} \end{aligned}$

形式	公式	特点
代码实现	$\sin\theta \cdot u^{\wedge} + (1 - \cos\theta) \cdot (u^{\wedge})^2$	计算紧凑，复用 K 矩阵
《十四讲》通用形式	$\frac{\sin\theta}{\theta} \phi^{\wedge} + \frac{1 - \cos\theta}{\theta^2} (\phi^{\wedge})^2$	直接处理旋转向量
《十四讲》几何形式	$cos⁡θ⋅I+(1−cos⁡θ)⋅uuT+sin⁡θ⋅u∧\cos\theta \cdot I + (1 - \cos\theta) \cdot uu^T + \sin\theta \cdot u^{\wedge}$	几何意义最清晰

四、对数映射（Log）：旋转矩阵 → 旋转向量

对数映射是指数映射的逆运算，即从旋转矩阵 $R$ 中提取旋转向量 $ϕ=θ⋅u\phi = \theta \cdot u$ 。这是本文件中最精妙、数值技巧最丰富的部分。

4.1 完整代码

template<typename T>
Eigen::Matrix<T,3,1> Log(const Eigen::Matrix<T, 3, 3> &R)
{
    // 步骤一：计算旋转角度 θ（含数值保护）
    T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1));
    
    // 步骤二：提取向量 K = [R - R^T]^vee
    Eigen::Matrix<T,3,1> K(
        R(2,1) - R(1,2),   // 对应 x 分量：R_21 - R_12
        R(0,2) - R(2,0),   // 对应 y 分量：R_02 - R_20
        R(1,0) - R(0,1)    // 对应 z 分量：R_10 - R_01
    );
    
    // 步骤三：计算最终旋转向量（小角度近似 vs 精确公式）
    return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);
}

4.2 步骤一：计算旋转角度 $θ\theta$ 的由来

数学推导

对于任意旋转矩阵 $R$ ，其迹（对角线元素之和）满足：

$\text{trace}(R) = 1 + 2\cos\theta$

这个公式的证明可以从罗德里格斯公式推导。取形式三：

$\cos\theta \cdot I + (1 - \cos\theta) \cdot uu^T + \sin\theta \cdot u^{\wedge}$

两边同时取迹：

$\text{trace}(I) = 3$

$\text{trace}(uu^T) = u^T u = 1$

$\text{trace}(u^{\wedge}) = 0 \quad (\text{反对称矩阵迹为 } 0)$

因此：

$\text{trace}(R) = 3\cos\theta + (1 - \cos\theta) \cdot 1 = 1 + 2\cos\theta$

反解得：

$\theta = \arccos\left(\frac{\text{trace}(R) - 1}{2}\right)$

浮点数陷阱与数值保护

理想情况： 在完美的数学世界中， $trace(R)−12\frac{\text{trace}(R) - 1}{2}$ 的值域严格限制在 $[- 1, 1]$ 之间。

残酷现实： 当 $θ\theta$ 非常小（ $R$ 接近单位矩阵）时，由于浮点数舍入误差，计算出的 R.trace() 可能略微大于 3.0，例如：

理论值：trace(R) = 3.0
计算值：trace(R) = 3.0000000000000004

此时 $trace(R)−12=1.0000000000000002>1\frac{\text{trace}(R) - 1}{2} = 1.0000000000000002 > 1$ ，std::acos(1.0000000000000002) 会返回 nan（Not a Number），导致整个算法崩溃。

代码的防御策略：

T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1));

阈值设定： $3.0 - 10^{-6} = 2.999999$

如果 trace > 2.999999：矩阵“足够接近”单位矩阵，直接令 $θ=0.0\theta = 0.0$
如果 trace ≤ 2.999999：角度足够大，浮点误差影响小，安全调用 acos

场景	trace 值	theta 结果	说明
完美单位矩阵	3.0	0.0	直接返回
浮点误差导致 trace > 3	3.0000000004	0.0（被截断）	防止 nan
小角度旋转	2.999995	~0.00316	正常计算
大角度旋转	1.0	π	正常计算

4.3 步骤二：提取旋转轴方向 $R^T]^{\vee} = 2\sin\theta \cdot u$

数学推导（从罗德里格斯公式出发）

采用代码中的简化形式（形式一）：

$\sin\theta \cdot u^{\wedge} + (1 - \cos\theta) \cdot (u^{\wedge})^2$

计算 $R$ 的转置 $R^T$ ：

$(u^{\wedge})^T = -u^{\wedge} \quad (\text{反对称矩阵的性质})$

$((u^{\wedge})^2)^T = (u^{\wedge})^2 \quad (\text{平方后是对称矩阵})$

因此：

$R^T = I - \sin\theta \cdot u^{\wedge} + (1 - \cos\theta) \cdot (u^{\wedge})^2$

作差 $R - R^T$ ：

$\begin{aligned} R - R^T &= \left(I + \sin\theta u^{\wedge} + (1 - \cos\theta)(u^{\wedge})^2\right) \\ &\quad - \left(I - \sin\theta u^{\wedge} + (1 - \cos\theta)(u^{\wedge})^2\right) \\ &= 2\sin\theta \cdot u^{\wedge} \end{aligned}$

两边同时应用 $∨\vee$ 运算符（反对称矩阵 → 三维向量）：

$R^T]^{\vee} = 2\sin\theta \cdot u$

代码中的 K 是什么？

Eigen::Matrix<T,3,1> K(
    R(2,1) - R(1,2),   // 对应 x 分量：R_21 - R_12
    R(0,2) - R(2,0),   // 对应 y 分量：R_02 - R_20
    R(1,0) - R(0,1)    // 对应 z 分量：R_10 - R_01
);

观察矩阵 $R - R^T$ 展开后的非对角线元素：

$R^T = \begin{bmatrix} 0 & R_{01} - R_{10} & R_{02} - R_{20} \\ R_{10} - R_{01} & 0 & R_{12} - R_{21} \\ R_{20} - R_{02} & R_{21} - R_{12} & 0 \end{bmatrix}$

对于三维向量 $v = [v_0, v_1, v_2]^T$ ，其反对称矩阵为：

$v^{\wedge} = \begin{bmatrix} 0 & -v_2 & v_1 \\ v_2 & 0 & -v_0 \\ -v_1 & v_0 & 0 \end{bmatrix}$

提取 vee 时，取的是：

$v_0 = -(\text{矩阵的 } [1,2] \text{ 位置}) = R_{21} - R_{12}$

$v_1 = -(\text{矩阵的 } [2,0] \text{ 位置}) = R_{02} - R_{20}$

$v_2 = -(\text{矩阵的 } [0,1] \text{ 位置}) = R_{10} - R_{01}$

结论： 代码中的 K 就是 $R^T]^{\vee}$ ，即 $2\sin\theta \cdot u$ 。

4.4 步骤三：计算旋转向量 $ϕ=θ2sin⁡θ⋅[R−RT]∨\phi = \frac{\theta}{2\sin\theta} \cdot [R - R^T]^{\vee}$

数学推导

我们要求的是完整的旋转向量 $ϕ=θ⋅u\phi = \theta \cdot u$ 。由步骤二已知：

$R^T]^{\vee} = 2\sin\theta \cdot u$

因此：

$\frac{K}{2\sin\theta}$

两边同时乘以 $θ\theta$ ：

$\phi = \theta \cdot u = \frac{\theta}{2\sin\theta} \cdot K$

这就是步骤三的完整推导。它告诉我们：只要知道角度 $θ\theta$ 和向量 $K$ ，就能恢复出完整的旋转向量。

代码返回值的两条分支

return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);

分支一：大角度情况（精确公式）

当 $θ≥0.001\theta \geq 0.001$ 时，直接使用推导出的闭式解：

$\phi = \frac{\theta}{2\sin\theta} \cdot K$

代码中的 0.5 * theta / std::sin(theta) * K 正是这个公式的直译。

分支二：小角度近似（避免除零）

当 $θ<0.001\theta < 0.001$ 时，角度极微小。此时利用等价无穷小：

$\sin\theta \approx \theta$

代入精确公式：

$\frac{\theta}{2\sin\theta} \approx \frac{\theta}{2\theta} = \frac{1}{2}$

因此直接返回：

$\phi \approx \frac{1}{2} \cdot K$

代码中的 0.5 * K 就是这个近似。

为什么小角度近似是安全的？

$0.001$ 弧度 ≈ $0.057$ 度，这是一个极其微小的角度。在这个范围内， $sin⁡θ\sin\theta$ 与 $θ\theta$ 的相对误差小于 $1.6 \times 10^{-7}$ 。使用 $0.5$ 近似造成的误差远小于双精度浮点数的舍入误差。更重要的是，彻底避免了除零风险。

4.5 完整推导流程图

已知：旋转矩阵 R
     |
     v
计算迹 trace(R)
     |
     v
判断 trace > 3 - 1e-6 ?
  |
  |-- 是：theta = 0（小角度归零，防止 acos 越界）
  |
  |-- 否：theta = acos((trace-1)/2) （正常计算）
     |
     v
计算矩阵差 D = R - R^T
     |
     v
提取 vee 向量 K = D^vee （代码三行赋值：R21-R12, R02-R20, R10-R01）
     |
     v
判断 theta 的大小：
  |
  |-- 若 theta < 0.001：phi = 0.5 * K （小角度近似）
  |
  |-- 若 theta >= 0.001：phi = (theta / (2*sin(theta))) * K （精确公式）
     |
     v
返回旋转向量 phi

4.6 Log 函数的局限性（工程警示）

当 $θ≈π\theta \approx \pi$ （180°）时， $sin⁡θ≈0\sin\theta \approx 0$ ，此时 theta / sin(theta) 会急剧膨胀，导致数值不稳定。这个特定的 SO3_MATH.h 库并没有处理这一问题。在高端 SLAM 库（如 Sophus 或 MTK）中，会实现更复杂的 Log 映射，通常利用 atan2 确定角度，并特殊处理 $θ≈π\theta \approx \pi$ 的边界情况。

五、欧拉角提取

template<typename T>
Eigen::Matrix<T, 3, 1> RotMtoEuler(const Eigen::Matrix<T, 3, 3> &rot)
{
    T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0));
    bool singular = sy < 1e-6;
    T x, y, z;

    if(!singular)
    {
        x = atan2(rot(2, 1), rot(2, 2));  // 横滚角 Roll
        y = atan2(-rot(2, 0), sy);        // 俯仰角 Pitch
        z = atan2(rot(1, 0), rot(0, 0));  // 偏航角 Yaw
    }
    else
    {
        // 万向锁：设偏航角为 0
        x = atan2(-rot(1, 2), rot(1, 1));
        y = atan2(-rot(2, 0), sy);
        z = 0;
    }
    return {x, y, z};
}

5.1 旋转顺序

此函数假设旋转矩阵为 Z-Y-X 顺序（即先绕 X 轴转，再绕 Y 轴，最后绕 Z 轴）：

$R_z(\gamma) \cdot R_y(\beta) \cdot R_x(\alpha)$

x = 横滚角（Roll）—— 绕 X 轴
y = 俯仰角（Pitch）—— 绕 Y 轴
z = 偏航角（Yaw）—— 绕 Z 轴

5.2 万向锁处理

当 $\sqrt{R_{00}^2 + R_{10}^2} \approx 0$ 时， $β≈±90°\beta \approx \pm 90°$ ，此时 $α\alpha$ 和 $γ\gamma$ 无法唯一确定（万向锁）。处理方法：强制令 $γ=0\gamma = 0$ （偏航角归零），然后计算 $α=atan2(−R12,R11)\alpha = \text{atan2}(-R_{12}, R_{11})$ 。

六、数值稳定性总结

6.1 代码中的数值保护机制汇总

位置	保护机制	目的
Exp	`if (norm > 1e-7)`	避免除零，小角度直接返回 I
Log	`if (trace > 3 - 1e-6)`	防止 acos 输入 > 1 导致 nan
Log	`if (theta < 0.001)`	小角度避免除零（sinθ）
RotMtoEuler	`if (sy < 1e-6)`	检测万向锁，避免除零

6.2 潜在改进空间

Exp 和 Log 的阈值不一致：

Exp 使用 1e-7
Log 使用 1e-6（迹判断）和 0.001（近似判断）

可能导致 Exp(Log(R)) ≠ R 的微小不一致。

θ ≈ π 时的数值问题：

当旋转角度接近 180° 时， $sin⁡θ≈0\sin\theta \approx 0$ ，Log 中的 $θ/sin⁡θ\theta / \sin\theta$ 会放大误差。建议增加 $θ≈π\theta \approx \pi$ 的特殊处理分支。

宏 SKEW_SYM_MATRX 的风险：

无类型检查
参数可能被多次求值
调试困难

建议：统一使用 skew_sym_mat 函数版本。

6.3 与 MTK 数值方案的对比

特性	本文件	MTK mtkmath.hpp
小角度 Exp	硬截断（阈值 1e-7）	泰勒展开（连续逼近）
小角度 Log	硬截断 + 近似	泰勒展开
cos_sinc_sqrt	无	有（核心工具）
数值精度	工程级	高精度
适用场景	快速原型/嵌入式	精密状态估计

七、工程使用建议

7.1 适用场景

✅ 推荐使用：

快速原型验证
对精度要求不高的应用
嵌入式系统（代码体积小）
学习 SO(3) 数学原理

❌ 不推荐使用：

高精度 SLAM 后端优化
误差状态卡尔曼滤波（ESKF）
需要严格保持 Exp/Log 互逆性的场景

7.2 改进建议

添加 cos_sinc_sqrt 工具函数，统一管理小角度计算：

template<typename T>
std::pair<T, T> cos_sinc_sqrt(const T& x2) {
    // 小角度使用泰勒展开，大角度直接计算
    // 返回 (cosθ, sinθ/θ)
}

移除宏 SKEW_SYM_MATRX，统一使用 skew_sym_mat 函数。
统一 Exp 和 Log 的数值阈值，保证互逆性。
在 Log 中处理 $θ≈π\theta \approx \pi$ 的边界情况，避免除零。

八、总结

SO3_MATH.h 是一个麻雀虽小，五脏俱全的 SO(3) 数学库。通过逐行分析其源码，我们不仅掌握了旋转矩阵与旋转向量之间的转换算法，更重要的是理解了工业级数值计算的核心哲学：

罗德里格斯公式有三种等价形式，分别适用于不同场景：
- 代码实现形式：计算紧凑
- 通用形式：直接处理旋转向量
- 几何形式：物理意义最清晰
对数映射的本质：
- $R^T]^{\vee} = 2\sin\theta \cdot u$
- $ϕ=θ2sin⁡θ⋅K\phi = \frac{\theta}{2\sin\theta} \cdot K$
- 小角度时 $ϕ≈0.5⋅K\phi \approx 0.5 \cdot K$
数值稳定性是工业级代码的生命线：
- 一个小小的 acos 越界就能让整个 SLAM 系统崩溃
- 阈值判断、分段计算是防御性编程的标准手法
- 工程实现是数学理论、浮点误差防御与计算效率的权衡产物

理解这些底层实现细节，是写出健壮状态估计算法的关键。无论你后续是使用 Sophus、MTK 还是自己实现旋转矩阵运算，本文所揭示的数学原理和数值技巧都将长期受益。

所有评论(0)

查看更多评论

aiccc

@aiccc

已为社区贡献2条内容

SO(3) 数学库源码深度剖析：从罗德里格斯公式到数值稳定性

aiccc

一、文件概览

二、反对称矩阵：SO(3) 运算的基石

2.1 宏定义版本

2.2 函数版本

2.3 数学意义

三、指数映射（Exp）：旋转向量 → 旋转矩阵

3.1 版本一：右值引用版本

3.2 版本二：角速度 × 时间增量

3.3 版本三：三个标量分量

3.4 罗德里格斯公式的三种等价形式

形式一：代码中的简化形式（使用单位轴 uuu 和角度 θ\thetaθ）

形式二：《视觉SLAM十四讲》中的通用形式（直接使用旋转向量 ϕ\phiϕ）

形式三：《十四讲》中的几何分解形式

三种形式的等价性证明

四、对数映射（Log）：旋转矩阵 → 旋转向量

4.1 完整代码

4.2 步骤一：计算旋转角度 θ\thetaθ 的由来

数学推导

浮点数陷阱与数值保护

4.3 步骤二：提取旋转轴方向 [R−RT]∨=2sin⁡θ⋅u[R - R^T]^{\vee} = 2\sin\theta \cdot u[R−RT]∨=2sinθ⋅u

数学推导（从罗德里格斯公式出发）

代码中的 K 是什么？

4.4 步骤三：计算旋转向量 ϕ=θ2sin⁡θ⋅[R−RT]∨\phi = \frac{\theta}{2\sin\theta} \cdot [R - R^T]^{\vee}ϕ=2sinθθ​⋅[R−RT]∨

数学推导

代码返回值的两条分支

为什么小角度近似是安全的？

4.5 完整推导流程图

4.6 Log 函数的局限性（工程警示）

五、欧拉角提取

5.1 旋转顺序

5.2 万向锁处理

六、数值稳定性总结

6.1 代码中的数值保护机制汇总

6.2 潜在改进空间

6.3 与 MTK 数值方案的对比

七、工程使用建议

7.1 适用场景

7.2 改进建议

八、总结

延伸阅读

所有评论(0)

温馨提示：您尚未绑定手机号

aiccc

形式一：代码中的简化形式（使用单位轴 $u$ 和角度 $θ\theta$ ）

形式二：《视觉SLAM十四讲》中的通用形式（直接使用旋转向量 $ϕ\phi$ ）

4.2 步骤一：计算旋转角度 $θ\theta$ 的由来

4.3 步骤二：提取旋转轴方向 $R^T]^{\vee} = 2\sin\theta \cdot u$

4.4 步骤三：计算旋转向量 $ϕ=θ2sin⁡θ⋅[R−RT]∨\phi = \frac{\theta}{2\sin\theta} \cdot [R - R^T]^{\vee}$