vins-mono中的初始化详解（1）_vinsmono初始化-CSDN博客

本文链接：https://blog.csdn.net/weixin_44587828/article/details/134106398

1.前言

本人硕士研究方向是slam，最近学习了vins-mono开源框架。对于整个工程的实现过程有了基本的认识，近期打算整理一下关于vins的一些学习笔记和个人理解，也包括如何用自己的设备跑该框架，以及在实际应用中应该做的一些改进，保证定位效果的鲁棒性。首先，从介绍vins中初始化模块开始。

本文主要介绍vins-mono中初始化都做了什么，具体如何实现，以及怎么构建出最小二乘问题。此外，还会详细介绍ceres如何进行求解其中的最小二乘问题。

2.vins初始化中做了哪些工作

2.1初始化的目的

首先，明确初始化的目的——给后端优化提供相对准确的初值。关于此点，原因有二：

其一，因为整个vio系统是一个紧耦合的问题，对于优化问题我们知道，初值的选取对于问题最后的求解影响很大。在后端优化中进行优化的变量有很多，包括滑窗内的位姿、速度和imu零偏bias（包含加速度计和陀螺仪）、imu和camera的外参、传感器时间延时Td、3d地图点的逆深度。这么多参数耦合在一起需要一组相对准确的优化初值，否则很容易陷入局部最优解。

其二，后端优化对于时间有要求，原则上在保证准确情况下越快越好。否则，前端的节点数据发布好久了上一次优化任务还没结束肯定是不太行的。而初始化提供了离准确值比较近的初值，那么后端优化经过少次数的迭代就可以收敛。初始化做得好就减轻了后端优化的压力。

2.2初始化都优化了哪些变量

虽然初始化是为了给后端优化提供初值，但是也并没有对每个参数都进行初值估计。在初始化中，进行优化的变量有：滑窗内的位姿、速度、陀螺仪bias、3d地图点的逆深度、imu和相机外参的旋转(当没有先验外参时)。经过对比可以看到，imu和camera外参的位移以及加速度计的bias没有进行优化。

关于平移外参可以用尺子大概测量一下也能得到相对准确的先验信息，也可以根据机械结构CAD得到准确的数据；关于加速度计的bias对于后端优化影响不大，而且很难从包含重力加速度中分离出来，但是该项参数在后端优化中也会进行处理。

2.3初始化流程

初始化部分主要包含在vins-mono工程vins_estimator功能包的estimator_node节点下。该节点除了包含前端里程计的所有代码，还包含后端优化模块。初始化的准备工作是光流追踪节点和imu数据发布节点。这两个节点发布出数据后，estimator_node节点开始处理。

2.3.1数据预处理

本节点订阅图像数据话题和imu数据话题，创建两个线程callback进行数据预处理，此外，还会开辟一个新的线程process进行处理camera和imu数据，初始化过程就是在process线程中进行。

先说接收话题的两个callback线程。

对于imu数据的预处理：

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
    if (imu_msg->header.stamp.toSec() <= last_imu_t)
    {
        ROS_WARN("imu message in disorder!");
        return;
    }
    last_imu_t = imu_msg->header.stamp.toSec();

    m_buf.lock();
    imu_buf.push(imu_msg);
    m_buf.unlock();
    con.notify_one();

    last_imu_t = imu_msg->header.stamp.toSec();

    {
        std::lock_guard<std::mutex> lg(m_state);
        predict(imu_msg);
        std_msgs::Header header = imu_msg->header;
        header.frame_id = "world";
        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
            pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
    }
}

可以看到，imu数据接收后一方面存进对应的imu_buf，这里包含线程锁和条件变量，保证线程安全同时提高了CPU运行效率。另一方面当solverflag为non_linear时，节点会通过imu积分得到最新位姿并发布，这里也弥补了优化速度较慢导致位姿更新较慢的问题。但是在实际测的过程中，位姿发布频率并不能达到接近imu频率，更确切的说，在本人电脑上运行位姿输出时间戳的规律是：得到第一帧位姿然后以高频输出多帧位姿（接近imu频率），持续大概10ms，再等40ms左右来下一帧位姿，再高频输出。这个问题目前我还没有去处理，猜测是因为在40ms这个间隔时间imu的buf大部分信息被占用，用来后端处理，也就是线程锁将其限制。可以考虑将buff数据再copy一份，然后根据状态量标志位进行处理。

