VINS_Fusion回环检测学习笔记

回环检测的任务主要是为了检测机器人是否到达之前相同的位置,并且消累计的误差。VINS_Fusion的回环检测和VINS_Mono基本相似。只是loop_fusion中读取的很多参数是直接设定好的。回环部分我们先从pose_graph_node.cpp开始。

一、pose_graph_node.cpp

1.1 读取参数

首先看主函数,主要开头是设置基本的参数和读取.yaml文件和字典文件等。这部分其实没有太多需要理解的,就是简单的读取参数和复制,并且完成了ROS节点的初始化工作。

ros::init(argc, argv, "loop_fusion");
    ros::NodeHandle n("~");
    posegraph.registerPub(n);
    //和mono不同之处是直接赋值
    VISUALIZATION_SHIFT_X = 0;
    VISUALIZATION_SHIFT_Y = 0;
    SKIP_CNT = 0;
    SKIP_DIS = 0;

    if(argc != 2)
    {
        printf("please intput: rosrun loop_fusion loop_fusion_node [config file] \n"
               "for example: rosrun loop_fusion loop_fusion_node "
               "/home/tony-ws1/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml \n");
        return 0;
    }
    
    string config_file = argv[1];
    printf("config_file: %s\n", argv[1]);

    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        std::cerr << "ERROR: Wrong path to settings" << std::endl;
    }
    //设置可视化参数
    cameraposevisual.setScale(0.1);
    cameraposevisual.setLineWidth(0.01);
    
    std::string IMAGE_TOPIC;
    int LOAD_PREVIOUS_POSE_GRAPH;
    //与Mono不同直接不要判断是否回环
    ROW = fsSettings["image_height"];
    COL = fsSettings["image_width"];
    std::string pkg_path = ros::package::getPath("loop_fusion");
    string vocabulary_file = pkg_path + "/../support_files/brief_k10L6.bin";
    cout << "vocabulary_file" << vocabulary_file << endl;
    BRIEF_PATTERN_FILE = pkg_path + "/../support_files/brief_pattern.yml";
    cout << "BRIEF_PATTERN_FILE" << BRIEF_PATTERN_FILE << endl;

    int pn = config_file.find_last_of('/');
    std::string configPath = config_file.substr(0, pn);
    std::string cam0Calib;
    fsSettings["cam0_calib"] >> cam0Calib;
    std::string cam0Path = configPath + "/" + cam0Calib;
    printf("cam calib path: %s\n", cam0Path.c_str());
    // 和前面一样,生成一个相机模型回环读入的yaml文件
    m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(cam0Path.c_str());
    fsSettings["image0_topic"] >> IMAGE_TOPIC;        
    fsSettings["pose_graph_save_path"] >> POSE_GRAPH_SAVE_PATH;
    fsSettings["output_path"] >> VINS_RESULT_PATH;
    fsSettings["save_image"] >> DEBUG_IMAGE;

    LOAD_PREVIOUS_POSE_GRAPH = fsSettings["load_previous_pose_graph"];
    VINS_RESULT_PATH = VINS_RESULT_PATH + "/vio_loop.csv";
    //保存路径文件
    std::ofstream fout(VINS_RESULT_PATH, std::ios::out);
    fout.close();

    int USE_IMU = fsSettings["imu"];
    posegraph.setIMUFlag(USE_IMU);
    fsSettings.release();

    if (LOAD_PREVIOUS_POSE_GRAPH)//如果有先前的位姿图
    {
        printf("load pose graph\n");
        m_process.lock();
        posegraph.loadPoseGraph();
        m_process.unlock();
        printf("load pose graph finish\n");
        load_flag = 1;
    }
    else       
    {
        printf("no previous pose graph\n");
        load_flag = 1;
    }

1.2 回调函数

这部分就是接收前端得到的地图点和特征点的话题,并且完成参数的赋值。

