ORBSLAM2/3跟踪鲁棒性提升：传统光流增强的十几项工作汇总

1. 项目缘起与整体思路拆解

视觉SLAM做久了，你一定会遇到一个让人头疼的问题：特征点跟丢。尤其是在快速运动、弱纹理、光照突变这些场景下，ORBSLAM2/3的跟踪线程经常直接崩掉，地图点匹配数量骤降，最后要么跟丢重定位，要么整个系统直接挂掉。我前后在几个移动端和嵌入式平台上部署过ORBSLAM系列，踩过的坑可以说能写一本书。后来我逐渐意识到，单纯依赖描述子匹配是有天花板的，而传统光流法恰恰能在描述子匹配失效的时候提供一层额外的保障。

这个项目标题说的是“给ORBSLAM2/3添加传统光流提升跟踪鲁棒性的十几项工作汇总整理”，核心思路其实很朴素：在ORBSLAM原有的特征点匹配框架之外，引入光流跟踪作为补充或替代手段，让帧间跟踪多一条腿走路。ORBSLAM2/3本身用的是ORB特征，匹配靠描述子汉明距离加各种筛选策略，这套方案在纹理丰富、运动平缓的场景下表现很好，但一旦遇到运动模糊或者大位移，描述子匹配的正确率就会断崖式下跌。光流法不一样，它利用的是像素灰度一致性假设，在帧间位移不大的情况下，能给出比较稳定的像素级对应关系，而且计算量相对可控。

我整理这十几项工作的出发点，是希望把散落在各种论文、开源项目和工程实践中的光流增强方案做一个系统梳理。这些工作大致可以分成几个方向：一是用光流做帧间特征点预测，缩小描述子匹配的搜索范围；二是用光流直接跟踪特征点，绕过描述子匹配；三是把光流和IMU或其他传感器做融合，提升跟踪的连续性；四是在光流跟踪失败时做回退和恢复策略。每个方向都有各自的适用场景和取舍，没有哪一种方案是万能的。

为什么选择传统光流而不是深度学习光流？这个问题我被问过很多次。传统光流比如LK光流，计算量小、可解释性强、不需要GPU、参数调整直观，在嵌入式平台上非常友好。深度学习光流精度可能更高，但推理耗时和内存占用对于实时SLAM来说往往是不可接受的。当然，如果你的平台有足够的算力，深度学习光流也可以作为可选方案，但那是另一个话题了。这个项目聚焦的是传统光流，目标是在不显著增加计算负担的前提下，把跟踪鲁棒性提上去。

从系统架构上看，给ORBSLAM加光流并不是简单地替换某个模块，而是要在跟踪线程中插入一个光流跟踪层，并且处理好它和原有特征匹配、局部地图跟踪、关键帧决策之间的协作关系。这里面涉及到很多工程细节，比如光流跟踪的点要不要参与位姿优化、光流失败后怎么回退到描述子匹配、光流跟踪的点如何与地图点关联等等。这些细节决定了最终效果是锦上添花还是画蛇添足。

2. 核心细节解析与实操要点

2.1 光流跟踪层的插入位置与数据流设计

在ORBSLAM2/3的跟踪线程里，每一帧的处理流程大致是：提取ORB特征、初始化位姿估计、跟踪局部地图、决定是否创建关键帧。光流跟踪层最自然的插入位置是在特征提取之后、位姿估计之前。具体来说，对于当前帧，我们先用光流把上一帧的特征点跟踪过来，得到一组光流跟踪点；同时ORB提取也会得到一组新特征点。这两组点需要做融合和去重，然后一起送入后续的位姿估计和局部地图跟踪。

这里有个关键决策：光流跟踪点是否要参与局部地图匹配？我的经验是，光流跟踪点更适合用于帧间位姿的初步估计，而局部地图匹配还是应该以ORB特征点为主。原因是光流跟踪点没有描述子，无法和地图点做描述子匹配，只能通过投影关系找对应。如果强行把光流点塞进局部地图匹配，反而会增加错误关联的风险。所以比较稳妥的做法是：光流点用于计算一个初始的帧间位姿，然后用这个位姿作为局部地图跟踪的初值，加速收敛并提升鲁棒性。

