VINS-mono代码阅读 -- 初始化（三）

这一部分来看看视觉-IMU对齐。

1. 视觉惯导联合初始化 VisualIMUAlignment

这个函数调用了两个函数，1）陀螺仪bias校正 2）初始化速度、重力和尺度因子

bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)
{
	//陀螺仪偏差标定
    solveGyroscopeBias(all_image_frame, Bgs);
	//速度、重力向量和尺度的初始化
    if(LinearAlignment(all_image_frame, g, x))
        return true;
    else 
        return false;
}

1）陀螺仪bias校正 solveGyroscopeBias

把这段代码分为两个部分，首先是构造这个求解的方程。

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();
		//对应公式中的四元数乘积运算： q_ij = (q^c0_bk)^-1 * (q^c0_bk+1)
        Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
		//tmp_A = J^r_bw													   3   *  12
        tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
		//tmp_b = 2 * ((r^bk_bk+1)^-1 * (q^c0_bk)^-1 * (q^c0_bk+1))_vec
		//		= 2 * ((r^bk_bk+1)^-1 * q_ij)_vec
        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
		//tmp_A * delta_bg = tmp_b
        A += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;

    }
}

将这部分对应的公式写在这里，以供对照：
目标函数为：视觉给出的相邻两帧之间的旋转等于预积分的旋转值
$$\underset{\delta b_w}{\min }\sum _{k\in B}{\Vert {q_{b_{k+1}}^{c_0}}^{-1}⨂q_{b_k}^{c_0}⨂{\gamma }_{b_{k+1}^{b_k}}\Vert }^2$$
其中
$${\gamma }_{b_{k+1}^{b_k}}={\hat{\gamma }}_{b_{k+1}}^{b_k}⨂\left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_{\omega }}^{\gamma }\delta b_{\omega }\end{array}\right]$$
目标函数的最小值为单位四元数，所以可以写成：
$${q_{b_{k+1}}^{c_0}}^{-1}⨂q_{b_k}^{c_0}⨂{\gamma }_{b_{k+1}^{b_k}}=\left[\begin{array}{c}1\\ 0\end{array}\right]$$
$${\hat{\gamma }}_{b_{k+1}}^{b_k}⨂\left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_{\omega }}^{\gamma }\delta b_{\omega }\end{array}\right]={q_{b_k}^{c_0}}^{-1}⨂q_{b_{k+1}}^{c_0}⨂\left[\begin{array}{c}1\\ 0\end{array}\right]$$
$$\left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_{\omega }}^{\gamma }\delta b_{\omega }\end{array}\right]={{\hat{\gamma }}_{b_{k+1}}^{b_k}}^{-1}⨂{q_{b_k}^{c_0}}^{-1}⨂q_{b_{k+1}}^{c_0}⨂\left[\begin{array}{c}1\\ 0\end{array}\right]$$
只考虑虚部：
$$J_{b_{\omega }}^{\gamma }\delta b_{\omega }=2({{\hat{\gamma }}_{b_{k+1}}^{b_k}}^{-1}⨂{q_{b_k}^{c_0}}^{-1}⨂q_{b_{k+1}}^{c_0}{)}_{vec}$$
将左边的矩阵转换为正定矩阵，就可以使用Cholesky分解了：
$${J_{b_{\omega }}^{\gamma }}^T{J}_{b_{\omega }}^{\gamma }\delta b_{\omega }=2{J_{b_{\omega }}^{\gamma }}^T({{\hat{\gamma }}_{b_{k+1}}^{b_k}}^{-1}⨂{q_{b_k}^{c_0}}^{-1}⨂q_{b_{k+1}}^{c_0}{)}_{vec}$$

上面的代码的所有解释都在公式里了，和公式对照起来，一步步理解，足以让人印象深刻。
下面的代码就是对等式进行ldlt分解，得到结果就是陀螺仪的bias。
在求出陀螺仪的bias后，需要对IMU与积分的值进行重新计算。

//ldlt分解
    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]);
    }

2）初始化速度，重力和尺度因子

和前面一样，我把这段代码也分为两部分，构造这个要求解的方程：