//vio回调函数根据pose_msg中的位姿得到imu位姿和cam位姿
    ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 2000, vio_callback);
    //将image_msg放入到image_buf中,同时更具时间挫检测是否有新的图像序列
    ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 2000, image_callback);
    //图像位姿回调函数,把pose_msg放入到pose_buf
    ros::Subscriber sub_pose = n.subscribe("/vins_estimator/keyframe_pose", 2000, pose_callback);
    //imu的外参回调函数
    ros::Subscriber sub_extrinsic = n.subscribe("/vins_estimator/extrinsic", 2000, extrinsic_callback);
    //地图的点云,存放到point_buf
    ros::Subscriber sub_point = n.subscribe("/vins_estimator/keyframe_point", 2000, point_callback);
    //可视化处理
    ros::Subscriber sub_margin_point = n.subscribe("/vins_estimator/margin_cloud", 2000, margin_point_callback);

这里我们主挑出一个讲解,比如vio_callback这个回调函数,当接收到/usb_cam/image_raw这个话题时,就会调用回调函数。这里设计ROS的话题基本通讯不了解的可以查看我之前写的文章:ROS学习笔记——通讯机制_每日亿学的博客-CSDN博客

这部分的代码也很容易理解,就是定义和话题类型相同的对象,接收数据并且完成赋值

void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
    //ROS_INFO("vio_callback!");
    Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);
    Quaterniond vio_q;
    vio_q.w() = pose_msg->pose.pose.orientation.w;
    vio_q.x() = pose_msg->pose.pose.orientation.x;
    vio_q.y() = pose_msg->pose.pose.orientation.y;
    vio_q.z() = pose_msg->pose.pose.orientation.z;

    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;

    nav_msgs::Odometry odometry;
    odometry.header = pose_msg->header;
    odometry.header.frame_id = "world";
    odometry.pose.pose.position.x = vio_t.x();
    odometry.pose.pose.position.y = vio_t.y();
    odometry.pose.pose.position.z = vio_t.z();
    odometry.pose.pose.orientation.x = vio_q.x();
    odometry.pose.pose.orientation.y = vio_q.y();
    odometry.pose.pose.orientation.z = vio_q.z();
    odometry.pose.pose.orientation.w = vio_q.w();
    pub_odometry_rect.publish(odometry);

    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, pose_msg->header);


}

1.3 创建线程

回环部分的核心内容就是在process这个线程中了。

    std::thread measurement_process;
    std::thread keyboard_command_process;

    measurement_process = std::thread(process);
    keyboard_command_process = std::thread(command);

二、process函数

2.1 时间戳对齐

一开始我们读取了,原始图像、前端的位姿和地图点。但是对应的时间戳并没有对齐,因为我们得到的位姿都是上一个原图处理的,所以位姿和地图点的时间都晚于原图的时间。所以VINS中一开始就是基于时间戳的大小来对齐,如果原图的时间戳大于则删除这帧的图像

if(!image_buf.empty() && !point_buf.empty() && !pose_buf.empty()&&!Mono_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");
            }
            // 上面确保了image_buf <= point_buf && image_buf <= pose_buf
            // 下面根据pose时间找时间戳同步的原图和地图点
            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
                pose_buf.pop();
                while (!pose_buf.empty())   // 清空所有的pose,回环的帧率慢一些没关系
                    pose_buf.pop();
                // 找到对应pose的原图
                while (image_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec())
                {
                    image_buf.pop();
              
                }
                

                Mono_msg = Mono_buf.front();//传递符合的一帧
                if(!Mono_buf.empty())
                    Mono_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();

               
            }

 最近在重新看了这部分,对于时间戳我们可以理解:其中

                        front            back

图像queue:。。。。。

位姿queue:         。 。。。。

地图点:                   。。。。。。

基本就是保持以上的时间顺序来对齐时间戳,对于图像的位姿会判断时间戳是相等还是是第一个大于pose的时间,之后就会将当前帧n-1,n-2...pop掉。地图点则是一直满足要大于位姿点。这样就一直保持各个buf里不会一直堆积数据。

 看到github上有人说内存泄露问题在这,就是简单的限制img_buf的大小。这里buf都有严格的时间戳来管理。不能通过大小来随意pop,所以如果你想减小loop的内存我建议不要在这里动手

2.2 降采样

