VINS代码主体结构和流程剖析——后端

代码剖析

首先一定要对Keyframe这个类有充分的认识，其中比较难理解就是一些pose的数据结构：

  /** @brief vio pose in backend */
  Eigen::Vector3d vio_T_w_i;
  Eigen::Matrix3d vio_R_w_i;
  /** @brief rectified pose in backend */
  Eigen::Vector3d T_w_i;
  Eigen::Matrix3d R_w_i;
  /** @brief vio pose in frontend */
  Eigen::Vector3d origin_vio_T;
  Eigen::Matrix3d origin_vio_R;

• 第一个pose表示原始vio的位姿，也就是说直接保留前端发过来的pose，之后不做任何修改；
• 第二个pose表示经过后端优化之后的位于map坐标系的pose；
• 第三个pose表示被后端修改之后转换到map坐标系的vio pose（没有经过优化），这个要结合posegraph::addKeyFrame中的代码理解，如下：

	cur_kf->getVioPose(vio_P_cur, vio_R_cur);
    vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
    vio_R_cur = w_r_vio *  vio_R_cur;
    cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
...
		if (cur_kf->findConnection(old_kf))
        {
            if (earliest_loop_index > loop_index || earliest_loop_index == -1)
                earliest_loop_index = loop_index;

            Vector3d w_P_old, w_P_cur, vio_P_cur;
            Matrix3d w_R_old, w_R_cur, vio_R_cur;
            old_kf->getVioPose(w_P_old, w_R_old);
            cur_kf->getVioPose(vio_P_cur, vio_R_cur);

            Vector3d relative_t;
            Quaterniond relative_q;
            relative_t = cur_kf->getLoopRelativeT();
            relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();
            w_P_cur = w_R_old * relative_t + w_P_old;
            w_R_cur = w_R_old * relative_q;
            double shift_yaw;
            Matrix3d shift_r;
            Vector3d shift_t; 
            if(use_imu)
            {
                shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();
                shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));
            }
            else
                shift_r = w_R_cur * vio_R_cur.transpose();
            shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur; 
            // shift vio pose of whole sequence to the world frame
            if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
            {  
                w_r_vio = shift_r;
                w_t_vio = shift_t;
                vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
                vio_R_cur = w_r_vio *  vio_R_cur;
                cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
                list<KeyFrame*>::iterator it = keyframelist.begin();
                for (; it != keyframelist.end(); it++)   
                {
                    if((*it)->sequence == cur_kf->sequence)
                    {
                        Vector3d vio_P_cur;
                        Matrix3d vio_R_cur;
                        (*it)->getVioPose(vio_P_cur, vio_R_cur);
                        vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
                        vio_R_cur = w_r_vio *  vio_R_cur;
                        (*it)->updateVioPose(vio_P_cur, vio_R_cur);
                    }
                }
                sequence_loop[cur_kf->sequence] = 1;
            }
            m_optimize_buf.lock();
            optimize_buf.push(cur_kf->index);
            m_optimize_buf.unlock();
        }
	}

其中shift_r和t表示当找到不同sequence之间的回环时，求得的两个sequence之间的tf，之后会把所有新增的KeyFrame的vio位姿转换到已有的sequence坐标下，保存在上文提到的 vio_T_w_i，vio_R_w_i中，于是KeyFrame中的两个vio pose就不一样了，一个是前端发过来的pose，一个是将前端发过来的pose转换到已有sequence坐标系（map坐标系，因为vins中暂时默认已有的sequence就是已经建立好的地图，该sequence坐标系就是map坐标系）下的pose。，如果不提前加载地图，那么所有的KeyFrame应该位于同一sequence，w_r_vio就是个单位矩阵，不起任何作用，KeyFrame中的两个vio pose就是相等的。
有个了这个理解之后，其他的算法才能理解清楚，还有个关键变量是r_drift，比较容易理解，看看optimize4DOF()。

...
            Vector3d cur_t, vio_t;
            Matrix3d cur_r, vio_r;
            cur_kf->getPose(cur_t, cur_r);
            cur_kf->getVioPose(vio_t, vio_r);
            m_drift.lock();
            yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();
            r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));
            t_drift = cur_t - r_drift * vio_t;
            ...

在优化完成之后，cur_kf的全局pose，也就是上文中的第二个pose被更新了，此时vio pose跟全局pose不相等了，r_drift就是两者的差值，之后进来的KeyFrame都通过这个差值从vio pose中推导出全局pose，直到全局pose再被优化函数更新。

再来看看代码整体逻辑和流程，主逻辑位于addKeyFrame()这个函数中:

如果不做回环这个后端基本就没做啥了，detectLoop就是用DBOW在找回环，代码逻辑很简单就不详解了，detectLoop如果通过DBOW找到了回环，会用回环找到的index索引到老的KeyFrame，并给到findConnection，由它去做进一步确认以及计算相对位姿，findConnection()也是一个很核心的函数，基本上论文中关于回环的部分都在这里实现的，先看下主体逻辑：
核心就是用老的KeyFrame与当前KeyFrame进行匹配得到结果，主要有以下几个主要流程，论文中也都讲过了：

• matchByBriefDes()，通过feature描述子匹配，对于论文中VII-B中2D-2D匹配，这里提一句，在构建KeyFrame时，会提取所有feature的描述子并保存在window_brief_descriptors，此外，额外在全图范围内提取了大量feature，并提取了这些feature的描述子并保存在brief_descriptors，这里的matchByBriefDes()就是用当前KeyFrame的window_brief_descriptors与老的KeyFrame的brief_descriptors一一计算HammingDistance，大于一定阈值则认为匹配上了，匹配上的feature数量大于一定阈值（默认值25）才进入下一步。
• PnPRANSAC()，对应论文中VII-B中3D-2D匹配，名字很直观了，通过上一步得到的匹配到的feature对，进行PnPRANSAC操作，用以剔除掉错误的匹配对，并计算出relative pose，同样的，经过这一步后剩余的匹配feature对数量大于一定阈值（默认值25）才进入下一步。
• 最后判断上一步得到的relative pose的yaw和t满足一定阈值（默认30度和20米以内），则认为得到回环。

之后就是把回环因子加入优化问题使用ceres进行优化的东西了，没有太多可以讲了。