★ FEATURED ARTICLE 这篇继续往源码深处走。前面我们聊过前端特征追踪、IMU预积分、初始化框架这次把目光单独锁定在“滑窗内的纯视觉SFM”上。之所以单独拎出来讲是因为我之前很长一段时间都用现成配置跑VINS-Fusion直到需要自采数据、频繁看到日志里初始化反复失败才意识到自己对滑窗内相机位姿和特征点3D估计的理解是“知其然不知其所以然”。SFM这段代码在整个系统里像是一个地基后端紧耦合优化、IMU尺度恢复、重定位全部建立在这份“初值”之上。这篇我会从任务边界开始把相对位姿求解、三角化、PnP接力、Ceres全局优化逐层拆开最后分享一些我在实际调试中踩过的坑。1. 滑窗内SFM的任务边界从“一串2D观测”到“有几何的稀疏地图”1.1 前端到底给了我们什么先理清楚输入输出。VINS-Fusion的特征跟踪模块最终维护的是一个个特征点在滑窗内的观测轨迹每个特征点有一个全局ID记录了它出现在哪几帧、每帧上对应的像素坐标和归一化坐标。但这一层信息没有几何意义——它只知道“第20号特征点在第3、4、5帧上都看到了坐标分别是多少”并不知道这个点在世界系里到底在哪也不知道第3帧和第4帧之间的相机运动是什么。SFM在这一步要解决的就是给定滑窗内所有帧的2D观测、相机内参恢复出每一帧的相机位姿以及共同可见特征点的3D坐标。这个输出非常关键没有它IMU预积分和视觉观测之间的尺度对齐无从谈起没有它后端滑窗优化拿到手的只是一堆没有几何约束的像素残差。滑窗的长度通常不大VINS系列中默认窗口是11帧左右。在这么小的范围里做SFM本质上是在“用过去的几帧图像重建一小段局部轨迹”。你不需要全局地图但需要足够可靠的局部位姿这比全图建图简单却比两视图几何复杂得多——因为窗口里不是只有两帧而是存在大量帧间约束。1.2 为什么必须先找“基准帧”而不是随便拿一帧当世界系单目视觉里世界坐标系只有相对意义。SFM要做的第一件事是确定一个参考帧把它当成世界系的起点其他帧的位姿都相对它表达。VINS的初始化里不是拿窗口最老的那一帧当基准而是会去寻找一个“和当前帧共视点足够多、视差又足够大”的历史帧。这一步普通人容易忽略但它直接决定了SFM成败。我举一个反例假设你拿着相机原地转了一圈几乎没有平移这时窗口内所有帧对视差极小两帧间的三角化角度接近于零。这样的几何结构下3D点的深度完全不可观求出来的相机轨迹数值上非常不稳定。VINS里专门有一段逻辑做这件事遍历窗口内帧与帧之间的共视关系和平移量从中挑出相对靠谱的一对作为初始骨架。为什么要“足够多的共视点”因为后续PnP和全局优化都要靠这些点提供约束共视太少位姿两端就会出现“拔河”式的拉扯为什么要“足够大的视差”因为它决定三角化的病态程度视差不够点云会整体沿光轴方向拉长。这里还有个隐含的细节SFM求出来的平移没有物理尺度。单目相机本身拿不到绝对尺度SFM只能恢复出“归一化尺度”下的轨迹和点云。VINS后续用IMU预积分和视觉平移之间的对齐才能恢复真实的米制尺度。所以SFM阶段你不需要追求绝对的轨迹精度但必须保证几何结构是自洽的否则后面IMU尺度拟合全部白搭。2. 相对位姿求解本质矩阵与单应矩阵的竞争、选择与恢复2.1 先把像素坐标归一化再谈对极几何用两帧恢复相对位姿时第一步不是直接拿像素坐标去算本质矩阵而是要把像素坐标变换到归一化平面。原因是本质矩阵的约束建立在相机内参被“消掉”之后p2^T * E * p1 0这里的p1、p2是归一化坐标本质矩阵E隐含了内参的影响被去除。实际代码中就是用相机内参矩阵K的逆去乘齐次像素坐标// 伪代码像素坐标转归一化坐标 cv::Mat x1 K_inv * p1_homogeneous; cv::Mat x2 K_inv * p2_homogeneous;这一步看起来简单但容不得半点马虎。内参不准归一化坐标就不准后面本质矩阵分解、三角化全部跟着偏。尤其用鱼眼或者广角相机时内参标定误差在图像边缘会被放大得很厉害这也是为什么我建议自采数据前一定要先做好相机标定不要直接拿标称焦距凑合。2.2 E矩阵和H矩阵为什么要做“双保险”很多读者以为两视图几何就是算个本质矩阵就完事了但实际在VINS这种需要应对复杂场景的系统里代码是同时计算本质矩阵和单应矩阵然后根据内点数量来二选一。本质矩阵对应非平面场景用五点法求解单应矩阵对应平面场景用四点法求解。所谓“平面场景”简单说就是大部分特征点都落在一个平面上比如你正对着一面墙移动或者车辆行驶时路面占了画面的大部分。单应矩阵在平面场景下能把两帧的对应关系描述得非常好但把它强行分解成R和t时往往存在多解和退化风险。反过来本质矩阵在平面场景下计算容易退化因为特征点分布近似共面对极几何的约束变得不稳定。所以代码里的策略很务实两个模型都跑一遍RANSAC统计各自的内点数量哪个模型能解释更多特征点就用哪个。这相当于让算法根据场景自适应选择几何模型而不是赌单一模型在当前场景下一定成立。对比项本质矩阵E单应矩阵H适用场景非平面一般场景近似平面场景最小点数5点4点模型自由度5个自由度8个自由度退化风险平面场景退化复杂场景描述能力弱常见判断依据内点数多则优选内点数持续占优时使用这种“竞争式选择”在工程里很常见——与其依赖一个复杂的决策规则不如让数据自己说话。VINS风格里对这种问题有一个判断阈值通常是比较内点数比例而不是绝对数量因为两帧的共视点数可能差很多。2.3 从E分解出R和t四种候选解为什么最终只剩一个拿到本质矩阵E之后位姿不是唯一的。E本身只包含了旋转和平移的约束信息而且在相差一个符号、甚至整体旋转的情况下会产生多解。经典方法是SVD分解E U * Σ * V^T结合本质矩阵的奇异值结构可以构造出四组可能的旋转和平移组合。这四组解里只有一组能让所有三角化出来的3D点都在两帧相机前方深度为正。这一步叫cheirality校验本质上就是用一个“先验常识”——相机看到的东西在摄像机前方而不是后方——来排除掉错误的几何解。实际代码里会先随便挑一对匹配点做快速三角化分别检查这组点在四个候选位姿下的深度符号留下唯一合理的那个。这里我要提醒一点从E分解恢复出来的平移方向只有“方向”可信“大小”完全不可信因为单目SFM的尺度是任意缩放的。你不用纠结为什么平移向量的模看起来很小或者很大这是正常的真正要关注的是平移方向和旋转是否正确因为后面和IMU对齐时尺度是单独估计出来的。3. 三角化与PnP的接力让3D点铺满整个滑窗3.1 线性三角化一条叉积方程和一个SVD有了基准帧之间的相对位姿下一步就是把两帧同时看到的特征点还原成3D坐标。三角化最直觉的思路是“从两个相机位置分别向各自的观测方向射出一条射线两条射线的交点就是3D点”。但实际观测有噪声两条射线在三维空间里往往不是真正相交的所以要用最小二乘去找一个“距离两条射线都最近的”点。在代码里这被转化成一个线性方程组的求解。对每一个特征点我们有x1 P1 * X 第一帧投影方程 x2 P2 * X 第二帧投影方程其中P是3x4的投影矩阵X是待求的3D齐次坐标。每个观测的叉积约束会把3D点约束到两条直线上把两帧观都堆叠起来就得到一个超定线性方程组最终用SVD求最小二乘解。代码里叫DAMP线性三角化本质就是这个思路。// 伪代码两帧三角化一个点的核心 cv::Mat X; triangulatePoint(P1, P2, x1, x2, X); // X是齐次坐标转成3D后检查深度是否为正这步有个容易被忽视的点三角化所用的两帧之间基线长度直接决定了点的精度。基线太短比如相邻两帧才隔了0.05秒三角化的角度极小3D点的深度误差会被放大几十倍。这也是为什么VINS在初始化阶段常常不是从“紧挨着的相邻两帧”开始而是要找一个有一定时间间隔的帧对。你如果自己改代码或者调试看到某个点的深度值剧烈跳动先想想是不是三角化时选的帧对基线太短。3.2 PnP接力一个3D点就是一根桩初始帧对之间的3D点恢复之后并没有覆盖整个滑窗。接下来要用PnP把窗口里其他帧的位姿一个个“钉”出来。PnPPerspective-n-Point解决的问题是已知一组3D点和它们在某一帧上的2D投影求该帧的相机位姿。这就好比地上钉了一排桩3D点你站在某个新位置看这些桩知道它们在你的画面里落在哪些像素上反推出你站的位置和朝向。VINS里的做法是先用cv::solvePnPRansac拿到一个初始解因为RANSAC能剔除一部分误匹配然后再把位姿丢给Ceres做一次局部细化。用RANSAC而不是直接对全部点求解的原因很直接特征匹配不是100%正确的少数outlier会让最小二乘彻底跑偏。需要注意PnP每恢复出一帧新位姿视野里又会出现更多新特征点这些点可以继续三角化变成下一帧PnP的“桩”。这就是滚雪球式的迭代两帧三角化出初始3D点PnP恢复第三帧第三帧和已有帧三角化出更多3D点再用更多3D点去PnP第四帧……直到滑窗里每一帧都位姿就绪所有共视点都被三角化。3.3 为什么顺序必须是“先局部后全局”有人可能会问为什么不直接拿所有帧、所有点一次性做全局BA一步到位答案很简单非线性优化需要好的初值。全局BA的代价函数曲面上有很多局部极小值如果初始位姿差得离谱优化根本收敛不到正确解附近。所以整个滑窗SFM的顺序其实遵循了一个朴素的工程原则先搭骨架再填肉。两帧相对位姿是骨架的第一根梁三角化的点是挂在梁上的肉PnP则不断把新梁接出去等整个窗口的梁都搭完之后再让全局BA上场把所有的梁和肉一次性微调到位。这个“从粗到精、从局部到全局”的思路在SLAM和SfM领域到处都是理解一次以后看任何建图算法都轻松。4. Ceres全局BA重投影误差的打磨与优化边界4.1 残差模型让3D点重新投回去当窗口内所有帧位姿和3D点都有了一个“差不多能用”的初值后就要进入全局BA。VINS里这一步用的是Ceres误差项是重投影误差把某个3D点按当前估计的位姿投影到图像上和实际观测到的像素坐标做差。// 伪代码Ceres残差块示意 struct SFMReprojectionFactor { SFMReprojectionFactor(double x, double y) : x_(x), y_(y) {} template typename T bool operator()(const T* pose, const T* point, T* residual) const { // 把四元数平移构成的pose变换3D点 // 然后除以z得到归一化平面坐标 // 和观测值x_、y_作差 residual[0] prediction_x - x_; residual[1] prediction_y - y_; return true; } };残差的维度是2因为图像观测是2D的优化变量包括位姿通常用四元数加平移7维和3D点3维。这个模型看起来简单但它把整个滑窗内的所有约束都串起来了一个特征点出现在第1、3、5帧它就会同时约束这三帧的位姿相当于在这三帧之间拉了一根“几何橡皮筋”。在VINS的SFM BA里通常会用Huber核函数来削弱误匹配的影响。Huber核的核心思想是小残差用平方损失大残差用线性损失这样个别离谱的外点不会像最小二乘那样把整个优化往错误方向拽。这个细节值得注意因为初始化阶段的特征匹配里其实混入了一些错误匹配鲁棒核是最后的防线。4.2 固定基准帧把“规范自由度”摁住单目BA有一个数学上的麻烦整个轨迹可以有任意一个刚体变换残差完全不变。你把所有位姿和3D点一起绕原点转个90度重投影误差一点都不会变。这种自由度叫规范自由度gauge freedom如果不处理优化会变得不稳定甚至在多方向上飘移。解决方式很简单固定某一帧的位姿让它成为世界系的锚点。比如固定基准帧的旋转和平移那么整个优化就只能在这个锚点周围微调其他变量。有时候还会额外固定某一帧的平移防止整体沿着某个方向滑动。理解这一点你就明白为什么代码里总有一部分位姿参数被“锁住”不动了——不是忘记优化它们而是刻意为之。优化完之后的输出是滑窗内所有位姿和3D点的“最大后验估计”。这个结果比直接PnP得到的位姿要平滑和自洽得多因为它把所有帧、所有点的约束都考虑进来了。但它的意义依然是“初始值”而不是最终答案。4.3 和紧耦合后端优化相比SFM BA到底差在哪很多初学者会把这里的SFM BA误认为就是后端的全部其实是两回事。我拿一个表格对比一下对比项SFM BA初始化阶段后端滑窗紧耦合优化是否使用IMU不使用使用IMU预积分因子优化的状态量位姿3D点位姿速度IMU零偏逆深度是否恢复尺度否纯视觉尺度是与IMU对齐恢复米制尺度作用给出可靠初值输出最终状态估计时间尺度一次性每一帧实时运行所以你会发现SFM阶段求出的位姿在后端跑起来之后会被再次优化、大幅修正。SFM在这里的角色不是“最终答案”而是“一个足够好的起跑线”。它保证IMU初始化和后端优化不会从一个离谱的初始猜测开始从而避免陷入错误局部极小值。5. 调试滑窗SFM的实操经验退化场景、参数旋钮与日志观察5.1 纯旋转、平面场景、低视差初始化失败的三大元凶纸上谈兵讲完原理聊聊实际调试时最常见的翻车场景。第一个坑是纯旋转。SFM要有3D信息本质上必须有多视角的视差而纯旋转时相机光心不动所有射线都从同一个点出发永远无法三角化定位。VINS里的初始化对这种运动非常敏感如果你拿着设备原地转动想让它初始化大概率会失败。解决办法是初始化阶段先做一段带明显平移的运动等系统稳定了再旋转。第二个坑是纯平面场景。对着白墙、桌面这类近似平面运动时本质矩阵退化代码会倾向于使用单应矩阵。但单应矩阵本身在平面场景下恢复出的相对位姿也有退化风险有时三角化出来的点会整体铺在平面上深度几乎没有区分度。这个问题的信号是虽然日志里显示初始化成功但3D点云看起来像一张“贴纸”深度分布极不均匀。第三个坑是低视差。前面反复提到视差决定三角化精度。如果你的相机帧率很高、运动又慢滑窗内的帧之间几乎没什么平移那么SFM算出来的位姿会反复抖动。我实测下来初始化阶段尽量让设备有足够的加减速和转向让滑窗里“拉开距离”的帧对多起来比调什么参数都管用。5.2 我调参时最常动的几个阈值调试SFM初始化时有几个参数值得关注视差阈值判断“某两帧之间是否有足够视差”的像素阈值。设得太小会把很多低视差的帧对误判为可靠设得太大很难找到合适的基准帧。经验上小分辨率图像用较小阈值高分辨率图像可以适当放大。最小共视点数两帧之间至少要有的共同特征点数量太少了求解本质矩阵都不够稳定。RANSAC内点阈值判定一个特征点是否为内点的重投影误差阈值。阈值太紧会让内点率看起来很低导致系统频繁误判为初始化失败。特征点数量上限前端能提取的特征点太少后面所有几何都无从谈起。如果自采数据纹理弱可以适当提高特征点数量上限。我自己的习惯是先不急着改这些阈值而是回头检查原始数据图像是否清晰、有没有运动模糊、帧率是否稳定。很多初始化失败的本质是数据质量不行而不是算法参数不对。5.3 如何从日志和可视化里快速定位问题VINS类系统的日志里初始化模块通常会打印出当前的状态找到的共视帧、相对位姿是否成功、三角化点云数量、内点比例等。我调试时会盯这几个关键信号如果日志长时间停留在“寻找足够视差”之类的状态说明前端特征太少或运动不够优先检查特征提取结果和图像质量。如果相对位姿求解反复失败但画面里特征看着很多多半是内参不准或者退化场景可以尝试换一个运动轨迹复现。如果初始化成功了但点云密集度低、分布只集中在少数几帧说明某几帧的PnP链条断了可能是中途出现了大量误匹配。可视化也很有用。我会把初始化输出的位姿和稀疏点云显示出来先看轨迹是否平滑再看点云深度是否有层次感。如果轨迹出现明显跳变或者点云呈“一串珠子”状态基本就是某次PnP或三角化出了问题。定位到具体是哪两帧之间的连接崩了再回去翻日志和图像匹配细节通常能很快找到原因。最后说一个我在实际使用中总结出来最有效的技巧自采数据时初始化阶段刻意让设备做一个“先平移、再旋转、再平移”的S形运动避免长时间纯旋转和低视差。这套动作不需要很精确但能让SFM拿到足够丰富的几何约束。代码层面的参数我反而很少动——大多数场景下默认参数在好的数据上都能一遍过。如果你发现默认参数下初始化一直失败建议优先怀疑数据和运动模式而不是立刻去调阈值。
阅读完成 · 觉得有帮助？
