在前一篇博文里介绍了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上看与地图重合度更高了，且更加平滑。
