一文详解VINS-Fusion初始化标定-技术圈

作者丨卢涛@知乎

来源丨https://zhuanlan.zhihu.com/p/358232587

编辑丨3D视觉工坊

本文基于VINS-Fusion解释VIO系统的初始化，包括在线标定IMU-Camera的外参旋转，IMU角速度偏置，重力方向，单目尺度。单目初始化相比于双目，多一个构建SFM问题优化位姿、3D点的过程。如有错误，请您指正。

一、IMU与Camera外参旋转标定

只估计旋转

，没有估计平移

，平移通常可以指定，并且平移量很小，影响不如旋转大。原理就是常说的手眼标定

，只不过这里只取旋转，丢掉了平移

推导很简单，用下面几个式子变换一下就能得到上面这个式子

对上面的式子做下变化，转换成

的形式

其中

是四元数的左乘、右乘矩阵，四元数向量相乘可以转换为一个矩阵与一个向量乘。得到上面这个形式就好办了，

用两帧图像间的IMU积分计算得到，

为两帧图像之间的位姿旋转，可以通过2d-2d计算H、E，恢复R。用多组数据通过SVD或者迭代最小二乘，就能把

算出来了，解

是

的SVD分解最小奇异值对应的右奇异向量。

在VINS中对每个

项还乘上了一个权重

是camera旋转与IMU旋转对应的角度差，理论上角度差应该为0。VINS中角度差阈值

设为5°，如果旋转角度误差大于5°，该项会乘上一个系数

，以降低该项的权重。相关函数如下，细节可以查看代码注释

https://github.com/smilefacehh/VINS-Fusion-DetailedNote

// initial_ex_rotation.cpp

/**
 * 在线标定外参旋转
 * 利用两帧之间的Camera旋转和IMU积分旋转，构建最小二乘问题，SVD求解外参旋转
 * 1、Camera系，两帧匹配点计算本质矩阵E，分解得到四个解，根据三角化成功点比例确定最终正确解R、t，得到两帧之间的旋转R
 * 2、IMU系，积分计算两帧之间的旋转
 * 3、根据旋转构建最小二乘问题，SVD求解外参旋转
 * @param corres            前一帧与当前帧匹配点
 * @param delta_q_imu       前一帧与当前帧IMU预积分得到的旋转
 * @param calib_ric_result  在线标定IMU与Camera之间的外参（旋转）
*/
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)；

二、IMU角速度偏置标定

这一步只标定角速度偏置，没有标定加速度偏置，加速度偏置相对于重力加速度量级太小，影响不如角速度偏置大。疑问点，角速度偏置是在imu-camera外参标定之后才进行标定的，而外参标定过程中通过角速度积分计算IMU姿态旋转，是需要用到角速度偏置的，说明用的偏置是不精确的。个人解释是，角速度偏置本身量级很小，之所以需要精确的值，是因为长时间之后小误差被放大，而外参标定只在初始化的时候进行，小误差可以容忍。

在第一步得到IMU-Camera外参旋转之后，将camera的旋转通过外参变换到IMU系下，理论上这个旋转应该与IMU系下对应的旋转一致，差为0，但是由于误差（角速度偏置目前还是个估计值）的存在，不会为0，那么构建最小二乘问题，最小化旋转差量，优化角速度偏置

最小二乘问题，写出它的正规方程（normal equation）

是旋转量残差，

是残差对优化变量的雅克比，此处我们只需要角速度偏置

，所以从雅克比中取出旋转残差对与角速度偏置部分的雅克比即可。

VINS中用LDLT直接解算

，然后更新

，完成角速度偏置的标定

回过头来看，旋转只跟角速度偏置有关，跟加速度偏置没有关系，所以用旋转约束只能估计角速度偏置。

// IMU角速度偏置标定
void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
    Matrix3d A;
    Vector3d b;
    Vector3d delta_bg;
    A.setZero();
    b.setZero();
    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;
    // 从滑窗第一帧遍历到倒数第二帧
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
    {
        frame_j = next(frame_i);
        MatrixXd tmp_A(3, 3);
        tmp_A.setZero();
        VectorXd tmp_b(3);
        tmp_b.setZero();
        Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
        tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
        A += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;
    }
    delta_bg = A.ldlt().solve(b);

    // 更新偏置
    for (int i = 0; i <= WINDOW_SIZE; i++)
        Bgs[i] += delta_bg;

    // 更新偏置之后，重新计算积分
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
    {
        frame_j = next(frame_i);
        frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
    }
}

三、标定重力向量、单目尺度

这一步要标定的是重力在第零帧下的坐标表示，单目的尺度，顺带还能标定IMU下的速度。旋转约束在前面标定外参、角速度偏置已经用过了。重力加速度用在速度、平移量中，尺度也是跟平移有关，所以这一步用速度、平移约束来标定。下面是IMU预积分中的平移项约束、速度项约束

将世界坐标系

换成零时刻相机坐标系

代入

可以写成

的形式，待优化变量

包括各帧速度、

、

。目标是最小化平移、速度误差。在VINS里面也是直接通过LDLT解算

。

上面得到的

没有加入模长

限制，所以还要把这个限制加进来，在这个估计的基础上得到一个更精确的解，相应的尺度

也会进行调整。如何加入这个约束呢

作为优化参数，那么优化后的重力加速度

就在原来

的附近空间里，相差不会很大。

还没完，VINS对优化后的

继续做了一点处理，还原了yaw角对应的旋转量。因为只靠平移、速度约束，yaw角是没有特定解的。只能将z轴方向对齐。如果旋转参与标定，yaw角是能求出来的。对应代码是LinearAlignment函数。

VINS初始化流程

上面标定的数据来自滑窗中所有帧，需要知道每帧的位姿，用图像帧的位姿变换与IMU的预积分量对齐，构建最小二乘问题求解外参、角速度偏置、重力加速度、尺度、速度。

对滑窗中的帧，构建SFM，优化位姿。具体流程如下，细节可参考代码注释。

/**
 * 系统初始化
 * 1、计算滑窗内IMU加速度的标准差，用于判断移动快慢
 * 2、在滑窗中找到与当前帧具有足够大的视差，同时匹配较为准确的一帧，计算相对位姿变换
 *   1) 提取滑窗中每帧与当前帧之间的匹配点（要求点在两帧之间一直被跟踪到，属于稳定共视点），超过20个则计算视差
 *   2) 两帧匹配点计算本质矩阵E，恢复R、t
 *   3) 视差超过30像素，匹配内点数超过12个，则认为符合要求，返回当前帧
 * 3、以上面找到的这一帧为参考系，Pnp计算滑窗每帧位姿，然后三角化所有特征点，构建BA（最小化点三角化前后误差）优化每帧位姿
 *   1) 3d-2d Pnp求解每帧位姿
 *   2) 对每帧与l帧、当前帧三角化
 *   3) 构建BA，最小化点三角化前后误差，优化每帧位姿
 *   4) 保存三角化点
 * 4、对滑窗中所有帧执行Pnp优化位姿
 * 5、Camera与IMU初始化，零偏、尺度、重力方向
*/
bool Estimator::initialStructure();