VINS-Mono融合轮速计和GPS（二）：鲁棒初始化

• 开篇
• 介绍
• 理论
• • 鲁棒初始化
  • • VINS-Mono的初始化
    • • 1. 陀螺仪bias校正
      • 2. 初始化速度和重力
      • 3. 初始化尺度因子
      • 4. 优化重力方向
• 实践
• • 配合代码查看

开篇

项目地址VINS-GPS-Wheel，欢迎交流学习。
VINS-Mono融合轮速计和GPS（一）：预积分
VINS-Mono融合轮速计和GPS（二）：鲁棒初始化
VINS-Mono融合轮速计和GPS（三）：优化和在线标定
VINS-Mono融合轮速计和GPS（四）：融合GPS

介绍

上一篇博客介绍了带有轮速计的预积分相关内容（工作有变动，托挺久的，实在抱歉），本篇用预积分的结果对初始化过程进行优化。

理论

鲁棒初始化

VINS-Mono的初始化

先说一下VINS-Mono的初始化过程。首先，用SFM求解滑窗内所有帧的位姿，和所有路标点的3D位置，然后跟IMU预积分的值对齐，求解重力方向、尺度因子、陀螺仪bias及每一帧对应的速度。
流程如下：

考虑到IMU初始化较慢，估计视觉尺度效果也容易受影响，所以改进思路是采用轮速计的观测数据进行视觉尺度的估计和参与初始速度的估计。具体改动为，图中前三个步骤采用原框架的视觉初始化模块，visualInitialAlign是对visualIMUAlignment之后的结果进行对齐。所以改动主要在visualIMUAlignment模块。
将visualIMUAlignment详细展开来说，又分为4个步骤：

1. 陀螺仪bias校正

（和VINS-Mono相同，暂略，有空再加）

2. 初始化速度和重力

p b k b 0 = p o k b 0 − R b k b 0 p o b p_{b_k}^{b_0}=p_{o_k}^{b_0}-R_{b_k}^{b_0}p_o^b pbk​b0​​=pok​b0​​−Rbk​b0​​pob​
α b j b i = q w b i ( p b j w − p b i w − v i w Δ t + 1 2 g w Δ t 2 ) \alpha_{b_j}^{b_i} = q_{w}^{b_i}(p_{b_j}^w-p_{b_i}^w-v_i^w\Delta t+\frac{1}{2}g^w\Delta t^2) αbj​bi​​=qwbi​​(pbj​w​−pbi​w​−viw​Δt+21​gwΔt2)
β b j b i = q w b i ( v j w − v i w + g w Δ t ) \beta_{b_j}^{b_i} = q_{w}^{b_i}(v_j^w-v_i^w+g^w\Delta t) βbj​bi​​=qwbi​​(vjw​−viw​+gwΔt)
η b j b i = q w b i ( p o j w − p o i w ) \eta_{b_j}^{b_i}=q_w^{b_i}(p_{o_j}^w-p_{o_i}^w) ηbj​bi​​=qwbi​​(poj​w​−poi​w​)