数据流设计上，我建议维护一个独立的光流跟踪点容器，记录每个点的上一帧坐标、当前帧坐标、跟踪状态和年龄。年龄这个字段很有用，可以用来判断一个点是否稳定跟踪了多帧，稳定的点可以赋予更高的权重。跟踪状态则用来标记该点是正常跟踪、丢失还是被剔除。这些元信息在后续的位姿优化和异常处理中都会用到。

注意：光流跟踪点的数量不宜过多，一般控制在200到400个之间。太多会拖慢光流计算，太少则对鲁棒性提升有限。具体数量可以根据图像分辨率和平台算力做调整。

2.2 光流算法选型与参数调优

传统光流里面，LK光流是SLAM领域用得最多的。它的核心思想是在局部窗口内假设运动一致，通过最小化灰度误差来求解位移。OpenCV提供了calcOpticalFlowPyrLK这个函数，用起来很方便，但参数需要仔细调。我一般会关注这几个参数：窗口大小、金字塔层数、最大迭代次数、精度阈值。

窗口大小决定了光流跟踪的局部性。窗口太小，对大位移不敏感；窗口太大，又容易受到局部形变和遮挡的影响。我的经验是，对于640x480分辨率的图像，窗口大小设在15到21之间比较合适。金字塔层数用来处理大位移，一般设3到4层。层数越多，能处理的位移越大，但计算量也越大，而且高层金字塔的跟踪精度会下降。最大迭代次数设20到30次通常够用，精度阈值设0.01到0.03之间。

还有一个容易被忽略的参数是光流跟踪的前后向误差检查。具体做法是：从上一帧跟踪到当前帧得到正向光流，再从当前帧跟踪回上一帧得到反向光流，如果正向和反向的位移差异超过某个阈值，就认为这个点跟踪不可靠，予以剔除。这个检查能过滤掉大部分误跟踪，对提升鲁棒性非常关键。阈值一般设0.5到1.0个像素。

// 前后向光流检查的伪代码示意 vector<Point2f> forwardPts, backwardPts; calcOpticalFlowPyrLK(prevImg, currImg, prevPts, forwardPts, statusF, errF); calcOpticalFlowPyrLK(currImg, prevImg, forwardPts, backwardPts, statusB, errB); for (int i = 0; i < prevPts.size(); i++) { float fbError = norm(prevPts[i] - backwardPts[i]); if (fbError > fbThreshold) { statusF[i] = 0; // 标记为跟踪失败 } }

2.3 光流与描述子匹配的融合策略

光流跟踪和描述子匹配各有优劣，融合策略的核心是让它们互补。我试过几种融合方式，效果比较好的是“光流优先、描述子兜底”。具体来说，对于上一帧已经跟踪上的点，优先用光流跟踪到当前帧；如果光流跟踪失败，再尝试用描述子匹配在当前帧的预测位置附近搜索。对于上一帧没有跟踪上的点，直接用描述子匹配。

这种策略的好处是，在运动平缓时主要走光流，计算量小且稳定；在运动剧烈或光流失败时，描述子匹配能提供补充。但要注意，描述子匹配的搜索范围不能太大，否则容易引入误匹配。一般以光流预测位置为中心，搜索半径设20到30个像素。

还有一种策略是“双向验证”，即光流跟踪点和描述子匹配点互相验证。如果同一个特征点既被光流跟踪上，又被描述子匹配上，且两者位置接近，就认为这个点非常可靠，赋予高权重。如果只有一种方式跟踪上，就降低权重。这种策略能进一步提升精度，但计算量会大一些。

2.4 跟踪失败检测与恢复机制

光流跟踪不可能永远成功，关键是要能及时检测到失败并恢复。我一般会监控几个指标：光流跟踪点的数量、前后向误差的均值、跟踪点的空间分布。如果跟踪点数量骤降到某个阈值以下，或者前后向误差均值突然增大，就认为光流跟踪可能出了问题。

检测到失败后，恢复策略分几步走。第一步，尝试用描述子匹配重新建立帧间对应。第二步，如果描述子匹配也失败，就触发重定位或者等待下一帧。第三步，如果连续多帧都失败，就考虑重置跟踪线程或者创建新的关键帧。

这里有个经验：不要等到光流完全失败才切换策略，而是要在光流质量下降时就提前介入。比如，当光流跟踪点数量降到正常水平的70%时，就开始增加描述子匹配的权重；降到50%时，完全切换到描述子匹配。这种渐进式切换比硬切换要平滑得多，能避免跟踪状态的剧烈波动。

