VINS-Mono 代码详细解读——初始化2：视觉惯性松耦合初始化 visualIntialAlign（）

visualIntialAlign（）函数视觉惯性联合初始化

这篇文章主要集中在讨论视觉部分和IMU部分之间的关联，如何对两部分进行对齐，使得系统完成初始化。

目录

visualIntialAlign（）函数视觉惯性联合初始化

理论：视觉IMU对齐流程

代码流程

VisualIMUAlignment（）

理论知识：

视觉惯性校准（IMU预积分与视觉结构对齐）

1、陀螺仪零偏bg标定

2、速度v、重力g和尺度初始化s

3、重力矢量修正

陀螺仪偏置标定solveGyroscopeBias（）函数

计算尺度、重力、速度 LinearAlignment（）函数

重力细化RefineGravity（）函数

 

---

理论：视觉IMU对齐流程

其中，步骤1的在线Cam到IMU的外参标定qbc 参考之前的博客。 

1、陀螺仪零偏bg标定

2、优化 速度v、重力g和尺度初始化s。我们得到了陀螺仪偏置bias的初始校准,需要将陀螺仪偏置bg代入到IMU预积分重新计算预积分

3、重力矢量修正

代码流程

bool Estimator::visualInitialAlign()主要过程为：

1. VisualIMUAlignment（）函数计算陀螺仪偏置bg，尺度s，重力加速度g和速度v

2. f_manager.triangulate（）计算特征点深度estimated_depth

3. repropagate（）陀螺仪的偏置bgs改变，重新计算预积分

4. 将Ps、Vs、depth进行更新

5. 将重力旋转到Z轴，将Ps、Vs、Rs从相机参考坐标系c0旋转到世界坐标系w。

1、计算陀螺仪偏置bg，尺度s，重力加速度g和速度v

 bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);
    if(!result)
    {
        ROS_DEBUG("solve g failed!");
        return false;
    }

这里的VisualIMUAlignment（）函数最重要，我们会在下一讲详细讲解。

这里先熟悉基本流程。

2、得到所有图像帧的位姿Ps、Rs，并将其置为关键帧

  for (int i = 0; i <= frame_count; i++)
    {
        Matrix3d Ri = all_image_frame[Headers[i].stamp.toSec()].R;// 是ROS吗？
        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、重新计算特征点的深度depth

 VectorXd dep = f_manager.getDepthVector();
    for (int i = 0; i < dep.size(); i++)
        dep[i] = -1;
    f_manager.clearDepth(dep);

    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]));
    // 尺度先假设
    double s = (x.tail<1>())(0);

4、陀螺仪的偏置bgs改变，重新计算预积分

 for (int i = 0; i <= WINDOW_SIZE; i++)
    {
        pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
    }

这里最要的repropagate（）函数很重要。

5、将Ps、Vs、depth进行更新

5.1 目的是将姿态从相机坐标系c0转换到IMU坐标系中。

之前所有帧的位姿(）为相对第一帧相机坐标系的，相机到IMU外参为（），姿态从相机坐标系转换到IMU坐标系关系为：

 // 5、将Ps、Vs、depth进行更新
    for (int i = frame_count; i >= 0; i--)
        // 5.1 Ps转变为第i帧imu坐标系到第0帧imu坐标系的变换
        // 之前相机第l帧为参考系，转换到IMU bo为基准坐标系
        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++;
            //5.2 Vs为优化得到的速度
            Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);
        }
    }
    // 5.3 逆深度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;
    }

6、所有变量从参考坐标系c0到世界坐标系。

    // 6 通过将重力旋转到z轴上，得到世界坐标系与摄像机坐标系c0之间的旋转矩阵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;
    // 7/所有变量从参考坐标系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];
    }

---

VisualIMUAlignment（）

本节主要介绍Estimator模块的视觉惯性联合初始化模块，visualIntialAlign（）函数内第一步VisualIMUAlignment（）函数上一节对者几个过程进行大致概括，本节主要对第一部分VisualIMUAlignment（）函数进行详细介绍。

理论知识：

本节主要介绍初始化中视觉惯性对齐求解陀螺仪偏置bg、重力加速度g、每帧速度v、尺度s和相机到IMU的外参估计。

主要流程为：

1. 利用旋转约束估计陀螺仪偏置bg
2. 利用平移约束估计重力加速度g、每帧速度v、尺度s
3. 对重力向量进一步优化

视觉惯性校准（IMU预积分与视觉结构对齐）

相机到IMU的外参矩阵求解出来，可以利用旋转的约束，估计陀螺仪的bias.

1、陀螺仪零偏bg标定

旋转两种方式：陀螺仪测量值和视觉观测值，二者的误差其实就是陀螺仪偏置bg。

目标函数：visual给出的相邻帧间的旋转应等于IMU预积分的旋转值Q之间的差。

   考虑到了陀螺仪bias噪声，

我们得到了陀螺仪偏置bias的初始校准,需要将陀螺仪偏置bg代入到IMU预积分重新计算预积分。

2、速度v、重力g和尺度初始化s

优化变量：速度、重力向量和尺度 

目标函数：相邻两帧IMU预积分增量与预测值之间平移、速度（P、V）的差。通过HX=B 利用cholesky分解获得

 

结合得到：

通过求解线性最小二乘问题：

3、重力矢量修正

重力向量的大小是已知的，加入了模长限制，这导致三维重力向量只剩2个自由度。

主要做的是优化方向，一个二维向量。

在其切线空间上用两个变量重新参数化重力，采用球面坐标进行参数化：

其中，是已知的重力的大小，为重力方向的单位向量。b1和b2是跨越切平面的两个正交基。w1和w2是待优化变量，表示沿着两个正交基方向的位移。

