最近接到一个任务，在激光检测回环失败时，比如黑色物体多，场景大等，可否利用视觉进行回环检测。如果只是检测回环，现有的许多框架都可以使用。ORB-SLAM本身就有单目模式，且效果不错。但是发现ORB在检测回环时，必须要进行pose计算，产生地图点，然后根据地图点和回环之间的关系进行回环检测。这样就比较耗费资源，可否只检测回环，并不计算位姿与地图点。然后想到VINS也是有单目检测回环功能，就着手从VINS开始。
1. feature_tracker模块
这部分模块无需较大改动，只需要在节点里增加激光数据触发信号，让激光关键帧与图象帧保持数据同步
2. estimator模块
此部分会融合IMU做pose计算，所以这个节点在回环检测中不需要使用。需要在launch文件中，删去此节点，不运行。
3. pose_graph模块
3.1 此部分为回环检测核心模块
首先修改订阅的topic，只需要订阅图像与feature_tracker发布的特征点信息。
ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 2000, image_callback);
ros::Subscriber sub_point = n.subscribe("/feature_tracker/feature", 2000, point_callback);
3.2 修改KeyFrame构造函数
以前的需要特征点对应的3D点，此时为了减少计算量，只需要特征点图像坐标与归一化坐标，以及特征点光流平均速度
KeyFrame::KeyFrame(double _time_stamp, int _index, cv::Mat &_image,vector<cv::Point2f> &_point_2d_uv, vector<cv::Point2f> &_point_2d_normal, double _aver_velocity, int _sequence)
{
time_stamp = _time_stamp;
index = _index;
image = _image.clone();
cv::resize(image, thumbnail, cv::Size(80, 60));
std::cout<<"this is resize"<<std::endl;
point_2d_uv = _point_2d_uv;
point_2d_norm = _point_2d_normal;
has_loop = false;
loop_index = -1;
has_fast_point = false;
loop_info << 0, 0, 0, 0, 0, 0, 0, 0;
sequence = _sequence;
computeWindowBRIEFPoint();
computeBRIEFPoint();
aver_velocity = _aver_velocity;
//这里可以把图像清除了，但是为了显示图像，可以暂且留着
}
3.3 改动的还有addkeyFrame
为了减少计算量，当机器人静止时，可以不需要进行回环检测，这里判断条件就是特征点的平均光溜速度，对于这个阈值可以根据最小视差来决定。
void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)
{
//先判断是否关键帧
std::cout<<"this is function of addKeyFrame"<<std::endl;
if(cur_kf->aver_velocity<=0.2&&keyframelist.size()>0)
{
std::cout<<"loop index unchanged"<<last_loop_index<<std::endl;
}else{
if (sequence_cnt != cur_kf->sequence)
{
sequence_cnt++;
sequence_loop.push_back(0);
}
cur_kf->index = global_index;
last_index= global_index;
int loop_index = -1;
if (flag_detect_loop)
{
TicToc tmp_t;
loop_index = detectLoop(cur_kf, cur_kf->index);
last_loop_index = loop_index;
std::cout<<"loop index is "<<loop_index<<std::endl;
KeyFrame * old_kf = getKeyFrame(loop_index);
if(loop_index !=-1)
{
if(cur_kf->findConnection(old_kf))
{
std::cout<<"this is true loop with index of : "<< loop_index<<std::endl;
}
}
}
global_index++;
keyframelist.push_back(cur_kf);
}
}
detectLoop函数没有做改动，对与findConnection函数做了如下改动。一开始以为可以不需要findConnection函数，直接用detectLoop得出的结果就能使用了，但是发现误匹配很多，VINS之前是使用
PnPRANSAC(matched_2d_old_norm, matched_3d, status, PnP_T_old, PnP_R_old);
函数进行两帧之间的真实相似度检测。但是此函数需要3D点，且计算了相对位姿，并不适合我的任务，正好VINS的老版本仅仅使用了两帧图像的2d点进行验证，虽然精度略微降低，但是无需3D点，减少了计算量。
bool KeyFrame::findConnection(KeyFrame* old_kf)
{
std::cout<<"this is function of findConnection"<<std::endl;
TicToc tmp_t;
//printf("find Connection
");
vector<cv::Point2f> matched_2d_cur, matched_2d_old;
vector<cv::Point2f> matched_2d_cur_norm, matched_2d_old_norm;
vector<uchar> status;
matched_2d_cur = point_2d_uv;
matched_2d_cur_norm = point_2d_norm;
TicToc t_match;
std::cout<<"old_kf keypoint : "<<old_kf->keypoints.size()<<std::endl;
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);
//printf("search by des finish
");
status.clear();
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
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);
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), 2, 8, 0);
}
cv::Mat notation(50, COL + gap + COL, CV_8UC3, cv::Scalar(255, 255, 255));
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);
cv::vconcat(notation, loop_match_img, loop_match_img);
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
cv::imshow("loop connection",loop_match_img);
cv::waitKey(100);
return true;
}
}
//printf("loop final use num %d %lf---------------
", (int)matched_2d_cur.size(), t_match.toc());
return false;
}
效果如图所示。
