VINS-MONO代码解读6----pose_graph
开始pose_graph部分，本部分记住一句话无论是快速重定位还是正常重定位，求出 T w 1 w 2 T_{w_1w_2} Tw1w2就是终极目标。
还剩一个整体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语句就不进行介绍。
- 查询字典，给出查询结果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);
- 判定是否找到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;//
- 返回 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 i i帧的2D点，而PnPRANSAC()最终输出在PnP_T_old, PnP_R_old中的是 T w 2 b i T_{w_2b_i} Tw2bi
//关键帧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 T_{w_2b_i} Tw2bi这个量是不是很熟悉？对，这就是estimator中的relo_Pose，有了这个量，就能求出 T b i b j T_{b_ib_j} Tbibj，存在loop_info中，后面就可以算出 T w 1 w 2 T_{w_1w_2} Tw1w2执行矫正了：
//相对位姿检验
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_{b_ib_j} Tbibj计算出 T w 1 w 2 T_{w_1w_2} Tw1w2并执行矫正，全部变换到 w 1 w_1 w1系下：
//若存在回环候选帧
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进行相关管理(重点)
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;
};
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：
本部分难点有二：
- 理解pose_graph和estimator中关于快速重定位msg的数据结构和组织形式；
- 理解estimator中的选点策略。
和前面一样，针对loop，old帧记为 i i i帧，cur帧记为 j j j帧。
在findConnection()函数中PnPRANSAC最后会输出 T w b i T_{wb_i} Twbi，存放于PnP_T_old, PnP_R_old中，在FAST_RELOCALIZATION之前会计算出relative pose T b i b j T_{b_ib_j} Tbibj，然后开始组装并pub match_points的msg：
其中
msg_match_points.header.stamp是 j j j帧的时间戳，用于重定位时与WINDOW内的msg_match_points.points前两维存的是loop上的点在 i i i帧中的归一化坐标的xy，z是该feature的idT,Q是 i i i帧的pose： T w b i T_{wb_i} Twbi，在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 i,j i,j两帧loop上的feature在 i i 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 T_{w_1b_i} Tw1bi，这里pts_j是i帧的归一化平面上的点，这里理解relo_Pose极其重要，relo_Pose实际上是 T w 2 b i T_{w_2b_i} Tw2bi，视觉重投影是从WINDOW内的start帧的camera(在w2系下)，投影到i帧(在w1系下)，relo_Pose中耦合了从 w 2 w_2 w2系到 w 1 w_1 w1系下的变换 T w 1 w 2 T_{w_1w_2} Tw1w2，而我们在pose_graph中是需要求出 T w 1 w 2 T_{w_1w_2} Tw1w2的。无论是快速重定位还是正常重定位，求出 T w 1 w 2 T_{w_1w_2} Tw1w2就是终极目标。
//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_{w_1w_2} Tw1w2和 T b i b j T_{b_ib_j} Tbibj，为什么叫快速重定位？就是因为此时在estimator中一轮优化之后得到了 T w 2 b i T_{w_2b_i} Tw2bi（即relo_Pose），就可以用 T w 2 b i T_{w_2b_i} Tw2bi和 T w 1 b i T_{w_1b_i} Tw1bi（即prev_relo_r, prev_relo_t）来算出 T w 1 w 2 T_{w_1w_2} Tw1w2，求出了终极目标就可以执行重定位了(在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 T_{b_ib_j} Tbibj时会更新
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_2 w2，在优化后由于保持了全局一致性，就变为 w 1 w_1 w1，代码和论文中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_{w1_bj} Tw1bj，与优化前的 T w 2 b j T_{w_2b_j} Tw2bj一起计算出 T w 1 w 2 T_{w_1w_2} Tw1w2即可进行全局pose矫正，注意更新的是VIO pose还是非VIO pose。
-
这里仍然使用loop中的叫法： i i i帧为old帧， j 帧 j帧 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_2 w2变为 w 1 w_1 w1，所以updatePose更新之后，此时*it中的T_w_i, R_w_i即为 T w 1 b j T_{w_1b_j} Tw1bj，而vio_T_w_i， vio_R_w_i仍然是 T w 2 b j T_{w_2b_j} Tw2bj，这样就可以更新 T w 1 w 2 T_{w_1w_2} Tw1w2了。 -
更新 T w 1 w 2 T_{w_1w_2} Tw1w2之后就可以矫正
keyframelist中的当前优化帧之后的帧的非VIO pose。 -
最后
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的整个框架解读也就完成了。