VINS-Mono+Fusion源码解析系列（十一）：后端processIMU与processImage

1. IMU后端优化estimator.processIMU

​ 主要是维持滑窗中的预积分量pre_integrations，为后续的优化提高状态初值。

vins_estimator/src/estimator.cpp
/**
 * @brief 对imu数据进行处理，包括更新预积分量，和提供优化状态量的初始值
 * 
 * @param[in] dt 
 * @param[in] linear_acceleration 
 * @param[in] angular_velocity 
 */
void Estimator::processIMU(
                    double dt, 
                    const Vector3d &linear_acceleration, 
                    const Vector3d &angular_velocity)
{
   
    // 若为第一个imu数据，则直接将imu测量值(加速度linear_acceleration和角速度angular_velocity)
    // 赋给相应变量(acc_0和gyr_0)
    if (!first_imu)
    {
   
        first_imu = true;
        acc_0 = linear_acceleration;
        gyr_0 = angular_velocity;
    }

    // 判断滑窗中第frame_count帧的预积分量是否存在
    // 若不存在，则new一个IntegrationBase，然后在下面调用push_back计算预积分量
    if (!pre_integrations[frame_count])  
    {
   
        pre_integrations[frame_count] = new IntegrationBase{
   acc_0, gyr_0, 
                                            Bas[frame_count], Bgs[frame_count]};
    }
/*
  滑窗中保留11帧，frame_count表示现在处理第几帧，一般处理到第11帧时就保持不变了(一直表示最后一帧)
  在滑窗中维持11个预积分量，由于预积分是帧间约束，因此第1个预积分量实际上是用不到的，同时避免第一帧对应的imu数据量过大造成出错
*/
    if (frame_count != 0)
    {
   
        // 调用push_back, 计算预积分量以及更新雅可比与协方差矩阵, 便于以imu的频率发布出去
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, 	
                                                     angular_velocity);
        // tmp_pre_integration中备份此时的预积分量，后面用来做初始化
        tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
        // 保存传感器数据
        dt_buf[frame_count].push_back(dt);
        linear_acceleration_buf[frame_count].push_back(linear_acceleration);  // 线加速度
        angular_velocity_buf[frame_count].push_back(angular_velocity);  // 角加速度
        // 又是一个中值积分，更新滑窗中状态量(Rs, Ps, Vs)，本质是给滑窗中的非线性优化提供可信的初始值 
        int j = frame_count;         
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
        Vs[j] += dt * un_acc;
    }
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

2. 图像帧后端优化estimator.processImage

2.1 VINS中特征点的管理方法

​ 如下图所示，对于当前帧的所有特征点，使用list<FeaturPerId>存储它们的id。对于每一个特征点，它具有的属性包括：

• 该特征点的id
• 第一个观测到该特征点的图像帧id（起始帧id）
• 该特征点在起始帧下可恢复的3D点的深度
• 该特征点在起始帧中的求解状态（是否可成功三角化恢复出3D点）
• 所有观测到该特征点的图像帧属性vector<FeaturePerFrame>，具体包括如下：
  • 该特征点在被观测帧中的像素坐标
  • 该特征点在被观测帧中的归一化相机坐标
  • 该特征点在被观测帧中的速度

2.2 processImage的实现

（1）大致流程

• 通过addFeatureCheckParallax进行关键帧的判断，从而确定边缘化的标志marginalization_flag。如果上一帧(倒数第2帧)是关键帧，则滑窗中最老的帧就要被移出滑窗；否则移出上一帧(倒数第2帧)。
• 维护变量all_image_frame和tmp_pre_integration。其中，tmp_pre_integration为前面在processIMU中计算出的当前帧预积分量，all_image_frame存储滑窗起始到当前的所有帧信息，它将图像帧与预积分绑定在一起。
• 在旋转外参标志ESTIMATE_EXTRINSIC为2的前提下，若当前帧不是第一帧，则通过getCorresponding获取当前帧与上一帧的特征点，然后在CalibrationExRotation中计算旋转外参。
• 根据是否需要进行初始化，执行相应的状态优化操作：
• • 若未初始化，则执行的操作如下：
    • 基于图像的纯视觉单目SLAM初始化（SFM）
    • 非线性优化求解VIO（solveOdometry）
    • 更新滑动窗口（slideWindow）
    • 移除无效地图点（removeFailures）
  • 若已进行了初始化，则执行的操作如下：
    • 非线性优化求解VIO（solveOdometry）
    • 更新滑动窗口（slideWindow）
    • 移除无效地图点（removeFailures）

