VINS-mono 位姿图 重利用测试

VINS-mono 位姿图 重利用测试

2018年03月20日 15:43:49 Watkinstord 阅读数：938

在前一篇博文里介绍了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’，帮助回环检测及重定位。

1. if (LOAD_PREVIOUS_POSE_GRAPH)

2. {

3. printf("load pose graph\n");

4. m_process.lock();

5. posegraph.loadPoseGraph();

6. m_process.unlock();

7. printf("load pose graph finish\n");

8. load_flag = 1;

9. }

1. void PoseGraph::addKeyFrameIntoVoc(KeyFrame* keyframe)

2. {

3. // put image into image_pool; for visualization

4. cv::Mat compressed_image;

5. if (DEBUG_IMAGE)

6. {

7. int feature_num = keyframe->keypoints.size();

8. cv::resize(keyframe->image, compressed_image, cv::Size(376, 240));

9. putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255));

10. image_pool[keyframe->index] = compressed_image;

11. }

12. db.add(keyframe->brief_descriptors);

13. }

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

快速重定位选项

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

1. if(FAST_RELOCALIZATION)

2. {

3. sensor_msgs::PointCloud msg_match_points;

4. msg_match_points.header.stamp = ros::Time(time_stamp);

5. for (int i = 0; i < (int)matched_2d_old_norm.size(); i++)

6. {

7. geometry_msgs::Point32 p;

8. p.x = matched_2d_old_norm[i].x;

9. p.y = matched_2d_old_norm[i].y;

10. p.z = matched_id[i];

11. msg_match_points.points.push_back(p);

12. }

13. Eigen::Vector3d T = old_kf->T_w_i;

14. Eigen::Matrix3d R = old_kf->R_w_i;

15. Quaterniond Q(R);

16. sensor_msgs::ChannelFloat32 t_q_index;

17. t_q_index.values.push_back(T.x());

18. t_q_index.values.push_back(T.y());

19. t_q_index.values.push_back(T.z());

20. t_q_index.values.push_back(Q.w());

21. t_q_index.values.push_back(Q.x());

22. t_q_index.values.push_back(Q.y());

23. t_q_index.values.push_back(Q.z());

24. t_q_index.values.push_back(index);

25. msg_match_points.channels.push_back(t_q_index);

26. pub_match_points.publish(msg_match_points);

27. }

1. if (FAST_RELOCALIZATION)

2. {

3. KeyFrame* old_kf = getKeyFrame(kf->loop_index);

4. Vector3d w_P_old, w_P_cur, vio_P_cur;

5. Matrix3d w_R_old, w_R_cur, vio_R_cur;

6. old_kf->getPose(w_P_old, w_R_old);

7. kf->getVioPose(vio_P_cur, vio_R_cur);

8.  

9. Vector3d relative_t;

10. Quaterniond relative_q;

11. relative_t = kf->getLoopRelativeT();

12. relative_q = (kf->getLoopRelativeQ()).toRotationMatrix();

13. w_P_cur = w_R_old * relative_t + w_P_old;

14. w_R_cur = w_R_old * relative_q;

15. double shift_yaw;

16. Matrix3d shift_r;

17. Vector3d shift_t;

18. shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();

19. shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));

20. shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur;

21.  

22. m_drift.lock();

23. yaw_drift = shift_yaw;

24. r_drift = shift_r;

25. t_drift = shift_t;

26. m_drift.unlock();

27. }

测试图像

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