Vins-Mono系列代码和理论解读＜三＞. vins初始化流程和代码实现细节

VINS系统初始化触发的目的

VINS系统初始化是估计Body坐标系下的重力方向，IMU陀螺仪和加速度计的偏置Bias，以及各帧速度，同时对齐视觉位姿，恢复视觉位姿的尺度。在初始化完成后，系统可以实时导出有真实Pose的位姿数据，并且可以在后端执行联合优化进行状态最优估计。

系统初始化触发机制

系统初始化需要满足4个条件:
a.当前求解模式为INITIAL；
b.滑动窗口内图像帧数目为WINDOWS_SIZE+1，这个算是系统层面的原因了，因为整个VINS后端就是要维护这个WINDOWS_SIZE+1的窗口的状态；
c.相机到IMU的外参旋转项参数已知(已经标定好)，否则无法建立视觉测量值和IMU预积分测量值之间的连接；
d.当前最新图像帧的时间戳距离系统上一次初始化的时间超过0.1s(时间不能太短，在一次初始化失败之后短时间内没必要继续初始化)。

    if (solver_flag == INITIAL)
    {
        // 确保有足够的frame参与初始化，有外参，且当前帧时间戳大于系统刚运行时时间戳+0.1秒
        // 凑满窗口数的图像帧进行初始化
        if (frame_count == WINDOW_SIZE)
        {
            bool result = false;
            if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
            {
                // 执行视觉惯性联合初始化
               result = initialStructure();
               initial_timestamp = header.stamp.toSec();
            }
         ...
         }
	｝

系统初始化触发时应具备的数据

站在设计者的角度考虑系统的初始化，可以更深入的了解理论和代码细节。比如我们需要想好在搭建一套VINS框架时，应该要储备什么数据用以支持完成系统初始化。回到正题，我们可以获取的数据包括视觉测量值(滑动窗口内各图像帧的数据关联——KLT跟踪建立的)以及IMU预积分测量值(连续图像帧之间的预积分测量值是由图像间IMU数据累加构成的)，在VINS-MONO中视觉/IMU测量值被封装成如下形式:

// 记录了所有图像帧信息，key为图像帧时间戳，value为图像帧视觉测量点和imu预积分测量数据
map<double, ImageFrame> all_image_frame;

需要注意的是，all_image_frame中存储的不仅仅是滑动窗口内的视觉/IMU数据，还包括窗口外的数据。

系统初始化触发并成功初始化后更新的数据

成功初始化后会更新滑动窗口内每一图像帧时刻的陀螺仪Bias，以及绝对的视觉位姿(拥有尺度scale，以及在世界惯性系的roll/pitch角)，同时还会计算滑动窗口内每一图像帧时刻的速度。在这个过程中，会利用估计的重力方向将视觉坐标系调整到世界惯性坐标系上。

系统初始化总流程概述

区别于ORB-SLAM3，VINS-MONO的初始化属于松耦合的初始化，即首先执行纯视觉的SFM从而获得up-to-scale的各图像帧位姿，然后将视觉位姿和IMU位姿对齐从而达到初始化的目的。

系统初始化各阶段详解

纯视觉sfm

VINS-MONO的sfm算法比较简单，基本步骤如下所示:
1.汇总特征管理器记录的所有图像帧的特征信息，整理得到所有特征轨迹sfm_f；

    vector<SFMFeature> sfm_f;
    // 遍历特征管理器中的每一个特征点
    for (auto &it_per_id : f_manager.feature)
    {
        // 获取该特征点起始图像帧的上一帧图像
        int imu_j = it_per_id.start_frame - 1;
        // 创建用于sfm的特征点对象，即一条特征轨迹track
        // 状态设置为未三角化，保留特征点索引
        SFMFeature tmp_feature;
        tmp_feature.state = false;
        tmp_feature.id = it_per_id.feature_id;
        // 往该特征点对象中填充对应的观测数据(图像帧索引，图像特征点2d位置)
        for (auto &it_per_frame : it_per_id.feature_per_frame)
        {
            imu_j++;
            Vector3d pts_j = it_per_frame.point;
            tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}));
        }
        sfm_f.push_back(tmp_feature);
    } 