for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)
    {
        frame_j = next(frame_i);
		//tmp_A 就是H矩阵
        MatrixXd tmp_A(6, 10);
        tmp_A.setZero();
        VectorXd tmp_b(6);
        tmp_b.setZero();

        double dt = frame_j->second.pre_integration->sum_dt;
		// -I * delta_tk
        tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
		// 1/2 * R_c0^bk * delta_tk^2
        tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();
		// R_c0^bk * (bar{p}_{c_{k+1}}^{c_0} - bar{p}_{c_{k}}^{c_0})
        tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;  

		// alpha_{b_{k+1}}^{b_k} + R_{c_0}^{b_k} * R_{b_{k+1}}^c_0 * p_c^b - p_c^b
        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;

		// -I
        tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
		// R_c0^{b_k} * R_{b_{k+1}}^c0
        tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
		// R_{ c_0 }^ {b_k} * delta tk
        tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();

		// beta_{b_{k+1}}^{b_k}
        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();
		// 10 * 6   6*6   6 * 10 = 10 * 10
        MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
		// 10 * 6  6 * 6  6 * 1 = 10 * 1
        VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;
		//构造 A 和 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>();
    }

要优化的变量
$$X_I^{3\ast (n+1)+3+1}=[v_{b_0}^{b_0},v_{b_1}^{b_1}\cdots v_{b_n}^{b_n},g^{c_0},s]$$
第k图像帧在body系下的速度，第0 帧相机坐标系下的重力向量
残差可以定义为：相邻两帧之间的IMU预积分算出的增量${\alpha }_{b_{k+1}}^{b_k},{\beta }_{b_{k+1}}^{b_k}$和预测值之间的误差：
$$\gamma ({\hat{z}}_{b_{k+1}}^{b_k,},X_I)=\left[\begin{array}{c}\delta {\alpha }_{b_{k+1}}^{b_k}\\ \delta {\beta }_{b_{k+1}}^{b_k}\end{array}\right]=\left[\begin{array}{c}{\alpha }_{b_{k+1}}^{b_k}-R_{c_0}^{b_k}(s({\bar{p}}_{c_{k+1}}^{c_0}-{\bar{p}}_{c_k}^{c_0})-R_{b_k}^{c_0}{v}_{b_k}^{b_k}\Delta t+\frac{1}{2}{g}^{c_0}\Delta t_k^2)\\ {\beta }_{b_{k+1}}^{b_k}-R_{c_0}^{b_k}(R_{b_{k+1}}^{c_0}{v}_{b_{k+1}}^{b_{k+1}}-R_{b_k}^{c_0}{v}_{b_k}^{b_k}{g}^{c_0}\Delta t_k)\end{array}\right]=\left[\begin{array}{c}{0}_{3\times 3}\\ 0_{3\times 3}\end{array}\right]$$
将文章中的公式14代进来，将相机第一帧$({)}^{c_0}作为参考帧，所有相机的位姿({\bar{p}}_{c_k}^{c_0},q_{c_k}^{c_0})$,有通过外参标定算出来的外参$(p_c^b,q_c^b)$因此，将位姿从相机坐标系转到IMU坐标系的公式：

$$\begin{array}{rl}\delta {\alpha }_{b_{k+1}}^{b_k}& ={\alpha }_{b_{k+1}}^{b_k}-R_{c_0}^{b_k}(s{\bar{p}}_{c_{k+1}^{c_0}}-R_{b_{k+1}^{c_0}}{p}_c^b-(s{\bar{p}}_{c_k^{c_0}}-R_{b_k^{c_0}}{p}_c^b)-R_{b_k}^{c_0}{v}_{b_k}^{b_k}\Delta t+\frac{1}{2}{g}^{c_0}\Delta t_k^2)\\ & ={\alpha }_{b_{k+1}}^{b_k}-R_{c_0}^{b_k}s({\bar{p}}_{c_{k+1}^{c_0}}-{\bar{p}}_{c_k^{c_0}})+R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}{p}_c^b-p_c^b+v_{b_k}^{b_k}\Delta t-\frac{1}{2}{R}_{c_0}^{b_k}{g}^{c_0}\Delta t^2\\ & =0_{3\times 3}\end{array}$$
想办法将上式转换成$Hx=b$的形式，这样便于直接利用cholesky分解。
将含有优化变量的项和不含有优化变量的项分开，
$$R_{c_0}^{b_k}s({\bar{p}}_{c_{k+1}^{c_0}}-{\bar{p}}_{c_k^{c_0}})-v_{b_k}^{b_k}\Delta t+\frac{1}{2}{R}_{c_0}^{b_k}{g}^{c_0}\Delta t^2={\alpha }_{b_{k+1}}^{b_k}-p_c^b+R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}{p}_c^b$$
将上式写成矩阵的形式：
$$\left[\begin{array}{cccc}-I\Delta t& 0& \frac{1}{2}{R}_{c_0}^{b_k}\Delta t^2& R_{c_0}^{b_k}({\bar{p}}_{c_{k+1}^{c_0}}-{\bar{p}}_{c_k^{c_0}})\end{array}\right]\left[\begin{array}{c}{v}_{b_k}^{b_k}\\ v_{b_{k+1}}^{b_{k+1}}\\ g^{c_0}\\ s\end{array}\right]={\alpha }_{b_{k+1}}^{b_k}-p_c^b+R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}{p}_c^b$$

