1. 为什么要在视觉里程计里聊半直接法第一次接触SVOSemi-direct Visual Odometry的人十有八九会被它那句“半直接法”绕晕。直接法阵营说它用了FAST角点特征点阵营又说它靠直接法对齐像素块两边都不太认它。我当年啃这块代码的时候也卡了很久后来把整条数据流在纸上画了三遍才想明白SVO的聪明之处恰恰在于它把“找点”和“配准”这两件事拆开用各自最擅长的方式去做。这个思路解决的是一个非常现实的问题。纯特征点法比如ORB-SLAM那一套在纹理丰富、光照稳定的场景里很稳但提取描述子、做匹配的开销不小尤其是想跑在嵌入式板子上时一帧几百个特征点的描述子计算和匹配能吃掉大半算力。纯直接法比如DSO省掉了描述子靠光度误差直接优化速度快、弱纹理下也能用但它对初始值极其敏感大位移或者快速旋转时容易掉进局部极小值而且金字塔层级一多计算量也不见得低。SVO走的是中间路线用FAST角点负责“在哪里有信息”用直接法负责“这个点跑到哪去了”。角点检测本身很便宜而且只做一次后续帧的追踪不再重新提点、不再算描述子而是拿上一帧的像素块patch在当前帧上做光度误差最小化。这样一来既保留了角点带来的良好分布和可重复性又避开了描述子匹配的高开销。实测下来在普通笔记本上单目跑个几十帧每秒是很轻松的事这也是为什么很多做无人机、AR眼镜的团队愿意拿SVO当基线。适合读这篇的人我大致分三类一是刚入门视觉SLAM、想搞懂“直接法到底怎么和角点结合”的学生二是手上有个算力有限的平台、想评估SVO能不能落地的工程师三是已经跑通过SVO demo、但对其中的patch对齐、逆深度滤波一知半解的开发者。下面我按自己复现和调参的顺序把核心机制、实操细节和踩过的坑一条条摊开讲。2. 整体架构拆解三个线程各干各的2.1 追踪、建图、深度滤波的分工逻辑SVO的代码结构一开始看会觉得线程有点多但理清职责后就顺了。它主要分三块帧到帧的位姿追踪、深度滤波建图、以及局部地图维护。追踪线程负责拿当前帧和上一帧做稀疏直接对齐估出相对位姿深度滤波线程对每个候选点维护一个逆深度分布随着多帧观测不断收敛地图维护则负责把收敛好的点转成正式地图点并做关键帧管理。为什么这么分因为直接法对齐对实时性要求高必须放在主线程快速跑而深度收敛是个统计过程慢一点没关系扔到独立线程里不影响追踪帧率。这种“快慢分离”的设计在工程上很常见类比一下就像收银台和仓库的关系——前台快速结账后台慢慢补货。注意多线程带来的是数据竞争风险。我早期改代码时随手在追踪线程里读了深度滤波正在写的点结果偶发段错误查了两天才定位到。所有跨线程共享的地图点读写都要加锁或者用原子操作别图省事。2.2 为什么追踪阶段不再提取新特征这是SVO区别于传统特征点法的关键决策。传统做法是每帧都提点、算描述子、匹配SVO只在关键帧上提FAST角点非关键帧完全靠投影和patch对齐来追踪已有地图点。省掉的不只是描述子计算还有匹配搜索的时间。代价是什么一旦某个点跟丢了这一帧就没法把它找回来只能等下一个关键帧重新提点。所以SVO对快速运动比较敏感因为大位移会让patch对齐的初始投影偏差过大直接法容易收敛到错误位置。这也是为什么后面要讲金字塔和粗到精对齐——本质上是在给直接法“兜底”。2.3 关键帧的判定标准关键帧不是随便选的。SVO的判据大致是当前帧相对上一关键帧的平移超过场景平均深度的某个比例或者旋转超过一定角度就插入新关键帧。这个“相对平均深度”的设计很巧妙因为它让阈值随场景尺度自适应——近处场景平均深度小平移阈值也小保证近距离时关键帧足够密。我实测时把平移比例从默认值调大过帧率确实上去了但快速转弯时追踪明显更容易丢。后来老老实实调回接近默认的值稳定性优先。这个参数没有万能值得看你的运动模式和场景尺度。3. FAST角点半直接法的“锚”3.1 FAST为什么比Harris、Shi-Tomasi更合适选FAST不是因为它检测质量最高而是因为快且够用。FAST的判定逻辑是看圆周上连续多少个像素比中心亮或暗纯整数比较没有浮点运算在ARM上跑起来非常轻。Harris和Shi-Tomasi要做梯度计算和矩阵特征值分析精度是好一些但开销大不少。对SVO来说角点的作用只是“标记哪里有可追踪的纹理”后续追踪靠的是patch光度对齐对角点本身的亚像素精度要求没那么高。所以FAST的“粗糙但快”正好匹配需求。这就像装修时用粉笔在墙上画个圈标记要打孔的位置你不需要用激光测距仪画这个圈画得差不多就行真正精确的是后面电钻对准的过程。3.2 网格化提取保证点的空间分布直接在全图跑FAST点会扎堆在纹理密集的区域比如桌角、文字边缘而平坦区域一个点都没有。SVO用了网格划分把图像切成固定大小的格子常见是30×30或40×40像素一格每个格子内最多保留响应最强的N个角点。这个设计的意义在于让地图点覆盖更均匀避免追踪时所有约束都来自图像一角导致位姿估计在另一侧方向上是病态的。我试过把每格上限从5调到10点多了追踪确实更稳但深度滤波线程压力明显增大帧率掉了大概一成。最后还是回到5左右平衡得比较好。3.3 角点响应阈值与金字塔层数的取舍FAST的阈值直接决定提点数量和噪声敏感度。阈值太低平坦区域的噪声也被当成角点patch对齐时这些点贡献的是垃圾约束阈值太高弱纹理场景提不出点追踪直接失效。SVO默认阈值一般在20到30之间不同实现有差异我建议先用默认值跑通再根据场景调。金字塔层数方面SVO通常用3到4层。层数多能应对更大位移但每层都要做patch对齐计算量线性增长。我的经验是室内慢速场景3层足够室外车载或者手持快速移动可以上到4层再往上收益递减明显。参数常见取值调大后的影响调小后的影响FAST阈值20~30点少、抗噪强、弱纹理易失效点多、噪声多、约束质量下降每格最大点数5~10分布更密、算力压力大点稀疏、位姿病态风险高金字塔层数3~4抗大位移强、耗时增加快速运动易跟丢4. 直接法对齐patch光度误差怎么最小化4.1 从投影初值到patch配准的完整链路追踪一帧时SVO对每个已有地图点做这么几步先用上一帧位姿和匀速模型预测当前帧位姿把地图点投影到当前帧图像上得到初值像素坐标然后以该坐标为中心取一个patch常见4×4到8×8和参考帧里对应patch做光度误差最小化用高斯牛顿或逆向组合法迭代求最优像素偏移最后把像素偏移转成重投影误差喂给位姿优化。这里的关键是初值。直接法是局部优化初值偏太远就收敛到错误位置。所以匀速模型预测很重要它假设帧间运动近似匀速给投影一个合理起点。我踩过的坑是在运动突变比如突然停住或急转时匀速模型预测反而帮倒忙初值偏得离谱patch对齐直接失败。后来加了个预测残差检查残差过大就退回用上一帧位姿做初值稳了不少。4.2 逆向组合法为什么能省计算SVO的patch对齐用的是逆向组合inverse compositional形式。普通高斯牛顿每次迭代都要重新计算当前帧patch的雅可比矩阵而逆向组合把雅可比固定在参考帧patch上迭代时只更新残差和增量雅可比可以预先算好复用。省多少假设patch有6×636个像素普通形式每迭代一次算一次雅可比逆向组合整个对齐过程只算一次。对于每帧要处理上百个点的情况这个优化相当可观。代价是逆向组合对光度变化比如曝光调整的鲁棒性稍弱因为它假设参考帧和当前帧的亮度关系固定。实际场景里自动曝光会破坏这个假设所以有些实现会加一个仿射亮度模型来补偿。4.3 金字塔粗到精给直接法上保险单层直接法对位移的容忍度大概就几个像素。为了应对更大位移SVO在金字塔上从粗到精逐层对齐先在最低分辨率层比如1/4尺寸做对齐得到的偏移乘上尺度因子作为下一层的初值逐层细化到原图。这个策略的本质是用分辨率换收敛域。低分辨率下同样的像素偏移对应原图更大的位移所以粗层能“看到”更大的运动范围细层则在粗层结果基础上做精修。类比一下就是先看地图找大致方向再走近看路牌确认门牌号。实操心得粗层对齐失败时细层基本没救。所以我在粗层加了个残差阈值判断粗层残差超过阈值就直接标记该点追踪失败不再往下迭代省算力也避免错误结果污染位姿优化。5. 逆深度滤波单目尺度的解法5.1 为什么单目必须用逆深度参数化单目相机没法直接测深度一个像素对应的是空间中的一条射线深度未知。SVO用逆深度深度的倒数来参数化这个不确定性原因是逆深度的分布更接近高斯滤波更新时数值更稳定。直观理解远处点的深度可能从10米到100米跨度巨大但逆深度就是从0.1到0.01范围紧凑高斯假设更成立。每个候选点维护一个逆深度均值和一个方差随着多帧观测不断更新。方差收敛到足够小这个点就从“候选”转正为“地图点”参与后续追踪。5.2 极线搜索与三角化的配合新点在关键帧上提取后深度是未知的。SVO在后续帧里沿极线搜索匹配极线是当前帧上该点可能出现的轨迹由两帧相对位姿决定。沿极线找光度最匹配的位置得到一个深度观测再喂给逆深度滤波器。这里有个细节极线搜索也是在金字塔上做的而且搜索范围受当前逆深度方差约束——方差大就搜得宽方差小就搜得窄。这个自适应搜索范围既保证收敛早期不漏掉正确匹配又保证后期不浪费算力。5.3 收敛判据与异常点剔除逆深度方差小于某个阈值常见是均值的某个比例就认为收敛。但收敛不等于正确错误匹配也可能让方差快速缩小。所以还要做一致性检查新观测的深度和当前均值偏差过大时不直接更新而是增加一个“外点计数”连续多次不一致就丢弃该点。我调这块时发现外点剔除太激进会把正确的慢收敛点误杀太宽松又会让错误点混进地图。折中办法是给外点计数设个上限比如连续3次不一致才丢同时对方差更新加个下限防止方差被单次观测压得过小。6. 实操复现从零跑通一条追踪链路6.1 环境与依赖的取舍复现SVO一般绕不开几个基础库Eigen做矩阵运算OpenCV做图像读写和金字塔构建Sophus处理李群李代数。版本上我建议Eigen用3.3以上OpenCV用3.x或4.x都行但注意4.x某些API有变动老代码可能要小改。编译这块早期SVO依赖ROS如果不想装整套ROS可以把核心算法抽出来单独编。我自己是把追踪和深度滤波两个模块拎出来用CMake单独构建只依赖Eigen和OpenCV跑数据集完全够用。这样调试时断点跟起来清爽很多不用在ROS那一堆节点里绕。6.2 数据流与坐标系约定跑之前一定要把坐标系约定搞清楚这是最容易埋雷的地方。SVO里涉及世界坐标系、相机坐标系、图像坐标系还有IMU坐标系如果用的话。我见过不少人因为把相机系和世界系搞反跑出来轨迹整个镜像。建议在代码里对每个变换矩阵的命名带上明确前后缀比如T_world_cam表示从相机到世界的变换读代码时一眼能看出方向。另外像素坐标的原点约定左上角还是中心也要统一patch取整时的边界处理很容易在这里出bug。6.3 关键参数配置与实测记录下面是我在一段室内手持数据集上跑通的一组参数供参考fast_threshold: 25 max_pts_per_cell: 5 grid_size: 30 pyramid_levels: 4 patch_size: 6 patch_align_max_iter: 10 patch_align_residual_thresh: 0.3 inverse_depth_var_thresh: 0.0001 keyframe_trans_ratio: 0.1 keyframe_rot_thresh: 0.15实测下来这套参数在普通笔记本上单目追踪能跑到40帧以上轨迹漂移在可接受范围。patch_align_residual_thresh这个值我调过几轮0.3左右对光照变化有一定容忍再小就容易把正常点误判为失败。注意参数不是抄来就能用。不同相机的内参、不同场景的纹理密度都会影响效果务必先用自己的数据跑一遍看提点数量和追踪成功率再微调。7. 常见问题与排查实录7.1 追踪频繁丢失的排查顺序追踪丢失是最常见的问题排查我一般按这个顺序走先看提点数量如果每帧只有几十个点多半是FAST阈值太高或者场景太弱纹理再看patch对齐的残差分布如果大量点残差超阈值可能是初值预测偏了或者光照变化大最后看关键帧插入频率如果关键帧几乎不插地图点会越来越旧追踪自然不稳。有一次我遇到追踪在特定转角必丢查了半天发现是那个方向纹理极弱网格化提取后该区域一个点都没有。解决办法是在弱纹理区域适当降低FAST阈值或者靠其他区域的点撑住位姿估计。7.2 patch对齐收敛到错误位置的识别直接法最隐蔽的失败是“收敛了但收敛错了”。表现是位姿看起来连续但轨迹慢慢漂移。识别方法是看patch对齐后的残差和偏移量如果偏移量接近搜索边界或者残差虽然小但和邻域点的偏移方向明显不一致就要警惕。我的做法是给每个点的对齐结果加个一致性投票邻域内多数点偏移方向一致才采纳孤立的异常偏移直接丢弃。这个后处理不复杂但对抑制漂移效果明显。7.3 逆深度不收敛的典型原因深度滤波不收敛常见原因有三个极线搜索范围设得太窄正确匹配在范围外光度匹配阈值太严弱纹理下找不到匹配或者基线太短两帧间视角变化不够三角化几何条件差。第三个最容易被忽略。如果关键帧插得太密相邻关键帧基线很短三角化出来的深度不确定性极大滤波收敛会非常慢。适当拉大关键帧间距给三角化留出足够基线反而收敛更快。现象可能原因排查手段处理方式追踪频繁丢失提点太少/初值偏差大统计每帧点数与残差调低FAST阈值、检查匀速模型轨迹缓慢漂移patch对齐收敛错误看偏移量与邻域一致性加一致性投票、剔除异常点深度长期不收敛基线太短/搜索范围窄检查关键帧间距与极线范围拉大关键帧间距、放宽搜索帧率骤降点数过多/金字塔层数高统计各线程耗时降每格点数、减金字塔层数8. 我踩过的几个坑和一点体会第一个坑是跨线程数据竞争。前面提过深度滤波线程写地图点、追踪线程读没加锁时偶发崩溃而且复现概率低特别难查。后来统一用读写锁保护地图点容器问题消失。这类问题在单线程demo里永远遇不到一上多线程就冒出来。第二个坑是光照突变。室内从窗边走到背光处自动曝光一调整patch光度误差整体偏移直接法对齐大面积失败。加仿射亮度补偿能缓解但根治还是靠关键帧及时更新参考patch。我的经验是检测到连续多帧对齐残差整体偏高时主动插一个关键帧刷新参考。第三个坑是参数过拟合。我在一段数据集上把参数调到追踪成功率95%以上换另一段数据直接掉到60%。后来明白参数要留余量别为了单场景的漂亮数字把阈值卡死。稳定性比峰值性能重要得多。SVO这套半直接法给我的最大启发是没有哪种方法在所有维度上都最优工程上的价值在于把不同方法的优势拼在合适的位置。FAST负责快速标记信息点直接法负责高效配准逆深度滤波负责稳健估计尺度各司其职。理解了这层分工再去读代码或者调参就不会被表面的复杂度吓住。后面如果想继续深挖可以往IMU紧耦合或者回环检测方向扩展那是另一条值得走的路。