α b k + 1 b k = R b 0 b k ( p o k + 1 b 0 − R b k + 1 b 0 p o b − p o k b 0 + R b k b 0 p o b − v k b 0 Δ t k + 1 2 g b 0 Δ t k 2 ) = η b k + 1 b k − R b k + 1 b k p o b + p o b − v k b k Δ t k + 1 2 R b k b 0 g b 0 Δ t k 2 ) \begin{aligned} \alpha_{b_{k+1}}^{b_k}&=R_{b_0}^{b_k}(p_{o_{k+1}}^{b_0}-R_{b_{k+1}}^{b_0}p_{o}^b-p_{o_{k}}^{b_0}+R_{b_k}^{b_0}p_{o}^b-v_k^{b_0}\Delta t_k+\frac{1}{2}g^{b_0}\Delta t_k^2)\\&=\eta_{b_{k+1}}^{b_k}-R_{b_{k+1}}^{b_k}p_o^b+p_{o}^b-v_k^{b_k}\Delta t_k+\frac{1}{2}R_{b_k}^{b_0}g^{b_0}\Delta t_k^2) \end{aligned} αbk+1​bk​​​=Rb0​bk​​(pok+1​b0​​−Rbk+1​b0​​pob​−pok​b0​​+Rbk​b0​​pob​−vkb0​​Δtk​+21​gb0​Δtk2​)=ηbk+1​bk​​−Rbk+1​bk​​pob​+pob​−vkbk​​Δtk​+21​Rbk​b0​​gb0​Δtk2​)​
[ α b k + 1 b k − η b k + 1 b k + R b k + 1 b k p o b − p o b β b k + 1 b k ] = [ − I Δ t 0 1 2 R b k b 0 Δ t k 2 − I R b k + 1 b k R b k b 0 Δ t ] [ v k b k v k + 1 b k + 1 g b 0 ] \begin{bmatrix} \alpha_{b_{k+1}}^{b_k}-\eta_{b_{k+1}}^{b_k}+R_{b_{k+1}}^{b_k}p_o^b-p_o^b\\ \beta_{b_{k+1}}^{b_k} \end{bmatrix}=\begin{bmatrix} -I\Delta t& 0& \frac{1}{2}R_{b_k}^{b_0}\Delta t_k^2 \\ -I& R_{b_{k+1}}^{b_k}& R_{b_k}^{b_0}\Delta t \end{bmatrix}\begin{bmatrix} v_k^{b_k}\\ v_{k+1}^{b_{k+1}}\\ g^{b_0} \end{bmatrix} [αbk+1​bk​​−ηbk+1​bk​​+Rbk+1​bk​​pob​−pob​βbk+1​bk​​​]=[−IΔt−I​0Rbk+1​bk​​​21​Rbk​b0​​Δtk2​Rbk​b0​​Δt​]⎣⎡​vkbk​​vk+1bk+1​​gb0​​⎦⎤​

3. 初始化尺度因子

R b 0 b k ( p o k + 1 b 0 − R b k + 1 b 0 p o b − p o k b 0 + R b k b 0 p o b ) = R c 0 b k ( s ( p ˉ b k + 1 c 0 − p ˉ b k c 0 ) ) = R c 0 b k ( s ( p ˉ c k + 1 c 0 − p ˉ c k c 0 ) − R b k + 1 c 0 p c b + R b k c 0 p c b ) \begin{aligned} R_{b_0}^{b_k}(p_{o_{k+1}}^{b_0}-R_{b_{k+1}}^{b_0}p_{o}^b-p_{o_{k}}^{b_0}+R_{b_k}^{b_0}p_{o}^b)&=R_{c_0}^{b_k}(s(\bar{p}_{b_{k+1}}^{c_0}-\bar{p}_{b_{k}}^{c_0}))\\&=R_{c_0}^{b_k}(s(\bar{p}_{c_{k+1}}^{c_0}-\bar{p}_{c_{k}}^{c_0})-R_{b_{k+1}}^{c_0}p_c^b+R_{b_{k}}^{c_0}p_c^b) \end{aligned} Rb0​bk​​(pok+1​b0​​−Rbk+1​b0​​pob​−pok​b0​​+Rbk​b0​​pob​)​=Rc0​bk​​(s(pˉ​bk+1​c0​​−pˉ​bk​c0​​))=Rc0​bk​​(s(pˉ​ck+1​c0​​−pˉ​ck​c0​​)−Rbk+1​c0​​pcb​+Rbk​c0​​pcb​)​

4. 优化重力方向

