| 887 | } |
| 888 | |
| 889 | void PoseGraph::updateKeyFrameLoop(int index, Eigen::Matrix<double, 8, 1 > &_loop_info) |
| 890 | { |
| 891 | KeyFrame* kf = getKeyFrame(index); |
| 892 | kf->updateLoop(_loop_info); |
| 893 | if (abs(_loop_info(7)) < 30.0 && Vector3d(_loop_info(0), _loop_info(1), _loop_info(2)).norm() < 20.0) |
| 894 | { |
| 895 | if (FAST_RELOCALIZATION) |
| 896 | { |
| 897 | KeyFrame* old_kf = getKeyFrame(kf->loop_index); |
| 898 | Vector3d w_P_old, w_P_cur, vio_P_cur; |
| 899 | Matrix3d w_R_old, w_R_cur, vio_R_cur; |
| 900 | old_kf->getPose(w_P_old, w_R_old); |
| 901 | kf->getVioPose(vio_P_cur, vio_R_cur); |
| 902 | |
| 903 | Vector3d relative_t; |
| 904 | Quaterniond relative_q; |
| 905 | relative_t = kf->getLoopRelativeT(); |
| 906 | relative_q = (kf->getLoopRelativeQ()).toRotationMatrix(); |
| 907 | w_P_cur = w_R_old * relative_t + w_P_old; |
| 908 | w_R_cur = w_R_old * relative_q; |
| 909 | double shift_yaw; |
| 910 | Matrix3d shift_r; |
| 911 | Vector3d shift_t; |
| 912 | shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x(); |
| 913 | shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0)); |
| 914 | shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur; |
| 915 | |
| 916 | m_drift.lock(); |
| 917 | yaw_drift = shift_yaw; |
| 918 | r_drift = shift_r; |
| 919 | t_drift = shift_t; |
| 920 | m_drift.unlock(); |
| 921 | } |
| 922 | } |
| 923 | } |
no test coverage detected