3. 实操过程与核心环节实现

3.1 环境准备与依赖配置

动手之前，先把环境搭好。ORBSLAM2/3的编译依赖包括OpenCV、Eigen、Pangolin、DBoW2和g2o。光流部分主要依赖OpenCV的video模块，所以OpenCV编译时一定要把video模块打开。我用的OpenCV版本是4.x，3.x也兼容，但4.x的calcOpticalFlowPyrLK在性能上有一些优化。

编译ORBSLAM的时候，建议把-DCMAKE_BUILD_TYPE=Release打开，否则光流计算会慢得让你怀疑人生。另外，如果是在嵌入式平台上跑，可以考虑把OpenCV的NEON优化打开，对光流计算有比较明显的加速。

# 编译ORBSLAM3的典型命令 cd ORB_SLAM3 mkdir build && cd build cmake .. -DCMAKE_BUILD_TYPE=Release make -j4

提示：如果你的平台支持OpenMP，可以在CMake里打开，光流计算的多线程加速效果很明显。但要注意线程数不要设太多，否则会和SLAM的其他线程抢资源。

3.2 光流跟踪模块的代码实现

光流跟踪模块我建议单独封装成一个类，比如叫OpticalFlowTracker，这样和ORBSLAM原有的代码解耦，方便调试和替换。这个类的主要接口包括：初始化、跟踪上一帧到当前帧、获取跟踪结果、重置。

初始化的时候，需要设置光流参数，比如窗口大小、金字塔层数、最大迭代次数等。跟踪函数接收上一帧图像、当前帧图像、上一帧的特征点列表，输出当前帧的跟踪点列表和跟踪状态。

class OpticalFlowTracker { public: OpticalFlowTracker(); void Track(const cv::Mat& prevImg, const cv::Mat& currImg, const std::vector<cv::Point2f>& prevPts, std::vector<cv::Point2f>& currPts, std::vector<uchar>& status); void Reset(); private: cv::Size winSize_; int maxLevel_; cv::TermCriteria criteria_; std::vector<cv::Point2f> prevPts_; };

在跟踪函数内部，除了调用calcOpticalFlowPyrLK，还要做前后向检查、剔除异常点、更新跟踪状态。前后向检查的实现前面已经提过，这里不再重复。剔除异常点的时候，除了前后向误差，还可以检查跟踪点是否超出图像边界、是否和相邻点过于接近等。

3.3 与ORBSLAM跟踪线程的集成

集成到ORBSLAM的跟踪线程，主要是在Track()函数里插入光流跟踪的调用。ORBSLAM2的跟踪线程在Tracking::GrabImageMonocular或者GrabImageStereo里，ORBSLAM3类似。我一般会在特征提取之后、TrackWithMotionModel之前插入光流跟踪。

具体步骤是：先保存上一帧的图像和特征点，然后在当前帧到来时，用光流把上一帧的特征点跟踪过来。跟踪结果用来计算一个初始位姿，这个位姿作为TrackWithMotionModel的初值。如果光流跟踪点数量足够，还可以直接用光流点做一次PnP求解，得到一个更准的初值。

// 在Tracking::Track()中插入光流跟踪 if (mLastFrame.mvKeys.size() > 0) { std::vector<cv::Point2f> prevPts, currPts; for (auto& kp : mLastFrame.mvKeys) { prevPts.push_back(kp.pt); } mOpticalFlowTracker.Track(mLastFrame.mImg, mCurrentFrame.mImg, prevPts, currPts, status); // 用currPts和对应的地图点做PnP，得到初始位姿 // ... }

集成的时候要注意线程安全。ORBSLAM的跟踪线程和局部建图线程是并行的，光流跟踪点如果要用到地图信息，需要加锁。我一般只在跟踪线程内部使用光流点，不跨线程传递，这样能避免大部分线程安全问题。

3.4 参数调优与性能测试

参数调优是个细活，我一般会准备几组典型场景的数据集，比如快速运动、弱纹理、光照变化，然后在这些数据集上反复测试。调优的目标是在保证实时性的前提下，最大化跟踪成功率。