（2）代码实现

void Estimator::processImage(const map<int, 
                             vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, 
                             const std_msgs::Header &header)
{
   
    // Step 1 将特征点信息加到f_manager这个特征点管理器中，同时进行是否关键帧的检查
    if (f_manager.addFeatureCheckParallax(frame_count, image, td))
        // 如果上一帧(倒数第2帧)是关键帧，则滑窗中最老的帧就要被移出滑窗
        marginalization_flag = MARGIN_OLD;
    else
        // 否则移除上一帧(倒数第2帧)
        marginalization_flag = MARGIN_SECOND_NEW;

    Headers[frame_count] = header;
    
    // 根据对齐的当前帧image和时间戳定义ImageFrame对象
    ImageFrame imageframe(image, header.stamp.toSec());  
    // 获取当前帧的预积分量    每一帧的预积分量tmp_pre_integration是在processIMU中计算得到的
    imageframe.pre_integration = tmp_pre_integration;    
    // all_image_frame: 维护滑窗起始帧到当前帧之间的所有帧状态，用于后续初始化
    all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));
    // 初始化时：当前帧处理完之后，无论当前帧是否为关键帧，都认为它已经得到了两帧之间的预积分结果
    // 那么需要将预积分复位，为下一帧做准备
    tmp_pre_integration = new IntegrationBase{
   acc_0, gyr_0, Bas[frame_count], 
                                              Bgs[frame_count]};
    
    // Step 2： 外参初始化  
    // 2表示没有任何外参的先验初值，需要进行初始化
    // 1表示初始外参比较可靠，不需要通过初始化来计算旋转外参，只需要将初始外参值送给后端进行滑窗优化即可
    // 0表示初始外参比较准确，在滑窗中固定住，不对其进行优化
    if(ESTIMATE_EXTRINSIC == 2)
    {
   
        ROS_INFO("calibrating extrinsic param, rotation movement is needed");
        if (frame_count != 0)
        {
   
/*
   标定imu和相机的旋转外参的初值  因为预积分是相邻图像帧的约束，因此这里得到的图像关联也是相邻的
   通过getCorresponding获取当前图像帧frame_count和上一图像帧frame_count-1观察到的特征点在各自图像帧下的坐标corres
*/
            vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(
                										frame_count - 1, frame_count);
            Matrix3d calib_ric;
            // 将预积分的delta_q和刚才求出的特征点在两相邻帧上的坐标corres作为参数
            // 传入CalibrationExRotation函数中进行外参初始化，得到标定后的外参结果calib_ric
            if (initial_ex_rotation.CalibrationExRotation(
                			corres, pre_integrations[frame_count]->delta_q, calib_ric))
            {
   
                ric[0] = calib_ric;
                RIC[0] = calib_ric;
                // 标志位设置成可信的外参初值
                ESTIMATE_EXTRINSIC = 1;
            }
        }
    }
    
    // 基于图像的纯视觉单目SLAM ———— SFM
    // solver_flag == INITIAL表示需要进行初始化  solver_flag = NON_LINEAR表示不需要进行初始化
    if (solver_flag == INITIAL)
    {
   
        // 是否有足够的帧数，滑窗是否已满(后面在滑窗中，frame_count会一直等于滑窗大小)
        if (frame_count == WINDOW_SIZE) 
        {
   
            // 要有可信的旋转外参值，同时距离上次初始化不成功至少相邻0.1s
            // Step 3： VIO初始化
            if( ESTIMATE_EXTRINSIC!=2 && (header.stamp.toSec()-initial_timestamp) > 0.1)
            {
   
                result = initialStructure();  // 单目视觉SLAM的三维重建
                initial_timestamp = header.stamp.toSec();
            }
            if(result)
            {
   
                // solver_flag = INITIAL表示处于初始化阶段 
                // solver_flag = NON_LINEAR表示初始化已完成  
                solver_flag = NON_LINEAR;
                // Step 4： 非线性优化求解VIO
                solveOdometry();
                // Step 5： 更新滑动窗口
                slideWindow();
                // Step 6： 移除无效地图点
                f_manager.removeFailures(); // 移除无效地图点
                ROS_INFO("Initialization finish!");
                last_R = Rs[WINDOW_SIZE];   // 滑窗里最新的位姿
                last_P = Ps[WINDOW_SIZE];
                last_R0 = Rs[0];    // 滑窗里最老的位姿
                last_P0 = Ps[0];
                
            }
            else
                // 更新滑动窗口
                slideWindow();
        }
        else
            frame_count++;
    }
    else  // 不需要进行初始化的清空
    {
   
        TicToc t_solve;
        solveOdometry();
        ROS_DEBUG("solver costs: %fms", t_solve.toc());
        // 检测VIO是否正常
        if (failureDetection())
        {
   
            ROS_WARN("failure detection!");
            failure_occur = 1;
            // 如果异常，重启VIO
            clearState();
            setParameter();
            ROS_WARN("system reboot!");
            return;
        }

        TicToc t_margin;
        slideWindow();
        f_manager.removeFailures();
        ROS_DEBUG("marginalization costs: %fms", t_margin.toc());
        // prepare output of VINS
        // 给可视化用的
        key_poses.clear();
        for (int i = 0; i <= WINDOW_SIZE; i++)
            key_poses.push_back(Ps[i]);

        last_R = Rs[WINDOW_SIZE];  // 保存最新帧信息
        last_P = Ps[WINDOW_SIZE];
        last_R0 = Rs[0];  // 保存最老帧信息
        last_P0 = Ps[0];
    }
}

