VINS-Mono代码阅读笔记(九):vins_estimator中的相机-IMU对齐

本篇笔记紧接上一篇VINS-Mono代码阅读笔记(八):vins_estimator中的相机-IMU初始化。阅读分析视觉惯导对齐部分的代码。相机-IMU对齐指的是将视觉SFM结构和IMU的预积分结果进行对齐,主要分为1)陀螺仪偏差的标定;2)速度、重力向量和尺度初始化;3)对重力进行修正三部分。效果如论文中提供的下图所示。

1.陀螺仪偏差的标定

1)论文中理论描述推导

从滑动窗口中根据视觉SFM可以获得两个连续的帧b_k,b_{k+1}相对于相机第一帧图像之间的旋转q_{b_k}^{c_0}q_{b_{k+1}}^{c_0},从IMU预积分中可以获得相邻帧间的旋转\widehat{\gamma }_{b_{k+1}}^{b_k}理论上来讲,视觉SFM获得的相邻帧间旋转应该和IMU预积分计算出的相邻帧间旋转相等因此,线性化IMU预积分项和陀螺仪偏差,可以得到最小化的代价函数如下:

                                                                      \large \underset{\delta b_w}{min}\underset{k\in \ss }{\sum }\left \|{q_{b_{k+1}}^{c_0}} ^{-1} \bigotimes q_{b_k}^{c_0}\bigotimes \gamma _{b_{k+1}}^{b_k}\right \|^2

                                                                       \large \gamma _{b_{k+1}}^{b_k}\approx \widehat{\gamma }_{b_{k+1}}^{b_k}\bigotimes \begin{bmatrix} 1\\ \frac{1}{2}J_{b_w}^\gamma \delta b_w \end{bmatrix}

其中,\large \ss表示了滑动窗口中所有帧的index。这样我们就获得了陀螺仪的偏差\large b_w的初始化标定,然后使用新获得的陀螺仪偏差对IMU的预积分项进行重新积分。

由于损失函数的最小值为单位四元数,所以代价函数可以写为:

                                                                    \large q_{b_{k+1}}^{c_0}} ^{-1} \bigotimes q_{b_k}^{c_0}\bigotimes \gamma _{b_{k+1}}^{b_k}=\begin{bmatrix} 1\\ 0 \end{bmatrix}

                                                             

                                \large \Rightarrow \widehat{\gamma }_{b_{k+1}}^{b_k}\bigotimes \begin{bmatrix} 1\\ \frac{1}{2}J_{b_w}^\gamma \delta b_w \end{bmatrix}={q_{b_k}^{c_0}}^{-1}\bigotimes q_{b_{k+1}}^{c_0}\bigotimes \begin{bmatrix} 1\\ 0 \end{bmatrix}            

                                \large \Rightarrow   \large \begin{bmatrix} 1\\ \frac{1}{2}J_{b_w}^\gamma \delta b_w \end{bmatrix}=(\hat{\gamma }_{b_{k+1}}^{b_k})^{-1}\bigotimes {q_{b_k}^{c_0}}^{-1}\bigotimes q_{b_{k+1}}^{c_0}\bigotimes \begin{bmatrix} 1\\ 0 \end{bmatrix}

此时,如果只考虑虚部则有下面式子:

                                        \large \frac{1}{2}J_{b_w}^\gamma \delta b_w=((\hat{\gamma }_{b_{k+1}}^{b_k})^{-1}\bigotimes {q_{b_k}^{c_0}}^{-1}\bigotimes q_{b_{k+1}}^{c_0})_{vec}

将左侧转为正定阵,这样就可以进行分解了。如下式:

                                       

上式就是我们最终要求解的方程,其中\large \delta b_w为要求解的未知数(陀螺仪偏差)。该式形式可理解为为:\large Ax=b

下面代码中就按照这个方式来进行求解。

2)代码分析

solveGyroscopeBias函数是进行陀螺仪偏差计算的函数,代码如下。

这里我有点不明白的是,论文公式中推出来的最终公式里是\large {\color{Red} \mathbf{}{q_{b_k}^{c_0}}^{-1}}应该是逆,代码中是frame_i->second.R.transpose(),意思是转置,这里不应该是逆吗??