将$\beta$也写成矩阵的形式
$$\left[\begin{array}{cccc}-I\Delta t& 0& \frac{1}{2}{R}_{c_0}^{b_k}\Delta t^2& R_{c_0}^{b_k}({\bar{p}}_{c_{k+1}^{c_0}}-{\bar{p}}_{c_k^{c_0}})\\ -I& R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}& R_{c_0}^{b_k}\Delta t_k& 0\end{array}\right]\left[\begin{array}{c}{v}_{b_k}^{b_k}\\ v_{b_{k+1}}^{b_{k+1}}\\ g^{c_0}\\ s\end{array}\right]=\left[\begin{array}{c}{\alpha }_{b_{k+1}}^{b_k}-p_c^b+R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}{p}_c^b\\ {\beta }_{b_{k+1}^{b_k}}\end{array}\right]$$

$$H^{6\times 10}{X}_I^{10\times 1}=b^{6\times 1}$$
这样就可以用cholesky分解下面的方程求$X_I$
$$H^{T}H{X}_I=H^{T}b$$

• 下面的代码就是用ldlt分解求解$X_I$，并做了一些判断，并对重力进行了修正。
  这里对A和b的值都放大了1000倍，，是为了避免数据过小，计算的过程中出现截断误差
  其它的代码注释也很清楚了。

    A = A * 1000.0;
    b = b * 1000.0;

	//对Ax = b 求解
    x = A.ldlt().solve(b);

	//从求解出来的 x 向量里面取出最后面的尺度 s 
    double s = x(n_state - 1) / 100.0;
    ROS_DEBUG("estimated scale: %f", s);
	// 取出对重力向量 g 的计算值 从n_state开始，取3个数
    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;

3）修正重力矢量

由于重力矢量的模大小是已知的，因此剩下两个自由度。在半径为9.81的半球切平面空间用两个正交的向量对重力进行参数化。
$${\hat{g}}^{3\times 3}=\Vert g\Vert {\bar{\hat{g}}}^{3\times 1}+{\omega }_1{\vec{b}}_1+{\omega }_2{\vec{b}}_2$$
其中$\Vert g\Vert$为重力向量的模，$\hat{\bar{g}}$为表示重力方向的单位向量，$w_1,w_2$为在两个正交向量$b_1,b_2$方向上的大小。
将新写的重力向量带入上面初始化速度，重力和尺度因子中推出的公式：
$$\left[\begin{array}{cccc}-I\Delta t& 0& \frac{1}{2}{R}_{c_0}^{b_k}\Delta t^2\vec{b}& R_{c_0}^{b_k}({\bar{p}}_{c_{k+1}^{c_0}}-{\bar{p}}_{c_k^{c_0}})\\ -I& R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}& R_{c_0}^{b_k}\Delta t_k\vec{b}& 0\end{array}\right]\left[\begin{array}{c}{v}_{b_k}^{b_k}\\ v_{b_{k+1}}^{b_{k+1}}\\ \omega \\ s\end{array}\right]=\left[\begin{array}{c}{\alpha }_{b_{k+1}}^{b_k}-p_c^b+R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}{p}_c^b-\frac{1}{2}{R}_{c_0}^{b_k}\Delta t^2\Vert g\Vert \cdot \bar{\hat{g}}\\ {\beta }_{b_{k+1}^{b_k}}-R_{c_0}^{b_k}\Delta t\Vert g\Vert \cdot \bar{\hat{g}}\end{array}\right]$$
然后用cholosky分解求解$H^{T}H{X}_I=H^{T}b$这样我们就获得了$C_0$系下的重力向量$g^{c_0}$，通过将$g^{c_0}$旋转到惯性坐标系中的z轴方向，可以计算相机坐标系到惯性坐标系的旋转矩阵$q_{c_0}^w$，这样就可以将所有的变量调整至惯性世界坐标系中。

