VINS-MONO代码解读6----pose_graph

开始pose_graph部分，本部分记住一句话无论是快速重定位还是正常重定位，求出$T_{w_1{w}_2}$就是终极目标。

还剩一个整体Pipeline~~

1. pose_graph_node.cpp

注意，定义全局变量时即实例化了一个对象

PoseGraph posegraph; //定义全局的位姿图优化对象

加载配置
订阅topic及对应的callback()
注册publisher
定义主线程和键盘线程，
若需要loop则
取出对应的msg，时间戳对齐
最重要的是新建KF，添加KF到posegraph中

2. pose_graph.cpp

2.1 回环检测

int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index)
相关的debug语句就不进行介绍。

1. 查询字典，给出查询结果ret，并将描述子添加到数据库中，db.query()的传参见注释。

    //first query; then add this frame into database!
    QueryResults ret;
    TicToc t_query;
    //查询字典数据库，得到与每一帧的相似度评分ret
    //param：描述子；返回结果(主要是Entry id和相似度评分Score)；最多返回几个；<=max_id的才被返回
    db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);

    TicToc t_add;
    //描述子加到数据库中
    db.add(keyframe->brief_descriptors);

2. 判定是否找到loop

    //至少两个，且第1个的score>0.05才可能是loop
    if (ret.size() >= 1 &&ret[0].Score > 0.05)
        for (unsigned int i = 1; i < ret.size(); i++)
        {
            //if (ret[i].Score > ret[0].Score * 0.3)
            //后面只要再有一个score>0.015的就判定找到了loop
            if (ret[i].Score > 0.015)
            {          
                find_loop = true;//

3. 返回 50帧之后，最远的，score>0.015的结果

    //只有50帧之后才返回loop查询结果
    if (find_loop && frame_index > 50)
    {
        int min_index = -1;
        for (unsigned int i = 0; i < ret.size(); i++)
        {
            //找最远的，且score>0.015的结果
            if (min_index == -1 || (ret[i].Id < min_index && ret[i].Score > 0.015))
                min_index = ret[i].Id;
        }
        return min_index;
    }
    else
        return -1;

2.2 PnP 求解相对位姿

bool KeyFrame::findConnection(KeyFrame* old_kf)
其中关键函数是searchByBRIEFDes()和PnPRANSAC()
需要说明，searchByBRIEFDes()找到的是$i$帧的2D点，而PnPRANSAC()最终输出在PnP_T_old, PnP_R_old中的是$T_{w_2{b}_i}$

//关键帧WINDOW内的描述子与回环帧的描述子(brief_descriptors，不知道后面有没有把WINDOW内的描述子合并到这里面来)进行BRIEF描述子匹配，剔除匹配失败的点
searchByBRIEFDes(matched_2d_old, matched_2d_old_norm, status, old_kf->brief_descriptors, old_kf->keypoints, old_kf->keypoints_norm);

......

//如果能匹配的特征点能达到最小回环匹配个数，则用RANSAC PnP检测再去除误匹配的点，求出的是(PnP_T_old, PnP_R_old)=Tw2_bi
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
	status.clear();
    PnPRANSAC(matched_2d_old_norm, matched_3d, status, PnP_T_old, PnP_R_old);
    reduceVector(matched_2d_cur, status);//根据status去除outlier
    reduceVector(matched_2d_old, status);
    reduceVector(matched_2d_cur_norm, status);
    reduceVector(matched_2d_old_norm, status);
    reduceVector(matched_3d, status);
    reduceVector(matched_id, status);
......   
}

对$T_{w_2{b}_i}$这个量是不是很熟悉？对，这就是estimator中的relo_Pose，有了这个量，就能求出$T_{b_i{b}_j}$，存在loop_info中，后面就可以算出$T_{w_1{w}_2}$执行矫正了：

  //相对位姿检验
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
      //Tbi_bj=Tw2_bi^(-1) * Tw2_bj
   relative_t = PnP_R_old.transpose() * (origin_vio_T - PnP_T_old);
   relative_q = PnP_R_old.transpose() * origin_vio_R;
   //Rbi_bj.yaw
   relative_yaw = Utility::normalizeAngle(Utility::R2ypr(origin_vio_R).x() - Utility::R2ypr(PnP_R_old).x());
   if (abs(relative_yaw) < 30.0 && relative_t.norm() < 20.0)
   {
   	has_loop = true;
   	loop_index = old_kf->index;
   	//更新this KFde loop_info  Tbi_bj，后面要用
   	loop_info << relative_t.x(), relative_t.y(), relative_t.z(),
   	             relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),
   	             relative_yaw;
          //快速重定位:组装msg，发送match_points topic，estimator接收到该消息后进行快速重定位
   	if(FAST_RELOCALIZATION)
......balabala

在addKeyFrame()中，拿loop_info中的$T_{b_i{b}_j}$计算出$T_{w_1{w}_2}$并执行矫正，全部变换到$w_1$系下：

    //若存在回环候选帧
	if (loop_index != -1)
	{
        //printf(" %d detect loop with %d \n", cur_kf->index, loop_index);
        //获得回环帧
        KeyFrame* old_kf = getKeyFrame(loop_index);
        //PnP 求解Tbi_w2，进而求出Tbi_bj存放于loop_info中
        if (cur_kf->findConnection(old_kf))
        {
            //更新最早的回环帧index
            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);//获得Tw1_bi
            cur_kf->getVioPose(vio_P_cur, vio_R_cur);//Tw2_bj

            //relative是Tbi_bj=Told_cur
            Vector3d relative_t;
            Quaterniond relative_q;
            //读取Tbi_bj
            relative_t = cur_kf->getLoopRelativeT();//这个loop_info是在findConnection()中更新的
            relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();
            //Tw1_bj = Tw1_bi * Tbi_bj
            w_P_cur = w_R_old * relative_t + w_P_old;
            w_R_cur = w_R_old * relative_q;

            //回环得到的位姿和VIO位姿之间的偏移量Tw1_w2
            double shift_yaw;
            Matrix3d shift_r;
            Vector3d shift_t;
            //Tw1_w2
            shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();
            shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));
            //tw1_bj - Rw1_bj * Rw2_bj^(-1) * tw2_bj = tw1_w2
            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
            // 如果存在多个图像序列，则将所有图像序列都从w2系拉回到w1系下
            if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
            {  
                w_r_vio = shift_r;
                w_t_vio = shift_t;
                //Tw1_bj = Tw1_w2 * Tw1_bj，执行relocation
                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++)   
                {
                    //TODO：这sequence到底是啥意思？是说跟着loop上的帧后面还有一些帧，都需要relo回来吗？
                    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();
        }
	}

