【SLAM】VINS-MONO解析——初始化(理论部分)

关注

VIP免费原创最新推荐文章于 2023-10-31 20:03:08 发布 2.2w 阅读 AI 写同款· GEO 优化 ›

AI权益加码！Claude Code、Cursor等20+工具免费用！购周边限时加赠Coding Plan Lite，畅享主流AI工具！学习进阶更高效！阅读详情

6.初始化

第一个问题，为什么要初始化？
对于单目系统而言，
(1)视觉系统只能获得二维信息，损失了一维信息(深度),所以需要动一下，也就是三角化才能重新获得损失的深度信息；
(2)但是，这个三角化恢复的深度信息，是个“伪深度”，它的尺度是随机的，不是真实的，所以就需要IMU来标定这个尺度；
(3)要想让IMU标定这个尺度，IMU也需要动一下，得到PVQ的P；
(4)另外，IMU存在bias，视觉获得的旋转矩阵不存在bias，所以可以用视觉来标定IMU的旋转bias；
(5)需要获得世界坐标系这个先验信息，通过初始化能借助g来确定；

对于双目系统而言，
初始化的难度会低一些，因为双目可以一下确定深度，只需要通过g的方向(先验)来确定世界坐标系。

注意，这个初始化只进行一次就够了，或者是在系统重启时执行一次，大部分时候，系统都处于NON_LINEAR的状态。因为，初始化的时候，就能确定尺度scaler和bias初始值，scaler确定后，在初始化获得的这些路标点都是准的了，后续通过PnP或者BA得到的特征点都是真实尺度的了。而bias初始值确定以后，在后续的非线性优化过程中，会实时更新。

6.1 基础原理

初始化的逻辑图如下：
在这里插入图片描述

6.1.1 如果旋转外参数 qbc 未知, 则先估计旋转外参数
实际上讲，这部分并不是vins代码中初始化的内容，而是初始化之前就需要完成的判断和操作，见5.2-4。根据IMU和视觉部分的旋转关系，可以得到下面的关系式：
在这里插入图片描述将多个帧之间的等式关系一起构建超定方程Ax=0。对A进行svd分解，其中最小奇异值对应的奇异向量便为需要求解的qbc。
虽然这里IMU的旋转部分并没有标定，得到的外参数可能不太准。但是问题不大，因为初始化所占用的总运行时间不长，而更长生命周期的后端会持续的优化这部分的值。

6.1.2 利用SfM确定各个pose和特征点的相对于c0帧的位置关系
这一部分和基于图像的三维重建比较像，可以用三角化和PnP把这一串的ck帧的位姿和特征点位置确定下来(特征点是伪深度)，在加上外参数qbc和pbc，一系列bk帧的位姿也确定下来。注意，这里把c0帧作为基础帧，实际上，c0帧旋转一下使gc0和gw方向一致时获得的坐标系就是vins的世界坐标系，也就是先验。在vins代码中，这一部分篇幅很长，但是这一部分也是很重要的部分。
在这里插入图片描述
首先我们先推导论文式(14)，所有帧的位姿(Rc0ck,qc0ck)表示相对于第一帧相机坐标系(·)c0。相机到IMU的外参为(Rbc,qbc)，得到姿态从相机坐标系转换到IMU坐标系的关系。

将T展开有成R与p有：

左侧矩阵的两项写开：
在这里插入图片描述

6.1.3 利用相机旋转约束标定IMU角速度bias
求解的目标函数如下公式所示：
在这里插入图片描述
在SfM完成且外参数标定完之后，头两个值是已知的了，而且我们假设头两个值是准的。理想状态下，这三个数乘积应该是单位四元数，很可惜，第三个值是IMU预积分得到的，而预积分里面是有bias的。所以，通过最小化这个目标函数的，可以把旋转bias标定出来！
在IMU预积分部分，也就是4.1.1最后的那个公式，有：
在这里插入图片描述
带入到损失函数里，可以得到：

或者是：

带入bias的残差后，得到，

实部没有需要标定的量，所以只用考虑虚部，也就是：

两侧再乘以，可以构造出Ax=B的形式，在采用LDLT分解，就可以求出状态量：
在这里插入图片描述
其实这里不像那种用高斯牛顿法迭求解，而更像是用直接法,也就是矩阵运算的方式来求待优化的状态量，6.1.4也是一样的思路。代具体代码见:initial_aligment.cpp 函数 solveGyroscopeBias()。

接下来是代码解析，
(1)参数的传入和容器的定义

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;

上式中，A和b对应的是Ax=b。注意，传入的参数是all_image_frame，不仅仅是滑窗内的帧。frame_i和frame_j分别读取all_image_frame中的相邻两帧。

(2)套本小节的最后一个公式，构造Ax=b等式

    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;
    }

注意到本小节的第一个公式了吗，那里有求和，所以需要遍历all_image_frame，然后叠加A和b。

(3)ldlt分解

    delta_bg = A.ldlt().solve(b);

(4)给滑窗内的IMU预积分加入角速度bias

    for (int i = 0; i <= WINDOW_SIZE; i++)
        Bgs[i] += delta_bg;

(5)重新计算所有帧的IMU积分(重要！)

    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]);
    }
}

repropagate()部分内容见4.3.2.。

6.1.4 利用IMU的平移估计重力/各bk帧速度/尺度scaler
首先要明确需要优化的状态量是什么，是各帧在bk坐标系下的速度，c0帧下的g和SfM的尺度scaler：
在这里插入图片描述
这块有个遗留问题，就是为什么要优化速度呢？
在IMU预积分部分，已经有如下的公式：

但是w坐标系我们不知道，只知道c0坐标系，所以需要把上面的公式转到c0坐标系上：

上式中，等号左边减去等号右边就是残差，理想状态下，这个残差是0，那么带入上式得：
在这里插入图片描述
把这个 XX = 0的等式也转为 A x = b这种线性方程组的形式，如下式：