----------后来我明白了,在此补充说明一下,由于旋转矩阵为正交矩阵,所以它的逆和转置是相等的,都描述了一个相反的旋转。

/**
 * 陀螺仪偏差标定
*/
void solveGyroscopeBias(map &all_image_frame, Vector3d* Bgs)
{
    Matrix3d A;
    Vector3d b;
    Vector3d delta_bg;
    A.setZero();
    b.setZero();
    map::iterator frame_i;
    map::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_j_bw
        //O_R的值为3 O_BG的值为12                                                 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;

    }
    //进行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.速度、重力向量和尺度的初始化

1)论文中理论描述推导

本部分需要优化求解速度、重力向量和尺度,如下所示:

                                                              \large \chi _I=\begin{bmatrix} v_{b_0}^{b_0},v_{b_1}^{b_1},\cdots v_{b_n}^{b_n},g^{c_0},s \end{bmatrix}

其中\large v_{b_k}^{b_k}是相机拍摄到第k帧图像对应的IMU体坐标中对应的速度,\large g^{c_0}是相机第\large c_0帧的重力向量,\large s是SFM的尺度。

这部分的损失函数构造是通过求解IMU预积分获得的值视觉SFM运动估计的值之间的残差来进行求解。

通过IMU预积分获取连续的两帧\large b_k\large b_{k+1}两帧之间的IMU测量值如下:

                     \large \alpha _{b_{k+1}}^{b_k}=\iint_{t\in [t_k,t_{k+1}]}R_t^{b_k}(\hat{a}_t-b_{a_t})dt^2                   这个也可以说是IMU预积分出来的位移的增量

                     \large \beta _{b_{k+1}}^{b_k}=\int _{t\in [t_k,t_{k+1}]}R_t^{b_k}(\hat{a}_t-b_{a_t})dt                        这个理解为是IMU预积分出来的速度的增量

在滑动窗口中,将相机第一帧\large \left ( . \right )^{c_0}作为SFM的参考帧,所有帧的位姿为\large \left ( \bar{p}_{c_k}^{c_0},q_{c_k}^{c_0} \right ),有因为通过相机和IMU之间外参标定计算出来的外参为\large \left ( p_c^b,q_c^b \right ),因此将位姿从相机坐标下转到IMU坐标后如下:

                                     \large q_{b_k}^{c_0}=q_{c_k}^{c_0}\bigotimes \left ( q_c^b \right )^{-1}

                                     \large s\bar{p}_{b_k}^{c_0}=s\bar{p}_{c_k}^{c_0}-R_{b_k}^{c_0}p_c^b

滑动窗口中连续的两帧\large b_k\large b_{k+1}之间位移和速度的估计增量计算如下:

\large \alpha _{b_{k+1}}^{b_k}=R_{c_0}^{b_k}(s\bar{p}_{b_{k+1}}^{c_0}+\frac{1}{2}g^{c_0}\Delta t_k^2-s\bar{p}_{b_k}^{c_0}-R_{b_k}^{c_0}v_{b_k}^{b_k}\Delta t_k)

           \large =R_{c_0}^{b_k}(s\bar{p}_{c_{k+1}}^{c_0}-R_{b_{k+1}}^{c_0}p_c^b+\frac{1}{2}g^{c_0}\Delta t_k^2-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_k)            

          \large =R_{c_0}^{b_k}s\bar{p}_{c_{k+1}}^{c_0}-R_{c_0}^{b_k}R_{b_{k+1}}^{c_0}p_c^b+\frac{1}{2}R_{c_0}^{b_k}g^{c_0}\Delta t_k^2-sR_{c_0}^{b_k}\bar{p}_{c_k}^{c_0}+p_c^b-v_{b_k}^{b_k}\Delta t_k

          \large =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_k+\frac{1}{2}R_{c_0}^{b_k}g^{c_0}\Delta t_k^2

\large \beta _{b_{k+1}}^{b_k}=R_{c_0}^{b_k}(R_{b_{k+1}}^{c_0}v_{b_{k+1}}^{b_{k+1}}+g^{c_0}\Delta t_k-R_{b_k}^{c_0}v_{b_k}^{b_k}).