2.执行纯视觉的两帧初始化，在滑动窗口内(共11帧)寻找合适的图像帧(和窗口最后一帧对应特征点的平均视差>视差阈值且特征点匹配数量>20且可利用solveRelativeRT解算出R/T)，记录下初始化图像对的索引(窗口内最后一帧和筛选得到的第L帧)和相对姿态(relative_R/relative_T)；

    // 保证具有足够的视差,由E矩阵恢复R、t
    // l是挑选出来的用来初始化的图像帧索引
    // 该函数判断每帧到窗口最后一帧对应特征点的平均视差大于30，且内点数目大于12则可进行初始化
    // 并同时返回当前最新帧到第l帧的坐标系变换R和T
    // l为0-10之间的一个值
    if (!relativePose(relative_R, relative_T, l))
    {
        ROS_INFO("Not enough features or parallax; Move device around");
        return false;
    }

3.执行GlobalSFM，恢复窗口内所有图像帧位姿和landmarks，这一步流程相对比较复杂，展开讲讲:

a.基于输入的初始化图像对和相对Pose，进行两帧内三角化triangulateTwoFrames
b.以第l帧为纯视觉坐标系原点，通过Pnp计算第l+1帧到窗口最后一帧中所有图像帧的姿态
c.以第l帧为纯视觉坐标系原点，通过Pnp计算窗口第0帧到第l-1帧中所有图像帧的姿态
d.基于滑动窗口内的所有带Pose的图像帧，三角化窗口内剩余的特征轨迹
e.执行全局BA，添加滑动窗口内各图像帧的四元数形式的旋转和向量形式的平移作为优化变量，遍历所有特征对建立残差项
f.将优化结果取出，计算滑动窗口内各帧到参考帧l的转换矩阵，并记录所有地图点sfm_tracked_points

GlobalSFM的实现定义在initial_sfm.cpp中，具体的调用如下

    GlobalSFM sfm;
    // param_1(in): 待求解位姿的图像帧总数目
    // param_2(out): 当前滑动窗口内的图像帧姿态，从滑动窗口内各图像帧坐标系到第l帧相机坐标系的旋转矩阵
    // param_3(out): 当前滑动窗口内的图像帧平移，从滑动窗口内各图像帧坐标系到第l帧相机坐标系的平移向量
    // param_4(in): 初始化图像对的左图索引，右图索引为窗口内最后一帧
    // param_5，param_6(in): 初始化图像对的相对位姿，注意relative_R表示从当前最新帧到第l帧的坐标系变换relative_R和relative_T
    // param_7(in,out): 特征管理器内获取的所有特征轨迹
    // param_8(out): sfm计算的到的稀疏点云
    if(!sfm.construct(frame_count + 1, Q, T, l,
              relative_R, relative_T,
              sfm_f, sfm_tracked_points))
    {
        ROS_DEBUG("global SFM failed!");
        marginalization_flag = MARGIN_OLD;
        return false;
    }