3. keyframe.cpp/h

构建了两个类：

• BriefExtractor：用于计算BRIEF描述子
• KeyFrame：用于对KF进行相关管理(重点)

1. BriefExtractor类主要使用node中加载的描述子的pattern来计算BRIEF描述子，其中重载了()运算符

class BriefExtractor
{
public:
    //读取 构建字典时使用的相同的Brief模板文件，构造BriefExtractord
    virtual void operator()(const cv::Mat &im, vector<cv::KeyPoint> &keys, vector<BRIEF::bitset> &descriptors) const;
    //运算符重载了“()”来计算描述子。
    BriefExtractor(const std::string &pattern_file);

    DVision::BRIEF m_brief;
};

---

2. KeyFrame类定义了一些函数，包括计算描述子，进行匹配，PnP计算relative pose等。

构造时会分别调用以下函数计算WINDOW内已有的features的描述子，新提features、去畸变、计算描述子。均使用到了BriefExtractor中重载的()运算符，函数较简单，不进行进一步解释。

computeWindowBRIEFPoint();//计算窗口中所有特征点的描述子
computeBRIEFPoint();//对新KF再重新检出500个FAST角点，并计算所有特征点的描述子，用于loop detection

---

3.1 快速重定位(涉及pose_graph、estimator两个模块)

