| 44 | } |
| 45 | |
| 46 | void PoseGraph::addKFToPoseGraph(Keyframe* cur_kf, bool flag_detect_loop) { |
| 47 | // shift to base frame |
| 48 | Eigen::Vector3d svin_P_cur; |
| 49 | Eigen::Matrix3d svin_R_cur; |
| 50 | if (sequence_cnt != cur_kf->sequence) { |
| 51 | sequence_cnt++; |
| 52 | sequence_loop.push_back(0); |
| 53 | w_t_svin = Eigen::Vector3d(0, 0, 0); |
| 54 | w_r_svin = Eigen::Matrix3d::Identity(); |
| 55 | |
| 56 | { |
| 57 | std::lock_guard<std::mutex> l(driftMutex_); |
| 58 | t_drift = Eigen::Vector3d(0, 0, 0); |
| 59 | r_drift = Eigen::Matrix3d::Identity(); |
| 60 | } |
| 61 | } |
| 62 | |
| 63 | cur_kf->getSVInPose(svin_P_cur, svin_R_cur); |
| 64 | svin_P_cur = w_r_svin * svin_P_cur + w_t_svin; |
| 65 | svin_R_cur = w_r_svin * svin_R_cur; |
| 66 | cur_kf->updateSVInPose(svin_P_cur, svin_R_cur); |
| 67 | cur_kf->index = global_index; |
| 68 | global_index++; |
| 69 | std::set<Keyframe*> loopCandidates; |
| 70 | int loop_index = -1; |
| 71 | |
| 72 | if (flag_detect_loop) { // at least 20 KF has been passed |
| 73 | loop_index = detectLoop(cur_kf, cur_kf->index); |
| 74 | } else { |
| 75 | db.add(cur_kf->brief_descriptors); |
| 76 | } |
| 77 | |
| 78 | if (loop_index != -1) { |
| 79 | Keyframe* old_kf = getKFPtr(loop_index); |
| 80 | if (cur_kf->findConnection(old_kf)) { |
| 81 | if (earliest_loop_index > loop_index || earliest_loop_index == -1) earliest_loop_index = loop_index; |
| 82 | |
| 83 | Eigen::Vector3d w_P_old, w_P_cur, svin_P_cur; |
| 84 | Eigen::Matrix3d w_R_old, w_R_cur, svin_R_cur; |
| 85 | old_kf->getSVInPose(w_P_old, w_R_old); // old_kf replaced by min_loop_kf |
| 86 | cur_kf->getSVInPose(svin_P_cur, svin_R_cur); |
| 87 | |
| 88 | Eigen::Vector3d relative_t; |
| 89 | Eigen::Quaterniond relative_q; |
| 90 | relative_t = cur_kf->getLoopRelativeT(); |
| 91 | relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix(); |
| 92 | w_P_cur = w_R_old * relative_t + w_P_old; |
| 93 | w_R_cur = w_R_old * relative_q; |
| 94 | double shift_yaw; |
| 95 | Eigen::Matrix3d shift_r; |
| 96 | Eigen::Vector3d shift_t; |
| 97 | shift_yaw = Utils::R2ypr(w_R_cur).x() - Utils::R2ypr(svin_R_cur).x(); |
| 98 | shift_r = Utils::ypr2R(Eigen::Vector3d(shift_yaw, 0, 0)); |
| 99 | shift_t = w_P_cur - w_R_cur * svin_R_cur.transpose() * svin_P_cur; |
| 100 | // shift svin pose of whole sequence to the world frame |
| 101 | if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0) { |
| 102 | w_r_svin = shift_r; |
| 103 | w_t_svin = shift_t; |
no test coverage detected