做视觉SLAM的各位应该都有过这种体验明明前端跟踪没毛病后端优化看起来也对但地图点就是飘、姿态就是抖。行内人都知道问题大概率出在滑窗里的SFM环节——相机位姿没锁准3D点自然白扯。这篇接着系列往下聊专门说说Vins-Fusion处理滑窗内SFM的关键动作从两两相对位姿估计到三角化再到位姿和3D点的联合优化同时把我调试过程中踩过的坑一并晒出来。这套东西适合谁看正准备接手VINS系代码、被初始化弄得头大的人或者已经在跑自己数据但是效果不稳、想搞明白“到底该查前端还是后端”的人。我把原理逻辑和实操经验揉在一起讲尽量让没有系统学过视觉几何的读者也能跟上。1. SFM在Vins-Fusion滑窗里到底扮演什么角色1.1 滑窗不是静态快照而是动态测量系统很多新手看Vins-Fusion的代码第一反应是找“建图模块”结果翻半天发现根本没什么显式的重建流程只有一堆位姿、状态量、残差。实际上整条pipeline的核心就是“边估位姿边估点”只不过它把这个问题限制在一个很小的滑窗里反复做。滑窗一般维持10到13帧关键帧不同版本和配置略有差异窗口每往前滑一步就丢掉最老的帧引入新帧。这个窗口本质上是一个动态的局部测量系统新关键帧带来新的观测老关键帧要被边缘化中间的所有帧和特征点需要持续更新。SFM就是在这个窗口里完成的“视觉结构恢复”——它既不是离线跑一次就完事也不是纯前端的跟踪而是整个状态估计器的初始化器和校正器。我习惯打一个比方全局BA是修一栋大楼滑窗SFM是每次只修你所在的那一层但是每修一层都会把承重墙浇好等修到顶楼时整栋楼不会塌。这个“每一层的承重墙”就是滑窗里被反复验证过的相机位姿和3D点。1.2 为什么必须用滑窗而不是全局BA理论上SFM最“干净”的做法是把所有帧和所有点一次性拿来做光束平差得到全局最优解。但现实里这根本跑不动——移动机器人连续跑一个小时可能积累上万帧、几十万个特征点全局BA的计算量爆炸。更麻烦的是环境是会变的光照会变、动态物体会进出视野你辛辛苦苦把所有历史帧锁在一起一旦某段轨迹漂了后期花多大力气也拉不回来。所以Vins-Fusion采用滑窗策略本质上是在精度和实时性之间找一个工程上可接受的平衡点。滑窗的尺寸决定了局部结构的记忆长度窗口太小位姿之间约束太少视觉退化和尺度漂移很容易爆发窗口太大后端优化的实时性就保不住尤其还要和IMU做紧耦合的时候。我实测下来10到13帧是一个很稳定的区间既能让视差足够覆盖同一个特征点的多次观测又不至于让后端优化单次迭代耗时超过几十毫秒。另一个经常被忽略的理由是“不确定性管理”。全局BA总假设所有误差是高斯、独立的但实际跑数据时早期帧的误差结构早就不准了硬拉进去参与优化反而会污染后面的估计。滑窗的意义在于老帧的占用信息被压缩成先验项而不是让原始观测“阴魂不散”地一直干扰新估计。1.3 数据流动从图像到位姿再到3D点的管线在滑窗SFM里数据流向是严格分层的。第一步前端拿到图像序列通过特征提取和光流跟踪得到每一帧上特征点的2D像素坐标并且把不同帧之间同一物理点的对应关系维护起来。第二步利用这些2D对应关系估计相邻关键帧之间的相对位姿。第三步有了位姿用多视图三角化恢复特征点的3D坐标。第四步把相机位姿和3D特征点都放进同一个优化目标里做局部BA让两者互相纠正。注意第四步不是说“第三步做完再优化一次”而是在滑窗内反复进行的。我把SFM和BA理解成一个连续交互的过程SFM提供初始几何BA把它打磨结实BA打磨完的位姿又会让下一轮SFM三角化更准确。这个循环才叫“联合估计”很多刚起步的人把SFM当作离线预处理概念上就跑偏了。2. 相机位姿估计从匹配点到相对运动2.1 特征跟踪产出的“测量值”长什么样进入位姿估计前得先搞清楚输入到底是什么。Vins-Fusion前端的特征点库里每个特征点记录了一串观测在哪些帧出现、每次出现时的像素坐标、归一化坐标、特征描述子或者光流跟踪时的patch信息。我们要用的是归一化坐标也就是把像素坐标经过内参变换到相机坐标系下的一个方向向量。为什么用归一化坐标因为本质矩阵和单应矩阵模型都建立在对极约束的几何关系上本身不关心内参提前归一化可以让问题更干净。实际代码里这一步通常是先将像素坐标减掉主点再除以焦距得到camera frame下的坐标即x(u-cx)/fx。这里有个常见坑如果你手里的内参是标定好的归一化只按原图尺寸做一次即可。如果用了图像缩放必须把内参同步缩放否则后面所有的阈值判断都会失效。跟踪质量直接决定位姿估计的上限。我通常要求一个关键帧被至少跟踪到15到20个特征点光流成功且通过双向一致性检查少于这个数量我会直接拒绝把该帧设为关键帧继续等待新帧积累视差。这看起来保守实际上能省掉大量RANSAC拉锯时间。2.2 本质矩阵与单应矩阵的选择逻辑两帧之间的相对位姿估计核心问题是从2D对应关系中恢复R和t。根据场景不同同一个问题可以用两种模型本质矩阵E适合大部分普通三维场景自由度为5最少5个点对可解即五点法。E t^× R分解后直接得到相对旋转和平移方向。单应矩阵H当场景近似平面或者相机发生纯旋转时本质矩阵模型会退化此时用H二维单应更稳定。H的自由度为8最少4个点对。VINs-Fusion的初始化模块里并不会彻底放弃任何一个模型而是把H和E都估出来用统一的评分机制比较RANSAC内点数和重投影残差谁更优就选谁。这个“双模型竞争”的做法我认为极其重要。如果只跑E模型你在走廊墙面前跑直线特征点几乎都落在一个平面上本质矩阵会非常病态初始化常常炸掉如果只跑H模型遇到真正的非平面场景单应矩阵拟合出的模型同样不靠谱。实操中的经验是模型选择不要只看内点数还要看残差的分布。如果H模型内点残差普遍很大而E模型内点残差小而集中哪怕H内点数更高也优先选择E。因为平面场景下H的内点可能很多但那些内点的几何约束本身就很弱用于位姿解算反而会把噪声放大。2.3 RANSAC内点阈值到底怎么给RANSAC阈值的选择经常被随便填实际上它直接关系到位姿的精度和鲁棒性。阈值过小真实匹配都被当成外点丢弃内点率难看阈值过大大量错匹配混进来E矩阵被带偏。在归一化平面坐标的情况下RANSAC的距离度量通常是对极线距离。我把阈值设在0.002到0.01之间归一化单位对应像素大约0.2到0.8个像素在640x480的图像上归一化1.0大约对应几百像素所以这个区间很敏感。具体值需要根据你图像噪声和标定精度来定。平时跑车载数据图像噪声大一点我会放到0.01如果是实验室高分辨率静物场景0.002就能跑得很好。还有一点容易被忽略RANSAC的迭代次数和置信度要配合。一般设置信度0.99初始内外点比例未知时迭代次数从几十到几百动态决定。我见过有人为了“省时间”把最大迭代次数锁死在50结果在内点率低的场景里解出来的模型完全是噪声拟合。RANSAC本身不贵贵的是你对它过度信任。2.4 分解出R和t之后不能直接用于建图从E分解出R和t是个数学上很优雅、工程上很麻烦的过程。E矩阵本质上是3x3矩阵但它必须满足两个奇异值相等、第三个为0的约束。实际计算出来的E由于噪声不会完美满足这个结构所以通常先做一次SVD然后把奇异值强制设为(1, 1, 0)附近再重新合成。分解时R存在两个候选解加上t的方向正负一共4种组合。正确的组合需要靠三角化后的“正深度检验”来挑选把特征点三角化看它是否同时在两个相机的正前方深度大于零一致的那个组合才被认为是正确相对位姿。这一步是失败高发区。我见过很多次初始化崩溃不是因为E分解算错了而是因为深度检验只做了单帧检查或者只检查了一小部分点。正确做法是对所有被模型覆盖的内点做三角化统计深度大于零的比例同时检查某几个点的重投影误差综合投票。只检查少数几个点一旦这几点恰好是外点你就选了一个完全错误的位姿组合后面整个滑窗都救不回来。3. 特征点3D位置估计三角化的工程实现3.1 三角化本质上是解超定方程有了相机位姿下一步是把特征点从2D映射成3D。这个动作看起来像是“两束射线求交点”但实际上因为观测噪声两条射线在空间里根本不交而是错开的。所以三角化本质是在解一个最小二乘问题找一个3D点让它在所有观测相机上的重投影误差尽可能小。工程上最通用的是线性三角化DLT一类的方法每个观测都提供两个方程x坐标和y坐标各一个把X的三个未知数或齐次形式的四个未知数代入最后得到一个超定线性方程组AX 0。解法是SVD分解取最小奇异值对应的右奇异向量归一化后得到3D坐标。这里我要特别提醒一个理论之外的问题输入点位必须先做归一化。直接在像素坐标上做DLT数值条件会很差误差大的离谱。正确的做法是先在每幅图像上做坐标归一化这不同于前面提到的相机内参归一化将所有2D坐标缩放和中心化解完再把结果变换回世界坐标。这一步在很多简化的教学代码里被省掉我教你一个判断方法如果三角化出的点深度经常是负的或者重投影误差不稳定优先检查有没有做归一化。3.2 微小视差会让解病态三角化的精度对“视差角”极其敏感。视差角就是相机在观测同一个点时两条光线的夹角。夹角太小意味着两帧相机位置几乎没变化3D点的深度就从噪声中彻底不可观了。我在实际数据里验证过对一个距离10米的点如果基线只有0.1米视差角不到0.6度三角化出的深度方差会大到一个离谱的程度——看起来重投影误差很小但3D点位置完全不可信。所以滑窗SFM里一定要加视差检验要么算两个相机光心到特征点的光线夹角要么更直观地在图像上数特征点位移像素数。VINS这类系统一般会判断“足够的视差才把当前帧设为关键帧”目的就是为了保证后续三角化和优化的几何条件不至于病态。我在自己的参数表里设了一个最小视差角阈值大约0.5度到1度具体取决于特征点噪声水平。噪声越大阈值越要往高调。如果你发现三角化出的点跟着噪声抖动先把视差阈值往上拉比你去调RANSAC阈值更管用。另一个经验是不要在只有2帧观测时就把3D点锁死。滑窗里一个特征点至少要被3到5帧连续观测到才值得把它纳入优化状态。观测次数太少三角化的不确定度没有充分约束加入BA只会增加计算量不会带来精度收益。3.3 给3D点做体检重投影、深度、共线三角化完毕不能直接“上户口”。我习惯对每个3D点做三项体检至少两项通过才留用重投影误差把3D点重新投影到每一帧图像上计算与原始观测的像素距离。一般要求小于2像素。这个指标很直观但只能反映“是否与观测一致”不能说明“是否真的准”。深度正负检查点在各个相机坐标系下的深度是否都大于零。这个必须严格通过允许少数异常点被剔除。共线检查检查3D点到相机光心的连线方向与2D观测方向的一致性尤其在多帧观测时如果观测之间的方向变化很乱说明跟踪大概率已经漂了。还有一个工程细节当一个3D点在后续滑窗中不断被新观测更新时我建议定期用滑窗内所有观测重新做一次带约束的三角化而不是只保留第一次三角化结果。因为位姿在BA中会更新初始三角化的点不再严格满足新位姿下的几何关系定期“换一次底”能显著减少累计误差。4. 滑窗内联合优化SFM和BA的衔接4.1 怎么把两两位姿拼成一致轨迹SFM输出的本质是相对位姿和3D点但要真正发挥价值必须把散落的相对位姿统一到滑窗的一个坐标系下。这个统一过程通常以滑窗里较老的一帧为参考系通过位姿乘法把所有帧的绝对位姿算出来。第一次初始化时可能用第一帧作为世界坐标系原点后续滑窗滑动时需要保证新老坐标系之间的一致性。这一步的坑在于你从E分解出的t是单位尺度它没有任何物理尺度信息。在纯视觉滑窗里尺度本来就是自由漂移的。Vins-Fusion之所以引入IMU预积分就是要把重力方向和尺度拉回真实的物理量纲。如果你的场景里只有纯视觉没有IMU那你必须接受sfm结果在无尺度空间内更新不能直接拿去当真实轨迹。拼接轨迹时我强烈建议用李群SE(3)上做插值和平均而不是把旋转矩阵拆成欧拉角来融合。欧拉角在接近90度时会奇异性爆表哪怕你数据里根本转不到那个角度数值上也会出现奇怪的跳动。代码里直接用四元数或旋转矩阵的log/exp映射至少能保证轨迹的连续性。4.2 滑窗BA为什么要走稀疏求解到位姿和3D点都有了初值接下来就是局部BA的活。BA的数学模型并不复杂最小化所有残差的平方和其中残差是3D点在某一帧的重投影误差。复杂的是怎么快速求解。假设滑窗里有11帧、200个特征点那么未知量就是11个位姿每个6自由度加尺度信息和200个3D点每个3自由度。这个规模看着不大但直接对Cartesian坐标求导并填充Hessian矩阵矩阵会非常庞大且稠密。幸好BA的Jacobian天然带有稀疏结构某个残差只连接一个3D点和一帧位姿于是整个Hessian矩阵呈块状稀疏布局绝大部分元素是零。利用这个稀疏性标准做法是先对特征点块做Schur消元也就是先在数学上把3D点的量“消”掉只剩下位姿相关的量最后组成一个维度小很多的位姿增量线性系统。我见过有人为了省事直接调用通用非线性优化库却不开稀疏求解器结果每轮迭代耗时几十毫秒滑窗变成幻灯片。正确姿势一定是用精求解器开启SCHUR_STRATEGY和稀疏线性代数把边缘化掉的路标点块提前消元速度可以提升一个数量级。这里再提一个能提升稳定性的技巧在BA迭代前把图像坐标投影误差乘以一个huber核函数鲁棒核从而抑制大误差项对解的牵引。Vins-Fusion的残差实现里也引入了这类鲁棒化处理我的经验是误匹配率在1%3%时加了huber核能明显改善优化收敛性。如果你发现BA结果在一次误匹配下就大跳八成是核函数没加或者阈值设置太宽松。4.3 边缘化把老帧的贡献折算成先验滑窗每前进一步最老的关键帧就要被移出窗口。直接把它丢掉会损失掉它与其他帧之间的约束信息系统会在某一刻“忘掉”过去导致漂移。所以这里要用边缘化Marginalization把被移除帧的所有约束信息压缩成一个先验概率项附加到后续的优化问题中。边缘化的数学工具是Schur补。把要删除的变量从信息矩阵中消掉剩下的信息矩阵加上测量贡献就形成新的先验。我听得懂但真正写代码时比较容易忘记的是边缘化的前提是线性化点固定。一旦先验项在你当前的雅可比线性化点生成之后不要再用新雅可比反复线性化它否则信息会反复放大导致所谓的“信息矩阵重复注入”系统会假性乐观——协方差变小、误差却不减。这个坑我在调参时撞过一次因为在边缘化之后没有冻结线性化点程序的RMSE看似越来越小但实际轨迹和真值越拉越远。排查了半天最后发现是先验项被重复线性化了几次信息矩阵里的值凭空大了好几倍。修复之后系统才恢复正常。所以你看边缘化这一环节正确性比代码性能更重要。5. 常见问题与排查记录5.1 初始化总是失败的常见原因如果你发现Vins-Fusion在第一次初始化时反复失败先别怀疑调参多数是数据本身不够格。我看过很多数据集特征是够多但相机几乎在原地转圈纯旋转基础上平移量很小这时候本质矩阵模型退化。你可以打印出H和E模型各自的内点数来确认如果H模型内点数远超E模型且场景看起来有大块平面那基本就是低视差或旋转退化问题。解决方法是等相机平移量积累起来再触发初始化。你可以手动控制机器人在初始化阶段做一个带明显平移的八字绕行而不是原地转圈。另外初始化时不要用模糊帧运动模糊会让光流跟踪的像素位置整体偏移RANSAC再怎么聪明也救不回系统性偏差。有个更隐蔽的原因内参没对准输出图像的尺寸。我遇到过一次图像被resize到一半分辨率标定内参却没跟着缩放出新主点和焦距初始化阶段特征点归一化之后全部偏了结果H/E模型内点数惨不忍睹。这个问题只靠肉眼看不出但我建议你在代码里打印几个点的归一化坐标跟真实物理方向对一对能快速定位是不是内参坐标系搞错了。5.2 3D点在两次三角化后跳变同一个特征点前几帧三角化出来的3D坐标在一个位置滑窗滚过几帧之后再做一次三角化3D点位置突然跳了几米。这是非常典型的“三角化时视差角不够”掷出来的病态解。我第一次遇到时以为是前端跟踪崩了但其实前端很稳问题就是当帧的基线和深度比太低3D点深度方向上的不确定性高达数米重投影误差却很小看起来很正常。排查方法是把“该特征点所有观测帧”标出来逐帧打印出三角化时的视差角。视差角小于阈值时哪怕重投影误差再小也不要更新这个3D点。另一个经验是使用多帧观测做三角化比只用两帧要稳得多。多出来的帧可以让最小二乘残差在深度方向上形成更好的约束。如果你只有一个特征库但不记得它被多少帧观测过可以统计一下“每个点在滑窗内的观测总数”低于3帧的一律不用它做定位。5.3 先验一致性破坏后的漂移特征边缘化先验出问题的时候系统的症状很有迷惑性姿态看起来很平滑但轨迹会随时间缓慢往一个方向偏越拉越远。这种漂移和前端特征跟踪错误的“突变性抖动”不一样是慢性病。你检查IMU看看没异常检查3D点重投影误差也不大但全世界点云和轨迹就像被一股无形的力缓慢拖拽。大概率出在先验项的线性化点被重复更新或者先验项残差的雅可比计算中使用的新位姿和生成先验时不一致。我自己的排错流程是把滑窗边缘化前后的信息矩阵导出看对角元素是否异常增大如果增大超过一个数量级就基本坐实了重复注入。再检查先验残差模块中“固定线性化点”那个开关有没有在代码里丢失。这种问题在纯视觉定位场景里更明显因为IMU还能提供一部分参考多少能掩盖先验错误。一旦你的部署环境里IMU噪声很大视觉边缘化先验的问题就会被放大你就更难怀疑到先验头上了。5.4 参数调优的心得控制不住的掉帧和漂移往往不是某一个参数的问题而是参数之间不匹配。我的经验是按下图这个顺序来调不要跳着调先确认滑窗大小1013不被拍脑袋改再确认关键帧的最小视差阈值0.5度到1度只做微调接着看RANSAC阈值和最大迭代次数归一化阈值0.0020.01最后才动特征点跟踪窗口尺寸和金字塔层数。乱调参数的后果是你换了阈值AB的问题被暴露你换B又让A的问题回来陷入死循环。另外一个心得是写一个小的“诊断日志”功能每隔几帧输出滑窗内平均重投影误差、内点数、最老帧到当前帧的平移量。调试时开着它跑一小段就能快速判断系统状态。我每接手一个新场景数据第一件事就是开着诊断日志走一遍看看初始化和滑窗内的残差变化趋势比盯着一堆轨迹图发呆有效太多。滑窗SFM这部分做到位最大的感受就是后端的BA优化变“聪明”了它不再会因为一次误匹配而翻车也不会因为几帧的低视差而产出飘移点。前期的特征跟踪、位姿估计、三角化这些看似基础的操作直接决定了全系统最终能跑多稳这不是公式推导能带给你的经验而是反复调数据积累出来的肌肉记忆。我个人到现在还保留着一个习惯每次改动三角化模块的参数我都会用一组带真值的仿真数据做回归测试看看深度误差随视差角变化的曲线。这能最快暴露你把阈值调过头了还是调不到位了。以后哪位朋友要是被困在“初始化失败”“点云漂移”“轨迹慢性漂”这三个坑里希望这篇文章能帮你绕开大部分雷区。 
长河君 · 2026/10/20 15:40:49 · 阅读 1.2k Vins-Fusion滑窗SFM全解析：从位姿估计到联合优化的工程实践
做视觉SLAM的各位应该都有过这种体验明明前端跟踪没毛病后端优化看起来也对但地图点就是飘、姿态就是抖。行内人都知道问题大概率出在滑窗里的SFM环节——相机位姿没锁准3D点自然白扯。这篇接着系列往下聊专门说说Vins-Fusion处理滑窗内SFM的关键动作从两两相对位姿估计到三角化再到位姿和3D点的联合优化同时把我调试过程中踩过的坑一并晒出来。这套东西适合谁看正准备接手VINS系代码、被初始化弄得头大的人或者已经在跑自己数据但是效果不稳、想搞明白“到底该查前端还是后端”的人。我把原理逻辑和实操经验揉在一起讲尽量让没有系统学过视觉几何的读者也能跟上。1. SFM在Vins-Fusion滑窗里到底扮演什么角色1.1 滑窗不是静态快照而是动态测量系统很多新手看Vins-Fusion的代码第一反应是找“建图模块”结果翻半天发现根本没什么显式的重建流程只有一堆位姿、状态量、残差。实际上整条pipeline的核心就是“边估位姿边估点”只不过它把这个问题限制在一个很小的滑窗里反复做。滑窗一般维持10到13帧关键帧不同版本和配置略有差异窗口每往前滑一步就丢掉最老的帧引入新帧。这个窗口本质上是一个动态的局部测量系统新关键帧带来新的观测老关键帧要被边缘化中间的所有帧和特征点需要持续更新。SFM就是在这个窗口里完成的“视觉结构恢复”——它既不是离线跑一次就完事也不是纯前端的跟踪而是整个状态估计器的初始化器和校正器。我习惯打一个比方全局BA是修一栋大楼滑窗SFM是每次只修你所在的那一层但是每修一层都会把承重墙浇好等修到顶楼时整栋楼不会塌。这个“每一层的承重墙”就是滑窗里被反复验证过的相机位姿和3D点。1.2 为什么必须用滑窗而不是全局BA理论上SFM最“干净”的做法是把所有帧和所有点一次性拿来做光束平差得到全局最优解。但现实里这根本跑不动——移动机器人连续跑一个小时可能积累上万帧、几十万个特征点全局BA的计算量爆炸。更麻烦的是环境是会变的光照会变、动态物体会进出视野你辛辛苦苦把所有历史帧锁在一起一旦某段轨迹漂了后期花多大力气也拉不回来。所以Vins-Fusion采用滑窗策略本质上是在精度和实时性之间找一个工程上可接受的平衡点。滑窗的尺寸决定了局部结构的记忆长度窗口太小位姿之间约束太少视觉退化和尺度漂移很容易爆发窗口太大后端优化的实时性就保不住尤其还要和IMU做紧耦合的时候。我实测下来10到13帧是一个很稳定的区间既能让视差足够覆盖同一个特征点的多次观测又不至于让后端优化单次迭代耗时超过几十毫秒。另一个经常被忽略的理由是“不确定性管理”。全局BA总假设所有误差是高斯、独立的但实际跑数据时早期帧的误差结构早就不准了硬拉进去参与优化反而会污染后面的估计。滑窗的意义在于老帧的占用信息被压缩成先验项而不是让原始观测“阴魂不散”地一直干扰新估计。1.3 数据流动从图像到位姿再到3D点的管线在滑窗SFM里数据流向是严格分层的。第一步前端拿到图像序列通过特征提取和光流跟踪得到每一帧上特征点的2D像素坐标并且把不同帧之间同一物理点的对应关系维护起来。第二步利用这些2D对应关系估计相邻关键帧之间的相对位姿。第三步有了位姿用多视图三角化恢复特征点的3D坐标。第四步把相机位姿和3D特征点都放进同一个优化目标里做局部BA让两者互相纠正。注意第四步不是说“第三步做完再优化一次”而是在滑窗内反复进行的。我把SFM和BA理解成一个连续交互的过程SFM提供初始几何BA把它打磨结实BA打磨完的位姿又会让下一轮SFM三角化更准确。这个循环才叫“联合估计”很多刚起步的人把SFM当作离线预处理概念上就跑偏了。2. 相机位姿估计从匹配点到相对运动2.1 特征跟踪产出的“测量值”长什么样进入位姿估计前得先搞清楚输入到底是什么。Vins-Fusion前端的特征点库里每个特征点记录了一串观测在哪些帧出现、每次出现时的像素坐标、归一化坐标、特征描述子或者光流跟踪时的patch信息。我们要用的是归一化坐标也就是把像素坐标经过内参变换到相机坐标系下的一个方向向量。为什么用归一化坐标因为本质矩阵和单应矩阵模型都建立在对极约束的几何关系上本身不关心内参提前归一化可以让问题更干净。实际代码里这一步通常是先将像素坐标减掉主点再除以焦距得到camera frame下的坐标即x(u-cx)/fx。这里有个常见坑如果你手里的内参是标定好的归一化只按原图尺寸做一次即可。如果用了图像缩放必须把内参同步缩放否则后面所有的阈值判断都会失效。跟踪质量直接决定位姿估计的上限。我通常要求一个关键帧被至少跟踪到15到20个特征点光流成功且通过双向一致性检查少于这个数量我会直接拒绝把该帧设为关键帧继续等待新帧积累视差。这看起来保守实际上能省掉大量RANSAC拉锯时间。2.2 本质矩阵与单应矩阵的选择逻辑两帧之间的相对位姿估计核心问题是从2D对应关系中恢复R和t。根据场景不同同一个问题可以用两种模型本质矩阵E适合大部分普通三维场景自由度为5最少5个点对可解即五点法。E t^× R分解后直接得到相对旋转和平移方向。单应矩阵H当场景近似平面或者相机发生纯旋转时本质矩阵模型会退化此时用H二维单应更稳定。H的自由度为8最少4个点对。VINs-Fusion的初始化模块里并不会彻底放弃任何一个模型而是把H和E都估出来用统一的评分机制比较RANSAC内点数和重投影残差谁更优就选谁。这个“双模型竞争”的做法我认为极其重要。如果只跑E模型你在走廊墙面前跑直线特征点几乎都落在一个平面上本质矩阵会非常病态初始化常常炸掉如果只跑H模型遇到真正的非平面场景单应矩阵拟合出的模型同样不靠谱。实操中的经验是模型选择不要只看内点数还要看残差的分布。如果H模型内点残差普遍很大而E模型内点残差小而集中哪怕H内点数更高也优先选择E。因为平面场景下H的内点可能很多但那些内点的几何约束本身就很弱用于位姿解算反而会把噪声放大。2.3 RANSAC内点阈值到底怎么给RANSAC阈值的选择经常被随便填实际上它直接关系到位姿的精度和鲁棒性。阈值过小真实匹配都被当成外点丢弃内点率难看阈值过大大量错匹配混进来E矩阵被带偏。在归一化平面坐标的情况下RANSAC的距离度量通常是对极线距离。我把阈值设在0.002到0.01之间归一化单位对应像素大约0.2到0.8个像素在640x480的图像上归一化1.0大约对应几百像素所以这个区间很敏感。具体值需要根据你图像噪声和标定精度来定。平时跑车载数据图像噪声大一点我会放到0.01如果是实验室高分辨率静物场景0.002就能跑得很好。还有一点容易被忽略RANSAC的迭代次数和置信度要配合。一般设置信度0.99初始内外点比例未知时迭代次数从几十到几百动态决定。我见过有人为了“省时间”把最大迭代次数锁死在50结果在内点率低的场景里解出来的模型完全是噪声拟合。RANSAC本身不贵贵的是你对它过度信任。2.4 分解出R和t之后不能直接用于建图从E分解出R和t是个数学上很优雅、工程上很麻烦的过程。E矩阵本质上是3x3矩阵但它必须满足两个奇异值相等、第三个为0的约束。实际计算出来的E由于噪声不会完美满足这个结构所以通常先做一次SVD然后把奇异值强制设为(1, 1, 0)附近再重新合成。分解时R存在两个候选解加上t的方向正负一共4种组合。正确的组合需要靠三角化后的“正深度检验”来挑选把特征点三角化看它是否同时在两个相机的正前方深度大于零一致的那个组合才被认为是正确相对位姿。这一步是失败高发区。我见过很多次初始化崩溃不是因为E分解算错了而是因为深度检验只做了单帧检查或者只检查了一小部分点。正确做法是对所有被模型覆盖的内点做三角化统计深度大于零的比例同时检查某几个点的重投影误差综合投票。只检查少数几个点一旦这几点恰好是外点你就选了一个完全错误的位姿组合后面整个滑窗都救不回来。3. 特征点3D位置估计三角化的工程实现3.1 三角化本质上是解超定方程有了相机位姿下一步是把特征点从2D映射成3D。这个动作看起来像是“两束射线求交点”但实际上因为观测噪声两条射线在空间里根本不交而是错开的。所以三角化本质是在解一个最小二乘问题找一个3D点让它在所有观测相机上的重投影误差尽可能小。工程上最通用的是线性三角化DLT一类的方法每个观测都提供两个方程x坐标和y坐标各一个把X的三个未知数或齐次形式的四个未知数代入最后得到一个超定线性方程组AX 0。解法是SVD分解取最小奇异值对应的右奇异向量归一化后得到3D坐标。这里我要特别提醒一个理论之外的问题输入点位必须先做归一化。直接在像素坐标上做DLT数值条件会很差误差大的离谱。正确的做法是先在每幅图像上做坐标归一化这不同于前面提到的相机内参归一化将所有2D坐标缩放和中心化解完再把结果变换回世界坐标。这一步在很多简化的教学代码里被省掉我教你一个判断方法如果三角化出的点深度经常是负的或者重投影误差不稳定优先检查有没有做归一化。3.2 微小视差会让解病态三角化的精度对“视差角”极其敏感。视差角就是相机在观测同一个点时两条光线的夹角。夹角太小意味着两帧相机位置几乎没变化3D点的深度就从噪声中彻底不可观了。我在实际数据里验证过对一个距离10米的点如果基线只有0.1米视差角不到0.6度三角化出的深度方差会大到一个离谱的程度——看起来重投影误差很小但3D点位置完全不可信。所以滑窗SFM里一定要加视差检验要么算两个相机光心到特征点的光线夹角要么更直观地在图像上数特征点位移像素数。VINS这类系统一般会判断“足够的视差才把当前帧设为关键帧”目的就是为了保证后续三角化和优化的几何条件不至于病态。我在自己的参数表里设了一个最小视差角阈值大约0.5度到1度具体取决于特征点噪声水平。噪声越大阈值越要往高调。如果你发现三角化出的点跟着噪声抖动先把视差阈值往上拉比你去调RANSAC阈值更管用。另一个经验是不要在只有2帧观测时就把3D点锁死。滑窗里一个特征点至少要被3到5帧连续观测到才值得把它纳入优化状态。观测次数太少三角化的不确定度没有充分约束加入BA只会增加计算量不会带来精度收益。3.3 给3D点做体检重投影、深度、共线三角化完毕不能直接“上户口”。我习惯对每个3D点做三项体检至少两项通过才留用重投影误差把3D点重新投影到每一帧图像上计算与原始观测的像素距离。一般要求小于2像素。这个指标很直观但只能反映“是否与观测一致”不能说明“是否真的准”。深度正负检查点在各个相机坐标系下的深度是否都大于零。这个必须严格通过允许少数异常点被剔除。共线检查检查3D点到相机光心的连线方向与2D观测方向的一致性尤其在多帧观测时如果观测之间的方向变化很乱说明跟踪大概率已经漂了。还有一个工程细节当一个3D点在后续滑窗中不断被新观测更新时我建议定期用滑窗内所有观测重新做一次带约束的三角化而不是只保留第一次三角化结果。因为位姿在BA中会更新初始三角化的点不再严格满足新位姿下的几何关系定期“换一次底”能显著减少累计误差。4. 滑窗内联合优化SFM和BA的衔接4.1 怎么把两两位姿拼成一致轨迹SFM输出的本质是相对位姿和3D点但要真正发挥价值必须把散落的相对位姿统一到滑窗的一个坐标系下。这个统一过程通常以滑窗里较老的一帧为参考系通过位姿乘法把所有帧的绝对位姿算出来。第一次初始化时可能用第一帧作为世界坐标系原点后续滑窗滑动时需要保证新老坐标系之间的一致性。这一步的坑在于你从E分解出的t是单位尺度它没有任何物理尺度信息。在纯视觉滑窗里尺度本来就是自由漂移的。Vins-Fusion之所以引入IMU预积分就是要把重力方向和尺度拉回真实的物理量纲。如果你的场景里只有纯视觉没有IMU那你必须接受sfm结果在无尺度空间内更新不能直接拿去当真实轨迹。拼接轨迹时我强烈建议用李群SE(3)上做插值和平均而不是把旋转矩阵拆成欧拉角来融合。欧拉角在接近90度时会奇异性爆表哪怕你数据里根本转不到那个角度数值上也会出现奇怪的跳动。代码里直接用四元数或旋转矩阵的log/exp映射至少能保证轨迹的连续性。4.2 滑窗BA为什么要走稀疏求解到位姿和3D点都有了初值接下来就是局部BA的活。BA的数学模型并不复杂最小化所有残差的平方和其中残差是3D点在某一帧的重投影误差。复杂的是怎么快速求解。假设滑窗里有11帧、200个特征点那么未知量就是11个位姿每个6自由度加尺度信息和200个3D点每个3自由度。这个规模看着不大但直接对Cartesian坐标求导并填充Hessian矩阵矩阵会非常庞大且稠密。幸好BA的Jacobian天然带有稀疏结构某个残差只连接一个3D点和一帧位姿于是整个Hessian矩阵呈块状稀疏布局绝大部分元素是零。利用这个稀疏性标准做法是先对特征点块做Schur消元也就是先在数学上把3D点的量“消”掉只剩下位姿相关的量最后组成一个维度小很多的位姿增量线性系统。我见过有人为了省事直接调用通用非线性优化库却不开稀疏求解器结果每轮迭代耗时几十毫秒滑窗变成幻灯片。正确姿势一定是用精求解器开启SCHUR_STRATEGY和稀疏线性代数把边缘化掉的路标点块提前消元速度可以提升一个数量级。这里再提一个能提升稳定性的技巧在BA迭代前把图像坐标投影误差乘以一个huber核函数鲁棒核从而抑制大误差项对解的牵引。Vins-Fusion的残差实现里也引入了这类鲁棒化处理我的经验是误匹配率在1%3%时加了huber核能明显改善优化收敛性。如果你发现BA结果在一次误匹配下就大跳八成是核函数没加或者阈值设置太宽松。4.3 边缘化把老帧的贡献折算成先验滑窗每前进一步最老的关键帧就要被移出窗口。直接把它丢掉会损失掉它与其他帧之间的约束信息系统会在某一刻“忘掉”过去导致漂移。所以这里要用边缘化Marginalization把被移除帧的所有约束信息压缩成一个先验概率项附加到后续的优化问题中。边缘化的数学工具是Schur补。把要删除的变量从信息矩阵中消掉剩下的信息矩阵加上测量贡献就形成新的先验。我听得懂但真正写代码时比较容易忘记的是边缘化的前提是线性化点固定。一旦先验项在你当前的雅可比线性化点生成之后不要再用新雅可比反复线性化它否则信息会反复放大导致所谓的“信息矩阵重复注入”系统会假性乐观——协方差变小、误差却不减。这个坑我在调参时撞过一次因为在边缘化之后没有冻结线性化点程序的RMSE看似越来越小但实际轨迹和真值越拉越远。排查了半天最后发现是先验项被重复线性化了几次信息矩阵里的值凭空大了好几倍。修复之后系统才恢复正常。所以你看边缘化这一环节正确性比代码性能更重要。5. 常见问题与排查记录5.1 初始化总是失败的常见原因如果你发现Vins-Fusion在第一次初始化时反复失败先别怀疑调参多数是数据本身不够格。我看过很多数据集特征是够多但相机几乎在原地转圈纯旋转基础上平移量很小这时候本质矩阵模型退化。你可以打印出H和E模型各自的内点数来确认如果H模型内点数远超E模型且场景看起来有大块平面那基本就是低视差或旋转退化问题。解决方法是等相机平移量积累起来再触发初始化。你可以手动控制机器人在初始化阶段做一个带明显平移的八字绕行而不是原地转圈。另外初始化时不要用模糊帧运动模糊会让光流跟踪的像素位置整体偏移RANSAC再怎么聪明也救不回系统性偏差。有个更隐蔽的原因内参没对准输出图像的尺寸。我遇到过一次图像被resize到一半分辨率标定内参却没跟着缩放出新主点和焦距初始化阶段特征点归一化之后全部偏了结果H/E模型内点数惨不忍睹。这个问题只靠肉眼看不出但我建议你在代码里打印几个点的归一化坐标跟真实物理方向对一对能快速定位是不是内参坐标系搞错了。5.2 3D点在两次三角化后跳变同一个特征点前几帧三角化出来的3D坐标在一个位置滑窗滚过几帧之后再做一次三角化3D点位置突然跳了几米。这是非常典型的“三角化时视差角不够”掷出来的病态解。我第一次遇到时以为是前端跟踪崩了但其实前端很稳问题就是当帧的基线和深度比太低3D点深度方向上的不确定性高达数米重投影误差却很小看起来很正常。排查方法是把“该特征点所有观测帧”标出来逐帧打印出三角化时的视差角。视差角小于阈值时哪怕重投影误差再小也不要更新这个3D点。另一个经验是使用多帧观测做三角化比只用两帧要稳得多。多出来的帧可以让最小二乘残差在深度方向上形成更好的约束。如果你只有一个特征库但不记得它被多少帧观测过可以统计一下“每个点在滑窗内的观测总数”低于3帧的一律不用它做定位。5.3 先验一致性破坏后的漂移特征边缘化先验出问题的时候系统的症状很有迷惑性姿态看起来很平滑但轨迹会随时间缓慢往一个方向偏越拉越远。这种漂移和前端特征跟踪错误的“突变性抖动”不一样是慢性病。你检查IMU看看没异常检查3D点重投影误差也不大但全世界点云和轨迹就像被一股无形的力缓慢拖拽。大概率出在先验项的线性化点被重复更新或者先验项残差的雅可比计算中使用的新位姿和生成先验时不一致。我自己的排错流程是把滑窗边缘化前后的信息矩阵导出看对角元素是否异常增大如果增大超过一个数量级就基本坐实了重复注入。再检查先验残差模块中“固定线性化点”那个开关有没有在代码里丢失。这种问题在纯视觉定位场景里更明显因为IMU还能提供一部分参考多少能掩盖先验错误。一旦你的部署环境里IMU噪声很大视觉边缘化先验的问题就会被放大你就更难怀疑到先验头上了。5.4 参数调优的心得控制不住的掉帧和漂移往往不是某一个参数的问题而是参数之间不匹配。我的经验是按下图这个顺序来调不要跳着调先确认滑窗大小1013不被拍脑袋改再确认关键帧的最小视差阈值0.5度到1度只做微调接着看RANSAC阈值和最大迭代次数归一化阈值0.0020.01最后才动特征点跟踪窗口尺寸和金字塔层数。乱调参数的后果是你换了阈值AB的问题被暴露你换B又让A的问题回来陷入死循环。另外一个心得是写一个小的“诊断日志”功能每隔几帧输出滑窗内平均重投影误差、内点数、最老帧到当前帧的平移量。调试时开着它跑一小段就能快速判断系统状态。我每接手一个新场景数据第一件事就是开着诊断日志走一遍看看初始化和滑窗内的残差变化趋势比盯着一堆轨迹图发呆有效太多。滑窗SFM这部分做到位最大的感受就是后端的BA优化变“聪明”了它不再会因为一次误匹配而翻车也不会因为几帧的低视差而产出飘移点。前期的特征跟踪、位姿估计、三角化这些看似基础的操作直接决定了全系统最终能跑多稳这不是公式推导能带给你的经验而是反复调数据积累出来的肌肉记忆。我个人到现在还保留着一个习惯每次改动三角化模块的参数我都会用一组带真值的仿真数据做回归测试看看深度误差随视差角变化的曲线。这能最快暴露你把阈值调过头了还是调不到位了。以后哪位朋友要是被困在“初始化失败”“点云漂移”“轨迹慢性漂”这三个坑里希望这篇文章能帮你绕开大部分雷区。