这部分稍微有些复杂，同时涉及到pose_graph和estimator两个模块，当时讲到estimator后端求解时在构建系统整体的ResidualBlock时遇到过，当时没有具体讲解，现在有了pose_graph的基础后对此部分进行详细探究。
rqt_graph中可以看出estimator接收了来自于pose_graph的topic：match_points

崔华坤的PDF：

本部分难点有二：

1. 理解pose_graph和estimator中关于快速重定位msg的数据结构和组织形式；
2. 理解estimator中的选点策略。

和前面一样，针对loop，old帧记为$i$帧，cur帧记为$j$帧。

在findConnection()函数中PnPRANSAC最后会输出$T_{w{b}_i}$，存放于PnP_T_old, PnP_R_old中，在FAST_RELOCALIZATION之前会计算出relative pose $T_{b_i{b}_j}$，然后开始组装并pub match_points的msg：
其中

• msg_match_points.header.stamp是$j$帧的时间戳，用于重定位时与WINDOW内的
• msg_match_points.points前两维存的是loop上的点在$i$帧中的归一化坐标的xy，z是该feature的id
• T,Q是$i$帧的pose：$T_{w{b}_i}$，在setReloFrame/estimator.cpp中赋给prev_relo_t, prev_relo_q

//keyframe.cpp
    //快速重定位:组装msg，发送match_points topic，estimator接收到该消息后进行快速重定位
   	if(FAST_RELOCALIZATION)
   	{
	    sensor_msgs::PointCloud msg_match_points;//特征点之间的匹配
        //这个是cur帧的时间戳，estimator靠该时间戳在WINDOW中找到需要重定位的帧，通过该时间戳和old_frame的index来将old与cur建立联系
	    msg_match_points.header.stamp = ros::Time(time_stamp);
	    //msg的point是归一化的xy和landmark的id
	    for (int i = 0; i < (int)matched_2d_old_norm.size(); i++)
	    {
            geometry_msgs::Point32 p;
            p.x = matched_2d_old_norm[i].x;//i帧的归一化xy
            p.y = matched_2d_old_norm[i].y;
            p.z = matched_id[i];
            msg_match_points.points.push_back(p);
	    }
        //Tw_bi
	    Eigen::Vector3d T = old_kf->T_w_i;
	    Eigen::Matrix3d R = old_kf->R_w_i;
	    Quaterniond Q(R);
	    sensor_msgs::ChannelFloat32 t_q_index;
	    t_q_index.values.push_back(T.x());
	    t_q_index.values.push_back(T.y());
	    t_q_index.values.push_back(T.z());
	    t_q_index.values.push_back(Q.w());
	    t_q_index.values.push_back(Q.x());
	    t_q_index.values.push_back(Q.y());
	    t_q_index.values.push_back(Q.z());
	    t_q_index.values.push_back(index);
	    msg_match_points.channels.push_back(t_q_index);
	    pub_match_points.publish(msg_match_points);
   	}

在estimator_node.cpp中接收到match_points的msg之后执行回调relocalization_callback将消息存入relo_buf，在主线程process中处理relo_buf中的数据，并setReloFrame()设置重定位帧。其中

• match_points中存的是$i,j$两帧loop上的feature在$i$帧中的归一化xy，z是feature id，在optimization中选点时需要使用。

