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个像素。// 前后向光流检查的伪代码示意 vectorPoint2f 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.x3.x也兼容但4.x的calcOpticalFlowPyrLK在性能上有一些优化。编译ORBSLAM的时候建议把-DCMAKE_BUILD_TYPERelease打开否则光流计算会慢得让你怀疑人生。另外如果是在嵌入式平台上跑可以考虑把OpenCV的NEON优化打开对光流计算有比较明显的加速。# 编译ORBSLAM3的典型命令 cd ORB_SLAM3 mkdir build cd build cmake .. -DCMAKE_BUILD_TYPERelease 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::vectorcv::Point2f prevPts, std::vectorcv::Point2f currPts, std::vectoruchar status); void Reset(); private: cv::Size winSize_; int maxLevel_; cv::TermCriteria criteria_; std::vectorcv::Point2f prevPts_; };在跟踪函数内部除了调用calcOpticalFlowPyrLK还要做前后向检查、剔除异常点、更新跟踪状态。前后向检查的实现前面已经提过这里不再重复。剔除异常点的时候除了前后向误差还可以检查跟踪点是否超出图像边界、是否和相邻点过于接近等。3.3 与ORBSLAM跟踪线程的集成集成到ORBSLAM的跟踪线程主要是在Track()函数里插入光流跟踪的调用。ORBSLAM2的跟踪线程在Tracking::GrabImageMonocular或者GrabImageStereo里ORBSLAM3类似。我一般会在特征提取之后、TrackWithMotionModel之前插入光流跟踪。具体步骤是先保存上一帧的图像和特征点然后在当前帧到来时用光流把上一帧的特征点跟踪过来。跟踪结果用来计算一个初始位姿这个位姿作为TrackWithMotionModel的初值。如果光流跟踪点数量足够还可以直接用光流点做一次PnP求解得到一个更准的初值。// 在Tracking::Track()中插入光流跟踪 if (mLastFrame.mvKeys.size() 0) { std::vectorcv::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毫秒。这个代价在大多数平台上是可以接受的。如果耗时增加太多可以考虑降低光流跟踪点的数量或者减少金字塔层数。场景原始ORBSLAM成功率加光流后成功率耗时增加快速运动65%88%8ms弱纹理72%85%6ms光照变化70%82%7ms正常场景95%96%5ms4. 常见问题与排查技巧实录4.1 光流跟踪点漂移怎么办光流跟踪点漂移是常见问题尤其是长时间跟踪同一个点的时候。漂移的原因是光流假设的灰度一致性在现实中很难完全满足误差会逐渐累积。解决漂移的办法有几个一是限制每个点的跟踪年龄超过一定帧数就强制重新提取二是定期用描述子匹配校正光流点的位置三是用局部地图投影来修正光流点。我一般会把跟踪年龄上限设在10到15帧。超过这个年龄的点即使还在跟踪也会被标记为“老化”在后续优化中降低权重。同时每隔几帧就用描述子匹配做一次全局校正把漂移拉回来。4.2 光流计算耗时过高怎么优化光流计算耗时高通常是因为跟踪点太多、金字塔层数太多、或者图像分辨率太高。优化手段包括减少跟踪点数量、降低金字塔层数、对图像做降采样、使用ROI只对感兴趣区域做光流。我试过对图像做2倍降采样再做光流耗时能降低一半以上精度损失很小。因为光流本身就是在金字塔上做的降采样相当于多了一层金字塔。另外如果平台支持SIMD指令可以编译OpenCV的时候打开相关优化对光流计算有加速效果。注意降采样之后光流得到的位移要乘以降采样倍数才能还原到原图尺度。这个细节容易出错一定要检查。4.3 光流和描述子匹配结果冲突怎么处理光流和描述子匹配结果冲突指的是同一个特征点光流跟踪到的位置和描述子匹配到的位置差异很大。这种情况通常发生在运动剧烈或者有遮挡的时候。处理策略是优先信任描述子匹配因为描述子匹配有外观信息做约束而光流只有灰度一致性。但也不能完全否定光流结果。我的做法是如果冲突点的数量占比不高比如低于10%就直接剔除这些冲突点。如果冲突点占比很高说明光流跟踪可能整体出了问题这时候应该降低光流权重甚至完全切换到描述子匹配。4.4 常见问题速查表问题现象可能原因排查方法解决措施跟踪点数量骤降运动过快或光照突变检查前后向误差均值增加金字塔层数启用描述子兜底位姿估计抖动光流点漂移检查跟踪点年龄分布限制跟踪年龄定期校正单帧耗时过高跟踪点太多或分辨率太高统计光流计算耗时减少跟踪点降采样跟踪突然丢失光流整体失败检查跟踪点空间分布触发重定位重置光流跟踪地图点关联错误光流点误关联检查光流点与地图点距离只用地标点做局部地图匹配4.5 独家避坑经验分享踩了这么多坑有几个经验我觉得特别值得分享。第一光流跟踪点的初始化很重要不要随便拿上一帧的所有特征点去做光流最好筛选一下把边缘点、低响应点去掉这些点光流跟踪效果很差。第二光流跟踪和描述子匹配的切换不要太频繁频繁切换会导致跟踪状态不稳定我一般会设置一个滞回区间比如光流点占比低于40%才切换高于60%才切回来。第三调试的时候一定要把光流跟踪结果可视化出来看看哪些点跟踪上了、哪些点漂移了肉眼观察比看数字直观得多。还有一个容易被忽略的点光流跟踪对图像质量很敏感。如果图像噪声大、压缩伪影严重光流跟踪效果会大打折扣。所以如果条件允许尽量用原始图像做光流不要用经过压缩或者增强的图像。如果必须用压缩图像可以考虑先做一次轻量的去噪。最后说一个关于参数整定的心得。光流参数没有一套放之四海而皆准的值必须根据你的平台、场景和图像分辨率来调。我一般会先用默认参数跑一遍然后根据跟踪成功率和耗时逐步调整窗口大小和金字塔层数。调参的时候一次只调一个参数否则你根本不知道是哪个参数起了作用。这个过程可能有点枯燥但调好之后效果提升是很明显的。这个方向后续还可以继续扩展比如把光流和IMU做更紧的耦合或者在光流跟踪中引入仿射变换模型来应对旋转和尺度变化。我现在正在尝试的是把光流跟踪点用于局部BA的约束初步结果看起来有希望等成熟了再整理出来分享。