用C++代码和Sophus库（2023版）彻底搞懂李群李代数

很多学习视觉SLAM的朋友第一次接触李群李代数时，都会被各种数学符号和抽象概念搞得晕头转向。SO3、SE3、指数映射、BCH公式...这些名词看起来高深莫测，但实际上它们都可以通过代码来直观理解。本文将带你用最新版Sophus库（2023.9.7）的C++代码，把这些抽象概念变成可以运行、调试的实际程序。

1. 为什么需要李群李代数

在三维空间刚体运动描述中，旋转是最基本的操作之一。传统欧拉角存在万向节锁问题，旋转矩阵又有冗余参数（9个参数描述3自由度）。四元数虽然解决了部分问题，但在连续运动和优化中仍不够直观。

李群李代数提供了一种优雅的解决方案：

• 李群 ：连续的平滑流形，如SO3（旋转群）、SE3（刚体运动群）
• 李代数 ：对应李群在单位元处的切空间，如so3、se3

它们之间的关系就像指数函数和对数函数：

SO3 = exp(so3)
so3 = log(SO3)

这种表示在SLAM中有三大优势：

1. 无奇异性 ：避免了欧拉角的万向节锁问题
2. 最小参数化 ：so3用3维向量，se3用6维向量
3. 便于优化 ：可以在切空间中进行导数计算

2. Sophus库基础与2023版变化

Sophus是目前最流行的李群李代数C++实现，基于Eigen开发。2023版有几个重要变化需要注意：

// 旧版构造方式（已废弃）
Sophus::SO3d SO3_v(0, 0, M_PI/2); 

// 新版正确构造方式
Eigen::Vector3d so3(0, 0, M_PI/2);
Sophus::SO3d SO3 = Sophus::SO3d::exp(so3);

主要API变化对照表：

功能	旧版API	2023版API
SO3构造	直接向量构造	必须通过exp映射
输出重载	支持<<操作符	取消<<操作符
模板参数	较少类型别名	增加类型安全

基础使用示例：

#include <sophus/so3.hpp>
#include <sophus/se3.hpp>

// 初始化一个绕Z轴旋转90度的SO3
Eigen::Vector3d so3(0, 0, M_PI/2);
Sophus::SO3d SO3 = Sophus::SO3d::exp(so3);

// 初始化一个SE3：旋转同上，平移(1,0,0)
Eigen::Vector3d t(1, 0, 0);
Sophus::SE3d SE3(SO3, t);

3. 从代码理解核心概念

3.1 指数映射与对数映射

指数映射将李代数映射到李群，对数映射则是其逆过程：

// so3 -> SO3
Eigen::Vector3d so3(0.1, 0.2, 0.3);
Sophus::SO3d SO3 = Sophus::SO3d::exp(so3);

// SO3 -> so3
Eigen::Vector3d so3_recovered = SO3.log();

// se3 -> SE3
Eigen::Vector6d se3(0.1, 0.1, 0.1, 0.3, 0.2, 0.1);
Sophus::SE3d SE3 = Sophus::SE3d::exp(se3);

// SE3 -> se3
Eigen::Vector6d se3_recovered = SE3.log();

数学上，SO3的指数映射对应罗德里格斯公式：

exp(φ^) = I + sinθ/θ φ^ + (1-cosθ)/θ² φ^²

其中θ=||φ||，φ^是φ的反对称矩阵

3.2 扰动模型与导数

李代数的一个重要应用是在优化中表示扰动：

// 原始位姿
Sophus::SE3d T;

// 扰动
Eigen::Vector6d xi;
xi << 0.01, 0.02, 0.03, 0.1, 0.1, 0.1;

// 应用扰动
Sophus::SE3d T_perturbed = Sophus::SE3d::exp(xi) * T;

这个操作在SLAM的位姿优化中非常关键，对应到BCH公式的近似：

exp(Δξ) · exp(ξ) ≈ exp(ξ + Jr⁻¹(ξ)Δξ)

3.3 伴随矩阵

伴随矩阵用于变换坐标系下的李代数：

Sophus::SE3d T;
Eigen::Matrix6d Ad_T = T.Adj();

// 使用伴随矩阵变换李代数
Eigen::Vector6d xi;
Eigen::Vector6d xi_transformed = Ad_T * xi;

伴随矩阵的数学形式：

Ad_T = [ R   t^R
         0     R ]