//estimator_node.cpp
            // 取relo_buf中最新的帧，设置重定位用的回环帧
            // set relocalization frame
            sensor_msgs::PointCloudConstPtr relo_msg = NULL;
            while (!relo_buf.empty())
            {
                relo_msg = relo_buf.front();
                relo_buf.pop();
            }
            if (relo_msg != NULL)
            {
                vector<Vector3d> match_points;
                double frame_stamp = relo_msg->header.stamp.toSec();
                for (unsigned int i = 0; i < relo_msg->points.size(); i++)
                {
                    Vector3d u_v_id;
                    u_v_id.x() = relo_msg->points[i].x;//i帧归一化xy
                    u_v_id.y() = relo_msg->points[i].y;
                    u_v_id.z() = relo_msg->points[i].z;//feature id
                    match_points.push_back(u_v_id);
                }
                //Tw_bi（i就是old帧，理解为loop frame v）
                Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);
                Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);
                Matrix3d relo_r = relo_q.toRotationMatrix();
                int frame_index;
                frame_index = relo_msg->channels[0].values[7];//old frame的帧号
                estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);//设置重定位帧(relo包括loop detection等操作)
            }


//estimator.cpp
void Estimator::setReloFrame(double _frame_stamp, int _frame_index, vector<Vector3d> &_match_points, Vector3d _relo_t, Matrix3d _relo_r)
{
    relo_frame_stamp = _frame_stamp;//与old frame loop上的WINDOW内的帧(j帧)的时间戳
    relo_frame_index = _frame_index;//j帧的帧号
    match_points.clear();
    match_points = _match_points;//i帧中与j帧中match上的点在i帧中的归一化(x,y,id)
    //Tw1_bi=Tw_b_old
    prev_relo_t = _relo_t;
    prev_relo_r = _relo_r;
    for(int i = 0; i < WINDOW_SIZE; i++)
    {
        if(relo_frame_stamp == Headers[i].stamp.toSec())//
        {
            relo_frame_local_index = i;//j帧在WINDOW中的下标
            relocalization_info = 1;
            for (int j = 0; j < SIZE_POSE; j++)
                relo_Pose[j] = para_Pose[i][j];//将WINDOW内用于relo的pose赋值给relo_Pose
        }
    }
}

在后端求解的optimization()函数中处理了上面设置的reloframe，其中，选点策略见下图，用到了match_points中z()保存的feature_id。

该部分的ResidualBlock目的是为了求$T_{w_1{b}_i}$，这里pts_j是i帧的归一化平面上的点，这里理解relo_Pose极其重要，relo_Pose实际上是$T_{w_2{b}_i}$，视觉重投影是从WINDOW内的start帧的camera(在w2系下)，投影到i帧(在w1系下)，relo_Pose中耦合了从$w_2$系到$w_1$系下的变换$T_{w_1{w}_2}$，而我们在pose_graph中是需要求出$T_{w_1{w}_2}$的。无论是快速重定位还是正常重定位，求出$T_{w_1{w}_2}$就是终极目标。

    //4.添加闭环检测残差，计算滑动窗口中与每一个闭环关键帧的相对位姿，这个相对位置是为后面的图优化(pose graph)准备 或者是 快速重定位(崔华坤PDF7.2节)
    //这里注意relo_pose是Tw2_bi = Tw2_w1 * Tw1_bi
    if(relocalization_info)
    {
        //printf("set relocalization factor! \n");
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(relo_Pose, SIZE_POSE, local_parameterization);
        int retrive_feature_index = 0;
        int feature_index = -1;
        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;
            ++feature_index;
            int start = it_per_id.start_frame;
            if(start <= relo_frame_local_index)//必须之前看到过
            {
                //1.先在i中match的点中找到可能是现在这个feature的id的index
                while((int)match_points[retrive_feature_index].z() < it_per_id.feature_id)//.z()存的是i，j两帧match上的feature的id
                {
                    retrive_feature_index++;
                }
                //2.如果是，则WINDOW内的it_per_id.feature_id这个id的landmark就是被loop上的landmark,取归一化坐标，
                if((int)match_points[retrive_feature_index].z() == it_per_id.feature_id)
                {
                    //pts_j是i帧的归一化平面上的点，这里理解relo_Pose及其重要，relo_Pose实际上是Tw2_bi，视觉重投影是从WINDOW内的start帧的camera(在w2系下)，投影到i帧(在w1系下)，耦合了Tw1_w2
                    Vector3d pts_j = Vector3d(match_points[retrive_feature_index].x(), match_points[retrive_feature_index].y(), 1.0);
                    Vector3d pts_i = it_per_id.feature_per_frame[0].point;//start中的点
                    
                    ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                    //relo_Pose是Tw2_bi
                    problem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]);
                    retrive_feature_index++;
                }     
            }
        }
    }

