9. 回环检测与重定位
本部分内容涉及到的代码大部分在pose_graph文件夹下，少部分在vins_estimator里。
原创内容，转载请先与我联系并注明出处，谢谢！
系列内容请点击：【SLAM】VINS-MONO解析——综述
知识点：
如何利用描述子-词袋进行回环检测运用PnP Ransac进行异常值剔除；
如何求出世界坐标系矫正矩阵和漂移矫正矩阵，和它们的区别；
如何进行图优化更新所有位姿实现位姿的平滑。
这部分最主要的作用就是求出漂移位姿矫正矩阵r_drift和t_drift。这么大一个工程，这么多行代码，保存这么多帧特征点和描述子，就是为了找出这6个自由度的变量。当然了，代码里还有w_r_vio和w_t_vio,同样也是位姿矫正矩阵，他俩是应对中途图像序列中断的情况，此时它俩会把新的图像序列的帧矫正到旧的图像序列的世界坐标系中，实现全局一致，这种情况出现的比较少，起作用的次数明显少于r_drift和t_drift。
9.1 主入口和回调函数
先进入到pose_graph_node.cpp里的main()：
开始是一些初始化ros node和从配置文件里读参数，这些我们不去关注它，然后是一些重要的订阅和回调函数：
//订阅topic并执行各自回调函数
ros::Subscriber sub_imu_forward = n.subscribe("/vins_estimator/imu_propagate", 2000, imu_forward_callback);
ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 2000, vio_callback);
ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 2000, image_callback);
ros::Subscriber sub_pose = n.subscribe("/vins_estimator/keyframe_pose", 2000, pose_callback);
ros::Subscriber sub_extrinsic = n.subscribe("/vins_estimator/extrinsic", 2000, extrinsic_callback);
ros::Subscriber sub_point = n.subscribe("/vins_estimator/keyframe_point", 2000, point_callback);
ros::Subscriber sub_relo_relative_pose = n.subscribe("/vins_estimator/relo_relative_pose", 2000, relo_relative_pose_callback);
这些订阅的数据从哪来，是什么含义，订阅之后要干什么，都非常重要。其中extrinsic_callback的含义比较明显，就不介绍了。
然后是发布的一些topic，在这里，我们先不管它发布的什么，在主流程碰到了再去研究：
//发布的topic
pub_match_img = n.advertise<sensor_msgs::Image>("match_image", 1000);
pub_camera_pose_visual = n.advertise<visualization_msgs::MarkerArray>("camera_pose_visual", 1000);
pub_key_odometrys = n.advertise<visualization_msgs::Marker>("key_odometrys", 1000);
pub_vio_path = n.advertise<nav_msgs::Path>("no_loop_path", 1000);
pub_match_points = n.advertise<sensor_msgs::PointCloud>("match_points", 100);
随后是创建回环检测和重定位的主流程函数process()，这里创建了一个线程干这个事情：
measurement_process = std::thread(process);
9.1.1 话题 “imu_propagate”的数据来源和回调函数imu_forward_callback()
数据来源是vins_estimator下visualization.cpp的pubLatestOdometry()，这个函数是在estimator_node.cpp的imu_callback()下被调用的。我们知道后端优化的位姿是以image的频率实现的，而IMU的频率是高于image的，所以在imu_callback()中，它会找一个最新优化的位姿，在这个基础上依托这个位姿之后收到的imu数据，积分在这个位姿上，实现输出一个高频的里程计的作用，所以 “imu_propagate”话题下接收的是一个高频的实时里程计。进入到imu_forward_callback()，你会发现这个回调函数把这个位姿先跟进重定位的结果进行了矫正：
vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio;
vio_q = posegraph.w_r_vio * vio_q;
vio_t = posegraph.r_drift * vio_t + posegraph.t_drift;
vio_q = posegraph.r_drift * vio_q;
这里有个非常有趣的位姿变换，就是先用w_r_vio和w_t_vio矫正一次，再用r_drift和t_drift矫正一次。我目前对他们的理解是前者是把当前位姿从错误的世界坐标系纠正到正确的坐标系上，后者是对短期位姿漂移的矫正。
然后把这个位姿从body->world变成cam->world：
Vector3d vio_t_cam;
Quaterniond vio_q_cam;
vio_t_cam = vio_t + vio_q * tic;
vio_q_cam = vio_q * qic;
最后用于可视化。
cameraposevisual.reset();
cameraposevisual.add_pose(vio_t_cam, vio_q_cam);
cameraposevisual.publish_by(pub_camera_pose_visual, forward_msg->header);
9.1.2 话题 “odometry”的数据来源和回调函数vio_callback()
数据来源是vins_estimator下visualization.cpp的pubOdometry()，这个函数是在estimator_node.cpp的主线程process()下调用的，我们看看它发的是哪个的位姿：
nav_msgs::Odometry odometry;
odometry.header = header;
odometry.header.frame_id = "world";
odometry.child_frame_id = "world";
Quaterniond tmp_Q;
tmp_Q = Quaterniond(estimator.Rs[WINDOW_SIZE]);
odometry.pose.pose.position.x = estimator.Ps[WINDOW_SIZE].x();
odometry.pose.pose.position.y = estimator.Ps[WINDOW_SIZE].y();
odometry.pose.pose.position.z = estimator.Ps[WINDOW_SIZE].z();
odometry.pose.pose.orientation.x = tmp_Q.x();
odometry.pose.pose.orientation.y = tmp_Q.y();
odometry.pose.pose.orientation.z = tmp_Q.z();
odometry.pose.pose.orientation.w = tmp_Q.w();
odometry.twist.twist.linear.x = estimator.Vs[WINDOW_SIZE].x();
odometry.twist.twist.linear.y = estimator.Vs[WINDOW_SIZE].y();
odometry.twist.twist.linear.z = estimator.Vs[WINDOW_SIZE].z();
pub_odometry.publish(odometry);
发布的位姿是滑窗的最新帧，假如说滑窗大小是10，那发布的是第11帧的位姿。再看看它的回调函数，实际上和前一个差不多，先根据重定位的结果进行矫正，再转化为cam->world的表示：
vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio;
vio_q = posegraph.w_r_vio * vio_q;
vio_t = posegraph.r_drift * vio_t + posegraph.t_drift;
vio_q = posegraph.r_drift * vio_q;
vio_t_cam = vio_t + vio_q * tic;
vio_q_cam = vio_q * qic;
把相机位姿的平移分类放到buffer里：
odometry_buf.push(vio_t_cam);
这个buffer是为了给key_odometrys提供相机运动过程中一系列空间坐标数据的：
for (unsigned int i = 0; i < odometry_buf.size(); i++)
{
geometry_msgs::Point pose_marker;
Vector3d vio_t;
vio_t = odometry_buf.front();
odometry_buf.pop();
pose_marker.x = vio_t.x();
pose_marker.y = vio_t.y();
pose_marker.z = vio_t.z();
key_odometrys.points.push_back(pose_marker);
odometry_buf.push(vio_t);
}
而key_odometrys的作用在vins中也是用于可视化的。
pub_key_odometrys.publish(key_odometrys)
9.1.3 话题"keyframe_pose"的数据来源和回调函数pose_callback()
数据来源是vins_estimator下visualization.cpp的pubKeyframe()，这个函数是在estimator_node.cpp的主线程process()下调用的，我们看看它发的是哪个的位姿：
int i = WINDOW_SIZE - 2;
Vector3d P = estimator.Ps[i];
Quaterniond R = Quaterniond(estimator.Rs[i]);
nav_msgs::Odometry odometry;
odometry.header = estimator.Headers[WINDOW_SIZE - 2];
odometry.header.frame_id = "world";
odometry.pose.pose.position.x = P.x();
odometry.pose.pose.position.y = P.y();
odometry.pose.pose.position.z = P.z();
odometry.pose.pose.orientation.x = R.x();
odometry.pose.pose.orientation.y = R.y();
odometry.pose.pose.orientation.z = R.z();
odometry.pose.pose.orientation.w = R.w();
pub_keyframe_pose.publish(odometry);
你发现了吗，是滑窗内倒数第二帧的位姿！也就是说，pose_graph收到的数据，处理的对象，都是由estimator滑窗在各个时刻的倒数第二帧提供的。我们再回到这个回调函数pose_callback()：
void pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
if(!LOOP_CLOSURE) return;
m_buf.lock();
pose_buf.push(pose_msg);
m_buf.unlock();
}
把位姿放到pose_buf里去，pose_buf会在process()里用到。
9.1.4 话题 “keyframe_point”的数据来源和回调函数point_callback()
数据来源是vins_estimator下visualization.cpp的pubKeyframe()，这个函数是在estimator_node.cpp的主线程process()下调用的，我们看看它发的是哪个的点：
sensor_msgs::PointCloud point_cloud;
point_cloud.header = estimator.Headers[WINDOW_SIZE - 2];
//遍历滑窗内的所有点
for (auto &it_per_id : estimator.f_manager.feature)
{
//如果在WINDOW_SIZE - 2这一帧上看到过
int frame_size = it_per_id.feature_per_frame.size();
if(it_per_id.start_frame < WINDOW_SIZE - 2 && it_per_id.start_frame + frame_size - 1 >= WINDOW_SIZE - 2 && it_per_id.solve_flag == 1)
{
//找到这些特征点的世界坐标
int imu_i = it_per_id.start_frame;
Vector3d pts_i = it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth;
Vector3d w_pts_i = estimator.Rs[imu_i] * (estimator.ric[0] * pts_i + estimator.tic[0])
+ estimator.Ps[imu_i];
geometry_msgs::Point32 p;
p.x = w_pts_i(0);
p.y = w_pts_i(1);
p.z = w_pts_i(2);
point_cloud.points.push_back(p);
//找到这些特征点在WINDOW_SIZE - 2这一帧上的归一化平面坐标，像素坐标，id
int imu_j = WINDOW_SIZE - 2 - it_per_id.start_frame;
sensor_msgs::ChannelFloat32 p_2d;
p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].point.x());
p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].point.y());
p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].uv.x());
p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].uv.y());
p_2d.values.push_back(it_per_id.feature_id);
point_cloud.channels.push_back(p_2d);
}
}
pub_keyframe_point.publish(point_cloud);
仍然是是滑窗内倒数第二帧的点。它首先扫描了滑窗内所有的点，如果某一个点在倒数第二帧前被看到，过了倒数第二帧仍然被看到，那么就把他的w系下的坐标，倒数第二帧的归一化坐标，像素坐标记录下来，发布出去。
再回到回调函数，你会发现和上一个也是类似的：
void point_callback(const sensor_msgs::PointCloudConstPtr &point_msg)
{
if(!LOOP_CLOSURE) return;
m_buf.lock();
point_buf.push(point_msg);
m_buf.unlock();
}
9.1.5 话题 IMAGE_TOPIC的数据来源和回调函数image_callback()
void image_callback(const sensor_msgs::ImageConstPtr &image_msg)
{
//ROS_INFO("image_callback!");
if(!LOOP_CLOSURE)
return;
m_buf.lock();
image_buf.push(image_msg);
m_buf.unlock();
//printf(" image time %f \n", image_msg->header.stamp.toSec());
// detect unstable camera stream
if (last_image_time == -1)
last_image_time = image_msg->header.stamp.toSec();
//若新到达的图像时间已超过上一帧图像时间1s或小于上一帧，则是新的图像序列
else if (image_msg->header.stamp.toSec() - last_image_time > 1.0 || image_msg->header.stamp.toSec() < last_image_time)
{
ROS_WARN("image discontinue! detect a new sequence!");
new_sequence();
}
last_image_time = image_msg->header.stamp.toSec();
}
你会发现，在这里，如果image时间戳错乱或者跟丢了的话，他会new_sequence()一下，sequence这个概念在pose_graph里面多次出现，而且概念非常绕：
void new_sequence()
{
printf("new sequence\n");
sequence++;
printf("sequence cnt %d \n", sequence);
if (sequence > 5)
{
ROS_WARN("only support 5 sequences since it's boring to copy code for more sequences.");
ROS_BREAK();
}
//重新初始化，重新构建地图。
posegraph.posegraph_visualization->reset();
posegraph.publish();
m_buf.lock();
while(!image_buf.empty())
image_buf.pop();
while(!point_buf.empty())
point_buf.pop();
while(!pose_buf.empty())
pose_buf.pop();
while(!odometry_buf.empty())
odometry_buf.pop();
m_buf.unlock();
}
它不仅会清空buffer里的数据，还会sequence++。在这里，作者把一系列连续输入的帧作为一个sequence，如果数据中断了，那么就会创建一个新的sequence。然后代码里只要找到机会(具有两个回环关系的两帧在不同的sequence里被发现了)，那么他会把新sequence里的帧的位姿全部矫正到与旧sequence一致的坐标系里。
9.2 主线程process()
首先经过时间戳的对比操作，在各个buffer里淘出具有相同时间戳的pose_msg、image_msg、point_msg ；
然后控制频率，每隔SKIP_CNT帧进行一次，让CPU别那么累：
if (skip_cnt < SKIP_CNT)
{
skip_cnt++;
continue;
}
然后当前的关键帧相对上一个处理过的关键帧的平移也得大于一个距离，系统才会继续处理当前帧：
(T - last_t).norm() > SKIP_DIS
这些都满足了，那就把当前关键帧的点找出来：
//将距上一关键帧距离（平移向量的模）超过SKIP_DIS的图像创建为关键帧
if((T - last_t).norm() > SKIP_DIS)
{
vector<cv::Point3f> point_3d; //世界坐标
vector<cv::Point2f> point_2d_uv; //归一化平面坐标(关键帧所在坐标系)
vector<cv::Point2f> point_2d_normal; //像素坐标(关键帧所在坐标系)
vector<double> point_id; //点id(为啥是double？)
for (unsigned int i = 0; i < point_msg->points.size(); i++)
{
cv::Point3f p_3d;
p_3d.x = point_msg->points[i].x;
p_3d.y = point_msg->points[i].y;
p_3d.z = point_msg->points[i].z;
point_3d.push_back(p_3d);
cv::Point2f p_2d_uv, p_2d_normal;
double p_id;
p_2d_normal.x = point_msg->channels[i].values[0];
p_2d_normal.y = point_msg->channels[i].values[1];
p_2d_uv.x = point_msg->channels[i].values[2];
p_2d_uv.y = point_msg->channels[i].values[3];
p_id = point_msg->channels[i].values[4];
point_2d_normal.push_back(p_2d_normal);
point_2d_uv.push_back(p_2d_uv);
point_id.push_back(p_id);
//printf("u %f, v %f \n", p_2d_uv.x, p_2d_uv.y);
}
利用当前关键帧点，位姿，时间戳和当前关键帧在pose_graph里的idx构建一个关键帧：
KeyFrame* keyframe = new KeyFrame(pose_msg->header.stamp.toSec(), frame_index, T, R, image,
point_3d, point_2d_uv, point_2d_normal, point_id, sequence);
我们进入到KeyFrame的构造函数里，除了传递输入数据给到当前keyframe对象外，还进行了brief描述子的计算：
KeyFrame::KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, cv::Mat &_image,
vector<cv::Point3f> &_point_3d, vector<cv::Point2f> &_point_2d_uv, vector<cv::Point2f> &_point_2d_norm,
vector<double> &_point_id, int _sequence)
{
time_stamp = _time_stamp;
index = _index;
vio_T_w_i = _vio_T_w_i;
vio_R_w_i = _vio_R_w_i;
T_w_i = vio_T_w_i;
R_w_i = vio_R_w_i;
origin_vio_T = vio_T_w_i;
origin_vio_R = vio_R_w_i;
image = _image.clone();
cv::resize(image, thumbnail, cv::Size(80, 60));
point_3d = _point_3d;
point_2d_uv = _point_2d_uv;
point_2d_norm = _point_2d_norm;
point_id = _point_id;
has_loop = false;
loop_index = -1;
has_fast_point = false;
loop_info << 0, 0, 0, 0, 0, 0, 0, 0;
sequence = _sequence;
computeWindowBRIEFPoint();
computeBRIEFPoint();
if(!DEBUG_IMAGE)
image.release();
}
computeWindowBRIEFPoint()是对外部传入的点的像素坐标进行brief描述子的计算，分别放到 window_keypoints和window_brief_descriptors里：
void KeyFrame::computeWindowBRIEFPoint()
{
BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());
for(int i = 0; i < (int)point_2d_uv.size(); i++)
{
cv::KeyPoint key;
key.pt = point_2d_uv[i];
window_keypoints.push_back(key);
}
extractor(image, window_keypoints, window_brief_descriptors);
}
而computeBRIEFPoint()会另外提取一些角点，计算描述子和归一化坐标，分别放到keypoints，brief_descriptors和keypoints_norm。
//额外检测新特征点并计算所有特征点的描述子，为了回环检测
void KeyFrame::computeBRIEFPoint()
{
BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());
const int fast_th = 20;
cv::FAST(image, keypoints, fast_th, true);
extractor(image, keypoints, brief_descriptors);
for (int i = 0; i < (int)keypoints.size(); i++)
{
Eigen::Vector3d tmp_p;
m_camera->liftProjective(Eigen::Vector2d(keypoints[i].pt.x, keypoints[i].pt.y), tmp_p);
cv::KeyPoint tmp_norm;
tmp_norm.pt = cv::Point2f(tmp_p.x()/tmp_p.z(), tmp_p.y()/tmp_p.z());
keypoints_norm.push_back(tmp_norm);
}
}
创建完关键帧之后，正戏开始了，往位姿图添加关键帧！
posegraph.addKeyFrame(keyframe, 1);
完事之后帧号+1：
frame_index++;
9.3 往位姿图添加关键帧posegraph.addKeyFrame(keyframe, 1)
posegraph对象是在pose_graph_node.cpp最前面定义的一个全局变量：
PoseGraph posegraph;
在Posegraph的构造函数里，它定义了几个重要的变量，同时创建了一个4Dof优化位姿的线程：
PoseGraph::PoseGraph()
{
t_optimization = std::thread(&PoseGraph::optimize4DoF, this);
earliest_loop_index = -1;
t_drift = Eigen::Vector3d(0, 0, 0);
yaw_drift = 0;
r_drift = Eigen::Matrix3d::Identity();
w_t_vio = Eigen::Vector3d(0, 0, 0);
w_r_vio = Eigen::Matrix3d::Identity();
global_index = 0;
sequence_cnt = 0;
sequence_loop.push_back(0);
base_sequence = 1;
}
earliest_loop_index：被发现是回环帧的那些帧中，时间最早的那一帧的global_index；
global_index：PoseGraph里面所有帧的序号；
r_drift/yaw_drift：相当于同一个东西，把当前里程计位姿转到纠正后位姿的旋转变换，由于只优化yaw角的旋转变换，所以这两者等价；
t_drift: 当前里程计位姿相对于纠正后位姿的平移变换；
w_r_vio/w_t_vio：如果说r_drift/t_drift是对位姿短期的快速矫正的话，w_r_vio/w_t_vio是对位姿长期的矫正，如果image跟丢了，那么就会新建一个sequence，w_r_vio/w_t_vio的作用就是找到新旧sequence之间的位姿变换，把新sequence的帧变换到旧sequence的坐标系中；
sequence_loop：如果当前sequence与之前的sequence的坐标系相同，也就是说新的sequence的帧都位姿矫正到旧sequence坐标系中了，则设为1；
我们进入到addKeyFrame()函数，这个函数主要做的事情是：
①先利用当前的坐标系矫正矩阵把最新帧的位姿的坐标系变换到正确的世界坐标系下；
②然后找回环帧并求出回环帧和最新帧的位姿变换；
③但是代码并没有把这个位姿变换乘以老帧的位姿得到的结果矩阵给到最新帧，而是计算现有最新帧位姿(经过①变换后的)和这个结果矩阵的变化量；
④如果回环两帧不在同一个序列下，就用③求得的变化量对新帧所在序列的所有帧都进行世界坐标系的矫正，包括最新帧；
⑤用现有的漂移矫正矩阵更新最新帧的位姿。漂移矫正矩阵的数据来源是vins_estimator.
一开始先判断最新帧的序列号和poseGraph里现有的序列号是否相同，如果是不同的序列，当前帧是全新的序列，那么世界坐标系矫正矩阵和漂移矫正矩阵都重置：
if (sequence_cnt != cur_kf->sequence)
{
sequence_cnt++;
sequence_loop.push_back(0);
w_t_vio = Eigen::Vector3d(0, 0, 0);
w_r_vio = Eigen::Matrix3d::Identity();
m_drift.lock();
t_drift = Eigen::Vector3d(0, 0, 0);
r_drift = Eigen::Matrix3d::Identity();
m_drift.unlock();
}
然后把当前帧的里程计进行世界坐标系的矫正，再还给它：
//获取当前帧的位姿vio_P_cur、vio_R_cur并更新
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);
cur_kf->index = global_index;
global_index++;
int loop_index = -1;
下一步，对当前帧进行回环检测，找到它的回环帧，具体的实现放到后面介绍，在正常的运行里，因为外部调用时flag_detect_loop一直为true，所以执行的都是detectLoop()， 而addKeyFrameIntoVoc()是预先在硬盘里加载现有位姿图时才执行的函数，所以先不看了。
if (flag_detect_loop)
{
TicToc tmp_t;
//回环检测，返回回环候选帧的索引
loop_index = detectLoop(cur_kf, cur_kf->index);
}
else
{
addKeyFrameIntoVoc(cur_kf);
}
如果说回环检测找到了，并且这两帧的描述子也匹配上了，那么才可以继续走下一步，findConnection()也在后面的部分分析:
if (loop_index != -1)
{
//printf(" %d detect loop with %d \n", cur_kf->index, loop_index);
//获取回环候选帧
KeyFrame* old_kf = getKeyFrame(loop_index);
//当前帧与回环候选帧进行描述子匹配
if (cur_kf->findConnection(old_kf))
{
如果找到的这个回环帧帧号是最早的，那么就记录这个帧号：
//earliest_loop_index为最早的回环候选帧
if (earliest_loop_index > loop_index || earliest_loop_index == -1)
earliest_loop_index = loop_index;
接下来，找到这两帧的位姿，计算两帧直接的位姿变换，把这个位姿变换作用到老帧上，获得新帧理论上的位姿w_P_cur和w_R_cur，getLoopRelativeT()我们仍然后面介绍，现在先只把主流程理解清楚：
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);
//获取当前帧与回环帧的相对位姿relative_q、relative_t
Vector3d relative_t;
Quaterniond relative_q;
relative_t = cur_kf->getLoopRelativeT();
relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();
//重新计算当前帧位姿w_P_cur、w_R_cur
w_P_cur = w_R_old * relative_t + w_P_old;
w_R_cur = w_R_old * relative_q;
代码里并没有把w_P_cur和w_R_cur直接给到最新帧的位姿，而是用它计算和之前原有位姿之间的变化shift_r和shift_t，其中旋转只提取yaw的变化：
//回环得到的位姿和VIO位姿之间的偏移量shift_r、shift_t
double shift_yaw;
Matrix3d shift_r;
Vector3d shift_t;
shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();
shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));
shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur;
接下来不好理解的来了，如果具有回环关系的两帧不在同一个序列，并且新帧所在的序列所在的世界坐标系没有进行过矫正，那么才用shift_r和shift_t对新帧所在的序列的所有的帧的位姿所在的世界坐标系进行矫正，否则shift_r和shift_t虽然求出来了，那么也不用它，这里是w_r_vio/w_t_vio被更新的地方：
// 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;
}
把当前帧放到优化缓存buffer里去，等待optimize4DoF()的优化：
//将当前帧放入优化队列中
m_optimize_buf.lock();
optimize_buf.push(cur_kf->index);
m_optimize_buf.unlock();
}
}
最后，把当前位姿进行漂移矫正，并把当前帧存起来，当然，有一些用于可视化和对外发布的代码，我们把它们跳过：
m_keyframelist.lock();
Vector3d P;
Matrix3d R;
//获取VIO当前帧的位姿P、R，根据偏移量得到实际位姿
cur_kf->getVioPose(P, R);
P = r_drift * P + t_drift;
R = r_drift * R;
//更新当前帧的位姿P、R
cur_kf->updatePose(P, R);
keyframelist.push_back(cur_kf);
m_keyframelist.unlock();
}
r_drift和w_drift从哪来的，我们后面分析。
9.3.1 回环检测detectLoop()
上一部分把posegraph.addKeyFrame()函数的主要流程讲了，现在继续抠里面一些函数的细节。detectLoop()的作用是找到当前帧的回环帧是哪一帧。
第一步：向词袋字典传入当前帧brief描述子，找到和当前帧最像的4个回环帧备胎，并且它们不能出现在与当前帧序号差50帧的范围内，搜索的结果返回给ret：
int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index)
{
//查询字典数据库，得到与每一帧的相似度评分ret
QueryResults ret;
db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);
第二步：搜索完成后，把当前帧的描述子加入到词袋字典里：
//添加当前关键帧到字典数据库中
db.add(keyframe->brief_descriptors);
bool find_loop = false;
cv::Mat loop_result;
第三步：如果有评分>0.015的话，就认为有回环帧：
//确保与相邻帧具有好的相似度评分
if (ret.size() >= 1 &&ret[0].Score > 0.05)
for (unsigned int i = 1; i < ret.size(); i++)
{
//评分大于0.015则认为是回环候选帧
if (ret[i].Score > 0.015) find_loop = true;
}
第四步：如果有多个帧的评分都>0.015，那么选择其中最早的那一帧：
//对于索引值大于50的关键帧才考虑进行回环
//返回评分大于0.015的最早的关键帧索引min_index
if (find_loop && frame_index > 50)
{
int min_index = -1;
for (unsigned int i = 0; i < ret.size(); i++)
{
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;
}
9.3.2 找到当前帧和回环帧的相对位姿findConnection()
这个函数在keyFrame.cpp中，里面很多内容在论文里基本都浓墨重彩讲的介绍过。
第一步：定义了一些用于保存当前帧和回环帧中具有对应关系的点的容器，名为cur的容器对应当前帧，old对应回环帧。matched_2d保存像素坐标，matched_2d_norm保存归一化坐标，matched_3d是3D点，注意，它们的3D坐标系是cur帧的世界坐标系，也就是位姿矫正前的世界坐标系。名为matched用于保存匹配成功的点的相关坐标，序号是一一对应的。
bool KeyFrame::findConnection(KeyFrame* old_kf)
{
TicToc tmp_t;
//printf("find Connection\n");
vector<cv::Point2f> matched_2d_cur, matched_2d_old;
vector<cv::Point2f> matched_2d_cur_norm, matched_2d_old_norm;
vector<cv::Point3f> matched_3d;
vector<double> matched_id;
vector<uchar> status;
matched_3d = point_3d;
matched_2d_cur = point_2d_uv;
matched_2d_cur_norm = point_2d_norm;
matched_id = point_id;
第二步，利用brief描述子找到当前帧和回环帧特征点的对应关系，然后去除匹配失败的点。
//关键帧与回环帧进行BRIEF描述子匹配，剔除匹配失败的点
searchByBRIEFDes(matched_2d_old, matched_2d_old_norm, status, old_kf->brief_descriptors, old_kf->keypoints, old_kf->keypoints_norm);
reduceVector(matched_2d_cur, status);
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);
匹配前，要知道两帧都是有各自特征点的，然后每一个特征点都有自己的描述子。searchByBRIEFDes()干的事情就是对于当前帧的每一个特征点，都和回环帧的每一个特征点进行描述子匹配，选择最接近的那一个，这样他俩就配对上了，它们就是同一个世界特征点在各自帧上的像素坐标。
void KeyFrame::searchByBRIEFDes(std::vector<cv::Point2f> &matched_2d_old,
std::vector<cv::Point2f> &matched_2d_old_norm,
std::vector<uchar> &status,
const std::vector<BRIEF::bitset> &descriptors_old,
const std::vector<cv::KeyPoint> &keypoints_old,
const std::vector<cv::KeyPoint> &keypoints_old_norm)
{
for(int i = 0; i < (int)window_brief_descriptors.size(); i++)
{
cv::Point2f pt(0.f, 0.f);
cv::Point2f pt_norm(0.f, 0.f);
// use cur descriptors to find corresponding keypoints in old frame
if (searchInAera(window_brief_descriptors[i], descriptors_old, keypoints_old, keypoints_old_norm, pt, pt_norm))
status.push_back(1);
else
status.push_back(0);
matched_2d_old.push_back(pt);
matched_2d_old_norm.push_back(pt_norm);
}
}
window_brief_descriptors是当前帧特征点的描述子，如果在老帧里找到了对应特征点，那么就保存下它的像素坐标和归一化坐标，我们再看看searchInAera()是怎么做到对于当前帧某一个特征点的描述子，在老帧找到对应点的：
bool KeyFrame::searchInAera(const BRIEF::bitset window_descriptor,
const std::vector<BRIEF::bitset> &descriptors_old,
const std::vector<cv::KeyPoint> &keypoints_old,
const std::vector<cv::KeyPoint> &keypoints_old_norm,
cv::Point2f &best_match,
cv::Point2f &best_match_norm)
{
cv::Point2f best_pt;
int bestDist = 128;
int bestIndex = -1;
for(int i = 0; i < (int)descriptors_old.size(); i++)
{
int dis = HammingDis(window_descriptor, descriptors_old[i]);
if(dis < bestDist)
{
bestDist = dis;
bestIndex = i;
}
}
//printf("best dist %d", bestDist);
//找到汉明距离小于80的最小值和索引即为该特征点的最佳匹配
if (bestIndex != -1 && bestDist < 80)
{
best_match = keypoints_old[bestIndex].pt;
best_match_norm = keypoints_old_norm[bestIndex].pt;
return true;
}
else
return false;
}
实际上就是拿当前帧的当前点的描述子和回环帧的每一个点的描述子进行汉明距离的计算，选择最小距离的那个就OK了：
int KeyFrame::HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b)
{
BRIEF::bitset xor_of_bitset = a ^ b;
int dis = xor_of_bitset.count();
return dis;
}
第三步，当前帧和回环帧的对应点找到了，相关的归一化坐标，像素坐标，世界坐标(cur帧的世界坐标系，有drift)也已知，接下来就是求当前帧和回环帧之间的相对位姿关系了。很多人都说这块是RANSAC PnP去除误匹配，的确是的，但是除此之外，还求出两帧之间的相对位姿变换。但是，求出的这个相对位姿变换，最后并不是用来求r_drift和t_drift，而是用来求w_r_vio和w_t_vio的，所以这个相对位姿没有那么的重要。
Eigen::Vector3d PnP_T_old; //回环帧在当前帧的世界坐标系下的位姿
Eigen::Matrix3d PnP_R_old; //回环帧在当前帧的世界坐标系下的位姿
Eigen::Vector3d relative_t; //回环帧和当前帧的相对平移变换
Quaterniond relative_q; //回环帧和当前帧的相对旋转变换
double relative_yaw; //回环帧和当前帧的相对旋转变换中的yaw角分量
//若达到最小回环匹配点数
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
//RANSAC PnP检测去除误匹配的点
status.clear();
PnPRANSAC(matched_2d_old_norm, matched_3d, status, PnP_T_old, PnP_R_old);
reduceVector(matched_2d_cur, status);
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);
}
PnPRANSAC()利用当前帧世界坐标系下的3D点和回环帧对应点的归一化坐标，求出回环帧在当前帧世界坐标系下的位姿，首先把当前帧位姿从body->world系转到camera->world系下，作为回环帧位姿的初始估计：
void KeyFrame::PnPRANSAC(const vector<cv::Point2f> &matched_2d_old_norm,
const std::vector<cv::Point3f> &matched_3d,
std::vector<uchar> &status,
Eigen::Vector3d &PnP_T_old, Eigen::Matrix3d &PnP_R_old)
{
//for (int i = 0; i < matched_3d.size(); i++)
// printf("3d x: %f, y: %f, z: %f\n",matched_3d[i].x, matched_3d[i].y, matched_3d[i].z );
//printf("match size %d \n", matched_3d.size());
cv::Mat r, rvec, t, D, tmp_r;
cv::Mat K = (cv::Mat_<double>(3, 3) << 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0);
Matrix3d R_inital;
Vector3d P_inital;
Matrix3d R_w_c = origin_vio_R * qic;
Vector3d T_w_c = origin_vio_T + origin_vio_R * tic;
R_inital = R_w_c.inverse();
P_inital = -(R_inital * T_w_c);
cv::eigen2cv(R_inital, tmp_r);
cv::Rodrigues(tmp_r, rvec);
cv::eigen2cv(P_inital, t);
然后调用OpenCV的函数，直接求出回环帧位姿：
cv::Mat inliers;
solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 100, 10.0 / 460.0, 0.99, inliers);
标记出利用PnP RANSAC去除outliner后，还有哪些点活着，然后会利用status去除outliers：
for (int i = 0; i < (int)matched_2d_old_norm.size(); i++)
status.push_back(0);
for( int i = 0; i < inliers.rows; i++)
{
int n = inliers.at<int>(i);
status[n] = 1;
}
最后求出回环帧在当前帧的世界坐标系下的位姿，是body->world的表示：
cv::Rodrigues(rvec, r);
Matrix3d R_pnp, R_w_c_old;
cv::cv2eigen(r, R_pnp);
R_w_c_old = R_pnp.transpose();
Vector3d T_pnp, T_w_c_old;
cv::cv2eigen(t, T_pnp);
T_w_c_old = R_w_c_old * (-T_pnp);
PnP_R_old = R_w_c_old * qic.transpose();
PnP_T_old = T_w_c_old - PnP_R_old * tic;
}
第四步，把相对位姿给到loop_info，注意了，此时loop_info保存的是当前帧与回环帧的相对位姿变换。
//若达到最小回环匹配点数
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
relative_t = PnP_R_old.transpose() * (origin_vio_T - PnP_T_old);
relative_q = PnP_R_old.transpose() * origin_vio_R;
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;
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;
第五步，把这些匹配点在回环帧上的坐标，回环帧的位姿(在他自己的世界坐标系下)发布出去，由vins_estimator订阅，用于计算r_drift和t_drift。这部分是一个复杂过程，请见9.4部分。
这一切完成之后，请回到9.3开头的部分，你会发现目前这个loop_info是用于计算w_r_vio和w_t_vio的。
9.4 位姿矫正矩阵r_drift和t_drift的计算/快速重定位
作者在代码里管这部分叫快速重定位，可能的确很快吧，但是我觉得这部分的作用是为了求出位姿矫正矩阵r_drift和t_drift的。我们回到9.3.2 的第五步，我们看看它是怎么做的。
9.4.1 初始数据由pose_graph发布
它把回环帧在自己世界坐标系的位姿(准),当前帧时间戳，RANSAC去除outliner后剩下的匹配的特征点的全局id，归一化坐标，封装到msg_match_points，由pub_match_points以话题"match_points"发布。
//快速重定位
if(FAST_RELOCALIZATION)
{
sensor_msgs::PointCloud msg_match_points;
msg_match_points.header.stamp = ros::Time(time_stamp); //当前帧时间戳
for (int i = 0; i < (int)matched_2d_old_norm.size(); i++)
{
geometry_msgs::Point32 p;
p.x = matched_2d_old_norm[i].x; //回环帧对应特征点的归一化坐标
p.y = matched_2d_old_norm[i].y; //回环帧对应特征点的归一化坐标
p.z = matched_id[i]; //回环帧对应特征点的全局id
msg_match_points.points.push_back(p);
}
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);
}
9.4.2 初始数据由vins_estimator订阅
在estimator_node.cpp中由sub_relo_points订阅，引发回调函数relocalization_callback()，这个函数会把9.4.1发布的数据保存到relo_buf里。在process()主线程中，它会取出relo_buf里最新的数据，给到estimator对象：
//取出最后一个重定位帧
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;
u_v_id.y() = relo_msg->points[i].y;
u_v_id.z() = relo_msg->points[i].z;
match_points.push_back(u_v_id);
}
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];
estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
}
进入到setReloFrame()，它把这些数据存到自己的内存里去：
void Estimator::setReloFrame(double _frame_stamp, int _frame_index, vector<Vector3d> &_match_points, Vector3d _relo_t, Matrix3d _relo_r)
{
relo_frame_stamp = _frame_stamp; //pose_graph里要被重定位的关键帧的时间戳
relo_frame_index = _frame_index; //回环帧在pose_graph里的id
match_points.clear();
match_points = _match_points; //两帧共同的特征点在回环帧上的归一化坐标
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())//如果pose_graph里要被重定位的关键帧还在滑窗里
{
relo_frame_local_index = i; //找到它的id
relocalization_info = 1; //告诉后端要重定位
for (int j = 0; j < SIZE_POSE; j++)
relo_Pose[j] = para_Pose[i][j]; //用这个关键帧的位姿(当前滑窗的世界坐标系下，不准)初始化回环帧的位姿
}
}
}
9.4.3 vins_estimator的后端求回环帧在当前帧世界坐标系下的位姿
数据现在estimator拿到了，那么依然是用最小化重投影误差的方式，即将滑窗内的3D特征点重投影到回环帧上来求回环帧在当前滑窗世界坐标系下的位姿，目的是让回环帧和重定位帧的位姿都在同一个世界坐标系下表示，从而求出这两帧之间的相对位姿，我们回到Estimator::optimization()下，梦开始的地方：
//添加闭环检测残差
if(relocalization_info)
{
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();
//当前点至少被观测到2次，并且首次观测不晚于倒数第二帧
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)
{
//如果都扫描到滑窗里第i个特征了，回环帧上还有id序号比i小，那这些肯定在滑窗里是没有匹配点的
while((int)match_points[retrive_feature_index].z() < it_per_id.feature_id)
{
retrive_feature_index++;
}
//如果回环帧某个特征点和滑窗内特征点匹配上了
if((int)match_points[retrive_feature_index].z() == it_per_id.feature_id)
{
//找到这个特征点在回环帧上的归一化坐标
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;
//构造重投影的损失函数
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
//构造这两帧之间的残差块
problem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]);
//回环帧上的这个特征点被用上了，继续找下一个特征点
retrive_feature_index++;
}
}
}
}
优化完成后，回环帧在当前帧世界坐标系下的位姿relo_Pose就求出来了，然后我们看Estimator::double2vector()：
// relative info between two loop frame
if(relocalization_info)
{
//回环帧在当前帧世界坐标系下的位姿
Matrix3d relo_r;
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;
//回环帧在当前帧世界坐标系和他自己世界坐标系下位姿的相对变化，这个会用来矫正滑窗内世界坐标系
double drift_correct_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));
drift_correct_t = prev_relo_t - drift_correct_r * relo_t;
//回环帧和重定位帧相对的位姿变换
relo_relative_t = relo_r.transpose() * (Ps[relo_frame_local_index] - relo_t);
relo_relative_q = relo_r.transpose() * Rs[relo_frame_local_index];
relo_relative_yaw = Utility::normalizeAngle(Utility::R2ypr(Rs[relo_frame_local_index]).x() - Utility::R2ypr(relo_r).x());
relocalization_info = 0;
}
9.4.4 获取drift矫正矩阵
这个drift_correct_r和drift_correct_t在vins_estimator文件夹下visualization.cpp的pubOdometry()函数使用，用于矫正滑窗内世界坐标系，而pubOdometry()会被estimator_node.cpp的process()主线程中被调用。
对于pose_graph而言，它关心relo_relative_t和relo_relative_q,这两个是在visualization.cpp的pubRelocalization()函数使用，用于矫正滑窗内世界坐标系，而pubRelocalization()也会被estimator_node.cpp的process()主线程中被调用：
void pubRelocalization(const Estimator &estimator)
{
nav_msgs::Odometry odometry;
odometry.header.stamp = ros::Time(estimator.relo_frame_stamp);
odometry.header.frame_id = "world";
odometry.pose.pose.position.x = estimator.relo_relative_t.x();
odometry.pose.pose.position.y = estimator.relo_relative_t.y();
odometry.pose.pose.position.z = estimator.relo_relative_t.z();
odometry.pose.pose.orientation.x = estimator.relo_relative_q.x();
odometry.pose.pose.orientation.y = estimator.relo_relative_q.y();
odometry.pose.pose.orientation.z = estimator.relo_relative_q.z();
odometry.pose.pose.orientation.w = estimator.relo_relative_q.w();
odometry.twist.twist.linear.x = estimator.relo_relative_yaw;
odometry.twist.twist.linear.y = estimator.relo_frame_index;
pub_relo_relative_pose.publish(odometry); //话题"relo_relative_pose"
}
9.4.5 pose_graph订阅回环帧和重定位帧相对的位姿变换
细心的读者会发现，在9.1部分我有一个重要的回调函数没有讲，因为他是在这起作用的。
ros::Subscriber sub_relo_relative_pose = n.subscribe("/vins_estimator/relo_relative_pose", 2000, relo_relative_pose_callback);
进入到这个回调函数：
//重定位回调函数，将重定位帧的相对位姿放入loop_info，updateKeyFrameLoop()进行回环更新
void relo_relative_pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
Vector3d relative_t = Vector3d(pose_msg->pose.pose.position.x,
pose_msg->pose.pose.position.y,
pose_msg->pose.pose.position.z);
Quaterniond relative_q;
relative_q.w() = pose_msg->pose.pose.orientation.w;
relative_q.x() = pose_msg->pose.pose.orientation.x;
relative_q.y() = pose_msg->pose.pose.orientation.y;
relative_q.z() = pose_msg->pose.pose.orientation.z;
double relative_yaw = pose_msg->twist.twist.linear.x;
int index = pose_msg->twist.twist.linear.y;
//printf("receive index %d \n", index );
Eigen::Matrix<double, 8, 1 > loop_info;
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;
posegraph.updateKeyFrameLoop(index, loop_info);
}
这个回调函数执行了个updateKeyFrameLoop()，你会发现漂移矫正矩阵r_drift和t_drift实际上是在这里更新的！到这里pose_graph最主要的算法就完成了。
void PoseGraph::updateKeyFrameLoop(int index, Eigen::Matrix<double, 8, 1 > &_loop_info)
{
//更新当前帧与回环帧的相对位姿到当前帧的loop_info
KeyFrame* kf = getKeyFrame(index);
kf->updateLoop(_loop_info);
if (abs(_loop_info(7)) < 30.0 && Vector3d(_loop_info(0), _loop_info(1), _loop_info(2)).norm() < 20.0)
{
if (FAST_RELOCALIZATION)
{
KeyFrame* old_kf = getKeyFrame(kf->loop_index);
Vector3d w_P_old, w_P_cur, vio_P_cur;
Matrix3d w_R_old, w_R_cur, vio_R_cur;
old_kf->getPose(w_P_old, w_R_old);
kf->getVioPose(vio_P_cur, vio_R_cur);
Vector3d relative_t;
Quaterniond relative_q;
relative_t = kf->getLoopRelativeT();
relative_q = (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;
shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();
shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));
shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur;
m_drift.lock();
yaw_drift = shift_yaw;
r_drift = shift_r;
t_drift = shift_t;
m_drift.unlock();
}
}
}
9.5 4DoF位姿图优化
整个工程里面，相对位姿的旋转都只选择了yaw，没有选择pitch和roll，因为IMU能够识别pitch和roll，所以矫正只针对剩下4个自由度。
第一步，取出buffer里第一帧，同时标记好最早的回环帧：
//四自由度位姿图优化
void PoseGraph::optimize4DoF()
{
while(true)
{
int cur_index = -1;
int first_looped_index = -1;
//取出最新一个待优化帧作为当前帧
m_optimize_buf.lock();
while(!optimize_buf.empty())
{
cur_index = optimize_buf.front();
first_looped_index = earliest_loop_index;
optimize_buf.pop();
}
m_optimize_buf.unlock();
第二步，设置图优化的size：
if (cur_index != -1)
{
printf("optimize pose graph \n");
TicToc tmp_t;
m_keyframelist.lock();
KeyFrame* cur_kf = getKeyFrame(cur_index);
int max_length = cur_index + 1;
// w^t_i w^q_i
double t_array[max_length][3]; //待优化的平移分量
Quaterniond q_array[max_length]; //待优化的旋转分量
double euler_array[max_length][3];//待优化的旋转分量
double sequence_array[max_length];//所在的图像序列号
设置ceres求解器：
ceres::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.max_num_iterations = 5;
ceres::Solver::Summary summary;
ceres::LossFunction *loss_function;
loss_function = new ceres::HuberLoss(0.1);
ceres::LocalParameterization* angle_local_parameterization =
AngleLocalParameterization::Create();
第三步，遍历位姿图的每一帧，但是只从第一个回环帧开始优化，对于被扫描的当前帧，拿到他的位姿，序列号，并构建参数块，对于处于第一个序列的帧或者是第一个回环帧，他们的参数块需要固定：
list<KeyFrame*>::iterator it;
int i = 0;
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
//找到第一个回环候选帧
if ((*it)->index < first_looped_index) continue;
(*it)->local_index = i;
Quaterniond tmp_q;
Matrix3d tmp_r;
Vector3d tmp_t;
(*it)->getVioPose(tmp_t, tmp_r);
tmp_q = tmp_r;
t_array[i][0] = tmp_t(0);
t_array[i][1] = tmp_t(1);
t_array[i][2] = tmp_t(2);
q_array[i] = tmp_q;
Vector3d euler_angle = Utility::R2ypr(tmp_q.toRotationMatrix());
euler_array[i][0] = euler_angle.x();
euler_array[i][1] = euler_angle.y();
euler_array[i][2] = euler_angle.z();
sequence_array[i] = (*it)->sequence;
problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
if ((*it)->index == first_looped_index || (*it)->sequence == 0)
{
problem.SetParameterBlockConstant(euler_array[i]);
problem.SetParameterBlockConstant(t_array[i]);
}
第四步，如果当前帧和它前面紧挨着的5帧处于同一个序列中，那么当前帧和这5帧都设置一个残差块：
//add edge
for (int j = 1; j < 5; j++)
{
if (i - j >= 0 && sequence_array[i] == sequence_array[i-j])
{
Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());
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]);
relative_t = q_array[i-j].inverse() * relative_t;
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)
{
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();
double relative_yaw = (*it)->getLoopRelativeYaw();
ceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(), relative_yaw, euler_conncected.y(), euler_conncected.z());
problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index], t_array[connected_index], euler_array[i], t_array[i]);
}
if ((*it)->index == cur_index) break;
i++;
}
m_keyframelist.unlock();
第六步，利用ceres求解，把优化后的位姿给到每一个参加优化的帧：
ceres::Solve(options, &problem, &summary);
m_keyframelist.lock();
i = 0;
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
if ((*it)->index < first_looped_index)
continue;
Quaterniond tmp_q;
tmp_q = Utility::ypr2R(Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2]));
Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);
Matrix3d tmp_r = tmp_q.toRotationMatrix();
(*it)-> updatePose(tmp_t, tmp_r);
if ((*it)->index == cur_index) break;
i++;
}
第七步，求出当前帧优化前后的位姿差异，这个差异再次更新给r_drift和t_drift：
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;
m_drift.unlock();
第八步，由于参与位姿图优化的最后一帧就是当前帧，所以对于时间戳更靠后的那些帧，再利用t_drift和r_drift进行位姿矫正：
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();
}
}
}
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