回环检测频率不需要很高,所以每帧的间隔需要一定的时间。首先一开始有一个简单的降频,SKIP_FIRST_CNT为10,skip_first_cnt加到大于10就向下。但是,skip_first_cnt它没有地方置位为0之后skip_cnt < SKIP_CNT这里在初始化开始skip_cnt和SKIP_CNT都是0意味着skip_cnt要加到溢出才能进入下面内容!这里如果你希望关键帧之间间隔时间加长可以在skip_cnt < SKIP_CNT将skip_first_cnt置位并且更改SKIP_FIRST_CNT大小。

if (skip_first_cnt < SKIP_FIRST_CNT)
            {
                skip_first_cnt++;
                continue;
            }

            if (skip_cnt < SKIP_CNT)// 降频,每隔SKIP_CNT帧处理一次超过SKIP_CNT=0
            {
                skip_cnt++;
                continue;
            }
            else
            {
                skip_cnt = 0;
            }
            // 通过cvbridge得到opencv格式的图像
            cv_bridge::CvImageConstPtr ptr;

完成降采样之后,就是对地图点和位姿进行赋值了,之后给关键帧进行初始化。

三、创建KeyFrame

创建关键帧需时间戳,关键帧序号,位姿、原始图像、地图点、特征点。

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的构造函数,基本就是对关键帧类中的成员变量进行赋值。主要重要的函数为:

computeWindowBRIEFPoint()和computeBRIEFPoint()

// create keyframe online
/**
 * @brief Construct a new Key Frame:: Key Frame object,创建一个KF对象,计算已有特征点的描述子,同时额外提取fast角点并计算描述子
 * 
 * @param[in] _time_stamp KF的时间戳
 * @param[in] _index KF的索引
 * @param[in] _vio_T_w_i vio节点中的位姿
 * @param[in] _vio_R_w_i 
 * @param[in] _image 对应的原图
 * @param[in] _point_3d KF对应VIO节点中的世界坐标
 * @param[in] _point_2d_uv 像素坐标
 * @param[in] _point_2d_norm 归一化相机坐标
 * @param[in] _point_id 地图点的idx
 * @param[in] _sequence 序列号
 */
// create keyframe online
KeyFrame::KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, cv::Mat &_image,
		           vector &_point_3d, vector &_point_2d_uv, vector &_point_2d_norm,
		           vector &_point_id, int _sequence)
{
	time_stamp = _time_stamp;//传递时间戳  单目相机的时间戳使用cam0的时间戳
	index = _index;	//索引
	vio_T_w_i = _vio_T_w_i;
	vio_R_w_i = _vio_R_w_i;		//转化到转化到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;	//地图点的idx
	has_loop = false;	// 注意默认还没有检测到回环 这里可以修改是否产生回环
	loop_index = -1;	//和那一帧发生回环
	has_fast_point = false;
	loop_info << 0, 0, 0, 0, 0, 0, 0, 0;
	sequence = _sequence;		//注意序列号暂时和单目相机同步 
	computeWindowBRIEFPoint();	//计算描述子   因为mono没有前端所以不提取特征点描述子
	computeBRIEFPoint();		//提取fast角点和描述子
	if(!DEBUG_IMAGE)
	{
		image.release();
	}
		
}

3.1 计算关键帧描述子

这里就是计算之前对齐时间戳的特征点进行描述子提取,BRIEF_PATTERN_FILE在main函数参数读取时已经玩出赋值。之后将之前的角点转化为opencv的keypoint类,之后调用extractor的仿函数(如果不太了解这个可以理解为把类的名字当作函数来使用,其实就是重构了()运算符号)。

/**
 * @brief 计算已有特征点的描述子
 * 
 */
void KeyFrame::computeWindowBRIEFPoint()
{
	// 定义一个描述子计算的对象
	BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());
	//point_2d_uv检测到角点的像素坐标,这循环相当于转化为opencv之后的keypoint
	for(int i = 0; i < (int)point_2d_uv.size(); i++)
	{
	    cv::KeyPoint key;	//转化为opencv的格式
	    key.pt = point_2d_uv[i];	// 像素坐标用来计算描述子
	    window_keypoints.push_back(key);	// 转成opencv格式的特征点坐标
	}
	//注意window_brief_descriptor输出的描述子
	//注意window_keypoints为当前帧的特征点
	extractor(image, window_keypoints, window_brief_descriptors);
}