在double2vector()中计算了$T_{w_1{w}_2}$和$T_{b_i{b}_j}$，为什么叫快速重定位？就是因为此时在estimator中一轮优化之后得到了$T_{w_2{b}_i}$（即relo_Pose），就可以用$T_{w_2{b}_i}$和$T_{w_1{b}_i}$（即prev_relo_r, prev_relo_t）来算出$T_{w_1{w}_2}$，求出了终极目标就可以执行重定位了(在pubOdometry()中)。

需要注意，pitch和roll因为是可观的，所以在estimator中一直都有优化，所以我们重定位只需要矫正yaw和t即可。代码注释和计算方法如下：

// relative info between two loop frame
if(relocalization_info)
{
    //按照WINDOW内第一帧的yaw角变化对j帧进行矫正
    Matrix3d relo_r;//j帧矫正之后的T
    Vector3d relo_t;
    relo_r = rot_diff * Quaterniond(relo_Pose[6], relo_Pose[3], relo_Pose[4], relo_Pose[5]).normalized().toRotationMatrix();
    relo_t = rot_diff * Vector3d(relo_Pose[0] - para_Pose[0][0],
                                 relo_Pose[1] - para_Pose[0][1],
                                 relo_Pose[2] - para_Pose[0][2]) + origin_P0;//保证第[0]帧不变之后，+origin_P0转为世界系下的t

    //由于pitch和roll是可观的，所以我们在BA中一直都在优化pitch和roll，但由于yaw不可观，
    //所以即使漂了，可能还是满足我们BA的最优解，所以需要手动进行矫正
    //prev_relo_r=Tw1_bi, relo_Pose=Tw2_bi，这两个pose间的yaw和t的漂移Rw1_w2,tw1_w2
    double drift_correct_yaw;
    //yaw drift, Rw1_bi.yaw() - Rw2_bi.yaw=Rw1_w2.yaw()
    drift_correct_yaw = Utility::R2ypr(prev_relo_r).x() - Utility::R2ypr(relo_r).x();
    drift_correct_r = Utility::ypr2R(Vector3d(drift_correct_yaw, 0, 0));
    //tw1_w2
    drift_correct_t = prev_relo_t - drift_correct_r * relo_t;


    //Tw2_bi^(-1) * Tw2_bj = Tbi_bj
    relo_relative_t = relo_r.transpose() * (Ps[relo_frame_local_index] - relo_t);
    relo_relative_q = relo_r.transpose() * Rs[relo_frame_local_index];
    //Rw2_bj.yaw() - Rw2_bi.yaw() = Rbi_bj.yaw()
    relo_relative_yaw = Utility::normalizeAngle(Utility::R2ypr(Rs[relo_frame_local_index]).x() - Utility::R2ypr(relo_r).x());
    relocalization_info = 0;    
}

pubOdometry()中的快速重定位

//重定位时计算出的Tw1_w2，把现在的拉回到之前的上面去
Vector3d correct_t;
Vector3d correct_v;
Quaterniond correct_q;
//Tdrift_correct_r = T_w1_w2把world系的漂移拉回去
correct_t = estimator.drift_correct_r * estimator.Ps[WINDOW_SIZE] + estimator.drift_correct_t;
correct_q = estimator.drift_correct_r * estimator.Rs[WINDOW_SIZE];
odometry.pose.pose.position.x = correct_t.x();
odometry.pose.pose.position.y = correct_t.y();
odometry.pose.pose.position.z = correct_t.z();
odometry.pose.pose.orientation.x = correct_q.x();
odometry.pose.pose.orientation.y = correct_q.y();
odometry.pose.pose.orientation.z = correct_q.z();
odometry.pose.pose.orientation.w = correct_q.w();

