开始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才可能是loopif (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的就判定找到了loopif (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;}elsereturn -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去除outlierreduceVector(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_bjrelative_t = PnP_R_old.transpose() * (origin_vio_T - PnP_T_old);relative_q = PnP_R_old.transpose() * origin_vio_R;//Rbi_bj.yawrelative_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)){//更新最早的回环帧indexif (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_bicur_kf->getVioPose(vio_P_cur, vio_R_cur);//Tw2_bj//relative是Tbi_bj=Told_curVector3d relative_t;Quaterniond relative_q;//读取Tbi_bjrelative_t = cur_kf->getLoopRelativeT();//这个loop_info是在findConnection()中更新的relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();//Tw1_bj = Tw1_bi * Tbi_bjw_P_cur = w_R_old * relative_t + w_P_old;w_R_cur = w_R_old * relative_q;//回环得到的位姿和VIO位姿之间的偏移量Tw1_w2double shift_yaw;Matrix3d shift_r;Vector3d shift_t;//Tw1_w2shift_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_w2shift_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，执行relocationvio_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模板文件，构造BriefExtractordvirtual 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的idfor (int i = 0; i < (int)matched_2d_old_norm.size(); i++){geometry_msgs::Point32 p;p.x = matched_2d_old_norm[i].x;//i帧的归一化xyp.y = matched_2d_old_norm[i].y;p.z = matched_id[i];msg_match_points.points.push_back(p);}//Tw_biEigen::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 framesensor_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帧归一化xyu_v_id.y() = relo_msg->points[i].y;u_v_id.z() = relo_msg->points[i].z;//feature idmatch_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_oldprev_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_biif(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的indexwhile((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_w2Vector3d 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_biproblem.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帧矫正之后的TVector3d 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_w2double 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_w2drift_correct_t = prev_relo_t - drift_correct_r * relo_t;//Tw2_bi^(-1) * Tw2_bj = Tbi_bjrelo_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 edgeif((*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的tdouble 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)的residualreturn (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的整个框架解读也就完成了。