VINS-mono 位姿图 重利用测试

在前一篇博文里介绍了VINS-mono pose_graph reuse功能的使用，这里接着贴出一些延伸的测试，并进行一些探讨。

延伸测试

一般来说，加载地图是进行非GPS定位必要的一步。这里根据新的VINS公开的代码，以EuRoC的MH_01_为例，贴上rviz的效果图。在不加载地图的情况下，使能回环检测，得到无地图情况下的位姿估计如下图。图中绿色为估计的位姿图路径，红色的为真值。

加载上一次的存储好的MH_01轨迹的位姿图，这个图是需要定位的路径重合度很高的地图。图中黄色的为加载的上一次得到的位姿图路径，存储在pose_graph.txt中。绿色的本次估计得到的位姿图路径，红色的为真值。

当加载与当前路径重合度不高的地图时，其位姿图估计效果如下图所示。其中黄色轨迹为加载的之前存储的MH_03_Medium.bag轨迹的位姿图。绿色的当前MH_01_easy路径的位姿估计轨迹，红色的为MH_01_easy序列对应的轨迹真值。

可以看出在地图与轨迹重合度不高的情况下，有可能会使估计的位姿轨迹变差。产生的原因，应该还是在部分轨迹区间出现了错误的回环估计，将位姿估计轨迹误差拉大了。

加载地图的代码 

将关键点及对应的Brief描述子添加到DBow2的数据库中‘db’，帮助回环检测及重定位。

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;
        }

void PoseGraph::addKeyFrameIntoVoc(KeyFrame* keyframe)
{
    // 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[keyframe->index] = compressed_image;
    }
    db.add(keyframe->brief_descriptors);
}

这里地图帧应该都被作为关键帧使用。

快速重定位选项

配置文件中说为了实现更好的实时和在大项目中的性能，增加 FAST_RELOCALIZATION 选项，相关代码

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);
			    pub_match_points.publish(msg_match_points);
	    	}

        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();
        }

测试图像

FAST_RELOCALIZATION 快速重定位选项在加载地图选项之后，也在回环检测配置这组里，可见与加载地图的重定位相关。在使能这一选项之后，位姿估计从RVIZ上看与地图重合度更高了，且更加平滑。