简介Sophus非模板类库是一套C三维几何运算库的源码与解析资料采用非模板设计让SO(3)旋转群和SE(3)特殊欧几里得群的表示与运算更为简洁直接同时允许用户自行选择float或double进行实例化降低编译复杂度。这套资料面向机器人学、SLAM、计算机视觉与三维重建开发者尤其适合需要理解刚体变换数学原理并快速上手落地的C工程师。压缩包共46个文件大小约9.55MB主要包含12个cpp核心实现、6个h头文件、11个sample示例、README说明及CMake配置脚本目录清晰方便直接阅读和编译验证。内容深入梳理了四元数与旋转矩阵互转、平移旋转组合、逆运算、雅可比矩阵计算等关键功能并结合测试用例展示各类变换的边界与误差校验方式。已有218人学习下载对于希望系统掌握SO(3)/SE(3)变换并提升三维几何编程能力的开发者是一份兼具理论解析与可运行源码的实用参考。 前阵子帮一个师弟排查视觉里程计代码的编译问题。他下载的项目里用的还是老版Sophus模板接口满屏的Sophus::SO3double、Sophus::SE3double配合Eigen模板报错光是看编译日志就够喝一壶。而我自己项目里用的是后来重构的那套非模板类库——同样干着SO(3)、SE(3)这些李群李代数的活代码却简洁直接定位问题也快得多。这篇就想聊聊非模板版Sophus的设计思路、日常用法和工程集成细节给正在接触或者准备用Sophus的人一个参考。Sophus这个库做视觉SLAM、机器人状态估计、位姿图优化的朋友应该都不陌生。它主要提供SO(2)、SO(3)、SE(2)、SE(3)、Sim(3)这些李群李代数类型的实现底层依赖Eigen作用就是把旋转矩阵、变换矩阵和对应的李代数向量之间的指数映射、对数映射、扰动模型这些“数学”变成“代码”。如果你要手写BA优化、做位姿插值、或者维护一个里程计系统Sophus基本是绕不开的工具。这篇文章适合正在学习李群李代数、想把理论落到代码里的同学也适合已经在项目里用过老版Sophus、想迁移到新版的人。1. 从模板类到非模板类库这次重构到底在解决什么问题1.1 老版模板接口的尴尬Sophus早期版本是典型的模板类设计核心类长这样template typename Scalar class SO3 { ... };于是使用的时候就得写Sophus::SO3double、Sophus::SE3float这种类型。看起来多了一个自由度好像能支持任意标量类型但实际项目里绝大多数人只用double这个模板参数并没有带来多少实际收益反而造成三个问题。第一编译时间明显变长。模板类的实例化会在头文件展开Eigen本身又是一堆模板两层套在一起每次编译几乎都在等待。第二报错信息极其可怕。模板嵌套一旦出错编译器会吐出几百行类型推导错误新手基本看不懂老手也要盯半天。第三代码本身变得很啰嗦。函数签名里到处是Sophus::SO3double稍微复杂一点的函数类型名比变量名还长读起来非常吃力。1.2 非模板化之后收敛成什么样后来Sophus做了一次大重构核心库从模板类变成了非模板类库标量类型固定为double。现在常见用法是直接用Sophus::SO3d、Sophus::SE3d。Sophus::SO3d R Sophus::SO3d::rotX(0.3); Sophus::SE3d T Sophus::SE3d::rotX(0.2) * Sophus::SE3d::trans(1.0, 0.0, 0.0);类型名短了头文件里的模板推导少了编译速度上来了报错信息也恢复正常水平。重构之后代码量肉眼可见地减少阅读和维护难度都降低不少。对于做工程的人来说这种“少一点灵活性、多一点实用性”的取舍我觉得是正确方向——毕竟在SLAM这种场景里几乎没有人需要float精度去跑位姿优化。1.3 新旧版本如何共存旧版代码并没有被彻底删除Sophus保留了旧版头文件的兼容路径。你依然可以在项目里#include sophus/old/so3.hpp来使用模板版本。这意味着老项目想平滑过渡并不需要一次性重写所有代码可以逐步迁移。但我的建议是新项目直接上非模板版老项目如果没有特殊的跨平台多精度需求也尽早迁过来。一套代码里同时混用新旧两个版本虽然能编译通过但开发体验会非常别扭——新旧接口的命名习惯不太一样混着用容易记混而且排查问题时多了一层类型转换的干扰。2. SO3d和SE3d的日常操作看完就能直接上手2.1 SO3d的构造、转换与常用方法SO3d表示三维旋转李群内部用Eigen四元数存储旋转。构造方式非常灵活可以直接从旋转矩阵、四元数、轴角或者旋转向量构建#include sophus/so3.hpp #include Eigen/Geometry // 从旋转矩阵构造 Eigen::Matrix3d M Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()).matrix(); Sophus::SO3d R1(M); // 从四元数构造 Eigen::Quaterniond q(0.966, 0.0, 0.0, 0.259); Sophus::SO3d R2(q); // 从轴角构造 Sophus::SO3d R3 Sophus::SO3d::exp(Eigen::Vector3d(0.0, 0.0, 0.5));旋转矩阵和四元数在代码里经常需要来回切SO3d提供了对应的转换方法Sophus::SO3d R; Eigen::Matrix3d mat R.matrix(); // 转为旋转矩阵 Eigen::Quaterniond quat R.unit_quaternion(); // 转回四元数 Eigen::Vector3d so3 R.log(); // 转为李代数向量旋转向量这里要特别提醒unit_quaternion()返回的是四元数的引用如果你用auto去接注意别做不必要的拷贝。另外SO3d内部会维护四元数的单位范数约束所以尽量不要通过直接修改四元数内部数据的方式去改旋转而是通过构造新对象或者用exp更新。2.2 SE3d的位姿操作与点的变换SE3d是三维刚体变换李群内部包含旋转部分和平移部分构造方式一般用四元数加平移向量或者直接用旋转矩阵加平移向量#include sophus/se3.hpp Eigen::Quaterniond q(0.966, 0.0, 0.0, 0.259); Eigen::Vector3d t(1.0, 2.0, 3.0); Sophus::SE3d T(q, t);位姿的分解和合成也很直观Eigen::Vector3d trans T.translation(); // 平移向量 Eigen::Matrix3d rot T.rotationMatrix(); // 旋转矩阵 Eigen::Matrix4d mat T.matrix(); // 4x4齐次变换矩阵SE3d对点的变换有两种写法一种是直接用operator*操作三维点一种是转换齐次坐标后用matrix()去乘。前者在代码里更常见Eigen::Vector3d p(1.0, 0.0, 0.0); Eigen::Vector3d p_transformed T * p; // 变换后的点注意SE3d的operator*不会隐式做齐次化再变回三维它对普通三维点的处理方式符合直觉。不要因为矩阵乘法习惯去写T.matrix() * p.homogeneous()那也不是不行只是代码更啰嗦而且如果p的维度不匹配容易出错。2.3 从李代数角度看log/exp/hat/vee怎么配合使用李群和李代数之间的指数映射、对数映射是Sophus最核心的价值。exp把李代数向量映射到李群log把李群映射回李代数向量Eigen::Vector3d omega(0.1, 0.2, 0.3); // 旋转向量李代数 Sophus::SO3d R Sophus::SO3d::exp(omega); // 指数映射 Eigen::Vector3d so3 R.log(); // 对数映射 // SE3的扩展6维李代数向量 Eigen::Matrixdouble, 6, 1 xi; // 前3维平移后3维旋转 Sophus::SE3d T Sophus::SE3d::exp(xi); Eigen::Matrixdouble, 6, 1 se3 T.log();在优化代码里比较常见的模式是用SE3d::exp(delta) * T做左扰动更新Eigen::Matrixdouble, 6, 1 delta; delta.setZero(); delta.head3() Eigen::Vector3d(0.001, 0.0, 0.0); Sophus::SE3d T_updated Sophus::SE3d::exp(delta) * T;这一步省去了一大堆手写罗德里格斯公式的麻烦也避免了欧拉角带来的万向锁问题。3. 从模板版迁移到非模板版的接口变化清单3.1 类型映射表原来写的代码现在对应什么很多人是从老版模板接口踩坑之后决定迁移的。迁移过程中最核心的事情是类型名的替换。我列了一张在实际迁移中直接能用的对照表老版模板写法新版非模板写法备注Sophus::SO3doubleSophus::SO3d旋转类Sophus::SE3doubleSophus::SE3d刚体变换类Sophus::SO2doubleSophus::SO2d二维旋转Sophus::SE2doubleSophus::SE2d二维变换Sophus::Sim3doubleSophus::Sim3d相似变换R.unit_quaternion()R.unit_quaternion()保持一致R.matrix()R.matrix()保持一致从表里可以看出来真正变化的其实主要是类型声明那部分。方法论层面的exp、log、matrix、unit_quaternion这些接口基本延续了下来没有太大改动。所以老项目迁移时大部分工作量是机械性的类型替换真正需要动脑筋的地方并不多。3.2 编译期常见的错误与踩坑记录迁移过程中最容易卡住的是这里几个编译错误。第一个Eigen::Matrix3d和Eigen::Quaterniond声明顺序不一致导致的类型不匹配。比如你用一个已经归一化的四元数构造SO3d但是四元数本身是Eigen::Quaterniond类型没问题可如果你传入的是Eigen::Quaterniondouble而SO3d构造函数期望的是Eigen::Quaterniond编译器会报一个长长的模板错误。解决办法是统一用Eigen::Quaterniond或者显式做一次类型转换。第二个SE3d的构造参数顺序。我见过有人把SE3d(rotation_matrix, translation_vector)写反了结果编译通过运行逻辑全错。新版构造函数签名通常是SE3d(const Sophus::SO3d so3, const Eigen::Vector3d translation)或者SE3d(const Eigen::Quaterniond quat, const Eigen::Vector3d translation)注意前一个参数是旋转部分后一个是平移顺序别换。第三个头文件路径问题。老版本的头文件分散在src/sophus/下新版本一般用sophus/so3.hpp、sophus/se3.hpp。如果你从老项目拷过来的代码还保留#include sophus/so3.hpp而cmake没更新include目录会提示找不到文件。确保你的CMakeLists里链接的是Sophus::Sophus目标而不是手动拼路径。3.3 一个真实迁移案例我之前把一个VIO前端的状态更新模块从模板版迁到非模板版。这个模块涉及大量的四元数积分、坐标变换原本代码里到处都是Sophus::SE3double几十个文件看得头疼。迁移时我做了三件事先把所有Sophus::SE3double替换成Sophus::SE3d这个用IDE的全局替换就行。然后把所有Sophus::SO3double替换成Sophus::SO3d。最后重新编译按照编译错误一个个清理类型声明和头文件。整个过程大概半天时间最耗时的反而不是类型替换而是有些地方原本依赖模板参数做隐式类型推导比如模板函数里写了templatetypename T void foo(const Sophus::SO3T)非模板版就得改成直接用Sophus::SO3d的版本。这种地方不多但有代表性——迁移的时候别只想着找库的差异也要审视自己的代码有没有过度依赖模板推导。4. 编译、链接与CMake集成我踩过的几个坑4.1 编译Sophus的依赖Sophus不是纯头文件库编译安装需要依赖Eigen3新版还依赖fmt库。如果你用系统包管理器安装在Ubuntu上大致是sudo apt-get install libeigen3-dev libfmt-dev然后从源码编译Sophusgit clone https://github.com/strasdat/Sophus.git cd Sophus mkdir build cd build cmake .. make -j4 sudo make install这里有个容易忽略的点Sophus新版默认会开启BUILD_TESTS和BUILD_EXAMPLES如果不想编译这些可以在cmake时显式关闭cmake -DBUILD_TESTSOFF -DBUILD_EXAMPLESOFF ..否则make的时候会额外编译大量测试代码尤其第一次编译时特别耗时。我在刚开始编译时没关测试结果等了好几分钟还带出一堆fmt版本相关的警告纯属浪费时间。4.2 CMake集成的推荐写法在自己的项目里集成Sophus最简单的写法是find_package(Sophus REQUIRED) target_link_libraries(your_target PRIVATE Sophus::Sophus)前提是你已经通过make install把Sophus装到了系统里。如果没有安装也可以直接用add_subdirectory把Sophus源码目录包含进来然后把目标链接给Sophus这种用法适合把Sophus代码一并放进仓库管理的场景。要注意一点find_package搜到Sophus配置后它内部的Eigen3依赖也会通过Sophus::Sophus这个target传递过来所以在你的CMakeLists里不一定需要显式再写find_package(Eigen3 REQUIRED)但是如果你的代码里直接用了Eigen的类型建议还是写上避免头文件搜索路径不完整。4.3 两个运行时崩溃的排查我之前在跑一个位姿图优化时遇到过一个诡异的崩溃程序偶尔在加载完地图后直接段错误而且不是必现。排查了一晚上最后发现原因是代码里有人直接修改了SE3d内部四元数的数据来更新旋转auto q T.unit_quaternion(); q.x() ...; // 直接改内部数据没有归一化 q.normalize(); // 忘了调用unit_quaternion()返回的是内部引用直接修改是可以编译的但Sophus内部很多方法都依赖四元数单位范数这个前提。一旦范数不等于1矩阵转换、乘法、对数映射都会产生错误结果轻则精度偏差重则触发Eigen的断言。排查这种问题特别折磨人因为错误不是立刻爆出来的而是等到后续某个乘法或者分解时才炸。另一个崩溃场景和logmap的输入有关。如果你把一个不满足单位范数的旋转矩阵直接丢给Sophus::SO3d构造函数Sophus内部可能会通过四元数的实部做分支判断范数不对时可能生成一个NaN后续使用就彻底崩了。所以我的经验是所有外部传入的旋转矩阵、四元数进了Sophus之前先做一次归一化或正交化别省这一步。5. 非模板Sophus在SLAM实战中的典型用法5.1 位姿优化中的扰动模型左乘右乘别搞混在基于图优化的SLAM系统里位姿需要被表示成优化变量通常我们会用左扰动模型对SE3d进行更新。比如在g2o或Ceres的代价函数里误差项对位姿的雅可比很多都基于李代数左扰动来推导// 左扰动exp(delta_xi) * T Sophus::SE3d T_new Sophus::SE3d::exp(delta_xi) * T;而右扰动模型使用T * Sophus::SE3d::exp(delta_xi)。两种模型在数学上等价但雅可比形式不一样和外部库接口对接时要注意保持一致。如果Ceres里你给的雅可比是基于左扰动推导的而代码里实际更新用的是右扰动结果就是优化迟迟不收敛甚至发散。这个问题在调SLAM系统时特别容易遇到。5.2 位姿插值和平滑slerp与对数映射非模板版Sophus也能很方便地做位姿插值。旋转部分用四元数slerp是先想到的但如果你需要在整个SE3变换上做插值就应该用对数映射Sophus::SE3d T0 ...; Sophus::SE3d T1 ...; Sophus::SE3d T_interp Sophus::SE3d::exp(T0.log() * alpha T1.log() * (1.0 - alpha));这里要注意alpha的取值范围。对于旋转部分log的结果在范数接近π时存在奇异性两个位姿之间角度差超过90度时插值结果可能看起来“绕远路”。在实际工程里如果插值范围较大建议先用旋转向量对齐方向或者分段插值别一口吃成胖子。5.3 新项目选型建议现在如果让我给新项目选型直接用非模板版。理由很简单团队里不是每个人都有模板元编程的底子代码的可读性和可维护性优先而且SLAM系统的数值类型基本锁定在double模板自由度用不上就是负担。除非你明确需要float做嵌入式端优化、或者有跨平台精度对比的需求才需要回头去看模板版本否则没必要给自己增加额外的认知负担。我自己在实际项目里还有个偏好在对外接口层用Sophus类型在内部计算数据交换时尽量用Eigen原生类型。这样既保证了核心算法方便调用Sophus的数学方法又不会被Sophus类型绑死。最后再分享一个小技巧。如果你用auto接Sophus的类型推导建议在IDE里显式标注类型比如Sophus::SE3d T_updated ...不要图省事写auto T_updated ...。因为Sophus的表达式里有隐式的临时对象用auto偶尔会把引用类型或者表达式模板带进来导致后续调用T_updated.matrix()时产生莫名其妙的编译错误。显式标注看着啰嗦但能在排查问题的时候帮你省下不少时间。本文还有配套的精品资源点击获取