此时,IMU预积分的增量和估计出的增量之间的误差可写成如下:

\large \begin{bmatrix} \delta \alpha _{b_{k+1}}^{b_k}\\ \delta \beta _{b_{k+1}}^{b_k} \end{bmatrix}=\begin{bmatrix} \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_k-\frac{1}{2}R_{c_0}^{b_k}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{bmatrix}=\large \begin{bmatrix} 0_{3\times 1}\\ 0_{3\times 1} \end{bmatrix}

此时,将上式调整为\large Hx=b的形式,这样便于直接利用Cholesky进行求解:

\large \delta \alpha_{b_{k+1}}^{b_k}=\large \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_k-\frac{1}{2}R_{c_0}^{b_k}g^{c_0}\Delta t_k^2\large =0_{3\times 1}

        \large =R_{c_0}^{b_k}s(\bar{p}_{c_{k+1}}^{c_0}-\bar{p}_{c_k}^{c_0})-\Delta t_kv_{b_k}^{b_k}+\frac{1}{2}R_{c_0}^{b_k}\Delta t_k^2g^{c_0}\large =\alpha _{b_{k+1}}^{b_k}-p_c^b+R_{c_0}^{b_k}R_{b_{k+1}}^{c_0}p_c^b

转成矩阵形式可以写成如下:

\large \begin{bmatrix} -I\Delta t_k & 0 & \frac{1}{2}R_{c_0}^{b_k}\Delta t_k^2 & R_{c_0}^{b_k}(\bar{p}_{c_{k+1}}^{c_0}-\bar{p}_{c_k}^{c_0}) \end{bmatrix}\begin{bmatrix} v_{b_k}^{b_k}\\ v_{b_{k+1}}^{b_{k+1}}\\ g^{c_0}\\ s \end{bmatrix}=\alpha_{b_{k+1}}^{b_k}-p_c^b+R_{c_0}^{b_k}R_{b_{k+1}}^{c_0}p_c^b

同样,也将\large \delta \beta _{b_{k+1}}^{b_k}转为矩阵形式,综合写成下式:

\large \begin{bmatrix} -I\Delta t_k& 0 & \frac{1}{2}R_{c_0}^{b_k}\Delta t_k^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{bmatrix}\begin{bmatrix} v_{b_k}^{b_k}\\ v_{b_{k+1}}^{b_{k+1}}\\ g^{c_0}\\ s \end{bmatrix}=\begin{bmatrix} \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{bmatrix}

这个矩阵乘积形式为\large H^{6\times 10}X_I^{10\times 1}=b^{6\times 1},这样,两侧同时左乘\large H^T 就可以利用Cholesky分解方程求解\large X:

                                                                          \large H^{T}HX_I^{10\times 1}=H^{T}b

此处最开始有不清楚的地方,借鉴了崔华坤发表在泡泡机器人上边的文章。下面对照着代码分析具体的求解。

2)代码分析

这部分代码如下:

bool LinearAlignment(map &all_image_frame, Vector3d &g, VectorXd &x)
{
    int all_frame_count = all_image_frame.size();
    int n_state = all_frame_count * 3 + 3 + 1;

    MatrixXd A{n_state, n_state};
    A.setZero();
    VectorXd b{n_state};
    b.setZero();

    map::iterator frame_i;
    map::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);
        //tmp_A为6*10的矩阵,就是H
        MatrixXd tmp_A(6, 10);
        tmp_A.setZero();
        //tmp_b对应b
        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*deltat_k^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 t
        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 cov_inv = Matrix::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>();
    }
    //不是很理解这里为什么要对A和b都同时乘个1000.0呢???
    A = A * 1000.0;
    b = b * 1000.0;
    //对Ax=b进行分解求出x
    x = A.ldlt().solve(b);
    //从求解出的x向量里边取出最后边的尺度s
    double s = x(n_state - 1) / 100.0;
    ROS_DEBUG("estimated scale: %f", s);
    //取出对重力向量g的计算值
    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;
}

我没有理解这块为什么要给A和b都乘以1000.0???

3.对重力进行修正

1)论文中相关描述

