这篇继续往源码深处走。前面我们聊过前端特征追踪、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把窗口里其他帧的位姿一个个“钉”出来。PnP（Perspective-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拿到足够丰富的几何约束。代码层面的参数我反而很少动——大多数场景下默认参数在好的数据上都能一遍过。如果你发现默认参数下初始化一直失败，建议优先怀疑数据和运动模式，而不是立刻去调阈值。
这篇继续往源码深处走。前面我们聊过前端特征追踪、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把窗口里其他帧的位姿一个个“钉”出来。PnP（Perspective-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拿到足够丰富的几何约束。代码层面的参数我反而很少动——大多数场景下默认参数在好的数据上都能一遍过。如果你发现默认参数下初始化一直失败，建议优先怀疑数据和运动模式，而不是立刻去调阈值。