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上看與地圖重合度更高了，且更加平滑。