一、关键帧信息
KeyFrame::KeyFrame(
double _time_stamp, // 关键帧的时间戳（用于时序对齐）
int _index, // 关键帧的唯一ID（全局索引）
Vector3d &_vio_T_w_i, // VIO模块估计的该关键帧在世界坐标系下的平移向量
Matrix3d &_vio_R_w_i, // VIO模块估计的该关键帧在世界坐标系下的旋转矩阵
cv::Mat &_image, // 关键帧对应的原始图像
vector<cv::Point3f> &_point_3d, // 该关键帧观测到的3D点（世界坐标系下）
vector<cv::Point2f> &_point_2d_uv, // 特征点在图像上的2D像素坐标（u,v）
vector<cv::Point2f> &_point_2d_norm, // 特征点的归一化平面坐标（去畸变后，x' = (u-cx)/fx, y' = (v-cy)/fy）
vector<double> &_point_id, // 特征点的唯一ID（用于跨帧匹配）
int _sequence) // 关键帧所属的序列号（多段轨迹时区分）
{
// 1. 基础信息赋值
time_stamp = _time_stamp; // 时间戳（用于回环检测时序对齐）
index = _index; // 关键帧全局唯一索引
// 2. VIO位姿初始化
vio_T_w_i = _vio_T_w_i; // 保存VIO模块估计的平移
vio_R_w_i = _vio_R_w_i; // 保存VIO模块估计的旋转
T_w_i = vio_T_w_i; // 初始化关键帧的全局平移（初始值与VIO一致）
R_w_i = vio_R_w_i; // 初始化关键帧的全局旋转（初始值与VIO一致）
origin_vio_T = vio_T_w_i; // 记录VIO的原始平移（用于后续漂移修正）
origin_vio_R = vio_R_w_i; // 记录VIO的原始旋转（用于后续漂移修正）
// 3. 图像处理
image = _image.clone(); // 深拷贝原始图像（用于回环检测中的特征匹配）
cv::resize(image, thumbnail, cv::Size(80, 60)); // 生成缩略图（用于可视化）
// 4. 特征点信息存储
point_3d = _point_3d; // 存储该关键帧观测到的3D点（世界坐标系）
point_2d_uv = _point_2d_uv; // 存储特征点的像素坐标（用于重投影）
point_2d_norm = _point_2d_norm; // 存储归一化平面坐标（用于BA优化）
point_id = _point_id; // 存储特征点ID（用于跨帧数据关联）
// 5. 回环检测相关标志位初始化
has_loop = false; // 标记该关键帧是否参与回环
loop_index = -1; // 回环关联的关键帧ID（-1表示无回环）
has_fast_point = false; // 标记是否提取了快速匹配特征点（用于加速回环检测）
loop_info << 0, 0, 0, 0, 0, 0, 0, 0; // 回环校正参数（如相对位姿的平移和旋转）
// 6. 序列管理
sequence = _sequence; // 关键帧所属的轨迹序列号（用于多段SLAM）
// 7. 计算特征描述子
computeWindowBRIEFPoint(); // 在图像局部窗口内计算BRIEF描述子（提高匹配鲁棒性）
computeBRIEFPoint(); // 全局计算BRIEF描述子（用于回环检测）
// 8. 内存优化（非调试模式下释放原始图像）
if(!DEBUG_IMAGE)
image.release(); // 释放原始图像数据，仅保留缩略图和描述子
}
主要包含几个模块：
(1) 基础信息赋值
(2) VIO位姿初始化
(3) 图像处理
(4) 特征点信息存储
(5) 回环检测相关标志位初始化
(6) 计算特征描述子，（computeWindowBRIEFPoint函数中做的），并在关键帧中新检测500个关键点并进行描述子计算（在computeBRIEFPoint函数中实现）
1）computeWindowBRIEFPoint计算关键帧中关键点的描述子
计算当前关键帧已有关键点的描述子，存放在window_brief_descriptors中，其他关键帧要计算和这个关键帧是否形成闭环，就可以用其他关键帧中的描述子和该关键帧中的描述子一算相似度，如果相似度评分达到设置的阈值，则可以认为该帧就是闭环候选帧。
/**
* 计算窗口中所有特征点的描述子
*/
void KeyFrame::computeWindowBRIEFPoint()
{
BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());
//遍历关键帧中的2d像素坐标关键点
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);
}
2）computeBRIEFPoint函数
由于闭环是使用BRIEF描述子的DBow2词袋进行检测的，而前端feature_tracker中检测到的关键点数太少，对于闭环检测远远不够。因此在posegraph当中会对新来的 KeyFrame 即后端非线性优化刚处理完的关键帧，再检测出 500 个 FAST 关键点进行闭环检测的时候使用。同时对所有新老角点进行 BRIEF描述子计算。然后，计算当前帧与词袋的相似度分数，并与关键帧数据库中所有帧进行对比,并进行闭环一致性检测，获得闭环的候选帧。
/**
* 在关键帧图像中检测了500个新的特征点并计算所有特征点的描述子
*/
void KeyFrame::computeBRIEFPoint()
{
BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());
const int fast_th = 20; // corner detector response threshold
if(1)
cv::FAST(image, keypoints, fast_th, true);
else
{
vector<cv::Point2f> tmp_pts;
/**
* 因为后边要做闭环检测，前端提取的关键点数太少，这里从图像image中提取500个角点
* cv::goodFeaturesToTrack参数介绍如下：
* 第一个参数是输入图像（8位或32位单通道图）。
* 第二个参数是检测到的所有角点，类型为vector或数组，由实际给定的参数类型而定。如果是vector，那么它应该是一个包含cv::Point2f的vector对象；如果类型是cv::Mat,那么它的每一行对应一个角点，点的x、y位置分别是两列。
* 第三个参数用于限定检测到的点数的最大值。
* 第四个参数表示检测到的角点的质量水平（通常是0.10到0.01之间的数值，不能大于1.0）。
* 第五个参数用于区分相邻两个角点的最小距离（小于这个距离得点将进行合并）。
* 第六个参数是mask，如果指定，它的维度必须和输入图像一致，且在mask值为0处不进行角点检测。
* 第七个参数是blockSize，表示在计算角点时参与运算的区域大小，常用值为3，但是如果图像的分辨率较高则可以考虑使用较大一点的值。
* 第八个参数用于指定角点检测的方法，如果是true则使用Harris角点检测，false则使用Shi Tomasi算法。
* 第九个参数是在使用Harris算法时使用，最好使用默认值0.04。
* */
cv::goodFeaturesToTrack(image, tmp_pts, 500, 0.01, 10);
//遍历图像中新提取的角点，存入keypoints当中
for(int i = 0; i < (int)tmp_pts.size(); i++)
{
cv::KeyPoint key;
key.pt = tmp_pts[i];
keypoints.push_back(key);
}
}
//计算该关键帧中对应特征点的描述子
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);
}
}
二、添加关键帧
/**
* 位姿图中加入关键帧，flag_detect_loop表示是否进行闭环检测
*/
void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)
{
//shift to base frame
Vector3d vio_P_cur;
Matrix3d vio_R_cur;
//创建新的图像序列，sequence_loop的定义为vector<bool> sequence_loop
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);
//设置当前关键帧的index
cur_kf->index = global_index;
global_index++;
int loop_index = -1;
//进行闭环检测，返回闭环候选帧的index存入loop_index中
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);
//根据查询到的和当前帧形成闭环的帧的index来获取旧的闭环帧作为候选帧
KeyFrame* old_kf = getKeyFrame(loop_index);
/**
* 判断当前帧与闭环候选帧之间匹配的特征点数是否大于形成闭环的最小匹配点数
* */
if (cur_kf->findConnection(old_kf))
{
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_old * relative_t + w_P_old;
w_R_cur = w_R_old * relative_q;
double shift_yaw;
Matrix3d shift_r;
Vector3d shift_t;
//计算根据闭环帧得到的位姿和当前vio之间的偏差
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();
//遍历keyframelist链上每一个keyframe
for (; it != keyframelist.end(); it++)
{
//遍历的keyframe和当前关键帧的序列号相同
if((*it)->sequence == cur_kf->sequence)
{
Vector3d vio_P_cur;
Matrix3d vio_R_cur;
//更新it的位姿
(*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;
//对当前帧根据计算出来的偏移量更新位姿
cur_kf->getVioPose(P, R);
P = r_drift * P + t_drift;
R = r_drift * R;
cur_kf->updatePose(P, R);
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
path[sequence_cnt].poses.push_back(pose_stamped);
path[sequence_cnt].header = pose_stamped.header;
//保存闭环路径
if (SAVE_LOOP_PATH)
{
//在euroc_config.yaml当中设置的VINS_RESULT_PATH路径为output_path: "/home/shaozu/output/"
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
//SHOW_S_EDGE表示是否在位姿图中展示sequential edge，就是一个关键帧和它之前的关键帧之间的相对变换关系
if (SHOW_S_EDGE)
{
//c.rbegin()功能是返回一个逆序迭代器，它指向容器c的最后一个元素
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);
//P是当前帧的位置，conncected_P是rit帧的位置
posegraph_visualization->add_edge(P, conncected_P);
}
rit++;
}
}
//SHOW_L_DEBUG表示是否在位姿图中展示loop-closure edge
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");
//P0是当前帧的位置，connected_P是和当前帧构成闭环的帧的位置
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);
//发布topic
publish();
m_keyframelist.unlock();
}
addKeyFrame函数中主要做了下面的工作：
1）初始化与序列管理
序列切换：当检测到新的图像序列时（sequence_cnt != cur_kf->sequence），系统会初始化新的序列，并重置相关的变量：
sequence_cnt 增加。
sequence_loop 中添加一个新的标志位，初始化为0。
重置世界坐标系下的平移 w_t_vio 和旋转 w_r_vio。
重置漂移变量 t_drift 和 r_drift。
2）获取当前关键帧的位姿
从当前关键帧 cur_kf 中获取其视觉惯性里程计（VIO）的位姿（位置 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
更新当前关键帧的位姿。
3）关键帧索引分配
为当前关键帧分配一个全局索引 global_index，并递增该索引。
4） 闭环检测
如果 flag_detect_loop 为 true，则进行闭环检测：
调用 detectLoop(cur_kf, cur_kf->index) 函数，检测是否存在闭环，返回闭环候选帧的索引 loop_index。
如果不进行闭环检测，则直接将当前关键帧加入到词典中（addKeyFrameIntoVoc(cur_kf)）。
5）闭环处理
如果检测到闭环（loop_index != -1）：
获取闭环候选帧 old_kf。
判断当前帧与闭环候选帧之间的特征点匹配数是否满足闭环条件。
如果满足条件，则计算当前帧与闭环帧之间的相对平移 relative_t 和相对旋转 relative_q。
根据闭环帧的位姿和相对变换，重新计算当前帧在世界坐标系下的位姿。
计算当前帧的VIO位姿与闭环优化后的位姿之间的偏差（shift_yaw、shift_r、shift_t）。
如果闭环帧和当前帧不在同一个序列中，并且当前序列尚未进行过闭环优化，则更新当前序列中所有关键帧的位姿。
将当前帧的索引放入优化队列 optimize_buf 中，等待后续的图优化。
6）更新关键帧位姿
根据计算出的漂移量 t_drift 和 r_drift，更新当前关键帧的位姿。
将更新后的位姿转换为ROS消息格式，并发布到 path 中。
7）保存闭环路径
如果 SAVE_LOOP_PATH 为 true，则将当前帧的位姿信息保存到文件中。
8）可视化处理
如果 SHOW_S_EDGE 为 true，则在位姿图中展示当前帧与前几帧之间的连接关系。
如果 SHOW_L_EDGE 为 true，并且当前帧存在闭环，则在位姿图中展示闭环边。
9）将关键帧加入列表
将当前关键帧 cur_kf 加入到关键帧列表 keyframelist 中。
10）发布消息
调用 publish() 函数，发布相关的ROS消息。
11）解锁
释放 m_keyframelist 锁，确保线程安全。
在addKeyFrame函数中，需要重点关注下面几个函数：
1）闭环检测函数
int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index)
该函数用于检测当前关键帧 keyframe 是否存在闭环（即是否与之前的关键帧匹配）。如果检测到闭环，返回闭环候选帧的索引；否则返回 -1。
//闭环检测
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();
//将当前关键帧的图像按照指定大小(376,240)缩放到compressed_image当中
cv::resize(keyframe->image, compressed_image, cv::Size(376, 240));
/**
* putText函数将指定的text信息画在图像中指定的位置
* void cv::putText ( InputOutputArray img, 图像信息
* const String & text, 文字信息
* Point org, Bottom-left corner of the text string in the image.
* int fontFace, 字体
* double fontScale, 文字大小
* Scalar color, 颜色
* int thickness = 1, 文字的线条粗细
* int lineType = LINE_8,
* bool bottomLeftOrigin = false
* )
*/
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;
//first query; then add this frame into database!
QueryResults ret;
TicToc t_query;
/**
* 在数据库(字典)中查询该关键帧的描述子，获得查询结果存入ret中
* frame_index - 50表示要查询的帧中index最大的值，说明当前帧的前50帧不在描述子匹配查询范围之内
*/
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当中
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));
//hconcat函数：用于两个Mat矩阵或者图像的水平拼接。hconcat（B,C，A）等同于A=[B C]
cv::hconcat(loop_result, tmp_image, loop_result);
}
}
// a good match with its nerghbour 和相邻帧具有好的匹配(相似度评分)
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;
//TODO这个if下面的代码应该永远不会被执行才对，不清楚这么写的意义何在？？？
if (DEBUG_IMAGE && 0)
{
//在image_pool中找到该图像帧
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);
}
*/
/**
* 检测到了闭环情况的判断
* 需要判断当前帧的frame_index > 50，说明开始的前50帧不做闭环判断
*/
if (find_loop && frame_index > 50)
{
int min_index = -1;
for (unsigned int i = 0; i < ret.size(); i++)
{
//在匹配的候选帧中取最小的index并且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;
}
主要流程：
- 特征提取与匹配：提取当前关键帧的特征点，并与之前的关键帧进行匹配,使用视觉词汇袋（BoW）或直接特征匹配来加速搜索。
- 几何验证： 对匹配的特征点进行几何验证（例如，使用 RANSAC 计算基础矩阵或本质矩阵），以排除误匹配。
- 闭环候选帧选择： 根据匹配分数和几何一致性，选择最可能的闭环候选帧。
- 返回结果：如果找到闭环候选帧，返回其索引；否则返回 -1。
2）闭环帧检测函数
bool KeyFrame::findConnection(KeyFrame* old_kf)
该函数用于验证当前关键帧 this 与闭环候选帧 old_kf 之间是否存在有效的闭环连接。主要检查两帧之间的特征点匹配数量是否满足闭环条件。
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;
TicToc t_match;
#if 0
if (DEBUG_IMAGE)
{
cv::Mat gray_img, loop_match_img;
cv::Mat old_img = old_kf->image;
cv::hconcat(image, old_img, gray_img);
cvtColor(gray_img, loop_match_img, CV_GRAY2RGB);
for(int i = 0; i< (int)point_2d_uv.size(); i++)
{
cv::Point2f cur_pt = point_2d_uv[i];
cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0));
}
for(int i = 0; i< (int)old_kf->keypoints.size(); i++)
{
cv::Point2f old_pt = old_kf->keypoints[i].pt;
old_pt.x += COL;
cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0));
}
ostringstream path;
path << "/home/tony-ws1/raw_data/loop_image/"
<< index << "-"
<< old_kf->index << "-" << "0raw_point.jpg";
cv::imwrite( path.str().c_str(), loop_match_img);
}
#endif
//printf("search by des\n");
//当前关键帧与闭环候选帧进行BRIEF描述子匹配，这里相当于是在2d-2d之间进行匹配
searchByBRIEFDes(matched_2d_old, matched_2d_old_norm, status, old_kf->brief_descriptors, old_kf->keypoints, old_kf->keypoints_norm);
//根据返回的status状态，删除匹配失败的点
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);
//printf("search by des finish\n");
#if 0
if (DEBUG_IMAGE)
{
int gap = 10;
cv::Mat gap_image(ROW, gap, CV_8UC1, cv::Scalar(255, 255, 255));
cv::Mat gray_img, loop_match_img;
cv::Mat old_img = old_kf->image;
cv::hconcat(image, gap_image, gap_image);
cv::hconcat(gap_image, old_img, gray_img);
cvtColor(gray_img, loop_match_img, CV_GRAY2RGB);
for(int i = 0; i< (int)matched_2d_cur.size(); i++)
{
cv::Point2f cur_pt = matched_2d_cur[i];
cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0));
}
for(int i = 0; i< (int)matched_2d_old.size(); i++)
{
cv::Point2f old_pt = matched_2d_old[i];
old_pt.x += (COL + gap);
cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0));
}
for (int i = 0; i< (int)matched_2d_cur.size(); i++)
{
cv::Point2f old_pt = matched_2d_old[i];
old_pt.x += (COL + gap);
cv::line(loop_match_img, matched_2d_cur[i], old_pt, cv::Scalar(0, 255, 0), 1, 8, 0);
}
ostringstream path, path1, path2;
path << "/home/tony-ws1/raw_data/loop_image/"
<< index << "-"
<< old_kf->index << "-" << "1descriptor_match.jpg";
cv::imwrite( path.str().c_str(), loop_match_img);
/*
path1 << "/home/tony-ws1/raw_data/loop_image/"
<< index << "-"
<< old_kf->index << "-" << "1descriptor_match_1.jpg";
cv::imwrite( path1.str().c_str(), image);
path2 << "/home/tony-ws1/raw_data/loop_image/"
<< index << "-"
<< old_kf->index << "-" << "1descriptor_match_2.jpg";
cv::imwrite( path2.str().c_str(), old_img);
*/
}
#endif
status.clear();
/*
FundmantalMatrixRANSAC(matched_2d_cur_norm, matched_2d_old_norm, status);
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);
*/
#if 0
if (DEBUG_IMAGE)
{
int gap = 10;
cv::Mat gap_image(ROW, gap, CV_8UC1, cv::Scalar(255, 255, 255));
cv::Mat gray_img, loop_match_img;
cv::Mat old_img = old_kf->image;
cv::hconcat(image, gap_image, gap_image);
cv::hconcat(gap_image, old_img, gray_img);
cvtColor(gray_img, loop_match_img, CV_GRAY2RGB);
for(int i = 0; i< (int)matched_2d_cur.size(); i++)
{
cv::Point2f cur_pt = matched_2d_cur[i];
cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0));
}
for(int i = 0; i< (int)matched_2d_old.size(); i++)
{
cv::Point2f old_pt = matched_2d_old[i];
old_pt.x += (COL + gap);
cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0));
}
for (int i = 0; i< (int)matched_2d_cur.size(); i++)
{
cv::Point2f old_pt = matched_2d_old[i];
old_pt.x += (COL + gap) ;
cv::line(loop_match_img, matched_2d_cur[i], old_pt, cv::Scalar(0, 255, 0), 1, 8, 0);
}
ostringstream path;
path << "/home/tony-ws1/raw_data/loop_image/"
<< index << "-"
<< old_kf->index << "-" << "2fundamental_match.jpg";
cv::imwrite( path.str().c_str(), loop_match_img);
}
#endif
Eigen::Vector3d PnP_T_old;
Eigen::Matrix3d PnP_R_old;
Eigen::Vector3d relative_t;
Quaterniond relative_q;
double relative_yaw;
//MIN_LOOP_NUM值为25是最小闭环匹配点数，表示要判定为闭环得有大于25个2d特征点相匹配
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);
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);
#if 1
/**
* 闭环匹配图像处理
* 结合系统运行的实际情况可以看出，这里是对当前图像和与其形成闭环的图像之间匹配的特征点用绿线进行了连接，
* 并将两帧图像进行了拼接
*/
if (DEBUG_IMAGE)
{
int gap = 10;
cv::Mat gap_image(ROW, gap, CV_8UC1, cv::Scalar(255, 255, 255));
cv::Mat gray_img, loop_match_img;
cv::Mat old_img = old_kf->image;
//hconcat函数：用于两个Mat矩阵或者图像的水平拼接,hconcat（B,C，A）;//等同于A=[B C]
cv::hconcat(image, gap_image, gap_image);
cv::hconcat(gap_image, old_img, gray_img);
cvtColor(gray_img, loop_match_img, CV_GRAY2RGB);
for(int i = 0; i< (int)matched_2d_cur.size(); i++)
{
cv::Point2f cur_pt = matched_2d_cur[i];
/**
* 在闭环匹配的图像中画圆
* void cv::circle ( InputOutputArray img, 要画圆圈的图像
* Point center, 圆圈的中心
* int radius, 圆圈的半径
* const Scalar & color, 圆圈的颜色
* int thickness = 1,//为正数表示圆圈的线条粗细，为负数表示填充效果
* int lineType = LINE_8, //圆的边界类型
* int shift = 0 //中心坐标和半径值中的小数位数。
* )
*/
cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0));
}
for(int i = 0; i< (int)matched_2d_old.size(); i++)
{
cv::Point2f old_pt = matched_2d_old[i];
//计算old中的要画圆的中心坐标，COL为图像的宽度
old_pt.x += (COL + gap);
cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0));
}
//在两帧匹配的图像之间建立特征点之间的连接线
for (int i = 0; i< (int)matched_2d_cur.size(); i++)
{
cv::Point2f old_pt = matched_2d_old[i];
old_pt.x += (COL + gap) ;
/**
* void cv::line ( InputOutputArray img, 要画线的图像
* Point pt1, 线的一端的端点
* Point pt2, 线的另一端的端点
* const Scalar & color, 线条的颜色
* int thickness = 1, 线条的粗细
* int lineType = LINE_8, 线条类型
* int shift = 0
* )
*/
cv::line(loop_match_img, matched_2d_cur[i], old_pt, cv::Scalar(0, 255, 0), 2, 8, 0);
}
cv::Mat notation(50, COL + gap + COL, CV_8UC3, cv::Scalar(255, 255, 255));
//在当前图和之前的两幅图上方写上文字title
putText(notation, "current frame: " + to_string(index) + " sequence: " + to_string(sequence), cv::Point2f(20, 30), CV_FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255), 3);
putText(notation, "previous frame: " + to_string(old_kf->index) + " sequence: " + to_string(old_kf->sequence), cv::Point2f(20 + COL + gap, 30), CV_FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255), 3);
/**
* vconcat函数：用于将两个Mat矩阵或者图像的垂直拼接。vconcat（B,C，A）;等同于A=[B ;C]
*/
cv::vconcat(notation, loop_match_img, loop_match_img);
/*
ostringstream path;
path << "/home/tony-ws1/raw_data/loop_image/"
<< index << "-"
<< old_kf->index << "-" << "3pnp_match.jpg";
cv::imwrite( path.str().c_str(), loop_match_img);
*/
//匹配的2d点数大于判断为闭环的最小数目
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
/*
cv::imshow("loop connection",loop_match_img);
cv::waitKey(10);
*/
cv::Mat thumbimage;
/**
* void cv::resize ( InputArray src, 输入图像
* OutputArray dst, 输出图像，它有大小dsize（非零时）或从src.size（）、fx和fy计算的大小；dst的类型与src的类型相同
* Size dsize, output image size; if it equals zero, it is computed as:
* 𝚍𝚜𝚒𝚣𝚎 = 𝚂𝚒𝚣𝚎(𝚛𝚘𝚞𝚗𝚍(𝚏𝚡*𝚜𝚛𝚌.𝚌𝚘𝚕𝚜), 𝚛𝚘𝚞𝚗𝚍(𝚏𝚢*𝚜𝚛𝚌.𝚛𝚘𝚠𝚜))
* Either dsize or both fx and fy must be non-zero.
* double fx = 0, 沿水平轴的尺度因子；当它等于0时，计算为
* (𝚍𝚘𝚞𝚋𝚕𝚎)𝚍𝚜𝚒𝚣𝚎.𝚠𝚒𝚍𝚝𝚑/𝚜𝚛𝚌.𝚌𝚘𝚕𝚜
* double fy = 0, 沿垂直轴的尺度因子；当它等于0时，计算为
* (𝚍𝚘𝚞𝚋𝚕𝚎)𝚍𝚜𝚒𝚣𝚎.𝚑𝚎𝚒𝚐𝚑𝚝/𝚜𝚛𝚌.𝚛𝚘𝚠𝚜
* int interpolation = INTER_LINEAR 插值法
* )
*/
cv::resize(loop_match_img, thumbimage, cv::Size(loop_match_img.cols / 2, loop_match_img.rows / 2));
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", thumbimage).toImageMsg();
msg->header.stamp = ros::Time(time_stamp);
//发布loop_match_image消息
pub_match_img.publish(msg);
}
}
#endif
}
/**
* 对相对位姿进行检测并发布/pose_graph/match_points，在vins_estimator中接收该topic
*/
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());
//printf("PNP relative\n");
//cout << "pnp relative_t " << relative_t.transpose() << endl;
//cout << "pnp relative_yaw " << relative_yaw << endl;
if (abs(relative_yaw) < 30.0 && relative_t.norm() < 20.0)
{
has_loop = true;
loop_index = old_kf->index;
//将当前帧与闭环帧的相对位姿存入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;
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];
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);
//发布/pose_graph/match_points topic
pub_match_points.publish(msg_match_points);
}
return true;
}
}
//printf("loop final use num %d %lf--------------- \n", (int)matched_2d_cur.size(), t_match.toc());
return false;
}
主要流程：
- 特征点匹配：提取当前关键帧和闭环候选帧的特征点，并进行匹配。
- 匹配数量检查：如果匹配的特征点数量超过设定的阈值，则认为两帧之间存在有效的闭环连接。
- 相对位姿计算：计算两帧之间的相对位姿（平移 relative_t 和旋转 relative_q），用于后续的位姿图优化。
- 返回结果：如果匹配数量满足条件，返回 true；否则返回 false。
3）闭环边添加函数
void CameraPoseVisualization::add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1)
该函数用于在可视化工具（如 Rviz）中添加闭环边，显示两帧之间的闭环连接。
/**
1. 闭环边添加函数
2. const Eigen::Vector3d& p0 当前关键帧的位置
3. const Eigen::Vector3d& p1 闭环帧的位置
*/
void CameraPoseVisualization::add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){
//m_markers.clear();
visualization_msgs::Marker marker;
marker.ns = m_marker_ns;
marker.id = m_markers.size() + 1;
//tmp_loop_edge_num++;
//if(tmp_loop_edge_num >= LOOP_EDGE_NUM)
// tmp_loop_edge_num = 1;
//LINE_STRIP值为4，表示线条的类型（点的连线）
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration();
//marker.scale.x = 0.4;
marker.scale.x = 0.02;
marker.color.r = 1.0f;
//marker.color.g = 1.0f;
//marker.color.b = 1.0f;
marker.color.a = 1.0;
geometry_msgs::Point point0, point1;
//将p0和p1这两个位置向量转化为点坐标point0和point1
Eigen2Point(p0, point0);
Eigen2Point(p1, point1);
//设置线条的两个端点的点坐标
marker.points.push_back(point0);
marker.points.push_back(point1);
//将marker添加到m_markers向量当中
m_markers.push_back(marker);
}
- 输入参数：p0 和 p1 分别表示两帧在世界坐标系下的位置。
- 可视化处理：在可视化工具中绘制一条从 p0 到 p1 的线段，表示闭环边。
- 与 add_edge 的区别：add_loopedge 专门用于绘制闭环边，而 add_edge 用于绘制普通的关键帧之间的连接边。
4）四自由度优化函数
void PoseGraph::optimize4DoF()
该函数是位姿图优化的入口函数，用于对优化队列 optimize_buf 中的关键帧进行四自由度（4DoF）优化。四自由度优化包括平移（x, y, z）和偏航角（yaw），忽略俯仰角（pitch）和横滚角（roll）。
---------------------------细节的下一章专门梳理-------------------------------------