闭环检测的主要流程如下图：
这个功能在一个独立的节点中，在VINS-Mono/pose_graph文件夹下面，具体的文件包括：
- pose_graph_node.cpp ROS 节点函数、回调函数、主线程
- keyframe.cpp 构建关键帧类、描述子计算、匹配关键帧与回环帧
- pose_graph.cpp 位姿图的建立与图优化、回环检测与闭环
节点文件（）负责管理整个闭环的流程，订阅/vins_estimator节点发布的多个topic，包括关键帧的位姿（keyframe_pose）、重定位位姿（relo_relative_pose）、相机到IMU的外参估计（extrinsic）、VIO里程计信息PQV（odometry）、关键帧中的3D点云（keyframe_point）、IMU传播值（imu_propagate），然后再把闭环计算得到的量发布出去。
通过下图可以清晰看出该节点订阅和发布的信息
下面看具体的实现
1. process：闭环主线程
该函数位于pose_graph_node.cpp文件中，它负责执行闭环中除订阅和发布程序以外的所有流程，主要包括以下步骤
1）得到具有相同时间戳的pose_msg、image_msg、point_msg
2）构建pose_graph中用到的关键帧：这里用到的策略是先剔除最开始的SKIP_FIRST_CNT帧，然后每隔SKIP_CNT，将将距上一关键帧距离（平移向量的模）超过SKIP_DIS的图像创建为关键帧。
3）在posegraph中添加关键帧，将flag_detect_loop=1即设置回环检测。
if(!image_buf.empty() && !point_buf.empty() && !pose_buf.empty())
{
if (image_buf.front()->header.stamp.toSec() > pose_buf.front()->header.stamp.toSec())
{
pose_buf.pop();
printf("throw pose at beginning\n");
}
else if (image_buf.front()->header.stamp.toSec() > point_buf.front()->header.stamp.toSec())
{
point_buf.pop();
printf("throw point at beginning\n");
}
else if (image_buf.back()->header.stamp.toSec() >= pose_buf.front()->header.stamp.toSec()
&& point_buf.back()->header.stamp.toSec() >= pose_buf.front()->header.stamp.toSec())
{
pose_msg = pose_buf.front();
pose_buf.pop();
while (!pose_buf.empty())
pose_buf.pop();
while (image_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec())
image_buf.pop();
image_msg = image_buf.front();
image_buf.pop();
while (point_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec())
point_buf.pop();
point_msg = point_buf.front();
point_buf.pop();
}
}
m_buf.unlock();
if (pose_msg != NULL)
{
//printf(" pose time %f \n", pose_msg->header.stamp.toSec());
//printf(" point time %f \n", point_msg->header.stamp.toSec());
//printf(" image time %f \n", image_msg->header.stamp.toSec());
// skip first few
//剔除最开始的SKIP_FIRST_CNT帧
if (skip_first_cnt < SKIP_FIRST_CNT)
{
skip_first_cnt++;
continue;
}
//每隔SKIP_CNT帧进行一次 SKIP_CNT=0
if (skip_cnt < SKIP_CNT)
{
skip_cnt++;
continue;
}
else
{
skip_cnt = 0;
}
cv_bridge::CvImageConstPtr ptr;
if (image_msg->encoding == "8UC1")
{
sensor_msgs::Image img;
img.header = image_msg->header;
img.height = image_msg->height;
img.width = image_msg->width;
img.is_bigendian = image_msg->is_bigendian;
img.step = image_msg->step;
img.data = image_msg->data;
img.encoding = "mono8";
ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
}
else
ptr = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8);
cv::Mat image = ptr->image;
// build keyframe
Vector3d T = Vector3d(pose_msg->pose.pose.position.x,
pose_msg->pose.pose.position.y,
pose_msg->pose.pose.position.z);
Matrix3d R = Quaterniond(pose_msg->pose.pose.orientation.w,
pose_msg->pose.pose.orientation.x,
pose_msg->pose.pose.orientation.y,
pose_msg->pose.pose.orientation.z).toRotationMatrix();
//将距上一关键帧距离（平移向量的模）超过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;
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);
}
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);
m_process.lock();
start_flag = 1;
//在posegraph中添加关键帧，flag_detect_loop=1回环检测
posegraph.addKeyFrame(keyframe, 1);
m_process.unlock();
frame_index++;
last_t = T;
}
}
2. addKeyFrame：添加关键帧
它位于文件pose_graph.cpp中，它被process函数调用。
回环检测是基于关键帧的，添加关键帧的主要流程包括：
1）如果sequence_cnt != cur_kf->sequence，则新建一个新的图像序列
2）获取当前帧的位姿vio_P_cur、vio_R_cur并更新
3）进行回环检测，返回回环候选帧的索引
4）如果存在回环候选帧，将当前帧与回环帧进行描述子匹配并计算位姿，并执行优化
5）获取优化后的位姿P、R，根据偏移量计算得到实际位姿。并进行位姿更新
6）发布计算后的结果
void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)
{
//shift to base frame
Vector3d vio_P_cur;
Matrix3d vio_R_cur;
//建一个新的图像序列
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;
if (flag_detect_loop)
{
TicToc tmp_t;
//回环检测，返回回环候选帧的索引
loop_index = detectLoop(cur_kf, cur_kf->index);
}
else
{
addKeyFrameIntoVoc(cur_kf);
}
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;
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;
//回环得到的位姿和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 vio pose of whole sequence to the world frame
//将所有图像序列都合并到世界坐标系下
if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
{
w_r_vio = shift_r;
w_t_vio = shift_t;
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
list<KeyFrame*>::iterator it = keyframelist.begin();
for (; it != keyframelist.end(); it++)
{
if((*it)->sequence == cur_kf->sequence)
{
Vector3d vio_P_cur;
Matrix3d vio_R_cur;
(*it)->getVioPose(vio_P_cur, vio_R_cur);
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
(*it)->updateVioPose(vio_P_cur, vio_R_cur);
}
}
sequence_loop[cur_kf->sequence] = 1;
}
//将当前帧放入优化队列中
m_optimize_buf.lock();
optimize_buf.push(cur_kf->index);
m_optimize_buf.unlock();
}
}
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);
//发布path[sequence_cnt]
Quaterniond Q{R};
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(cur_kf->time_stamp);
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X;
pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y;
pose_stamped.pose.position.z = P.z();
pose_stamped.pose.orientation.x = Q.x();
pose_stamped.pose.orientation.y = Q.y();
pose_stamped.pose.orientation.z = Q.z();
pose_stamped.pose.orientation.w = Q.w();
path[sequence_cnt].poses.push_back(pose_stamped);
path[sequence_cnt].header = pose_stamped.header;
//保存闭环轨迹到VINS_RESULT_PATH
if (SAVE_LOOP_PATH)
{
ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file.precision(0);
loop_path_file << cur_kf->time_stamp * 1e9 << ",";
loop_path_file.precision(5);
loop_path_file << P.x() << ","
<< P.y() << ","
<< P.z() << ","
<< Q.w() << ","
<< Q.x() << ","
<< Q.y() << ","
<< Q.z() << ","
<< endl;
loop_path_file.close();
}
//draw local connection
if (SHOW_S_EDGE)
{
list<KeyFrame*>::reverse_iterator rit = keyframelist.rbegin();
for (int i = 0; i < 4; i++)
{
if (rit == keyframelist.rend())
break;
Vector3d conncected_P;
Matrix3d connected_R;
if((*rit)->sequence == cur_kf->sequence)
{
(*rit)->getPose(conncected_P, connected_R);
posegraph_visualization->add_edge(P, conncected_P);
}
rit++;
}
}
//当前帧与其回环帧连线
if (SHOW_L_EDGE)
{
if (cur_kf->has_loop)
{
//printf("has loop \n");
KeyFrame* connected_KF = getKeyFrame(cur_kf->loop_index);
Vector3d connected_P,P0;
Matrix3d connected_R,R0;
connected_KF->getPose(connected_P, connected_R);
//cur_kf->getVioPose(P0, R0);
cur_kf->getPose(P0, R0);
if(cur_kf->sequence > 0)
{
//printf("add loop into visual \n");
posegraph_visualization->add_loopedge(P0, connected_P + Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0));
}
}
}
//posegraph_visualization->add_pose(P + Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0), Q);
keyframelist.push_back(cur_kf);
publish();
m_keyframelist.unlock();
}
3. detectLoop：检测回环
它位于文件pose_graph.cpp中，被addKeyFrame函数调用。
它的主要作用是确定哪些是回环候选帧，并返回其索引，具体实现流程是：
1）查询字典数据库，得到与每一帧的相似度评分ret
2）添加当前关键帧到字典数据库中
3）通过相似度评分判断是否存在回环候选帧
4）如果在先前检测到回环候选帧再判断：当前帧的索引值是否大于50，即系统开始的前50帧不进行回环
int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index)
{
// put image into image_pool; for visualization
cv::Mat compressed_image;
if (DEBUG_IMAGE)
{
int feature_num = keyframe->keypoints.size();
cv::resize(keyframe->image, compressed_image, cv::Size(376, 240));
putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255));
image_pool[frame_index] = compressed_image;
}
TicToc tmp_t;
QueryResults ret;
TicToc t_query;
//查询字典数据库，得到与每一帧的相似度评分ret
db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);
//printf("query time: %f", t_query.toc());
//cout << "Searching for Image " << frame_index << ". " << ret << endl;
//添加当前关键帧到字典数据库中
TicToc t_add;
db.add(keyframe->brief_descriptors);
//printf("add feature time: %f", t_add.toc());
// ret[0] is the nearest neighbour's score. threshold change with neighour score
bool find_loop = false;
cv::Mat loop_result;
if (DEBUG_IMAGE)
{
loop_result = compressed_image.clone();
if (ret.size() > 0)
putText(loop_result, "neighbour score:" + to_string(ret[0].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255));
}
// visual loop result
if (DEBUG_IMAGE)
{
for (unsigned int i = 0; i < ret.size(); i++)
{
int tmp_index = ret[i].Id;
auto it = image_pool.find(tmp_index);
cv::Mat tmp_image = (it->second).clone();
putText(tmp_image, "index: " + to_string(tmp_index) + "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255));
cv::hconcat(loop_result, tmp_image, loop_result);
}
}
//确保与相邻帧具有好的相似度评分
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)
//评分大于0.015则认为是回环候选帧
if (ret[i].Score > 0.015)
{
find_loop = true;
int tmp_index = ret[i].Id;
if (DEBUG_IMAGE && 0)
{
auto it = image_pool.find(tmp_index);
cv::Mat tmp_image = (it->second).clone();
putText(tmp_image, "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255));
cv::hconcat(loop_result, tmp_image, loop_result);
}
}
}
/*
if (DEBUG_IMAGE)
{
cv::imshow("loop_result", loop_result);
cv::waitKey(20);
}
*/
//对于索引值大于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;
}
评论（0）
您还未登录，请登录后发表或查看评论