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<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++;
            //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