4.对所有图像帧(包括滑动窗口外的图像帧)，给定初始的R/T，然后执行solvePnp进行优化，这个过程修正了all_image_frame中存储的每一帧的R/T(使用相机到imu的标定外参做了转换)。

    //solve pnp for all frame
    // 对于所有的图像帧，包括不在滑动窗口中的，提供初始的RT估计，然后solvePnP进行优化
    map<double, ImageFrame>::iterator frame_it;
    map<int, Vector3d>::iterator it;
    frame_it = all_image_frame.begin( );
    // 遍历所有frame数据
    // 这些frame数据有三种情况
    // a.时间戳小于滑动窗口第一帧时间戳
    // b.时间戳在滑动窗口内第i/i+1帧之间
    // c.时间戳大于滑动窗口最后一帧时间戳(不可能发生，因为滑动窗口内最后一帧就是当前最新帧)
    for (int i = 0; frame_it != all_image_frame.end( ); frame_it++)
    {
        // provide initial guess
        cv::Mat r, rvec, t, D, tmp_r;
        // 若当前frame在滑动窗口内(即关键帧keyframe)，则直接使用construct中更新的Q/T
        if((frame_it->first) == Headers[i].stamp.toSec())
        {
            frame_it->second.is_key_frame = true;
            frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose();
            frame_it->second.T = T[i];
            i++;
            continue;
        }
        if((frame_it->first) > Headers[i].stamp.toSec())
        {
            i++;
        }
        // 以距离当前普通帧时序上最近的滑动窗口内对应关键帧的Pose作为初值，执行Pnp求解当前普通帧的位姿
        Matrix3d R_inital = (Q[i].inverse()).toRotationMatrix();
        Vector3d P_inital = - R_inital * T[i];
        cv::eigen2cv(R_inital, tmp_r);
        cv::Rodrigues(tmp_r, rvec);
        cv::eigen2cv(P_inital, t);
        // 标记当前图像帧为普通帧
        frame_it->second.is_key_frame = false;
        // 建立特征点3d位置和对应2d位置的向量，用以Pnp计算位姿
        vector<cv::Point3f> pts_3_vector;
        vector<cv::Point2f> pts_2_vector;
        for (auto &id_pts : frame_it->second.points)
        {
            int feature_id = id_pts.first;
            for (auto &i_p : id_pts.second)
            {
                it = sfm_tracked_points.find(feature_id);
                if(it != sfm_tracked_points.end())
                {
                    Vector3d world_pts = it->second;
                    cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));
                    pts_3_vector.push_back(pts_3);
                    Vector2d img_pts = i_p.second.head<2>();
                    cv::Point2f pts_2(img_pts(0), img_pts(1));
                    pts_2_vector.push_back(pts_2);
                }
            }
        }
        cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);     
        if(pts_3_vector.size() < 6)
        {
            cout << "pts_3_vector size " << pts_3_vector.size() << endl;
            ROS_DEBUG("Not enough points for solve pnp !");
            return false;
        }
        if (! cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1))
        {
            ROS_DEBUG("solve pnp fail!");
            return false;
        }
        cv::Rodrigues(rvec, r);
        MatrixXd R_pnp,tmp_R_pnp;
        cv::cv2eigen(r, tmp_R_pnp);
        // R_pnp表示当前普通帧的相机坐标系到视觉坐标系(第l帧的相机坐标系)的旋转矩阵
        // T_pnp表示当前普通帧的相机坐标系到视觉坐标系(第l帧的相机坐标系)的平移向量
        R_pnp = tmp_R_pnp.transpose();
        MatrixXd T_pnp;
        cv::cv2eigen(t, T_pnp);
        T_pnp = R_pnp * (-T_pnp);
        // frame_it->second.R表示从当前帧的IMU坐标系到视觉坐标系(第l帧相机坐标系)的旋转矩阵
        // 因为VINS-MONO中相机和IMU之间的外参平移项被忽略
        // 因此frame_it->second.Ｔ(T_pnp)表示从当前帧的IMU坐标系到视觉坐标系(第l帧相机坐标系)的平移向量
        frame_it->second.R = R_pnp * RIC[0].transpose();
        frame_it->second.T = T_pnp;
    }

视觉IMU对齐

在执行视觉IMU对齐visualInitialAlign前，已经得到了所有图像帧的视觉位姿，同时在前端也记录了图像帧的预积分信息，因此后续就是对齐视觉位姿和预积分信息，从而估计陀螺仪Bias(VINS-MONO中不估计加速度计bias)，以及各帧速度和尺度信息。具体流程如下所示：

相机IMU旋转外参与IMU陀螺仪Bias求解

相机IMU旋转外参估计

1.基于连续两帧的匹配关系，求得本质矩阵E，本质矩阵分解得到四组解(R/R’，t/-t)，针对四组解分别进行两帧三角化并计算正深度点的比例，选择正深度点比例最高的那组解，从而得到视觉测量上的帧间旋转矩阵；
2.基于连续两帧的预积分信息，可以获取到Imu测量上的帧间旋转矩阵；
3.根据旋转变换关系，可以构建方程:
相应的代码实现主要在initial_ex_rotation.cpp中的CalibrationExRotation，详细流程如下所示

bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    frame_count++;
    // Rc中存储了当前帧相机到上一帧相机的视觉旋转矩阵
    Rc.push_back(solveRelativeR(corres));// 帧间cam的R，由对极几何得到
    // Rimu中存储了当前帧Imu到上一帧Imu的imu旋转矩阵
    Rimu.push_back(delta_q_imu.toRotationMatrix());// 帧间IMU的R，由IMU预积分得到
    // Rc_g中存储了由相机到Imu的旋转矩阵计算得到的当前帧相机到上一帧相机的混合旋转矩阵，理论上它应该等于视觉旋转矩阵
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);// 每帧IMU相对于起始帧IMU的R

    Eigen::MatrixXd A(frame_count * 4, 4);
    A.setZero();
    int sum_ok = 0;
    for (int i = 1; i <= frame_count; i++)
    {
        Quaterniond r1(Rc[i]);
        Quaterniond r2(Rc_g[i]);
        // 若当前估计值偏离视觉测量值太大，认为它是外点则调低权重，降低外点的影响
        double angular_distance = 180 / M_PI * r1.angularDistance(r2);
        ROS_DEBUG(
            "%d %f", i, angular_distance);

        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        ++sum_ok;
        Matrix4d L, R;

        double w = Quaterniond(Rc[i]).w();
        Vector3d q = Quaterniond(Rc[i]).vec();
        L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
        L.block<3, 1>(0, 3) = q;
        L.block<1, 3>(3, 0) = -q.transpose();
        L(3, 3) = w;

        Quaterniond R_ij(Rimu[i]);
        w = R_ij.w();
        q = R_ij.vec();
        R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
        R.block<3, 1>(0, 3) = q;
        R.block<1, 3>(3, 0) = -q.transpose();
        R(3, 3) = w;

        A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
    }
    //svd分解中最小奇异值对应的右奇异向量作为旋转四元数
    JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
    Matrix<double, 4, 1> x = svd.matrixV().col(3);
    Quaterniond estimated_R(x);
    // 取逆求解得到相机到IMU的外参旋转矩阵
    ric = estimated_R.toRotationMatrix().inverse();
    //cout << svd.singularValues().transpose() << endl;
    //cout << ric << endl;
    Vector3d ric_cov;
    ric_cov = svd.singularValues().tail<3>();
    // 至少迭代计算了WINDOW_SIZE次，且R的奇异值大于0.25才认为标定成功
    if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
    {
        calib_ric_result = ric;
        return true;
    }
    else
        return false;
}

陀螺仪Bias估计

首先明确一点，在初始化之前的预积分测量值都是不准确的，因为它们受IMU偏置影响。VINS-MONO在计算预积分时，设置Bias_g=Bias_a=0，同时会计算预积分对Bias_g的雅克比矩阵，因此可以利用这一信息来求解Bias_g，当然这里也会存在问题，即若实际Bias_g偏离0太远的话利用雅克比来估计可能会带来偏差。最好的当然是，离线标定时给一个Bias_g初值就最好了。
VINS-MONO对应的代码实现也比较简单，在initial_aligment.cpp中solveGyroscopeBias有具体的实现细节，和上述公式一一对应：

// Bgs表示了窗口内的每帧对应的陀螺仪偏置
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;
    // 遍历每组连续图像对frame_i,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();
        // frame_it->second.R表示从当前帧的IMU坐标系到视觉坐标系(第l帧相机坐标系)的旋转矩阵
        // frame_it->second.T表示从当前帧的IMU坐标系到视觉坐标系(第l帧相机坐标系)的平移向量
        // q_ij表示从frame_j时刻的IMU坐标系到frame_i时刻的IMU坐标系的旋转矩阵
        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);
        // frame_j->second.pre_integration->delta_q表示从frame_i时刻的IMU坐标系到frame_j时刻的IMU预积分旋转值
        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
        // 左乘雅克比转置的目的是为了构造方阵A，从而可以调用ldlt分解快速求解变量x
        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;
    // 基于最新的陀螺仪偏置，更新所有图像帧的预积分信息
    // 这里的计算会比较慢，需要遍历图像间的所有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]);
    }
}

重力方向，尺度和速度求解

重力方向，尺度以及速度的求解主要依赖于up-to-scale的视觉位姿和预积分的位移项和速度项建立约束方程，具体流程如下图所示:
在上述公式推导中，需要理解尺度scale怎么放到预积分位移方程中，起始就是简单的空间变换。此外，需要注意VINS-MONO代码中all_image_frames中存储的R和T分别表示的是各帧IMU到c0的旋转和各帧相机到c0的平移！！！另外从代码中有看到作者本想利用预积分的协方差信息来更好地估计这些状态，但是并没有真正使用，后续调试可以对比两者的差异性！！！

bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
    int all_frame_count = all_image_frame.size();
    // n_state包含了所有图像的速度，重力方向以及尺度
    int n_state = all_frame_count * 3 + 3 + 1;
    MatrixXd A{n_state, n_state};
    A.setZero();
    VectorXd b{n_state};
    b.setZero();

    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;
    int i = 0;
    // 遍历每一组连续图像帧
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)
    {
        frame_j = next(frame_i);

        MatrixXd tmp_A(6, 10);
        tmp_A.setZero();
        VectorXd tmp_b(6);
        tmp_b.setZero();

        double dt = frame_j->second.pre_integration->sum_dt;

        tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
        tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();
        tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;     
        tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0];
        //cout << "delta_p   " << frame_j->second.pre_integration->delta_p.transpose() << endl;
        tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
        tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
        tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();
        tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v;
        //cout << "delta_v   " << frame_j->second.pre_integration->delta_v.transpose() << endl;

        Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
        //cov.block<6, 6>(0, 0) = IMU_cov[i + 1];
        //MatrixXd cov_inv = cov.inverse();
        cov_inv.setIdentity();

        MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
        VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;

        A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
        b.segment<6>(i * 3) += r_b.head<6>();

        A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();
        b.tail<4>() += r_b.tail<4>();

        A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();
        A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();
    }
    A = A * 1000.0;
    b = b * 1000.0;
    x = A.ldlt().solve(b);
    double s = x(n_state - 1) / 100.0;
    ROS_DEBUG("estimated scale: %f", s);
    g = x.segment<3>(n_state - 4);
    ROS_DEBUG_STREAM(" result g     " << g.norm() << " " << g.transpose());
    // 如果重力加速度与参考值差太大或者尺度为负则说明计算错误
    if(fabs(g.norm() - G.norm()) > 1.0 || s < 0)
    {
        return false;
    }
	// 基于重力先验的重力方向和尺度优化
    RefineGravity(all_image_frame, g, x);
    s = (x.tail<1>())(0) / 100.0;
    (x.tail<1>())(0) = s;
    ROS_DEBUG_STREAM(" refine     " << g.norm() << " " << g.transpose());
    if(s < 0.0 )
        return false;   
    else
        return true;
}

基于重力先验的重力方向优化

这里重点提一下，基于重力先验的重力方向优化的理论细节，具体的代码实现在RefineGravity函数中。
在上一步已经求得重力方向的初值，但其模长和实际值肯定存在差异，为了得到更准确的重力方向，可以引入重力模长$$||g||=(0,0,9.81)$$作为先验进行参数化来表示重力方向，从而得到更准确的重力方向和其他状态量。
具体的参数化表达方式可以参考论文，其实就是建立一个半径为9.81的圆球，并且建立重力方向初值的切面，并在该切面上进行搜索从而确定最优的重力方向。具体的重力方向表达方式如下图所示:
将重力方向新的参数化表达方式带入到上一步，重新构建新的线性方程和状态量，可以求得更优的状态量:

系统最终状态

在视觉IMU对齐后，得到all_image_frames各帧速度，重力方向，尺度信息以及更新后的预积分信息，接下来就可以调整滑动窗口内所有关键帧的状态了。
大体流程如下
step1: 更新vins_estimator的成员变量Ps/Rs，并且在all_image_frame中标记滑窗内的图像帧为关键帧；
step2:重置特征管理器中的逆深度列表；
step3:特征点三角化，计算逆深度值；
step4:更新预积分信息(其实之前做过了，这里没必要再做一遍)；
step5:利用尺度更新滑窗内所有帧的平移信息，修正成各图像帧IMU到第0帧IMU的平移
step6:利用尺度更新特征点深度值；
step7:恢复滑窗内所有帧的速度(在cl图像坐标系下的速度)；
step8:通过将重力旋转到z轴上，得到世界坐标系与摄像机坐标系cl之间的旋转矩阵rot_diff，并调整相关状态量(速度，平移，旋转)；这里值得注意的是坐标系之间的转换，网上很多博客都混淆了，