进入这个仿函数,就是直接调用DBOW的接口函数从而来计算描述子。 

/// @brief 提取描述子
/// @param im 输入图像
/// @param keys 角点
/// @param descriptors 输出的描述子
void BriefExtractor::operator() (const cv::Mat &im, vector &keys, vector &descriptors) const
{
	//调用dbow的接口计算描述子
  m_brief.compute(im, keys, descriptors);
}

3.3  额外提取FAST角点和描述子

VINS中为了防止前端传递特征点数目过少,这里格外检测FAST角点并且计算它的描述子。代码中直接调用opencv中的cv::FAST检测,并且将检测的角点归一化存储。

// 额外提取fast特征点并计算描述子
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);//提取fast特征点
	else
	{
		vector tmp_pts;
		cv::goodFeaturesToTrack(image, tmp_pts, 500, 0.01, 10);
		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);
	}
}

这里就完成了关键帧的初始化。之后开始检测回环。

四、添加关键帧

4.1      addKeyFrame

        我们构建出了关键帧之后,就需要添加。这个函数输入为关键帧的指针,和是否检测回环默认flag_detect_loop=true。

void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)        

 首先要获取当前帧的平移和旋转,但是以防传递来的不是当前的时序。这里因为loop是独立的什么时候启动停止都可以。如果不是一个时序的我们就复位一些变量。

    Vector3d vio_P_cur;
    Matrix3d vio_R_cur;
    if (sequence_cnt != cur_kf->sequence)//发生了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();
    }

 通过Keyfram中的函数获取当前帧在时间坐标系下的旋转和平移。这里vio_P_cur和vio_R_cur我们要搞清楚是word->i帧下的坐标,更新到i+1下的旋转和平移。还要更新好当前关键帧中的位姿。为了记录关键帧序号和个数global_index会随着添加的关键帧一直叠加。

    // 更新一下VIO位姿
    cur_kf->getVioPose(vio_P_cur, vio_R_cur);//从word->到vio坐标
    //update 回环修正后的消除累计误差的位姿
    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);    // 更新VIO位姿
    cur_kf->index = global_index;   // 赋值索引
    global_index++;
	int loop_index = -1;    //重置回环索引号

 4.2 detectLoop

        这个函数输入关键帧,和关键帧的当前序号,返回int类型,如果检测到回环则返回当前帧的序号。

loop_index = detectLoop(cur_kf, cur_kf->index);

其实这里的步骤很简单,我简化如下:

//step1 调用词袋查询接口,查询结果是ret,最多返回4个备选KF,查找距离当前至少50帧的KF

db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);

//step2 在这里不断加入描述子

db.add(keyframe->brief_descriptors);

 通过上面几步我们就可以得到候选的4个关键帧,这里选取的是当前序号的后50帧。所以这里其实可以释放回环的内存!!但是我尝试还是存在问题,关键帧因为需要优化所以不能随意释放

//step3 ret按照得分大小降序排列的,这里确保返回的候选KF数目至少一个且得分满足要求


    //注意 保证其中的一个关键帧的得分要大于0.05 最少备选项大于1
    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)
            // 如果有大于一个候选帧且得分满足要求
            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);
                }
            }

        }

这里希望,有一个关键帧的得分要大于0.05,当然作者希望其他候选关键帧也能大于0.015。因为如果当前为回环,附近的图像也应该有和当前帧有一定的相似程度

//step4 认为找到了回环并且当前帧是第50帧以后


    if (find_loop && frame_index > 50)
    {
        int min_index = -1;
        // 寻找得分大于0.015的idx最小的那个帧,这样可以尽可能调整更多帧的位姿
        
            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;   //当前帧和min_index形成了回环
    }

这里希望关键帧暂满50帧才检测回环,作者对前50帧一般都在初始化所以不需要回环功能。这里希望找到得分大于0.015的最小帧,这样可以优化到最远的回环帧。如果满足则返回序号,否则返回-1。

4.3 回环校验

        等待更新...

你可能感兴趣的:(Vins-mono,机器人,人工智能,计算机视觉,SLAM)