利用球面坐标对三维的重力向量进行参数化，自由度变为2
g b 0 = ∥ g ∥ ⋅ g b 0 + w 1 b 1 + w 2 b 2 g^{b_0}=\left \|g\right \|\cdot g^{b_0}+w_1{b_1}+w_2{b_2} gb0​=∥g∥⋅gb0​+w1​b1​+w2​b2​
b 1 = { g b 0 × [ 1 , 0 , 0 ] , g b 0 × [ 1 , 0 , 0 ] T , g b 0 × [ 0 , 0 , 1 ] , o t h e r w i s e . b 2 = g b 0 × b 1 \begin{aligned} & b_{1}=\left\{ \begin{aligned} g^{b_0}\times \left [ 1, 0, 0 \right ] & , & g^{b_0}\times \left [ 1, 0, 0 \right ]^T, \\ g^{b_0}\times \left [ 0, 0, 1 \right ] & , & otherwise. \end{aligned} \right.\\ & b_2=g^{b_0}\times b_1 \end{aligned} ​b1​={gb0​×[1,0,0]gb0​×[0,0,1]​,,​gb0​×[1,0,0]T,otherwise.​b2​=gb0​×b1​​
[ α b k + 1 b k − η b k + 1 b k + R b k + 1 b k p o b − p o b − 1 2 R b k b 0 ∥ g ∥ ⋅ g b 0 Δ t k 2 β b k + 1 b k − R b k b 0 ∥ g ∥ ⋅ g b 0 Δ t k ] = [ − I Δ t 0 1 2 R b k b 0 B Δ t k 2 − I R b k + 1 b k R b k b 0 B Δ t ] [ v k b k v k + 1 b k + 1 w b 0 ] \begin{bmatrix} \alpha_{b_{k+1}}^{b_k}-\eta_{b_{k+1}}^{b_k}+R_{b_{k+1}}^{b_k}p_o^b-p_o^b-\frac{1}{2}R_{b_k}^{b_0}\left \|g\right \|\cdot g^{b_0}\Delta t_k^2 \\ \beta_{b_{k+1}}^{b_k}-R_{b_k}^{b_0}\left \|g\right \|\cdot g^{b_0}\Delta t_k \end{bmatrix}=\begin{bmatrix} -I\Delta t& 0& \frac{1}{2}R_{b_k}^{b_0}B\Delta t_k^2 \\ -I& R_{b_{k+1}}^{b_k}& R_{b_k}^{b_0}B\Delta t \end{bmatrix}\begin{bmatrix} v_k^{b_k}\\ v_{k+1}^{b_{k+1}}\\ w^{b_0} \end{bmatrix} [αbk+1​bk​​−ηbk+1​bk​​+Rbk+1​bk​​pob​−pob​−21​Rbk​b0​​∥g∥⋅gb0​Δtk2​βbk+1​bk​​−Rbk​b0​​∥g∥⋅gb0​Δtk​​]=[−IΔt−I​0Rbk+1​bk​​​21​Rbk​b0​​BΔtk2​Rbk​b0​​BΔt​]⎣⎡​vkbk​​vk+1bk+1​​wb0​​⎦⎤​

实践

配合代码查看

具体对应代码在initial_alignment.cpp文件中（代码中注释的部分是源代码，相应的代码行是修改后的）

其中LinearAlignment函数对应于初始化速度和重力

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

    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;
    
    Eigen::Quaterniond q_b0bk;
    q_b0bk.setIdentity();
    q_b0bk = q_b0bk * all_image_frame.begin()->second.pre_integration->delta_q;

    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);
        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, 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];
        // 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;

        q_b0bk = q_b0bk * frame_j->second.pre_integration->delta_q;
        tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
        tmp_A.block<3, 3>(0, 6) = 0.5 * frame_i->second.R.transpose() * RIC[0].transpose() * dt * dt * Matrix3d::Identity();
        // tmp_A.block<3, 3>(0, 6) = 0.5 * q_b0bk.inverse().toRotationMatrix() * dt * dt * Matrix3d::Identity();
        tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p - frame_j->second.pre_integration->delta_eta + 
                                  frame_j->second.pre_integration->delta_q.toRotationMatrix() * TIO - TIO;
        tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
        tmp_A.block<3, 3>(3, 3) = frame_i->second.pre_integration->delta_q.toRotationMatrix();
        tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * RIC[0].transpose() * dt * Matrix3d::Identity();
        // tmp_A.block<3, 3>(3, 6) = q_b0bk.inverse().toRotationMatrix() * 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.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;
    VectorXd tmp_x;
    tmp_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);
    g = tmp_x.segment<3>(n_state - 3);
    ROS_INFO_STREAM(" result g     " << g.norm() << " " << g.transpose());
    // if(fabs(g.norm() - G.norm()) > 1.0 || s < 0)
    // {
    //     return false;
    // }
    if(fabs(g.norm() - G.norm()) > 1.0)
    {
        return false;
    }

    RefineGravity(all_image_frame, g, tmp_x);

    // 这里补充了尺度信息，保证外部接口调用的一致性
    double s = solveScale(all_image_frame);
    x.resize(tmp_x.rows() + 1, tmp_x.cols());
    x = tmp_x;
    (x.tail<1>())(0) = s;
    // s = (x.tail<1>())(0) / 100.0;
    // (x.tail<1>())(0) = s;
    g = RIC[0].transpose() * g; // 再转换到camera坐标系下
    ROS_INFO_STREAM(" refine     " << g.norm() << " " << g.transpose());
    if(s < 0.0 )
        return false;   
    else
        return true;
}

