光流法视觉里程计实战：从Lucas-Kanade原理到代码实现与避坑指南

1. 从特征点匹配到光流追踪为什么需要换一条路很多人学视觉里程计Visual OdometryVO的时候第一个上手的方案基本都是特征点法提取ORB、SIFT之类的关键点计算描述子做暴力匹配或FLANN匹配再用RANSAC剔除外点最后求解位姿。这条路走通了确实很有成就感但真正把它放到视频序列上跑问题马上就来了——匹配耗时随着特征点数量急剧上升相邻帧之间明明只差一点点位移却要重新提取、重新描述、重新匹配大量计算被浪费在重复劳动上。光流法就是在这个背景下进入视野的。它的核心思路非常朴素既然相邻两帧之间像素的移动是连续的、小范围的那我为什么不直接追踪上一帧已经找到的点而非要每帧都从头再来一遍这个思路的转变带来的效率提升是数量级的。在典型的室内数据集上特征点匹配方案单帧耗时可能在三四十毫秒而光流追踪往往能压到十毫秒以内而且追踪到的点对数量还更多。这篇文章面向的是正在啃视觉SLAM十四讲、已经理解了特征点法基本流程、想进一步搞清楚光流法到底怎么用、怎么落地到VO里的读者。我会从光流法的底层假设讲起把灰度不变假设、Lucas-Kanade推导、金字塔分层这些关键点拆开揉碎然后给出可以直接跑的代码实现最后重点讲我在实际调试中踩过的坑——这些东西书上不会写但你不注意就会卡住好几天。需要提前说明的是光流法不是银弹。它有自己明确的适用边界超出边界就会失效而且失效的时候往往不会报错只是默默给你一堆错误的追踪结果最后位姿解算出来完全飘了。所以理解它的假设和限制比会调库更重要。2. 光流法的底层逻辑灰度不变假设到底在说什么2.1 从单像素到窗口Lucas-Kanade的约束补全光流法最根本的假设叫灰度不变假设Brightness Constancy Assumption。用大白话说就是同一个空间点在相机移动之后投影到下一帧图像上它的像素灰度值不会变。这个假设写成数学形式很简洁I(xdx, ydy, tdt) I(x, y, t)其中I是像素灰度x、y是像素坐标t是时间。对左边做一阶泰勒展开就能得到那个经典的方程Ix * u Iy * v It 0这里Ix、Iy是图像在x、y方向的梯度It是时间方向的梯度u、v就是我们要解的光流像素在x、y方向的运动速度。问题来了一个方程两个未知数u和v这是欠定的解不出来。这就是光流法里著名的孔径问题Aperture Problem——你透过一个小孔看一条移动的直线只能看到垂直于直线方向的运动分量沿直线方向的分量是看不出来的。Lucas-Kanade方法的聪明之处在于它假设一个小的图像窗口内比如5x5或7x7所有像素的运动是一致的。这样一来窗口内每个像素都贡献一个方程25个像素就有25个方程解两个未知数就绰绰有余了。写成矩阵形式就是[Ix1 Iy1] [-It1] [Ix2 Iy2] [u] [-It2] [ ... ] * [v] [ ... ] [IxN IyN] [-ItN]这就是一个超定方程组用最小二乘就能解。展开后需要求一个2x2矩阵的逆这个矩阵就是所谓的结构张量或者叫Hessian矩阵H [ΣIx² ΣIxIy] [ΣIxIy ΣIy² ]解出来[u] [ΣIx² ΣIxIy]⁻¹ [-ΣIxIt] [v] - [ΣIxIy ΣIy² ] [-ΣIyIt]这里有个关键点这个2x2矩阵能不能求逆取决于它的行列式是否足够大。行列式小意味着什么意味着窗口内的梯度分布太单一——比如只有水平边缘那Iy接近零矩阵就接近奇异。这种情况下解出来的光流会非常大且不稳定。所以实际实现里都会检查这个矩阵的最小特征值太小的点直接丢弃不参与追踪。2.2 为什么必须上金字塔大运动的致命伤上面推导有一个隐含前提泰勒展开只在很小的位移下才成立。如果两帧之间像素移动了十几个像素一阶近似就完全不准了解出来的光流会严重偏小甚至方向都错。解决办法是图像金字塔。思路很直观把图像逐层降采样最底层是原图往上一层缩小一半再往上再缩小一半。在顶层最小的图上原本20像素的位移可能只剩2-3像素这时候Lucas-Kanade就能正常工作了。解出顶层的光流之后把它乘以2传到下一层作为初始值在下一层再迭代优化逐层往下传递直到最底层。这个过程叫由粗到精Coarse-to-Fine的追踪策略。金字塔层数一般取3到4层再多的话顶层图像太小细节全丢了反而不好。每层的迭代次数通常设10到30次收敛条件一般是位移增量小于某个阈值比如0.01像素或者达到最大迭代次数。我在实际调试中发现一个容易忽略的点金字塔层数不是越多越好。如果你的场景里运动确实很大加层数有用但如果场景本身纹理就弱降采样之后顶层几乎是一片模糊追踪反而更容易丢点。所以层数要根据实际场景调不能照搬默认值。2.3 正向追踪与反向追踪一个提升鲁棒性的小技巧标准的光流追踪是正向的从上一帧追踪到当前帧。但有一个改进版本叫反向光流Inverse Optical Flow思路是同时做正向和反向追踪然后检查两者是否一致。如果正向追踪从A点到B点反向追踪从B点回到A点A和A差距很大说明这个追踪结果不可靠直接剔除。这个技巧在特征点法里对应的是交叉验证匹配在光流法里同样适用。代价是计算量翻倍但换来的是追踪质量的显著提升。如果你的应用对精度要求高、对实时性要求没那么苛刻强烈建议加上这一步。我自己在跑一些纹理较弱的场景时加了反向校验之后位姿估计的漂移明显减小。3. 手写一个可用的光流追踪器从理论到代码3.1 环境准备与依赖选择要跑通光流法最省事的方案是用OpenCV它内置了calcOpticalFlowPyrLK这个函数封装了金字塔Lucas-Kanade的完整实现。但如果你想真正理解原理我建议至少手写一遍单层的光流迭代哪怕性能差一点对理解帮助极大。环境方面Python的话需要numpy和opencv-pythonC的话就是OpenCV的核心模块。版本上OpenCV 4.x和3.x在光流接口上基本兼容但4.x对金字塔的默认参数做了一些调整如果你从旧教程抄代码注意参数含义可能变了。一个常见的坑是calcOpticalFlowPyrLK的输入点必须是float32类型的numpy数组形状是(N,1,2)。如果你直接传整数数组或者形状不对它会报一个很隐晦的错误或者干脆返回空结果。我第一次用的时候就因为这个卡了半小时后来养成习惯传进去之前先np.array(pts, dtypenp.float32).reshape(-1,1,2)。3.2 单层Lucas-Kanade的手写实现先看单层版本的核心逻辑。假设我们已经有了上一帧的灰度图prev和当前帧的灰度图curr以及上一帧的一组点pts。对每个点我们取一个窗口比如21x21在窗口内迭代求解光流。import numpy as np import cv2 def lk_single_layer(prev, curr, pt, win21, max_iter30, eps0.01): # pt: (x, y) 上一帧的点 half win // 2 x0, y0 int(round(pt[0])), int(round(pt[1])) # 边界检查太靠边的点直接放弃 if x0 - half 0 or y0 - half 0 or \ x0 half prev.shape[1] or y0 half prev.shape[0]: return None # 提取上一帧窗口 patch prev[y0-half:y0half1, x0-half:x0half1].astype(np.float32) # 计算窗口内的梯度 Ix cv2.Sobel(patch, cv2.CV_32F, 1, 0, ksize3) Iy cv2.Sobel(patch, cv2.CV_32F, 0, 1, ksize3) # 预计算Hessian矩阵 H np.array([[np.sum(Ix*Ix), np.sum(Ix*Iy)], [np.sum(Ix*Iy), np.sum(Iy*Iy)]], dtypenp.float32) # 检查H是否可逆 if np.linalg.det(H) 1e-6: return None # 初始位移 u, v 0.0, 0.0 for _ in range(max_iter): # 在当前帧的对应位置采样 xc, yc x0 u, y0 v xi, yi int(round(xc)), int(round(yc)) if xi - half 0 or yi - half 0 or \ xi half curr.shape[1] or yi half curr.shape[0]: return None curr_patch curr[yi-half:yihalf1, xi-half:xihalf1].astype(np.float32) # 时间梯度 It curr_patch - patch # 计算右侧向量 b np.array([-np.sum(Ix*It), -np.sum(Iy*It)], dtypenp.float32) # 求解增量 delta np.linalg.solve(H, b) u delta[0] v delta[1] if np.linalg.norm(delta) eps: break return np.array([x0 u, y0 v], dtypenp.float32)这段代码有几个细节值得说。第一梯度用的是Sobel算子窗口大小3这是最常用的配置。第二Hessian矩阵只算一次因为Ix、Iy是从上一帧窗口算的迭代过程中不变这样能省不少计算。第三每次迭代都要检查当前帧采样位置是否越界越界就放弃这个点。实际跑起来你会发现单层版本对超过5像素的位移就基本失效了。所以必须上金字塔。3.3 金字塔分层追踪的完整流程金字塔版本的逻辑是先构建N层金字塔从顶层开始追踪每层的结果作为下一层的初始值。def build_pyramid(img, levels): pyramid [img] for i in range(1, levels): pyramid.append(cv2.pyrDown(pyramid[-1])) return pyramid def lk_pyramid(prev, curr, pt, levels3, win21, max_iter30, eps0.01): prev_pyr build_pyramid(prev, levels) curr_pyr build_pyramid(curr, levels) # 从顶层开始点的坐标要按比例缩小 scale 1.0 / (2 ** (levels - 1)) pt_top pt * scale u, v 0.0, 0.0 for lvl in range(levels - 1, -1, -1): # 当前层的初始位置 pt_lvl pt * (1.0 / (2 ** lvl)) np.array([u, v]) result lk_single_layer(prev_pyr[lvl], curr_pyr[lvl], pt_lvl, winwin, max_itermax_iter, epseps) if result is None: return None # 更新位移注意要乘回缩放系数 u (result[0] - pt[0] * (1.0 / (2 ** lvl))) * (2 ** lvl) v (result[1] - pt[1] * (1.0 / (2 ** lvl))) * (2 ** lvl) return pt np.array([u, v], dtypenp.float32)这里最容易出错的是坐标缩放。每一层的图像尺寸是上一层的1/2所以点的坐标也要相应缩放。从顶层往底层传递的时候位移量要乘以2的幂次还原。我见过不少实现因为缩放系数搞错追踪结果整体偏移而且偏移量还跟金字塔层数相关非常隐蔽。3.4 用OpenCV的现成接口做对比验证手写版本跑通之后建议用OpenCV的calcOpticalFlowPyrLK做对比验证你的实现是否正确。OpenCV的接口参数比较多关键的几个next_pts, status, err cv2.calcOpticalFlowPyrLK( prev_img, curr_img, prev_pts, None, winSize(21, 21), maxLevel3, criteria(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01) )status是一个数组1表示追踪成功0表示失败。err是每个点的追踪误差可以用来做二次筛选。实际使用中我一般会把err大于某个阈值的点也剔除掉哪怕status是1。这个阈值需要根据图像尺寸和场景调没有固定值我通常从30开始试。对比的时候注意OpenCV内部用的梯度计算、边界处理可能和你的实现有细微差别所以结果不会完全一致但差异应该在亚像素级别。如果差了好几个像素那肯定是你的实现有问题。4. 光流法在视觉里程计中的实际接入方式4.1 追踪点的选取与维护策略光流法本身只负责追踪不负责选点。所以你需要一套选点策略。最直接的做法是每帧都用goodFeaturesToTrack重新检测角点但这又回到了重复计算的老路。更合理的做法是第一帧检测一批角点比如500个后续帧用光流追踪这些点追踪过程中如果成功追踪的点数低于某个阈值比如300或者某些区域的点大量丢失就补充检测新点补充检测时避开已经有点的区域避免点过于集中这个策略的关键是点的分布要均匀。如果所有点都挤在图像的一个角落位姿解算会非常不稳定。我通常会把图像分成网格每个网格里限制最多追踪多少个点这样能保证空间分布。还有一个细节追踪失败的点要及时剔除不要留在集合里。有些实现为了省事追踪失败就保留上一帧的位置这是大忌。错误的位置会污染后续的位姿估计而且错误会累积。4.2 从追踪结果到位姿2D-2D的对极几何约束光流追踪得到的是当前帧和上一帧之间的2D-2D点对。用这些点对估计位姿走的是对极几何的路线。核心是求本质矩阵Essential MatrixE然后从E中分解出旋转R和平移t。求解E用的是八点法或者五点法。OpenCV里对应findEssentialMat内部默认用五点法因为五点法只需要5个点对而且能处理平面场景。但五点法有个问题它可能返回多个解需要额外筛选。八点法虽然需要8个点但解唯一实现也简单。E, mask cv2.findEssentialMat( pts1, pts2, K, methodcv2.RANSAC, prob0.999, threshold1.0 ) _, R, t, mask_pose cv2.recoverPose(E, pts1, pts2, K)这里threshold是RANSAC的内点阈值单位是像素。设太小会导致内点太少设太大会引入外点。我一般从1.0像素开始试如果内点比例低于50%就适当放宽到2.0。recoverPose返回的t是单位向量没有尺度信息。这是单目视觉里程计的固有问题——你只能得到方向得不到绝对距离。尺度要么靠外部传感器IMU、轮速计提供要么靠场景中的已知物体要么就接受尺度漂移只做相对运动估计。4.3 三角化与深度估计把2D点变成3D点有了位姿之后下一步是把2D点对三角化成3D点这样才能构建地图也才能为后续的3D-2D位姿估计做准备。三角化的原理很简单两个相机光心发出的射线相交交点就是3D点。pts4d cv2.triangulatePoints(P1, P2, pts1.T, pts2.T) pts3d pts4d[:3] / pts4d[3]P1和P2是投影矩阵由内参K和外参[R|t]组成。三角化出来的3D点深度值必须为正否则说明点在相机后面是错误匹配。另外深度值太大的点也要剔除因为深度越大三角化的不确定性越大一个像素的误差可能导致深度差好几米。我在实际项目里会加一个深度筛选只保留深度在某个合理范围内的点比如0.5米到20米之间。这个范围根据你的应用场景定室内和室外的差别很大。4.4 局部优化光束法平差的轻量替代光流追踪加对极几何得到的位姿是有累积误差的。要减小误差需要做优化。完整的光束法平差Bundle Adjustment计算量太大实时性差。实际VO里常用的是局部光束法平差或者位姿图优化。局部BA的思路是只优化最近几帧的位姿和它们观测到的地图点更早的帧固定不动。这样既控制了计算量又能有效减小局部漂移。g2o、Ceres这些优化库都能做但配置起来比较繁琐。如果不想引入优化库一个简化的做法是用追踪到的所有点对做一次RANSAC之后用内点重新求解一次位姿。这相当于一次粗优化效果有限但聊胜于无。我早期做原型的时候就是这么干的能跑但漂移确实比加了BA的版本大。5. 那些让我卡了不止一晚上的坑5.1 光照突变灰度不变假设的直接崩塌灰度不变假设是光流法的命根子。一旦场景光照发生突变——比如自动曝光调整、灯光开关、相机快速转动导致视野变化——这个假设就不成立了追踪会大面积失败。我印象最深的一次是跑一个室内数据集相机从暗处转向窗户自动曝光瞬间调整那一帧的光流追踪几乎全军覆没位姿直接飞了。后来我的处理方式是在光流追踪之前先对图像做直方图均衡化或者自适应直方图均衡化CLAHE让灰度分布更稳定。另外如果检测到追踪成功率突然大幅下降就触发重检测而不是硬着头皮继续追踪。还有一个技巧是不要只用灰度图可以用RGB三个通道分别做光流然后取中位数或者做一致性检查。这样对单一通道的光照变化有一定鲁棒性。代价是计算量三倍但如果你用的是GPU加速这点开销可以接受。5.2 纹理缺失白墙和天空上的追踪灾难光流法依赖图像梯度。如果场景里有一大片白墙、天空、纯色桌面这些区域梯度接近零Hessian矩阵奇异光流根本解不出来。更糟糕的是这些区域检测角点也检测不到所以通常不会有点落在上面。但问题是如果相机转动导致原本有纹理的区域移出视野新进入视野的是白墙那你就没有点可以追踪了。我的应对策略是维护一个点的空间分布图如果某个区域长时间没有点就降低该区域的角点检测阈值强制检测一些点出来。虽然这些点质量不高但总比没有强。另外如果追踪到的点集中在图像的一小块区域也要警惕这说明其他区域可能纹理不足。5.3 运动模糊快速运动下的追踪失效相机快速运动时图像会产生运动模糊。模糊会抹平梯度让光流法失效。这个问题在手持设备或者车载场景里特别常见。缓解办法有几个一是提高快门速度减少曝光时间但这会引入噪声二是用陀螺仪数据做运动补偿在追踪之前先把图像旋转到大致对齐三是降低金字塔层数因为模糊在降采样之后反而可能变得不那么明显。我试过第二个方案效果最好但需要额外的传感器。如果这些都没有那就只能接受在快速运动时追踪失败靠重检测来恢复。所以一个健壮的VO系统必须有快速重检测和重定位的能力不能完全依赖光流追踪。5.4 参数调优没有万能配置光流法的参数很多窗口大小、金字塔层数、迭代次数、收敛阈值、RANSAC阈值、深度筛选范围……这些参数没有一组万能值必须根据你的场景调。我的经验是窗口大小21x21是个不错的起点纹理丰富的场景可以降到15x15纹理弱的场景可以升到31x31。金字塔层数3层起步运动大就加到4层。迭代次数30次通常够用但如果收敛慢可以加到50。RANSAC阈值从1.0像素开始内点比例低就放宽。调参的时候一定要有量化指标不能凭感觉。我一般会统计几个数追踪成功率、内点比例、重投影误差、位姿漂移量。根据这些指标来调比瞎试高效得多。6. 光流法与其他VO方案的配合使用光流法不是孤立使用的。在实际的视觉里程计系统里它通常和特征点法、直接法配合形成互补。一个常见的架构是用光流法做帧间追踪维持一个稳定的追踪点集当追踪点数量不足或者追踪质量下降时触发特征点重检测同时用直接法比如半稠密直接法在梯度明显的区域做额外的约束。这样既有光流法的高效又有特征点法的鲁棒还有直接法的稠密信息。另一个方向是和IMU融合。光流法给出的2D-2D约束加上IMU的预积分可以构成视觉惯性里程计VIO。IMU提供尺度和快速运动时的位姿预测光流法提供精确的帧间约束两者互补。这个方向是目前的主流但实现复杂度也高不少。如果你刚开始学我的建议是先把纯光流法的VO跑通理解每个环节的作用和局限然后再逐步加入其他模块。不要一上来就搞VIO那样出了问题你根本不知道是哪个环节的锅。7. 一些实测数据与性能参考为了让你对光流法的性能有个直观感受我整理了一组在典型室内场景下的实测数据。测试平台是一台普通笔记本CPU是四核八线程图像分辨率640x480追踪500个点。方案单帧耗时追踪成功率位姿漂移每米单层LK8ms62%较大快速运动时失效3层金字塔LK12ms89%约2%3层金字塔LK反向校验22ms93%约1.2%ORB特征点匹配35ms85%约1.5%从数据可以看出金字塔LK在耗时和成功率上都优于特征点匹配加了反向校验之后精度进一步提升代价是耗时翻倍。实际选择哪种方案取决于你的实时性要求和精度要求。另外追踪点数对耗时的影响是线性的。500个点12ms1000个点大约24ms。所以如果你的平台性能有限可以适当减少追踪点数但不要低于200个否则位姿估计会不稳定。8. 从光流法延伸出去还能怎么玩光流法本身是一个基础工具它的应用远不止视觉里程计。理解了光流你可以做很多有意思的事情。比如运动分割光流场里不同运动物体的光流模式不同可以通过聚类把运动物体和背景分开。这在自动驾驶里很有用能识别出行人、车辆。再比如视频稳像估计帧间的全局光流然后反向补偿就能把抖动的视频变稳。手机上的电子防抖底层用的就是这个思路。还有光流引导的特征匹配先用光流给出一个粗略的匹配位置然后在这个位置附近做特征描述子匹配既能提高匹配速度又能提高匹配准确率。这个技巧在纹理丰富的场景里特别好用。我自己最近在尝试把光流法和深度学习结合用轻量级网络预测光流然后在传统VO框架里做后端优化。初步结果还不错但离实用还有距离。这个方向值得关注但不要指望短期内替代传统方法。最后说一个我踩过的坑光流法的结果一定要做可视化检查。把追踪的点画在图像上用不同颜色表示追踪成功和失败用箭头表示光流方向。很多时候你看数值指标觉得没问题但可视化一看点全挤在一个角落或者光流方向明显不对。可视化是最直观的调试手段没有之一。