对于图像数据的预处理：

void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
    if (!init_feature)
    {
        //skip the first detected feature, which doesn't contain optical flow speed
        init_feature = 1;
        return;
    }
    m_buf.lock();
    feature_buf.push(feature_msg);
    m_buf.unlock();
    con.notify_one();
}

对于图像数据的处理较为简单，只是在保证线程安全状态下将图像数据信息存进图像对应的feature_buf。

对于图像的数据信息可以简单说明一下，它包含图像中特征点id，归一化坐标、像素坐标以及特征点速度。

2.3.2process线程处理

数据经过预处理后都存进了对应的buf，接下来由process线程进行集中处理。

void process()
{
    while (true)
    {
        //std::vector<  std::pair<  std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr  >  > measurements;
        std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
        std::unique_lock<std::mutex> lk(m_buf);
        con.wait(lk, [&]
                 {
            //处理的是imu_buf 和 feature_buf里面的数据 目的是将对应的数据对齐 存储到measures里面
            return (measurements = getMeasurements()).size() != 0;  
                 });
        lk.unlock();

首先要将imu数据和image数据打包，存放到measurements数据结构里面。因为在进行process线程之前可能已经来了很多imu数据和camera数据，而且最后要构建约束，imu作为两帧图像之间的约束，所以要把对应的imu数据和camera数据进行绑定处理。经过处理后，measurements里面存储的数据样式如下图：

如上图，所有数据的前后排列顺序都是按照时间顺序排列。每个数据对保留的是一帧图像数据和它前面的两帧图像之间的imu数据，此外，还会加入在该图像数据后紧相邻的下一帧imu数据，目的是根据线性插值获得更加准确的图像帧对应的imu数据。

然后开始遍历刚刚打包好的measurements里面的每一对数据。遍历过程进行对imu和camera数据分别处理：

imu处理：简单讲，除了滑窗内第一帧图像前的imu数据不进行特殊处理外（因为在后端优化中并不构成约束），其余图像帧之间的imu数据都要经过中值积分存储到滑窗内的预积分数据中，同时也会更新对应的协方差和雅克比，这里包含vins中imu的预积分过程，这里先不展开说明，在之后的文章中再补充说明。此外，所有原始帧的imu数据也会备份到Estimator类里面对应的buf中，其主要目的是在边缘时将两个imu合并成一个（边缘化掉次新帧）。根据imu数据还会更新滑窗内对应位姿和速度Ps、Vs、Rs，此时的位姿为Twi或记录为 $T_{i}^{w}$ 。

接着处理回环检测的数据处理这里先不进行说明。

图像数据的处理estimator.processImage函数：

1.根据视差判断是否为关键帧，并将图像中的特征点信息添加到特征点管理中；

添加特征点信息：在vins的Estimator类中有一个特征点管理器的对象f_manager,对应的数据类型是FeatureManager，这里面包含对特征点的一些处理，最主要的是包含一个 list<FeaturePerId> feature对象，该对象对特征点进行管理。每一个FeaturePerId的对象包含特征点处理过程中的一些信息，包括特征点的ID，观测到该特征点在滑窗中的起始帧，特征点是否被三角化成功标志位等。最主要的是里面的vector<FeaturePerFrame> feature_per_frame，这个容器包含观测到该特征点的每一图像帧信息，包括归一化平面坐标，像素坐标，特征点速度信息等。

理清了特征点管理的结构后，FeatureManager::addFeatureCheckParallax函数会先根据ID在特征点管理器中寻找是否有该ID的特征点存在，如果存在说明该帧观测不是起始帧了，那么把该帧下的FeaturePerFrame类型的信息添加到它对应的vector<FeaturePerFrame>容器中；如果特征点管理器之前没有该特征点ID，那么就新建一个FeaturePerId，放进list<FeaturePerId> feature中，并且要将当前观测的信息放到它的vector<FeaturePerFrame>容器中。

根据视差判断是否是关键帧：

判断一帧图像帧是关键帧有以下三种情况：该帧图像追踪的特征点个数小于20个、该帧图像位于滑窗中的第一或第二帧、该帧和它前面的图像帧视差大于一定阈值。

是关键帧那么 marginalization_flag会设置为MARGIN_OLD，否则设置为MARGIN_SECOND_NEW，这两个标志位代表在滑窗边缘化时候应该滑掉最老帧或者次新帧。无论是否是关键帧，所有的图像信息都会存到all_image_frame里，目的是在进行初始化的时候提供更多的信息，尽量保证初始化的准确性。同时Estimator里面也有对应的容器记录预积分信息和时间戳信息等。

2.没有先验外参时，进行imu和camera的旋转外参估计

该部分程序只有当ESTIMATE_EXTRINSIC为2时才有效，具体功能就是估计旋转外参。

当滑窗内来了至少两帧图像数据后，可以通过预积分得到的旋转、两帧图像追踪到的匹配的3d坐标点对完成外参的估计。

vins中对于旋转外参标定原理：根据imu积分得到的旋转和图像对极几何得到的旋转信息通过一定的变换，最终构建一个齐次方程，齐次方程的解就是旋转外参。

计算得到外参后将ESTIMATE_EXTRINSIC标志为置为1，并把计算得到的旋转外参赋值给ric和Ric。

接下来就等图像帧能够填满滑窗，以上可以看做是初始化的准备工作。

3.开始初始化

初始化对应的函数是initialStructure，初始化过程中需要进行的操作有：判断imu激励是否能观、纯单目视觉恢复位姿和视觉惯性对齐三部分。这三部分中第一部分在实际应用中并没有作用，第二部分是为了给出所有图像帧相对准确的位姿（平移仍然是带尺度的），第三部分是图像帧信息和imu信息进行耦合得到相对准确的重力加速度、陀螺仪的偏置、尺度、滑窗内关键帧速度。

判断imu的可观性：

虽然该部分在vins中并没有应用，但是判断可观性的思想还是十分有价值的。因为在实际应用中，会出现imu“失灵”情况，这时候如果不进行一定处理整个vio会死掉，那么可以根据该部分作为判断imu失效的一种方案，下面简单介绍其原理。

基本原理就是计算图像帧之间预积分的标准差，如果标准差过小就认为激励不够。这个也比较好理解，标准差或方差都表示数据偏离均值的程度，比如如果只考虑x轴加速度计，假如得到一组数据是1 1.1 1.1 1 0.9这明显属于激励不够，车体处于接近匀速运动，在这种情况imu实际已经不太能构成约束了，因为由于噪声的存在imu获得的加速度并不为0，在后端构成的约束就是错误的，此时就需要特殊处理。

纯单目视觉恢复位姿：

该部分虽然处于vins的初始化阶段，但是对于视觉slam可以作为后端一部分。在程序中construct函数实现纯视觉的sfm（structure from motion）具体实现如下。

首先根据共视关系找到滑窗内的枢纽帧，并计算枢纽帧和滑窗内最后一帧的位姿。所谓枢纽帧，要满足两个条件：条件1：和最后一帧图像的共视点个数不能小于20，条件2：在满足条件1基础上视差越大越好。这两个条件一个是满足能够通过对极几何求出基础矩阵然后分解得到位姿，另一个条件是为了保证对地图点的三角化要尽可能准确。这里引用slam14讲里面提到的三角化的矛盾中的结论，对于同样精度提取的特征点，视差越大，三角化精度越高。这里也就是权衡三角化的矛盾得到最优解。

得到枢纽帧和滑窗内最后一帧位姿后，进行该部分的主要函数 GlobalSFM::construct函数处理。具体实现流程是：先三角化枢纽帧和滑窗内最后一帧共视的地图点，再根据得到的3d点通过pnp算出枢纽帧后一帧的位姿，然后根据后一帧的位姿和最后一帧的位姿再次三角化出更多的3d点，以此类推直到把滑窗内所有位姿求解完毕。然后，再从枢纽帧开始向前求解枢纽帧前面的滑窗内的位姿，以及三角化出前面的图像和枢纽帧图像共视的3d点。此时，并不是所有地图点都完成了三角化，接着就将能够被两帧及以上的图像观测到的地图点都进行三角化，得到更多的3d点。至此，特征点管理器里面能够进行三角化的地图点都已经完成了三角化。注：该部分所有的位姿和3d点坐标都是以枢纽帧作为参考坐标系。得到的位姿为Twc，w代表枢纽帧。

在进行下一步之前需要搞清两个问题：为什么把这么多地图点都进行三角化？和 vins如何实现三角化地图点？

首先，第一个问题是为了给下一步的优化提供更多的约束。理论上讲，对于优化问题，合理的约束越多优化出来的结果越准确，就比如vins因为加入了imu的约束所以比纯单目视觉slam要更准、鲁棒性更好，因为即使出现约束丢失情况还有其他约束存在，也能在一定范围运行。

然后，关于地图点的三角化问题，vins中仍然是构建一个超定齐次方程，然后求解该方程得到3d点。

到目前，滑窗内的位姿和特征点管理器里面的能被两帧以上图像帧观测到的地图点已经有了一个基本值。接下来根据上面求得的滑窗内位姿和3d点坐标通过构建重投影误差构建最小二乘问题再使用ceres进行优化。优化具体实现在之后的文章讲解，这里暂时不展开。只要知道经过优化后，滑窗内的位姿和3d点坐标都更加准确，但是仍然要注意，此时的世界坐标系仍然是以枢纽帧为参考，得到的q和T表示的是Twc，w表示枢纽帧。

接下来，根据滑窗内的位姿，求解出那些非关键帧的位姿，也就是all_image_frame里面其他帧的位姿，这一步的目的是为了给下一步视觉惯性对齐提供更多可靠的约束。

最后，将得到的所有位姿转化到从imu系到枢纽帧系的位姿变换，即Twi并存储在all_image_frame的ImageFrame属性里，接下来进行初始化的最后一步，进行视觉惯性对齐，求出陀螺仪的bias、重力加速度和视觉的尺度、图像帧的速度。

视觉惯性对齐：

在这部分，分为两个函数，计算陀螺仪的偏置bias并更新预积分量、优化重力加速度，尺度和图像帧速度。

计算陀螺仪bias：

计算bias也好，计算下面的尺度、速度、重力加速度也好，基本思想就是先考虑这些待优化变量和哪些计算过程有关，然后再想办法构建包含待优化变量的最小二乘问题或求解方程。

按照上面的思路，陀螺仪的bias毫无疑问会影响旋转，也就是位姿中的R。在vins里计算旋转有两种方式，imu积分和camera对极约束求解。那么，这两种做法得到的旋转理论上就不应该偏差太大，由此就可以构建最小二乘方程。

在实际代码中，在all_image_frame中既包含预积分计算得到的旋转，又包含通过单目视觉slam计算得到的位姿。

$\gamma_{bk+1}^{bk}=\hat{\gamma} _{bk+1}^{bk}\bigotimes \begin{pmatrix} 1\\ \frac{1}{2}J_{bw}^{\gamma }\delta bw\\ \end{pmatrix}$

上式表示imu积分计算得到的旋转经过陀螺仪bias作用下得到的新的旋转值，这点源自泰勒展开

$f(x+\delta x)=f(x)+\frac{\partial f}{\partial x}\delta x$

只不过这里的加是广义下的加法，而广义加后面的项是近似的结果（因为bias比较小）。

根据前面提到的思路，可以将图像计算得到的位姿和imu计算得到的位姿进行融合，即：

$\gamma_{bk+1}^{bk}=R_{w,bk}^{-1}R_{w,bk+1}$

构建整理得：

$({\gamma}_{bk+1}^{bk})^{-1}\bigotimes R_{bk+1}^{bk}=\begin{pmatrix} 1\\ 0\\ 0\\ 0 \end{pmatrix}$

因为最小二乘构建的是等于0的等式，所以对上式左边取虚部即可。

整理得到以下等式，非齐次方程：

$J_{bw}^{\gamma }\delta b=2(({\gamma}_{bk+1}^{bk})^{-1}\bigotimes R_{c0}^{bk} R_{bk+1}^{c0})_{vec}$

具体代码比较简单，如下：

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);//求解得到新的偏置
    ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());

    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]);//加速度计偏置置成0重新得到所有帧间的预积分约束和雅克比 协方差
    }
}