2.3 关键帧判断addFeatureCheckParallax

（1）大致流程

• 遍历传入的特征点信息image，完善特征点属性
  • 对遍历到的每个特征点，将它的xyz_uv_velocity信息封装到f_per_fra中
  • 对遍历到的每个特征点，在特征点存放容器feature（list<FeaturePerId>类型）中查找是否已经存在这个特征点
    • 若没有查找到，则表明遍历到的这个特征点是一个新特征点，则向特征点存放容器feature中添加这个特征点的一整套属性，即FeaturePerId属性与对应的FeaturePerFrame属性
    • 若查找到的，则只需完善该特征点的FeaturePerFrame属性，然后添加到特征点存放容器feature中，同时更新成功追踪的特征点数目last_track_num
• 在完善传入的特征点属性之后， 进行关键帧判断
  • 通过上一帧在滑窗中的索引frame_count和成功追踪的特征点数目，先提前判断倒数第二帧（上一帧）是否为关键帧
  • 遍历feature中的所有特征点，对于每个特征点，在它能够被滑窗中倒数第二帧和倒数第三帧观测到的前提下，在compensatedParallax2中计算该特征点在滑窗中倒数第二帧与倒数第三帧中的视差，然后累加每个特征点的视差
    • 若累积视差为0，则直接判定倒数第二帧为关键帧
    • 否则，若平均视差大于视差阈值，则也认为倒数第二帧是关键帧

（2）addFeatureCheckParallax代码实现

// vins_estimator/src/feature_manager.cpp
/**
 * @brief 增加特征点信息，同时检查上一帧是否时关键帧
 * 
 * @param[in] frame_count 
 * @param[in] image 
 * @param[in] td 
 * @return true 
 * @return false 
 */

bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{
   
    double parallax_sum = 0;
    int parallax_num = 0;
    last_track_num = 0;
    // 遍历每个特征点    
    for (auto &id_pts : image)
    {
   
        // 用遍历到的当前特征点信息(xyz_uv_velocity)以及时间戳td构造一个对象，即它在最新帧中的属性
        // xyz: 归一化像素坐标    uv: 像素坐标系下的坐标    velocity: 特征点速度
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td);

        // first: feature_id    
        // second: [camera_id0--xyz_uv_velocity0, ..., camera_idn--xyz_uv_velocityn]
        // 前面的id_pts.second[0].second就表示xyz_uv_velocity0
        int feature_id = id_pts.first;  // 获取特征点的id  
        // 在已有的特征点id中寻找是否有相同的特征点    
        // feature表示存储当前帧中所有特征点的list容器
        auto it = find_if(feature.begin(), feature.end(), 
                          				   [feature_id](const FeaturePerId &it)
                          {
   
            return it.feature_id == feature_id;    // 若查找到了，则直接返回
                          });
        // 若it为feature.end()，表示没有找到相同id的特征点，即这是一个新的特征点
        // 因此，需要将这个新的特征点更新到特征点管理器中
        if (it == feature.end())
        {
   
            // 根据FeaturePerId的构造函数，新创建一个特征点信息，然后将创建的对象push到feature中
            // 这里的frame_count就是该特征点在滑窗中的当前位置，作为这个特征点的起始位置
            // feature_id: 特征点id    frame_count: 第一个观测到该特征点的图像帧，即起始帧
            feature.push_back(FeaturePerId(feature_id, frame_count));
            // 对于feature中的最后一个元素feature.back(), 也是刚才上一步push进去的特征点
            // 添加观测到该特征点的所有图像帧的属性feature_per_frame
            feature.back().feature_per_frame.push_back(f_per_fra);
        }
        // 如果这是一个已有的特征点，就在对应的“组织”下增加一个帧属性
        else if (it->feature_id == feature_id)
        {
   
            // 在观测到当前特征点it的图像帧中添加对应的特征点属性f_per_fra
            it->feature_per_frame.push_back(f_per_fra);  // 增加帧属性
            last_track_num++;   // 更新追踪到上一帧的特征点数目
        }
    }
    // 判断当前帧是否为KF: 前两帧全部设置为KF，追踪过少也认为是KF
    // 若当前帧为前两帧(第0帧或第1帧)，则认为当前帧为关键帧
    // 若追踪到上一帧的特征点数目小于20，表示当前帧的特征关联比较弱了，则认为当前帧为关键帧
    if (frame_count < 2 || last_track_num < 20)
        return true;
    // 进行简单的视差计算: 判断倒数第2帧和倒数第3帧之间的视差是否足够大，以此来决定倒数第2帧是否为关键帧
    for (auto &it_per_id : feature)  // 遍历所有的特征点
    {
   
/*
   通过判断倒数第二帧(frame_count - 1)和倒数第三帧(frame_count - 2)的视差是否足够大，来确定倒数第二帧是否为关键帧。因此起始帧至少得是frame_count - 2(至少要从倒数第三帧开始被看到),同时至少覆盖到frame_count - 1帧(倒数第二帧)
*/
        if (it_per_id.start_frame <= frame_count - 2 && it_per_id.start_frame +
            int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)
        {
   
            parallax_sum += compensatedParallax2(it_per_id, frame_count);  // 计算视差
            parallax_num++;
        }
    }
    // 若累计视差为0，则表示倒数第二帧与倒数第三帧之间不存在特征关联，认为倒数第二帧为关键帧
    if (parallax_num == 0)
    {
   
        return true;
    }
    else
    {
   
        // 看看平均视差是否超过所设阈值MIN_PARALLAX  总视差 / 总特征点数  
        // 若为true，表明视差较大，二者之间的特征关联较弱，也认为倒数第2帧为关键帧
        return parallax_sum / parallax_num >= MIN_PARALLAX;
    }
}

计算视差compensatedParallax2

• 大致思路

​ 首先，获取该特征点在滑窗中倒数第二帧与倒数第三帧中的索引。然后，根据索引获取它在倒数第二帧与倒数第三帧中的归一化坐标。最后，根据归一化坐标，计算这两个归一化点之间的距离，即为视差。

• 实现代码