替换后，Hx=b，变化为：之后采用最小二乘对变量重新优化。

其中，待优化变量变为：

4、将相机坐标系对齐世界坐标系

已知量：

1）上一步得到了重力向量在相机初始时刻系下的大小, 

 2）重力向量在世界坐标系下的绝对向量为g=[0,0,9.81]

通过将旋转至惯性坐标系中的z轴方向，可以计算相机系到惯性系的旋转矩阵，从而将所有变量调整至惯性坐标系中。

最后一步

---

VisualIMUAlignment()函数在 vins_estimator/src/initial/initial_aligment.cpp/.h。

该函数主要调用了两个函数，分别是对陀螺仪偏置的标定和估计尺度、重力加速度和速度。

//视觉和IMU对齐
bool VisualIMUAlignment(map &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;
}

其中，ImageFrame图像帧类主要在initial_alignment.h ，这个头文件主要就两部分：ImageFrame类、VisualIMUAlignment（）函数。ImageFrame图像帧为特征点、时间戳、相机位姿、预积分对象、是否关键帧。

陀螺仪偏置标定solveGyroscopeBias（）函数

SFM得到的旋转矩阵和IMU预积分得到的旋转量q求Ax=B分解得到最小误差。

注意得到了新的Bias后保存在Bgs[]中，对应的预积分需要重新计算一遍repropagate。

 

/**
 * @brief   陀螺仪偏置校正
 * @optional    根据视觉SFM的结果来校正陀螺仪Bias -> Paper V-B-1
 *              主要是将相邻帧之间SFM求解出来的旋转矩阵与IMU预积分的旋转量对齐
 *              注意得到了新的Bias后对应的预积分需要repropagate
 * @param[in]   all_image_frame所有图像帧构成的map,图像帧保存了位姿、预积分量和关于角点的信息
 * @param[out]  Bgs 陀螺仪偏置
 * @return      void
*/
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();

        //R_ij = (R^c0_bk)^-1 * (R^c0_bk+1) 转换为四元数 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
        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)
        //      = 2 * (r^bk_bk+1)^-1 * q_ij
        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;
    // 得到了新的Bias后对应的预积分需要repropagate
    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]);
    }
}

之所以 A +=tmp_A.transpose() * tmp_A，其实就是 ,

在求解 Ax=b的最小二乘解时，两边同乘以A矩阵的转置得到的AT*A一定是可逆的。

计算尺度、重力、速度 LinearAlignment（）函数

主要就是对于Ax=b的填充

/**
 * @brief   计算尺度，重力加速度和速度
 * @optional    速度、重力向量和尺度初始化Paper -> V-B-2
 *              相邻帧之间的位置和速度与IMU预积分出来的位置和速度对齐，求解最小二乘
 *              重力细化 -> Paper V-B-3    
 * @param[in]   all_image_frame 所有图像帧构成的map,图像帧保存了位姿，预积分量和关于角点的信息
 * @param[out]  g 重力加速度
 * @param[out]  x 待优化变量，窗口中每帧的速度V[0:n]、重力g、尺度s
 * @return      void
*/
bool LinearAlignment(map &all_image_frame, Vector3d &g, VectorXd &x)
{
    int all_frame_count = all_image_frame.size();
    //优化量x的总维度
    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);

        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(6,10) = H^bk_bk+1 = [-I*dt           0             (R^bk_c0)*dt*dt/2   (R^bk_c0)*((p^c0_ck+1)-(p^c0_ck))  ] 
        //                           [ -I    (R^bk_c0)*(R^c0_bk+1)      (R^bk_c0)*dt                  0                    ]
        // tmp_b(6,1 ) = z^bk_bk+1 = [ (a^bk_bk+1)+(R^bk_c0)*(R^c0_bk+1)*p^b_c-p^b_c , (b^bk_bk+1)]^T
        // tmp_A * x = tmp_b 求解最小二乘问题
        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 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<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（）函数

/**
 * @brief   重力矢量细化
 * @optional    重力细化，在其切线空间上用两个变量重新参数化重力 -> Paper V-B-3 
                g^ = ||g|| * (g^-) + w1b1 + w2b2 
 * @param[in]   all_image_frame 所有图像帧构成的map,图像帧保存了位姿，预积分量和关于角点的信息
 * @param[out]  g 重力加速度
 * @param[out]  x 待优化变量，窗口中每帧的速度V[0:n]、二自由度重力参数w[w1,w2]^T、尺度s
 * @return      void
*/
void RefineGravity(map &all_image_frame, Vector3d &g, VectorXd &x)
{
    //g0 = (g^-)*||g||
    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;

    for(int k = 0; k < 4; k++)//迭代4次
    {
        //lxly = b = [b1,b2]
        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(6,9) = [-I*dt           0             (R^bk_c0)*dt*dt*b/2   (R^bk_c0)*((p^c0_ck+1)-(p^c0_ck))  ] 
            //              [ -I    (R^bk_c0)*(R^c0_bk+1)      (R^bk_c0)*dt*b                  0                    ]
            // tmp_b(6,1) = [ (a^bk_bk+1)+(R^bk_c0)*(R^c0_bk+1)*p^b_c-p^b_c - (R^bk_c0)*dt*dt*||g||*(g^-)/2 , (b^bk_bk+1)-(R^bk_c0)dt*||g||*(g^-)]^T
            // tmp_A * x = tmp_b 求解最小二乘问题
            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);
            //dg = [w1,w2]^T
            VectorXd dg = x.segment<2>(n_state - 3);
            g0 = (g0 + lxly * dg).normalized() * G.norm();
            //double s = x(n_state - 1);
    }   
    g = g0;
}