由于重力矢量的模大小是已知的,因此剩下两个自由度。在半径为\large g=9.81的半球面的切面空间上用两个正交的向量对重力进行参数化描述,如论文中提供的下图所示:

VINS-Mono代码阅读笔记(九):vins_estimator中的相机-IMU对齐_第1张图片

此时重力向量表示为:

                                       \large \hat{g}=\left \| g \right \|\cdot \hat{\bar{g}}^{3\times 1}+w_1\vec{b_1}^{3\times 1}+w_2\vec{b_2}^{3\times 1}=\left \| g \right \|\cdot \hat{\bar{g}}^{3\times 1}+\vec{b}^{3\times 2}w^{2\times 1}

其中\large \left \| g \right \|为重力向量的模,\large \hat{\bar{g}}为表示重力方向的单位向量,\large w_1,w_2为在两个正交向量\large b_1,b_2方向上的大小。

将新写的重力向量带入上边2中所推得的公式中,得如下结果:

\large \begin{bmatrix} -I\Delta t_k& 0 & \frac{1}{2}R_{c_0}^{b_k}\Delta t_k^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{bmatrix}\begin{bmatrix} v_{b_k}^{b_k}\\ v_{b_{k+1}}^{b_{k+1}}\\ w\\ s \end{bmatrix}=\begin{bmatrix} \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_{k}\left \| g \right \|\hat{\bar{g}}\\ \beta _{b_{k+1}}^{b_k}-R_{c_0}^{b_k}\Delta t_k\left \| g \right \| \hat{\bar{g}}\end{bmatrix}

此时,该矩阵乘法表现形式为\large H^{6\times 9}X_I^{9\times 1}=b^{6\times 1}, w^{2\times 1}=[w_1,w_2]^T,然后用LDLT分解下面方程求解\large X_I

                                                                               \large H^THX_I=H^{T}b

2)代码实现

void RefineGravity(map &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::iterator frame_i;
    map::iterator frame_j;
    //迭代求解4次
    for(int k = 0; k < 4; k++)
    {
        MatrixXd lxly(3, 2);
        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 cov_inv = Matrix::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=9.81的半球上找到切面的一对正交基,存放在bc当中
*/
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;
}

在将重力向量进行修正以后,我们就可以通过将重力旋转到z轴的方向上,从而获得相机第0帧到世界坐标系之间的旋转\large q_{c_0}^w。这时候就可以把所有变量从以相机第0帧为参考帧的坐标系中旋转到世界坐标系下。IMU帧中的速度也可以旋转到世界坐标系中。这时候从视觉SFM中获得的平移量将达到米级单位的刻度。

4.视觉SFM运动和IMU预积分结果对齐后位姿的计算

在将从视觉SFM中估计出来的位姿信息和IMU预积分的结果对齐之后,也就意味着本篇笔记开头的那张图中展示的视觉结构和IMU预积分结果匹配完成了。此时,我们需要获得世界坐标系中的位姿,也就是计算出PVQ,这样就完成了位姿的初始化估计,后边将用于进行单目紧耦合的VIO操作。

visualInitialAlign函数代码如下:

/**
 * 视觉惯导对齐
*/
bool Estimator::visualInitialAlign()
{
    TicToc t_g;
    VectorXd x;
    //solve scale 1.视觉惯性联合初始化,计算陀螺仪的偏置,尺度,重力加速度和速度
    bool result = VisualIMUAlignment(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.这里陀螺仪的偏差Bgs改变了,需遍历滑动窗口中的帧,重新预积分
    for (int i = 0; i <= WINDOW_SIZE; i++)
    {
        pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
    }
    //5.计算各帧相对于b0的位姿信息,前边计算的都是相对于第l帧的位姿
    /**
     * 前面初始化中,计算出来的是相对滑动窗口中第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帧的平移量
     * 注意:如果运行的脚本是配置文件中无外参的脚本,那么这里的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::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轴方向,
     * 这样就可以计算相机系到世界坐标系的旋转矩阵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;
}

VINS-Mono代码阅读笔记(十):vins_estimator中的非线性优化

 

 

你可能感兴趣的:(SLAM,slam,vins-mono)