4. 实战：用代码验证SLAM理论

让我们用Sophus验证几个SLAM教材中的经典结论。

4.1 验证SO3的指数映射

根据理论，旋转向量φ=(0,0,π/2)应该对应绕Z轴90度的旋转：

Eigen::Vector3d phi(0, 0, M_PI/2);
Sophus::SO3d R = Sophus::SO3d::exp(phi);

Eigen::Matrix3d R_matrix = R.matrix();
// 应该近似于：
// [0 -1  0]
// [1  0  0]
// [0  0  1]

4.2 验证SE3的指数映射

对于se3 ξ=[ρ,φ]，其中ρ=(1,0,0)，φ=(0,0,π/2)，对应的SE3应为：

Eigen::Vector6d xi;
xi << 1, 0, 0, 0, 0, M_PI/2;
Sophus::SE3d T = Sophus::SE3d::exp(xi);

Eigen::Matrix4d T_matrix = T.matrix();
// 旋转部分应与SO3例子相同
// 平移部分应为 [1, 1, 0]（注意Jρ效应）

4.3 验证BCH公式近似

小扰动时，BCH公式可以线性近似：

Sophus::SO3d R = Sophus::SO3d::exp(Eigen::Vector3d(0.1, 0.2, 0.3));
Eigen::Vector3d w(0.01, 0.02, 0.03);

// 直接组合
Sophus::SO3d R1 = Sophus::SO3d::exp(w) * R;

// BCH近似
Eigen::Matrix3d Jr = Sophus::SO3d::leftJacobian(w);
Sophus::SO3d R2 = Sophus::SO3d::exp(w + Jr.inverse() * R.log());

// 比较两者的差异
double error = (R1.matrix() - R2.matrix()).norm();

5. 常见陷阱与最佳实践

在实际使用Sophus时，有几个容易踩坑的地方：

1. 构造函数变化 ：2023版取消了直接向量构造SO3的方式

   // 错误！新版不再支持
   // Sophus::SO3d R(0.1, 0.2, 0.3);
   
   // 正确方式
   Sophus::SO3d R = Sophus::SO3d::exp(Eigen::Vector3d(0.1, 0.2, 0.3));
   

2. 归一化问题 ：从旋转矩阵构造SO3时需确保矩阵正交

   Eigen::Matrix3d R = ...;
   // 最好先做QR分解确保正交性
   R = Eigen::Quaterniond(R).normalized().toRotationMatrix();
   Sophus::SO3d SO3_R(R);
   

3. 性能优化 ：频繁调用exp/log时考虑预计算Jacobian

   // 低效方式
   for(int i=0; i<N; ++i) {
       Sophus::SO3d::exp(v[i]);
   }
   
   // 高效方式：预计算Jacobian
   Eigen::Matrix3d J = Sophus::SO3d::leftJacobian(v);
   for(int i=0; i<N; ++i) {
       Sophus::SO3d::exp(J * v[i]);
   }
   

4. 类型安全 ：新版Sophus加强了类型检查

   // 混用float/double会报错
   Sophus::SO3f R_f;
   // Sophus::SO3d R_d = R_f; // 错误！
   Sophus::SO3d R_d = R_f.cast<double>(); // 正确
   

6. 进阶应用：SLAM中的实际案例

最后，我们看一个ORB-SLAM3中IMU预积分的实际应用片段：

// 近似计算小旋转的指数映射和右雅可比
const float eps = 1e-4;
Eigen::Vector3f w(x, y, z);
float d = w.norm();

if(d < eps) {
    // 小值近似
    deltaR = Eigen::Matrix3f::Identity() + Sophus::SO3f::hat(w);
    rightJ = Eigen::Matrix3f::Identity();
} else {
    // 精确计算
    float d2 = d * d;
    Eigen::Matrix3f W = Sophus::SO3f::hat(w/d);
    deltaR = Eigen::Matrix3f::Identity() 
           + sin(d) * W 
           + (1-cos(d)) * W * W;
           
    rightJ = Eigen::Matrix3f::Identity() 
           - (1-cos(d))/d2 * W 
           + (d-sin(d))/(d2*d) * W * W;
}

这段代码展示了：

1. 小旋转量的高效近似
2. 右雅可比矩阵的计算
3. Sophus的hat算子应用

在实际项目中，类似的代码用于IMU预积分中的协方差传播，是紧耦合VIO系统的核心之一。