Sophus SO3::exp

Sophus库，标准的 SO3::exp 源码文件。基于罗德里格斯公式（Rodrigues' formula）的反对称矩阵形式。

基于 Sophus 库经典实现（非模板类版本）的 SO3::exp 函数完整源码解析：

📖 `SO3::exp` 函数完整源码与逐行注释

cpp

复制代码

// 假设命名空间和类型定义如下
// namespace Sophus {
// using Vector3d = Eigen::Vector3d;
// using Matrix3d = Eigen::Matrix3d;

static SO3 exp(const Vector3d& omega) {
    // 1. 计算输入旋转向量 omega 的模长（即旋转角度 theta）
    double theta = omega.norm();

    // 2. 初始化旋转矩阵为 3x3 的单位阵
    Matrix3d R = Matrix3d::Identity();

    // 3. 处理接近零旋转的情况（小角度近似），避免数值问题
    //    当 theta 小于一个很小的阈值（如 1e-10）时，使用泰勒展开近似
    if (theta < Sophus::Constants<double>::epsilon()) {
        // 3.1 使用一阶泰勒展开：R ≈ I + [omega]_x
        //    这在优化迭代的微小步长中非常常见且高效
        R += SO3::hat(omega);
        return SO3(R);
    }

    // 4. 一般情况：使用精确的罗德里格斯公式
    //    4.1 计算单位旋转轴向量 a = omega / theta
    Vector3d a = omega / theta;

    //    4.2 计算罗德里格斯公式中的系数
    //        c1 = sin(theta) / theta
    //        c2 = (1 - cos(theta)) / (theta^2)
    double c1 = sin(theta) / theta;
    double c2 = (1 - cos(theta)) / (theta * theta);

    //    4.3 构造旋转向量 omega 的反对称矩阵 [omega]_x
    //        hat(omega) = [omega]_x
    Matrix3d omega_hat = SO3::hat(omega);

    //    4.4 应用罗德里格斯公式计算旋转矩阵
    //        R = I + c1 * [omega]_x + c2 * [omega]_x^2
    //        这是指数映射 exp([omega]_x) 的闭式解
    R = Matrix3d::Identity() + c1 * omega_hat + c2 * (omega_hat * omega_hat);

    // 5. 返回构造的 SO3 对象
    return SO3(R);
}

💡 核心逻辑解读

输入与预处理 ：函数接收一个三维向量 omega（李代数 so(3)），并计算其模长 theta，即旋转角度。
小角度处理（数值稳定性）：
- 当 theta 非常小时，直接使用 sin 和 cos 会因theta在分母而产生数值不稳定的问题（如除零）。
- 因此，代码采用一阶泰勒展开 R ≈ I + [omega]_x 进行近似。这正是在高斯-牛顿法中，我们计算雅可比矩阵时使用的线性化模型。
一般情况（精确计算）：
- 计算单位旋转轴 a。
- 计算罗德里格斯公式的两个关键系数 c1 和 c2。
- 构造 omega 的反对称矩阵 omega_hat。
- 最终，通过 I + c1 * omega_hat + c2 * (omega_hat * omega_hat) 精确计算出旋转矩阵 R。

这个实现完美地体现了我们之前讨论的所有数学原理：它将李代数 so(3) 中的旋转向量，通过指数映射 ，精确地转换为了李群 SO(3) 中的旋转矩阵。

📖 SO3::exp 函数完整源码与逐行注释

💡 核心逻辑解读

📖 `SO3::exp` 函数完整源码与逐行注释