MatrixXd TangentBasis(Vector3d &g0)
{
    Vector3d b, c;
    Vector3d a = g0.normalized(); //归一化
    Vector3d tmp(0, 0, 1);
    if(a == tmp)
        tmp << 1, 0, 0;//避免平行
    //bc向量是同时正交于a向量的相互正交的单位向量
    b = (tmp - a * (a.transpose() * tmp)).normalized();
    c = a.cross(b);
    MatrixXd bc(3, 2);
    bc.block<3, 1>(0, 0) = b;
    bc.block<3, 1>(0, 1) = c;
    return bc;
}

由于每两帧图像即可以构建一个约束方程，all_image_frame中包含多个图像帧，将这些图像帧都构建成约束，就可以求解出相对准确的陀螺仪bias。在代码中非齐次方程左右同时乘以A的转置，保证前面的系数矩阵是对称正定的，再用LDLT分解求。

在得到陀螺仪的bias后，需要考虑陀螺仪的bias并重新计算预积分有关的量。

优化重力加速度，尺度和图像帧速度：

这部分优化的思想和上面的一致，仍然是利用imu和camera的耦合进行相应参数的最小二乘求解。首先，思考重力加速度、尺度和速度发生改变会影响哪里的计算。那一定是预积分阶段了，除了预积分其他部分没有公式包含这些参数。预积分公式如下：