solveScale函数对应于初始化尺度因子

// 估计初始尺度
double solveScale(map<double, ImageFrame> &all_image_frame)
{
    MatrixXd A(1, 1);
    VectorXd b(1, 1);
    VectorXd s(1, 1);
    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, 1);
        tmp_A.setZero();
        VectorXd tmp_b(3, 1);
        tmp_b.setZero();
        tmp_A = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T);
        tmp_b = frame_j->second.pre_integration->delta_eta - frame_i->second.pre_integration->delta_q * TIO + TIO
              + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0];
        A += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;
    }
    s = A.ldlt().solve(b);
    ROS_INFO_STREAM("A = " << A);
    ROS_INFO_STREAM("b = " << b);
    ROS_INFO("Visual scale %f", s[0]);
    return s[0];
}

RefineGravity函数对应于优化重力方向

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;
    int n_state = all_frame_count * 3 + 2;

    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;
    for(int k = 0; k < 4; k++)
    {
        MatrixXd lxly(3, 2);
        lxly = TangentBasis(g0);
        int i = 0;

        Eigen::Quaterniond q_b0bk;
        q_b0bk.setIdentity();
        q_b0bk = q_b0bk * all_image_frame.begin()->second.pre_integration->delta_q;

        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);
            MatrixXd tmp_A(6, 8);
            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, 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] - 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;

            q_b0bk = q_b0bk * frame_j->second.pre_integration->delta_q;
            tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
            tmp_A.block<3, 2>(0, 6) = 0.5 * frame_i->second.R.transpose() * RIC[0].transpose() * dt * dt * Matrix3d::Identity() * lxly;
            // tmp_A.block<3, 2>(0, 6) = 0.5 * q_b0bk.inverse().toRotationMatrix() * dt * dt * Matrix3d::Identity() * lxly;
            tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p - frame_j->second.pre_integration->delta_eta + 
                                    frame_j->second.pre_integration->delta_q.toRotationMatrix() * TIO - TIO -
                                    0.5 * frame_i->second.R.transpose() * RIC[0].transpose() * dt * dt * g0;
            // tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p - frame_j->second.pre_integration->delta_eta + 
            //                         frame_j->second.pre_integration->delta_q.toRotationMatrix() * TIO - TIO -
            //                         0.5 * q_b0bk.inverse().toRotationMatrix() * dt * dt * g0;
            tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
            tmp_A.block<3, 3>(3, 3) = frame_i->second.pre_integration->delta_q.toRotationMatrix();
            tmp_A.block<3, 2>(3, 6) = frame_i->second.R.transpose() * RIC[0].transpose() * dt * Matrix3d::Identity() * lxly;
            // tmp_A.block<3, 2>(3, 6) = q_b0bk.inverse().toRotationMatrix() * dt * Matrix3d::Identity() * lxly;
            tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v -
                                      frame_i->second.R.transpose() * RIC[0].transpose() * dt * Matrix3d::Identity() * g0;
            // tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v -
            //                           q_b0bk.inverse().toRotationMatrix() * 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.bottomRightCorner<2, 2>() += r_A.bottomRightCorner<2, 2>();
            b.tail<2>() += r_b.tail<2>();

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

标签：tmp,Mono,frame,transpose,second,鲁棒,VINS,dt,block
来源： https://blog.csdn.net/ewtewtewrt/article/details/117428612