视觉SLAM中Sophus库SO(3)与SE(3)位姿表示及优化实战指南

1. 视觉SLAM里的位姿表示为什么绕不开Sophus做视觉SLAM的人迟早会撞上李群李代数这堵墙。相机位姿、地图点变换、优化过程中的增量更新全都离不开旋转矩阵和平移向量的组合。刚开始你可能觉得用Eigen::Matrix3d和Eigen::Vector3d拼一拼就够了但一旦进入后端优化需要求导、需要迭代更新、需要处理约束裸矩阵那套写法很快就会让你崩溃。Sophus这个库就是来解决这个问题的。它把SO(3)和SE(3)这两个李群封装成了类似矩阵的类让你能像操作普通矩阵一样做乘法、求逆、取对数、求伴随同时还能通过指数映射在流形上做无约束优化。说白了它把“旋转矩阵必须正交且行列式为1”这个约束通过李代数参数化给消掉了优化器只需要在三维或六维向量空间里随便跑映射回去自然满足约束。这篇文章适合谁看如果你正在学视觉SLAM已经写过简单的PnP或者ICP但一到g2o、Ceres或者自己手写高斯牛顿的时候就被Sophus::SO3d的各种API搞晕那这篇就是给你准备的。我会把SO(3)和SE(3)的构造、更新、求导、常见坑点全部拆开讲一遍代码可以直接抄。2. Sophus的安装与工程集成别在第一步卡住2.1 安装方式选择与依赖处理Sophus本身是header-only的模板库但它的编译和安装过程有几个容易踩的坑。最稳妥的方式是源码编译安装因为不同版本对Eigen和fmt的依赖不一样。git clone https://github.com/strasdat/Sophus.git cd Sophus mkdir build cd build cmake .. -DCMAKE_BUILD_TYPERelease -DBUILD_SOPHUS_TESTSOFF make -j4 sudo make install这里有几个关键点。第一BUILD_SOPHUS_TESTS建议关掉测试用例编译很慢而且容易因为fmt版本问题报错。第二Sophus新版本依赖fmt库如果系统里没有先装libfmt-dev。第三如果你用的是ROS环境注意ROS自带的Sophus可能版本很老API和新版有差异建议统一用源码安装的版本在CMakeLists里显式指定路径。在CMakeLists.txt里的集成方式find_package(Sophus REQUIRED) find_package(Eigen3 REQUIRED) include_directories(${Sophus_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) target_link_libraries(your_target ${Sophus_LIBRARIES})注意Sophus的CMake配置文件在不同版本里导出的变量名可能不同有的是Sophus_INCLUDE_DIRS有的是Sophus_INCLUDE_DIR。如果find_package成功但include报错用message(STATUS ${Sophus_INCLUDE_DIRS})打印一下确认。2.2 头文件包含与命名空间约定Sophus的核心头文件就两个#include sophus/so3.hpp和#include sophus/se3.hpp。所有类型都在Sophus命名空间下。常用的类型别名有Sophus::SO3d双精度SO(3)内部用四元数存储Sophus::SE3d双精度SE(3)内部用四元数加平移向量存储Sophus::SO3f、Sophus::SE3f单精度版本嵌入式场景用我个人的习惯是在文件开头加using namespace Sophus;但如果你在大型项目里建议还是老老实实写Sophus::SO3d避免和别的库冲突。3. SO(3)的核心用法旋转的构造、更新与求导3.1 四种构造方式与适用场景SO(3)表示三维旋转Sophus提供了多种构造入口每种对应不同的数据来源// 方式一从旋转矩阵构造最常用 Eigen::Matrix3d R Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitZ()).toRotationMatrix(); Sophus::SO3d SO3_R(R); // 方式二从四元数构造 Eigen::Quaterniond q(R); Sophus::SO3d SO3_q(q); // 方式三从旋转向量轴角构造内部自动做指数映射 Eigen::Vector3d omega(0, 0, M_PI/4); Sophus::SO3d SO3_exp Sophus::SO3d::exp(omega); // 方式四直接构造单位旋转 Sophus::SO3d SO3_I; // 默认是单位旋转这四种方式里exp是最需要理解的。它把李代数so(3)里的三维向量映射到李群SO(3)上物理意义是“绕轴omega旋转|omega|角度”。在优化过程中我们求出来的增量就是这种三维向量用exp映射回去更新位姿。实操心得从旋转矩阵构造时Sophus内部会做正交化处理但如果你给的矩阵离正交矩阵太远比如数值误差累积很大结果可能不是你预期的。我一般会在构造前用SVD重新正交化一下尤其是从优化器里拿到的矩阵。3.2 对数映射与旋转向量提取反过来从SO(3)取李代数用log()Sophus::SO3d SO3_R(R); Eigen::Vector3d omega SO3_R.log();这里有个细节log()返回的向量方向是旋转轴模长是旋转角度角度范围在[0, π]。当旋转角度接近π时数值上会有点不稳定因为轴的方向会翻转。实际用的时候如果发现log结果跳变检查一下是不是接近奇异点。另外Sophus还提供了SO3_R.angle()直接返回旋转角度SO3_R.axis()返回单位旋转轴比手动从log结果里算方便。3.3 乘法、逆与伴随矩阵SO(3)的乘法就是旋转的复合Sophus::SO3d R1 Sophus::SO3d::exp(Eigen::Vector3d(0.1, 0.2, 0.3)); Sophus::SO3d R2 Sophus::SO3d::exp(Eigen::Vector3d(0.4, 0.5, 0.6)); Sophus::SO3d R12 R1 * R2; // 先R2后R1的旋转 Sophus::SO3d R1_inv R1.inverse();伴随矩阵Adj()在推导雅可比和坐标系变换时经常用到Eigen::Matrix3d Adj R1.Adj();伴随矩阵满足R * exp(omega) * R^T exp(Adj * omega)这个性质在推导扰动模型的时候是核心工具。很多教程讲到这里一笔带过但你如果自己推一遍SO(3)上的右扰动雅可比就会发现伴随矩阵无处不在。3.4 在流形上做增量更新这是Sophus在SLAM里最核心的用法。优化器每次迭代算出一个三维增量delta然后更新旋转Sophus::SO3d R_updated Sophus::SO3d::exp(delta) * R_current; // 左乘更新 // 或者 Sophus::SO3d R_updated R_current * Sophus::SO3d::exp(delta); // 右乘更新左乘和右乘的区别在于扰动定义在哪个坐标系。左乘对应“在世界坐标系下施加扰动”右乘对应“在局部坐标系下施加扰动”。视觉SLAM里两种都有用取决于你的雅可比是怎么推的。我个人的经验是BA优化里用右乘更直观因为增量直接对应相机局部坐标系下的微小旋转。常见坑千万不要写成R_current delta这种裸矩阵加法结果不再是旋转矩阵后续所有计算全错。必须通过exp映射回SO(3)再乘。4. SE(3)的实战用法位姿变换与优化更新4.1 SE(3)的构造与数据访问SE(3)表示刚体变换包含旋转和平移// 从旋转矩阵和平移向量构造 Eigen::Matrix3d R Eigen::Matrix3d::Identity(); Eigen::Vector3d t(1, 2, 3); Sophus::SE3d SE3_Rt(R, t); // 从四元数和平移向量构造 Eigen::Quaterniond q(R); Sophus::SE3d SE3_qt(q, t); // 从李代数构造六维向量前三维平移后三维旋转 Eigen::Matrixdouble, 6, 1 xi; xi 1, 2, 3, 0.1, 0.2, 0.3; Sophus::SE3d SE3_exp Sophus::SE3d::exp(xi);访问内部数据Eigen::Matrix3d rotation_matrix SE3_Rt.rotationMatrix(); Eigen::Quaterniond quaternion SE3_Rt.unit_quaternion(); Eigen::Vector3d translation SE3_Rt.translation(); Eigen::Matrix4d matrix4x4 SE3_Rt.matrix(); // 4x4齐次矩阵这里要注意matrix()返回的是4x4矩阵而rotationMatrix()只返回3x3旋转部分。在给OpenGL或者别的库传数据的时候别搞混了。4.2 点变换与坐标系变换SE(3)作用在三维点上Eigen::Vector3d p(1, 0, 0); Eigen::Vector3d p_transformed SE3_Rt * p;这个操作等价于R * p t但Sophus内部做了优化比手动展开快。如果你要批量变换大量点建议用SE3_Rt.matrix()取出4x4矩阵然后做齐次坐标乘法比逐个调用operator*效率高。坐标系变换用乘法Sophus::SE3d T_wc; // 世界到相机 Sophus::SE3d T_cw T_wc.inverse(); // 相机到世界 Sophus::SE3d T_wa T_wb * T_ba; // 链式变换注意SE(3)的乘法顺序不能随便换。T1 * T2表示先做T2的变换再做T1的变换。在SLAM里写坐标变换的时候建议在变量名里把坐标系标清楚比如T_wc表示world到camera这样不容易搞反。4.3 SE(3)上的扰动模型与雅可比SE(3)的优化更新和SO(3)类似但增量是六维的Eigen::Matrixdouble, 6, 1 delta; delta 0.01, 0.02, 0.03, 0.001, 0.002, 0.003; Sophus::SE3d T_updated Sophus::SE3d::exp(delta) * T_current;在推导雅可比的时候SE(3)的伴随矩阵是6x6的Eigen::Matrixdouble, 6, 6 Adj T_current.Adj();这个伴随矩阵在把扰动从一个坐标系转到另一个坐标系时非常关键。比如你在世界坐标系下定义了扰动但雅可比是在相机坐标系下算的就需要用伴随矩阵做转换。4.4 实际优化中的更新策略对比在视觉SLAM的BA优化里SE(3)的更新有两种常见策略策略更新方式优点缺点左乘更新T_new exp(delta) * T_old扰动在世界坐标系推导直观需要额外处理伴随矩阵右乘更新T_new T_old * exp(delta)扰动在局部坐标系雅可比形式简洁需要明确局部坐标系定义我实测下来在Ceres里用右乘更新更顺手因为Ceres的自动求导对局部参数化支持很好Sophus::SE3d可以直接配合LocalParameterization使用。在g2o里两种都有现成的顶点类型看你的边是怎么定义的。5. 常见报错与排查技巧实录5.1 编译期问题速查报错信息原因解决方法fatal error: sophus/so3.hpp: No such file头文件路径没配检查CMakeLists里的include路径undefined reference to Sophus::SO3d::exp链接库没加确认target_link_libraries包含Sophusstatic assertion failed: YOU_MIXED_MATRICES_OF_DIFFERENT_SIZESEigen矩阵维度不匹配检查旋转矩阵是不是3x3平移是不是3x1fmt/core.h: No such file缺少fmt依赖安装libfmt-dev或源码编译fmt5.2 运行期数值问题排查问题一优化不收敛位姿乱飞。最常见的原因是增量更新写错了。检查是不是直接用了T delta而不是T * exp(delta)。另一个可能是雅可比的符号搞反了导致优化器往错误方向走。我一般会先固定住其他变量只优化一个位姿看能不能收敛到正确值逐步排查。问题二log()结果跳变。当旋转角度接近π时log的轴方向会突然翻转。这不是bug是李代数在π处的固有奇异性。解决方法是在接近π时用四元数插值或者双覆盖处理或者干脆避免让旋转角度累积到π附近。问题三SE(3)的exp和log不对应。检查你用的李代数向量顺序。Sophus的SE(3)李代数前三维是平移后三维是旋转但有些教材写的是反过来的。如果你从别的地方抄的代码一定要确认顺序。独家避坑在写单元测试的时候一定要加一条T * T.inverse() Identity的检查以及exp(log(T)) T的往返测试。这两个测试能帮你抓住90%的API误用问题。5.3 性能相关的注意事项Sophus的exp和log涉及三角函数和矩阵运算在优化内循环里调用频率很高。如果你发现优化速度慢可以考虑以下优化用单精度SO3f和SE3f替代双精度速度能快一倍左右精度损失在视觉SLAM里通常可以接受避免在循环里反复构造SO(3)对象尽量复用如果只需要旋转矩阵直接用rotationMatrix()不要先转四元数再转矩阵6. 从SO(3)到SE(3)的进阶理解6.1 两者的代数结构关系SO(3)是SE(3)的子群SE(3)可以看成SO(3)和R^3的半直积。这个数学结构决定了SE(3)的李代数不是简单的平移加旋转而是有一个耦合项。具体来说SE(3)的exp映射里平移部分会被旋转部分影响// SE(3)的exp不是简单地把平移和旋转分开exp再拼起来 // 内部有V矩阵耦合这个耦合在推导雅可比的时候必须考虑否则优化会出问题。很多初学者在这里翻车以为SE(3)就是SO(3)加个平移向量结果雅可比推错了。6.2 在视觉SLAM中的典型应用场景场景用的群典型操作相机位姿表示SE(3)构造、求逆、乘法特征点三角化SE(3)点变换BA优化SE(3)exp更新、伴随矩阵IMU预积分SO(3)SE(3)旋转更新、雅可比传递回环检测位姿图SE(3)相对变换、优化6.3 和其他库的互操作Sophus和Eigen是无缝衔接的但和ROS的tf、geometry_msgs转换时需要手动处理// Sophus转ROS geometry_msgs::Pose pose; pose.position.x SE3.translation().x(); // ... 四元数赋值反过来从ROS消息构造Sophus对象也很直接。关键是注意ROS用的四元数顺序是(x, y, z, w)而Eigen的构造函数也是这个顺序但有些库用的是(w, x, y, z)别搞混。7. 我踩过的那些坑和最后的小技巧第一个坑是版本兼容性。Sophus在2020年前后的API有过一次大改老版本的SO3d::exp返回类型和新版本不一样如果你从网上抄的代码编译不过先检查版本。我现在的习惯是在CMakeLists里锁定Sophus的版本号避免不同机器上行为不一致。第二个坑是SE3d::exp的输入向量顺序。我至少见过三种不同的约定平移在前旋转在后、旋转在前平移在后、还有把平移除以旋转角度再放的。Sophus用的是平移在前旋转在后但如果你从论文里抄公式一定要确认论文用的哪种约定。第三个坑是优化过程中的数值稳定性。当旋转增量很小时exp的泰勒展开和直接计算会有微小差异长时间迭代可能累积误差。我的做法是每隔几百次迭代把SE(3)重新正交化一次用SVD把旋转矩阵投影回SO(3)。最后分享一个小技巧如果你只需要旋转不要平移用SO(3)比SE(3)快很多因为SE(3)的exp和log涉及更复杂的矩阵运算。在IMU预积分里旋转部分单独用SO(3)处理平移部分单独算比整体用SE(3)效率高不少。这个优化在我自己的VIO系统里实测能省15%左右的计算时间。