// vins_estimator/src/feature_manager.cpp
// it_per_id: 遍历到的当前特征点    frame_count: 当前帧索引
double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)
{
   
// 找到相邻两帧
// frame_count - 2 - it_per_id.start_frame表示倒数第3帧在it_per_id.feature_per_frame中的索引
// frame_count - 1 - it_per_id.start_frame表示倒数第2帧在it_per_id.feature_per_frame中的索引
    const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame];  // 取出倒数第3帧
    const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[frame_count - 1 - it_per_id.start_frame];  // 取出倒数第2帧

    double ans = 0;
    Vector3d p_j = frame_j.point;  // 倒数第2帧中的归一化坐标

    double u_j = p_j(0);
    double v_j = p_j(1);

    Vector3d p_i = frame_i.point;  // 倒数第3帧中的归一化坐标
    Vector3d p_i_comp;
    
    p_i_comp = p_i;
    // 归一化操作，其实没必要，因为存的就是归一化值
    double dep_i = p_i(2);
    double u_i = p_i(0) / dep_i;
    double v_i = p_i(1) / dep_i;
    // 特征点在倒数第三帧和倒数第二帧中归一化点的坐标差
    double du = u_i - u_j, dv = v_i - v_j;  
    // 当都是归一化坐标系时，他们两个都是一样的
    double dep_i_comp = p_i_comp(2);
    double u_i_comp = p_i_comp(0) / dep_i_comp;
    double v_i_comp = p_i_comp(1) / dep_i_comp;
    double du_comp = u_i_comp - u_j, dv_comp = v_i_comp - v_j;

    // 当前特征点it_per_id在倒数第2帧和倒数第3帧归一化平面上的距离(视差)
    ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));

    return ans;
}

获取当前帧与倒数第二帧中的特征点getCorresponding

/**
 * @brief 得到同时被frame_count_l frame_count_r帧看到的特征点在各自帧下的坐标
 * 
 * @param[in] frame_count_l 
 * @param[in] frame_count_r 
 * @return vector<pair<Vector3d, Vector3d>> 
 */

vector<pair<Vector3d, Vector3d>> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r)
{
   
    vector<pair<Vector3d, Vector3d>> corres;
    for (auto &it : feature)  // 遍历所有的特征点
    {
   
        // 保证需要的特征点被这两帧都观察到
        if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r)
        {
   
            Vector3d a = Vector3d::Zero(), b = Vector3d::Zero();
           // 获取两图像帧frame_count_l和frame_count_r在feature_per_frame（vector类型）中的索引
            int idx_l = frame_count_l - it.start_frame;
            int idx_r = frame_count_r - it.start_frame;

            // 获取当前特征点it在第idx_l帧中去畸变后的归一化坐标
            a = it.feature_per_frame[idx_l].point;  
			// 获取当前特征点it在第idx_r帧中去畸变后的归一化坐标
            b = it.feature_per_frame[idx_r].point;  
            
            corres.push_back(make_pair(a, b));  // 返回该两帧下同时观测到的特征点的各自归一化坐标
        }
    }
    return corres;
}

2.4 外参标定CalibrationExRotation

（1）大致思路

• 根据传入的两相邻图像帧之间的特征点，通过对极约束计算该两相邻图像帧之间的旋转

• 根据传入的预积分，获取两相邻图像帧对应的imu旋转

• 根据公式将两相邻图像帧对应的IMU之间的旋转转到图像帧上

• 构建求取外参方程的系数矩阵

• 使用SVD分解求取旋转外参，并根据特征值大小判断求解出的旋转外参是否有效（需要有足够大的置信度）

（2）求取外参的原理

​ 在两关键帧之间，可通过IMU积分得到IMU旋转为$q_{b_k{b}_{k+1}}$。通过光流追踪可以得到两相邻图像帧之间的特征点匹配，然后使用对极约束可以得到两相邻图像帧之间的位姿变换。其中，平移是带尺度的，但在标定旋转时不需要平移，故可假设旋转为$q_{c_k{c}_{k+1}}$。

​ 由于相机与IMU之间的旋转外参$q_{cb}$在标定完成之后是不会变的，所以 q c b = q c k b k = q c k + 1 b k + 1 q_{cb}=q_{c_kb_k} = q_{c_{k+1}b_{k+1}}