---

loop_info更新的几个地方：

• KeyFrame()的第2个构造函数，在load pose garph时会使用
• updateKeyFrameLoop中，在estimator完成FAST_RELOCATION后发送msg，pose_graph线程接收到msg后的回调函数中调用updateKeyFrameLoop更新
• findConnection中更新，在找到KF之后求$T_{b_i{b}_j}$时会更新

---

3.2 4DoF pose_graph优化

此部分和重定位跑在两个不同的线程

(1). 4DOF优化的残差边的构建，这里面包括两类，一是序列边(即每个关键帧和时序上前四帧之间的相对Pose)，二是回环边(即回环检测成功产生的相对Pose)；
(2). 另外需要注意的是，最早回环检测成功的回环关键帧之前的历史关键帧对此处的Pose_graph优化没有太大的帮助(很直观的理解)；
(3). 还需要注意的是要考虑线程异步的问题，从代码中可以发现，在Pose_graph优化后只是更新关键帧列表中各个关键帧的T_w_i和R_w_i，并不直接改变关键帧的vio_T_w_i和vio_R_w_i，主要是因为若直接修正关键帧的vio_T_w_i和vio_R_w_i可能会导致VIO后端优化时出现异常，特别是当存在比较大的位姿调整时这个问题更加明显。

4DoF PoseGraph可以这样理解：观测和预测实际上计算方法是一样的，观测使用了yaw,pitch,roll,tx,ty,tz，而预测中，pitch和roll不能动（因为estimator已经在优化了），所以只能调整yaw,tx,ty,tz来减小residual。这样来理解pose graph就好理解了。

---

factor构建部分，其中angle_local_parameterization是定义的yaw的计算更新方式（具体解释见ceres博客4.1.2节），本部分BA使用的是autodiff。

  problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);
  problem.AddParameterBlock(t_array[i], 3);

  //第1次loop上的帧和sequence=0的帧的欧拉角和t不优化
  if ((*it)->index == first_looped_index || (*it)->sequence == 0)
  {   
      problem.SetParameterBlockConstant(euler_array[i]);
      problem.SetParameterBlockConstant(t_array[i]);
  }

  //add sequential edge 找i前面的4帧[i-j]，计算(b[i-j])tb[i-j]_b[i], (w)Rb[i-j]_b[i].yaw都是从[i-j]<-[i]和论文顺序相反
  for (int j = 1; j < 5; j++)
  {
    if (i - j >= 0 && sequence_array[i] == sequence_array[i-j])
    {
      //Rw_b[i-j]
      Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());
      //tw_b[i] - tw_b[i-j] = (w)tb[i-j]_b[i]
      Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);
      //计算观测t_diff：Rw_b[i-j]^(-1) * (w)tb[i-j]_b[i] = (b[i-j])tb[i-j]_b[i]
      relative_t = q_array[i-j].inverse() * relative_t;
      //计算观测yaw_diff：Rw_b[i].yaw - Rw_b[i-j].yaw = (w)Rb[i-j]_b[i].yaw*(是个标量，也无所谓参考系，只是知道从[i]到[i-j]的yaw即可)
      double relative_yaw = euler_array[i][0] - euler_array[i-j][0];
      ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
                                     relative_yaw, euler_conncected.y(), euler_conncected.z());
      problem.AddResidualBlock(cost_function, NULL, euler_array[i-j], 
                              t_array[i-j], 
                              euler_array[i], 
                              t_array[i]);
    }
  }

  //add loop edge
  
  if((*it)->has_loop)
  {
      //loop上的帧号肯定大于第一次loop的帧号
      assert((*it)->loop_index >= first_looped_index);
      int connected_index = getKeyFrame((*it)->loop_index)->local_index;
      Vector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix());
      Vector3d relative_t;
      relative_t = (*it)->getLoopRelativeT();//Tbibj的t
      double relative_yaw = (*it)->getLoopRelativeYaw();//Tbibj的yaw(i,j按照loop的理解)
      //构建pose_graph factor
      //注意，ceres里面构建的Rwi是由Create()中的euler_conncected.y(), euler_conncected.z()和AddResidualBlock中的euler_array[connected_index]构建的
      ceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
                                                                 relative_yaw, euler_conncected.y(), euler_conncected.z());
      //分别是loop边和序列边(sequential edge)
      problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index], 
                                                    t_array[connected_index], 
                                                    euler_array[i], 
                                                    t_array[i]);
      
  }

