1. 从零写VIO第七节VINS初始化到底在做什么做视觉惯性里程计VIO做到初始化这一块很多人会卡住。前面几节把IMU预积分、视觉跟踪、滑动窗口的框架搭起来了到了VINS初始化这里突然发现代码跑不通、尺度对不上、重力方向飘得离谱。这不是你一个人的问题初始化本身就是整个VIO系统里最“玄学”的一环。这一节的核心任务是把视觉SFMStructure from Motion和IMU预积分的结果对齐估计出系统的尺度、重力方向、速度、陀螺仪偏置和加速度计偏置。说白了就是给整个系统找一个合理的起始状态让后续的滑动窗口优化能在一个靠谱的起点上跑起来。适合谁看如果你已经跟着前几节把VIO的基础框架写完了现在卡在初始化这里不知道怎么下手或者代码跑起来结果不对但找不到原因那这篇内容就是给你准备的。我会把VINS初始化的完整流程拆开从纯视觉SFM到视觉惯性对齐每一步的参数怎么算、代码怎么写、坑在哪里都讲清楚。2. VINS初始化的整体思路拆解2.1 为什么初始化不能直接用IMU积分很多人第一反应是IMU不是有加速度计和陀螺仪吗直接积分不就能得到位姿和速度理论上可以但实际完全不行。IMU积分有两个致命问题。第一加速度计测的是比力不是纯加速度。它包含重力分量而重力方向你事先不知道。第二偏置和噪声会随时间累积。哪怕陀螺仪偏置只有0.01度每秒积分60秒后姿态误差就有0.6度位移误差更是平方级增长。所以IMU单独积分只能撑很短时间必须借助视觉信息来“锚定”尺度、重力和偏置。这就是VINS初始化要解决的核心矛盾用视觉的长期稳定性来校准IMU的短期精度。2.2 视觉惯性对齐的基本框架VINS-Mono的初始化方案是松耦合的分两步走第一步纯视觉SFM。用滑动窗口里的图像帧做特征跟踪三角化得到路标点然后用PnP或者本质矩阵分解得到各帧的相对位姿。这一步得到的是没有尺度的相机位姿和路标点。第二步视觉惯性对齐。把SFM的结果和IMU预积分的结果放在一起构建一个优化问题求解尺度、重力方向、速度、偏置。这一步的关键在于IMU预积分提供了帧间的旋转和位移约束而SFM提供了视觉观测约束。注意这里说的“松耦合”是指初始化阶段后续的滑动窗口优化是紧耦合的。初始化只负责给一个靠谱的初值不需要一步到位。2.3 为什么选择这种方案对比几种常见的初始化方案方案优点缺点适用场景纯IMU积分简单直接漂移快无法估计尺度几乎不可用纯视觉SFM尺度可估精度高无法估计IMU偏置和重力仅视觉任务松耦合对齐计算量小鲁棒性好依赖SFM质量VINS主流方案紧耦合初始化理论最优计算复杂容易陷入局部最优研究阶段VINS-Mono选择松耦合对齐核心原因是鲁棒性。紧耦合初始化虽然理论更优但对初值敏感容易发散。松耦合先分别求解视觉和惯性再对齐相当于把问题解耦降低了求解难度。3. 纯视觉SFM部分的实操细节3.1 特征跟踪与关键帧选择初始化阶段视觉SFM的质量直接决定后续对齐的精度。特征跟踪用的是光流法这里有几个关键参数需要调最大跟踪点数一般设100到150。太少会导致三角化不稳定太多会增加计算量。最小跟踪点数低于20个点就认为跟踪失败需要重新初始化。关键帧选择策略当前帧与上一关键帧的平均视差超过一定阈值比如10像素或者跟踪点数低于阈值就选为新的关键帧。我实测下来视差阈值设10到15像素比较合适。太小会导致关键帧太密三角化精度不够太大则关键帧太少IMU预积分的时间间隔太长对齐误差大。3.2 三角化与PnP求解有了关键帧和特征点下一步是三角化。这里用的是滑动窗口内的所有关键帧不是只两帧。具体做法是以最后一帧为参考帧用对极几何约束求解本质矩阵。分解本质矩阵得到相对位姿。三角化所有特征点得到3D坐标。用PnP求解其他帧的位姿。实操心得三角化时一定要检查重投影误差和深度值。深度为负或者重投影误差超过2像素的点直接剔除否则会严重污染后续的对齐结果。代码层面OpenCV的triangulatePoints和solvePnPRansac可以直接用。但要注意solvePnPRansac的迭代次数和重投影误差阈值需要根据你的图像分辨率调整。一般1080P图像阈值设2到3像素比较合适。3.3 SFM结果的尺度问题纯视觉SFM得到的位姿是没有真实尺度的。比如相机移动了1米SFM可能告诉你移动了0.5个单位。这个尺度因子就是后续要对齐的关键参数之一。尺度不确定的原因是单目视觉无法区分“小物体近距离”和“大物体远距离”。只有引入IMU的加速度信息才能确定真实尺度。4. 视觉惯性对齐的核心实现4.1 构建对齐方程视觉惯性对齐的核心方程是$$ \mathbf{p}{c{k1}} \mathbf{p}_{c_k} \mathbf{v}_k \Delta t \frac{1}{2} \mathbf{g} \Delta t^2 \mathbf{R}_k \boldsymbol{\alpha}_k $$其中$\mathbf{p}_{c_k}$是第k帧相机位置$\mathbf{v}_k$是第k帧速度$\mathbf{g}$是重力向量$\mathbf{R}_k$是第k帧旋转$\boldsymbol{\alpha}_k$是IMU预积分的位移增量这个方程把视觉的位移和IMU的预积分结果联系起来。未知量是尺度$s$、重力$\mathbf{g}$、速度$\mathbf{v}_k$和偏置。4.2 重力方向与尺度的联合估计实际实现中重力向量不是直接优化的而是参数化为球坐标$$ \mathbf{g} |\mathbf{g}| \cdot [\cos\theta \cos\phi, \cos\theta \sin\phi, \sin\theta]^T $$这样做的好处是减少优化变量维度从3维降到2维而且能保证重力模长固定为9.81。尺度$s$和重力方向一起估计。具体做法是先假设陀螺仪偏置为零用旋转约束估计陀螺仪偏置。把尺度、重力、速度放在一个线性系统里求解。用最小二乘得到初值。再用非线性优化 refine。注意重力模长一定要固定。我见过有人不固定模长结果优化出来的重力是9.5或者10.2整个系统就飘了。4.3 速度与偏置的求解速度的求解依赖于位移方程。对于滑动窗口里的每一帧都有一个速度未知量。帧数越多未知量越多但方程也越多。偏置的求解分两步陀螺仪偏置用视觉旋转和IMU旋转的差异来估计。加速度计偏置在对齐过程中一起优化。这里有个经验陀螺仪偏置的估计精度直接影响后续姿态精度。如果陀螺仪偏置估计不准滑动窗口优化时姿态会一直漂。所以初始化阶段一定要把陀螺仪偏置估准。5. 完整实操流程与代码实现5.1 初始化触发条件初始化不是一上来就做的需要满足一定条件滑动窗口里至少有10帧关键帧。特征跟踪的平均视差超过10像素。IMU预积分的时间间隔不超过0.5秒。满足这些条件后触发初始化。如果初始化失败清空滑动窗口重新开始。5.2 纯视觉SFM的代码实现// 伪代码纯视觉SFM bool VisualSFM(vectorFrame frames, vectorVector3d points3d) { // 1. 选择参考帧 int ref_idx frames.size() - 1; // 2. 求解本质矩阵 Mat E findEssentialMat(frames[0].pts, frames[ref_idx].pts, K, RANSAC); // 3. 分解本质矩阵 recoverPose(E, frames[0].pts, frames[ref_idx].pts, K, R, t); // 4. 三角化 for (auto pt : frames[0].pts) { Vector3d pt3d triangulate(frames[0].pose, frames[ref_idx].pose, pt); if (pt3d.z() 0 reprojError(pt3d) 2.0) { points3d.push_back(pt3d); } } // 5. PnP求解其他帧 for (int i 1; i ref_idx; i) { solvePnPRansac(points3d, frames[i].pts, K, R, t); frames[i].pose compose(R, t); } return points3d.size() 20; }5.3 视觉惯性对齐的代码实现// 伪代码视觉惯性对齐 bool VisualInertialAlignment(vectorFrame frames, vectorIMUPreintegration imu_preints, double scale, Vector3d gravity, vectorVector3d velocities, Vector3d gyro_bias, Vector3d acc_bias) { // 1. 构建线性系统 A * x b MatrixXd A(6 * (frames.size() - 1), 10); VectorXd b(6 * (frames.size() - 1)); for (int i 0; i frames.size() - 1; i) { // 位移约束 A.block3, 3(6*i, 0) -Matrix3d::Identity() * imu_preints[i].dt; A.block3, 3(6*i, 3) 0.5 * Matrix3d::Identity() * imu_preints[i].dt * imu_preints[i].dt; A.block3, 1(6*i, 6) frames[i].R * imu_preints[i].alpha; // 旋转约束 A.block3, 3(6*i3, 6) Matrix3d::Identity(); b.segment3(6*i) frames[i1].p - frames[i].p; b.segment3(6*i3) frames[i].R * imu_preints[i].beta; } // 2. 最小二乘求解 VectorXd x A.colPivHouseholderQr().solve(b); // 3. 提取结果 scale x(6); gravity x.segment3(3); velocities ...; // 4. 非线性优化 refine ceres::Problem problem; // 添加残差项... ceres::Solve(options, problem, summary); return true; }5.4 参数计算过程详解以尺度$s$的计算为例。假设IMU预积分得到的位移增量是$\boldsymbol{\alpha}$视觉SFM得到的位移是$\mathbf{p}{c{k1}} - \mathbf{p}_{c_k}$那么$$ s \cdot (\mathbf{p}{c{k1}} - \mathbf{p}_{c_k}) \mathbf{v}_k \Delta t \frac{1}{2} \mathbf{g} \Delta t^2 \mathbf{R}_k \boldsymbol{\alpha}_k $$把$s$提出来写成线性形式$$ (\mathbf{p}{c{k1}} - \mathbf{p}_{c_k}) \cdot s - \mathbf{v}_k \Delta t - \frac{1}{2} \mathbf{g} \Delta t^2 \mathbf{R}_k \boldsymbol{\alpha}_k $$这样$s$、$\mathbf{v}_k$、$\mathbf{g}$都是线性的可以用最小二乘直接求解。实操心得最小二乘求解时一定要用QR分解或者SVD不要用正规方程。正规方程的数值稳定性差容易出问题。6. 常见问题与排查技巧实录6.1 初始化失败的原因排查问题现象可能原因排查方法解决方案SFM三角化点太少特征跟踪质量差检查跟踪点数降低跟踪阈值增加特征点尺度估计为负重力方向错误检查重力向量固定重力模长重新初始化速度估计发散IMU预积分错误检查预积分代码验证预积分与直接积分的一致性偏置估计过大视觉旋转不准检查旋转约束增加关键帧数量提高SFM精度初始化后系统飘重力方向不准检查重力模长固定模长9.81重新优化6.2 重力方向估计不准的排查重力方向估计不准是最常见的问题。表现是初始化后系统跑一会儿就飘了或者尺度明显不对。排查步骤检查重力模长是否接近9.81。如果差太多说明优化有问题。检查重力方向是否与加速度计静止时的读数一致。静止时加速度计应该测到约9.81的模长方向指向地心。检查IMU坐标系和相机坐标系的外参是否正确。外参错了重力方向肯定错。避坑技巧初始化阶段先让设备静止几秒钟用加速度计的读数粗略估计重力方向作为优化的初值。这样能大幅提高收敛速度和精度。6.3 尺度估计不稳定的处理尺度估计不稳定通常是因为视觉SFM的位移太小。如果相机移动距离太短三角化精度不够尺度就估不准。解决办法初始化时让相机充分移动至少移动0.5米以上。增加关键帧数量让IMU预积分的时间跨度更长。如果还是不行可以固定尺度为1只优化重力和偏置。虽然不完美但至少能跑起来。6.4 陀螺仪偏置估计的注意事项陀螺仪偏置的估计精度直接影响姿态精度。这里有几个经验静止初始化初始化前让设备静止2到3秒用陀螺仪的平均读数作为偏置初值。旋转充分初始化时要有足够的旋转运动否则陀螺仪偏置不可观。迭代优化陀螺仪偏置和视觉旋转是耦合的需要迭代优化。一般迭代3到5次就能收敛。7. 初始化后的验证与调优7.1 如何验证初始化结果初始化完成后需要验证结果是否靠谱。几个关键指标重力模长应该在9.81附近误差不超过0.1。尺度应该为正且与实际移动距离一致。速度应该平滑没有突变。偏置陀螺仪偏置一般在0.01度每秒以内加速度计偏置在0.1米每二次方秒以内。如果这些指标都正常说明初始化成功。否则需要重新初始化。7.2 初始化后的滑动窗口优化初始化只是给了一个初值后续的滑动窗口优化才是重头戏。初始化后把尺度、重力、速度、偏置作为初值放入滑动窗口优化。滑动窗口优化时重力方向可以继续优化但模长要固定。偏置也可以继续优化但一般会加一个先验约束防止偏置漂移太大。7.3 调优经验分享我踩过的几个坑第一个坑初始化时相机移动太少。结果尺度估计为0.3实际移动了1米。后来让相机多移动了一会儿尺度就准了。第二个坑重力模长没固定。优化出来的重力是9.5系统跑起来一直往下掉。后来固定模长为9.81问题解决。第三个坑陀螺仪偏置没估准。初始化后姿态一直漂后来发现是陀螺仪偏置估计成了0.05度每秒实际只有0.01。重新初始化后就好了。最后分享一个小技巧初始化阶段多打印中间结果。比如重力向量、尺度、速度、偏置每步都打印出来。这样出问题的时候一眼就能看出是哪一步不对。这个内容后续还可以这样扩展把初始化放到GPU上加速或者用更鲁棒的估计方法比如鲁棒核函数来处理外点。如果要做双目VIO初始化会更简单因为尺度可以直接从双目视差得到不需要IMU来估计尺度。