void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
    Vector3d g0 = g.normalized() * G.norm();
    Vector3d lx, ly;
    //VectorXd x;
    int all_frame_count = all_image_frame.size();
    int n_state = all_frame_count * 3 + 2 + 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;
	// 迭代求解 4 次
    for(int k = 0; k < 4; k++)
    {
        MatrixXd lxly(3, 2);
		// 在半径为 9.81 的半球上 找到切面的一对正交基，， 存放在bc当中
        lxly = TangentBasis(g0);
        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, 9);
            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, 2>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly;
            tmp_A.block<3, 1>(0, 8) = 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] - frame_i->second.R.transpose() * dt * dt / 2 * g0;

            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, 2>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly;
            tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0;


            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<3, 3>() += r_A.bottomRightCorner<3, 3>();
            b.tail<3>() += r_b.tail<3>();

            A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>();
            A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>();
        }
            A = A * 1000.0;
            b = b * 1000.0;
            x = A.ldlt().solve(b);
            VectorXd dg = x.segment<2>(n_state - 3);
            g0 = (g0 + lxly * dg).normalized() * G.norm();
            //double s = x(n_state - 1);
    }   
    g = g0;
}

找正交基的时候应该是使用了施密特正交化吧，

//在半径为G的半球找到切面的一对正交基
MatrixXd TangentBasis(Vector3d &g0)
{
    Vector3d b, c;
    Vector3d a = g0.normalized();
    Vector3d tmp(0, 0, 1);
    if(a == tmp)
        tmp << 1, 0, 0;
    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;
}

视觉-IMU对齐之后的处理

计算出世界坐标系下的PVQ

bool Estimator::visualInitialAlign()
{
    TicToc t_g;
    VectorXd x;
    //solve scale  1. 视觉惯性联合初始化， 计算陀螺仪的偏置，尺度，重力加速度和 速度
    bool result = bool VisualIMUAlignment(map<double, ImageFrame> & all_image_frame, Vector3d * Bgs, Vector3d & g, VectorXd & x)
	{
		//陀螺仪偏差标定
		solveGyroscopeBias(all_image_frame, Bgs);
		//速度、重力向量和尺度的初始化
		if (LinearAlignment(all_image_frame, g, x))
			return true;
		else
			return false;
	}(all_image_frame, Bgs, g, x);
    if(!result)
    {
        ROS_DEBUG("solve g failed!");
        return false;
    }

    // change state  2. 获取滑动窗口内所有帧相对于第l帧的位姿信息，并设置为关键帧
    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;
    }
	// 3. 获取特征点深度
    VectorXd dep = f_manager.getDepthVector();
    for (int i = 0; i < dep.size(); i++)
        dep[i] = -1;
    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中存放的是 相机到IMU 的旋转， 在相机-IMU外参标定环节得到
    ric[0] = RIC[0];
    f_manager.setRic(ric);
	// 三角化计算地图点的深度，Ps 中存放的是 各个帧相对于参考帧之间的平移，RIC[0] 为 相机-IMU 之间的旋转
    f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));

    double s = (x.tail<1>())(0);
	// 4. 这里陀螺仪的偏差Bags 改变了，需要遍历滑窗中的帧，重新进行预积分，
    for (int i = 0; i <= WINDOW_SIZE; i++)
    {
        pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
    }
	// 5. 计算各帧相对于b0的位置信息，前面计算的都是相对于第l帧的位姿，在这里转换到b0帧坐标系下
	/*
	* 前面的初始化中，计算出来的是相对滑动窗中第l帧的位姿，在这里转换到b0帧的坐标系下
	* s*p_bk^​b0​​=s*p_bk^​cl​​−s*p_b0^​cl​​=(s*p_ck^​cl​​−R_bk​^cl​​*p_c^b​)−(s*p_c0^​cl​​−R_b0​^cl​​*p_c^b​)
	* TIC[0]是相机到IMU的旋转量
	* Rs 是IMU第k帧到滑动窗口中图像第l帧的旋转
	* Ps 是滑动窗口中第k帧到第l帧的平移
	* 如果 launch文件中配置的 无外参，那么TIC 都是 0 
	*/
    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++)
    {
        if(frame_i->second.is_key_frame)
        {
            kv++;
			//存储速度
            Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);
        }
    }
	// 更新每个地图点被观测到的帧数(used_num) 和预测的深度 (estimated_depth)
    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;
    }
	/**
	* refine 之后就获得了C_0坐标系下的中立g^{c_0},此时通过将g^{c_0}旋转值z轴方向
	* 这样就可以i计算相机系到世界系的旋转矩阵 q_{c_0}^w, 这里求得的是rot_diff， 这样就可以将所有的变量调整到世界系中
	*/
    Matrix3d R0 = Utility::g2R(g);
    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();
    Matrix3d rot_diff = R0;
	//所有的变量从参考坐标系c_0 旋转到世界坐标系 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;
}