$\left\{\begin{matrix}R_{c0}^{bk}P_{bk+1}^{c0}=R_{c0}^{bk}(P_{bk}^{c0}+V_{bk}^{c0}\delta t_{k}-\frac{1}{2}g^{c0}\delta t_{k}^{2})+\alpha _{bk+1}^{bk} \\ R_{c0}^{bk}V_{bk+1}^{c0}=R_{c0}^{bk}(V_{bk}^{c0}-g^{c0}\delta t_{k})+\beta \\ \end{matrix}\right.$

为了方便讲明白这里，仍然用图示讲解，如下：

到这里还没有结束，上面求解速度、尺度和重力加速度过程中忽略了一个先验信息——重力的模为9.8（不同地区重力加速度略有差异），上面重力加速度自由度是3，加入该约束后，重力的自由度变为2，然后再去重新求解速度、尺度和重力加速度，得到的结果置信度会更高。

在此可能很多人和笔者有共同的疑问，既然有该先验信息，为什么不在上一步视觉惯性对齐就直接引入该先验信息？据说，orb3是直接加进去然后得到最大后验估计，具体有什么影响本人也未曾验证。下面介绍如何利用重力先验信息进行修正视觉惯性对齐的结果。

修正重力加速度，重新计算参数：

vins中的做法是，先保持上一步计算得到的重力方向，通过归一化再将重力加速度的模长固定为9.805，作为迭代开始的初值。然后，保持模长不变，使得重力加速度在一个球面上进行修正。修正的方法是找到此时重力加速度的切平面上的两个相互正交的单位向量然后重新构建上一步中的方程，此时上面的方程未知数维度减一，也就是重力用两个相互正交的单位向量（和重力向量也正交）乘以它的初值代替，每次求解出单位向量对应的尺度后叠加到重力初值上，但是仍然要保证重力的模为9.805，如此迭代4次得到最终视觉惯性对齐求得的速度、尺度、重力加速度。

至此，视觉惯性对齐的算法部分结束，接下来要恢复一些变量。恢复的原因是当前所有的位姿、地图点等都是以枢纽帧作为参考系，恢复就是让

1）将滑窗内对应的位姿从all_image_frame中取出并赋值给Estimator里面的Ps[]和Rs[]，这里的位姿为Rwi,twc，w对应枢纽帧；

2）将有效地图点深度赋值为-1；

3）把特征点管理器里面的点三角化，目的是把3d点坐标的参考系转变为第一帧观测到该点的图像帧所在坐标系；

4）滑窗内预积分量的更新，这里重新加入bias计算一遍，之前虽然也重新计算了但那是all_image_frame里面的预积分量，也不能直接拿过来赋值；

5）更新滑窗内的速度，转化到世界坐标系下（枢纽帧）；

6）把尺度模糊的3d点恢复尺度；

7）将滑窗内位姿、速度、重力加速度都转化到滑窗内第0帧坐标系下，并且保证第零帧的yaw为0，重力向下（该部分变换绕的有点晕，不是十分清晰过程，如有大佬指点感激不尽）。

至此，初始化部分圆满结束！在下一篇文章补充一下ceres咋去求解优化问题，并对初始化做个总结。