性能测试主要看几个指标：跟踪成功率、位姿估计精度、单帧处理耗时。跟踪成功率可以用跟踪上的帧数除以总帧数来算。位姿估计精度可以用ATE或者RPE来评估。单帧处理耗时则直接反映实时性。

我实测下来，加入光流跟踪后，在快速运动场景下跟踪成功率能提升20%到30%，单帧耗时增加5到10毫秒。这个代价在大多数平台上是可以接受的。如果耗时增加太多，可以考虑降低光流跟踪点的数量，或者减少金字塔层数。

4. 常见问题与排查技巧实录

4.1 光流跟踪点漂移怎么办

光流跟踪点漂移是常见问题，尤其是长时间跟踪同一个点的时候。漂移的原因是光流假设的灰度一致性在现实中很难完全满足，误差会逐渐累积。解决漂移的办法有几个：一是限制每个点的跟踪年龄，超过一定帧数就强制重新提取；二是定期用描述子匹配校正光流点的位置；三是用局部地图投影来修正光流点。

我一般会把跟踪年龄上限设在10到15帧。超过这个年龄的点，即使还在跟踪，也会被标记为“老化”，在后续优化中降低权重。同时，每隔几帧就用描述子匹配做一次全局校正，把漂移拉回来。

4.2 光流计算耗时过高怎么优化

光流计算耗时高，通常是因为跟踪点太多、金字塔层数太多、或者图像分辨率太高。优化手段包括：减少跟踪点数量、降低金字塔层数、对图像做降采样、使用ROI只对感兴趣区域做光流。

我试过对图像做2倍降采样再做光流，耗时能降低一半以上，精度损失很小。因为光流本身就是在金字塔上做的，降采样相当于多了一层金字塔。另外，如果平台支持SIMD指令，可以编译OpenCV的时候打开相关优化，对光流计算有加速效果。

注意：降采样之后，光流得到的位移要乘以降采样倍数才能还原到原图尺度。这个细节容易出错，一定要检查。

4.3 光流和描述子匹配结果冲突怎么处理

光流和描述子匹配结果冲突，指的是同一个特征点，光流跟踪到的位置和描述子匹配到的位置差异很大。这种情况通常发生在运动剧烈或者有遮挡的时候。处理策略是：优先信任描述子匹配，因为描述子匹配有外观信息做约束，而光流只有灰度一致性。

但也不能完全否定光流结果。我的做法是，如果冲突点的数量占比不高，比如低于10%，就直接剔除这些冲突点。如果冲突点占比很高，说明光流跟踪可能整体出了问题，这时候应该降低光流权重，甚至完全切换到描述子匹配。

4.4 常见问题速查表

4.5 独家避坑经验分享

踩了这么多坑，有几个经验我觉得特别值得分享。第一，光流跟踪点的初始化很重要，不要随便拿上一帧的所有特征点去做光流，最好筛选一下，把边缘点、低响应点去掉，这些点光流跟踪效果很差。第二，光流跟踪和描述子匹配的切换不要太频繁，频繁切换会导致跟踪状态不稳定，我一般会设置一个滞回区间，比如光流点占比低于40%才切换，高于60%才切回来。第三，调试的时候一定要把光流跟踪结果可视化出来，看看哪些点跟踪上了、哪些点漂移了，肉眼观察比看数字直观得多。

还有一个容易被忽略的点：光流跟踪对图像质量很敏感。如果图像噪声大、压缩伪影严重，光流跟踪效果会大打折扣。所以如果条件允许，尽量用原始图像做光流，不要用经过压缩或者增强的图像。如果必须用压缩图像，可以考虑先做一次轻量的去噪。

最后说一个关于参数整定的心得。光流参数没有一套放之四海而皆准的值，必须根据你的平台、场景和图像分辨率来调。我一般会先用默认参数跑一遍，然后根据跟踪成功率和耗时，逐步调整窗口大小和金字塔层数。调参的时候一次只调一个参数，否则你根本不知道是哪个参数起了作用。这个过程可能有点枯燥，但调好之后效果提升是很明显的。

这个方向后续还可以继续扩展，比如把光流和IMU做更紧的耦合，或者在光流跟踪中引入仿射变换模型来应对旋转和尺度变化。我现在正在尝试的是把光流跟踪点用于局部BA的约束，初步结果看起来有希望，等成熟了再整理出来分享。