其中关于残差边的构建，我对于此处world系的理解：优化参数在给初值时的world是$w_2$，在优化后由于保持了全局一致性，就变为$w_1$，代码和论文中residual构建的方向相反，论文中translation是“小-大”，代码中确是“大-小”，不过只要外层观测relative_t，relative_yaw和内层operator()中的预测t_i_ij保持一致就没有问题(都是“大-小”)。

//4为residual，输入参数分别为1(yaw_i)，3(tw2_i)，1(yaw_j)，3(tw2_j)，注意这里的w在优化前是w2，在优化后由于保持了全局一致性，就变为w1
static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
								   const double relative_yaw, const double pitch_i, const double roll_i) 
{
     //分别是sequential和loop边的yaw(1)和t(3)的residual
  return (new ceres::AutoDiffCostFunction<
          FourDOFError, 4, 1, 3, 1, 3>(
          	new FourDOFError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));
}

优化完之后，就变为$T_{w{1}_{b}j}$，与优化前的$T_{w_2{b}_j}$一起计算出$T_{w_1{w}_2}$即可进行全局pose矫正，注意更新的是VIO pose还是非VIO pose。

1. 这里仍然使用loop中的叫法：$i$帧为old帧，$j帧$为WINDOW内的loop帧。
   solve之后更新了pose graph中所有的pose的成员T_w_i, R_w_i，并未更新vio_T_w_i， vio_R_w_i，这是因为优化后，即进行了重定位，如果有漂移的话，会被拉回去，导致world系改变，从$w_2$变为$w_1$，所以updatePose更新之后，此时*it中的T_w_i, R_w_i即为$T_{w_1{b}_j}$，而vio_T_w_i， vio_R_w_i仍然是$T_{w_2{b}_j}$，这样就可以更新$T_{w_1{w}_2}$了。

2. 更新$T_{w_1{w}_2}$之后就可以矫正keyframelist中的当前优化帧之后的帧的非VIO pose。

3. 最后updatePath，遍历keyframelist，获取矫正之后的非VIO pose，并pub出去。

//这里计算的应该是Tw1_w2
Vector3d cur_t, vio_t;
Matrix3d cur_r, vio_r;
cur_kf->getPose(cur_t, cur_r);//pose graph优化后的pose Tw1_bj
cur_kf->getVioPose(vio_t, vio_r);//优化前的pose  Tw2_bj
m_drift.lock();
yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();//Rw1_bj.yaw - Rw2_bj.yaw= Rw1_w2.yaw
r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));
t_drift = cur_t - r_drift * vio_t;//(w1)tw1_bj - Rw1_w2 * (w2)tw2_bj = (w1)tw1_w2
m_drift.unlock();


//更新重定位帧之后的KF的非VIO pose
it++;
for (; it != keyframelist.end(); it++)
{
    Vector3d P;
    Matrix3d R;
    (*it)->getVioPose(P, R);
    P = r_drift * P + t_drift;
    R = r_drift * R;
    (*it)->updatePose(P, R);
}
m_keyframelist.unlock();
updatePath();

至此，pose_graph的整体工作就完成，而VINS-MONO的整个框架解读也就完成了。