bool Estimator::visualInitialAlign()
{
    TicToc t_g;
    VectorXd x;
    //solve scale
    // 计算陀螺仪偏置，尺度，重力加速度和速度
    bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);
    if(!result)
    {
        ROS_DEBUG("solve g failed!");
        return false;
    }

    // change state
    // 获取Headers[i]中对应的所有图像帧的位姿Ps、Rs，并将其置为关键帧
    // 从all_image_frame中取出滑动窗口内对应时间戳的位姿信息更新vins_estimator的成员信息Ps，Rs
    // 并在all_image_frame中记录对应帧为关键帧
    for (int i = 0; i <= frame_count; i++)
    {
        Matrix3d Ri = all_image_frame[Headers[i].stamp.toSec()].R;
        Vector3d Pi = all_image_frame[Headers[i].stamp.toSec()].T;
        Ps[i] = Pi;
        Rs[i] = Ri;
        all_image_frame[Headers[i].stamp.toSec()].is_key_frame = true;
    }

    // 重置所有特征点的逆深度为-1
    VectorXd dep = f_manager.getDepthVector();
    for (int i = 0; i < dep.size(); i++)
        dep[i] = -1;
    // 使用dep更新特征点管理器中对应特征的深度值
    f_manager.clearDepth(dep);

    //triangulat on cam pose , no tic
    //重新计算特征点的深度
    Vector3d TIC_TMP[NUM_OF_CAM];
    for(int i = 0; i < NUM_OF_CAM; i++)
        TIC_TMP[i].setZero();
    ric[0] = RIC[0];
    f_manager.setRic(ric);
    f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));
    // 陀螺仪的偏置bgs改变，重新计算预积分
    // 其实在intial_aligment中已经做过一次预积分更新了，这里没必要再做一次！！！
    double s = (x.tail<1>())(0);
    for (int i = 0; i <= WINDOW_SIZE; i++)
    {
        pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
    }
    // 将Ps、Vs、depth尺度s缩放后转变为相对于第0帧图像坐标系下(V是速度)
    // s * Ps[i] - Rs[i] * TIC[0]表示第i帧IMU在第l帧相机坐标系的绝对位置
    // (s * Ps[0] - Rs[0] * TIC[0])表示第0帧IMU在第l帧相机坐标系的绝对位置
    // s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0])表示第i帧imu在第0帧IMU坐标系的绝对位置
    for (int i = frame_count; i >= 0; i--)
        Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);
    int kv = -1;
    map<double, ImageFrame>::iterator frame_i;
    for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)
    {
        // 只更新滑动窗口内图像帧的速度，提高效率
        // 因为在初始化时计算的图像帧速度是对应时刻IMU在IMU坐标系上的速度
        // 因此需要转换一下得到对应时刻IMU在cl相机坐标系上的速度
        if(frame_i->second.is_key_frame)
        {
            kv++;
            Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);// x.segment<3>(kv * 3)表示v_bk_bk
        }
    }
    // 特征点深度值也需要等比例缩放
    for (auto &it_per_id : f_manager.feature)
    {
        it_per_id.used_num = it_per_id.feature_per_frame.size();
        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
            continue;
        it_per_id.estimated_depth *= s;
    }
    // 重力方向对齐世界惯性系z正半轴上，得到世界坐标系与摄像机坐标系cl之间的旋转矩阵rot_diff
    // g表示重力矢量在cl相机坐标系上的向量表示
    // R0表示第cl帧相机坐标系到世界坐标系的旋转矩阵
    Matrix3d R0 = Utility::g2R(g);
    // Rs[0]表示第0帧IMU到第cl帧相机坐标系的旋转矩阵
    // R0 * Rs[0]表示第0帧IMU到世界惯性系的旋转矩阵
    // yaw表示第0帧IMU到世界惯性系的旋转矩阵的航向角
    // 利用这个yaw角将滑动窗口内第0帧的IMU坐标系和世界惯性系完全对齐(z,x,y三轴完全对齐)
    double yaw = Utility::R2ypr(R0 * Rs[0]).x();
    R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
    g = R0 * g;
    //Matrix3d rot_diff = R0 * Rs[0].transpose();
    // rot_diff表示从cl相机坐标系到世界坐标系的旋转矩阵
    Matrix3d rot_diff = R0;
    // 所有变量从参考坐标系c0旋转到世界坐标系w
    for (int i = 0; i <= frame_count; i++)
    {
        Ps[i] = rot_diff * Ps[i];
        Rs[i] = rot_diff * Rs[i];
        Vs[i] = rot_diff * Vs[i];
    }
    ROS_DEBUG_STREAM("g0     " << g.transpose());
    ROS_DEBUG_STREAM("my R0  " << Utility::R2ypr(Rs[0]).transpose()); 

    return true;
}