| 577 | } |
| 578 | |
| 579 | void PoseGraph::updateKeyFrameLoop(int index, Eigen::Matrix<double, 8, 1>& _loop_info) { |
| 580 | Keyframe* kf = getKFPtr(index); |
| 581 | kf->updateLoop(_loop_info); |
| 582 | if (abs(_loop_info(7)) < 30.0 && Eigen::Vector3d(_loop_info(0), _loop_info(1), _loop_info(2)).norm() < 20.0) { |
| 583 | if (is_fast_localization_) { |
| 584 | Keyframe* old_kf = getKFPtr(kf->loop_index); |
| 585 | Eigen::Vector3d w_P_old, w_P_cur, svin_P_cur; |
| 586 | Eigen::Matrix3d w_R_old, w_R_cur, svin_R_cur; |
| 587 | old_kf->getPose(w_P_old, w_R_old); |
| 588 | kf->getSVInPose(svin_P_cur, svin_R_cur); |
| 589 | |
| 590 | Eigen::Vector3d relative_t; |
| 591 | Eigen::Quaterniond relative_q; |
| 592 | relative_t = kf->getLoopRelativeT(); |
| 593 | relative_q = (kf->getLoopRelativeQ()).toRotationMatrix(); |
| 594 | w_P_cur = w_R_old * relative_t + w_P_old; |
| 595 | w_R_cur = w_R_old * relative_q; |
| 596 | double shift_yaw; |
| 597 | Eigen::Matrix3d shift_r; |
| 598 | Eigen::Vector3d shift_t; |
| 599 | shift_yaw = Utils::R2ypr(w_R_cur).x() - Utils::R2ypr(svin_R_cur).x(); |
| 600 | shift_r = Utils::ypr2R(Eigen::Vector3d(shift_yaw, 0, 0)); |
| 601 | shift_t = w_P_cur - w_R_cur * svin_R_cur.transpose() * svin_P_cur; |
| 602 | |
| 603 | { |
| 604 | std::lock_guard<std::mutex> l(driftMutex_); |
| 605 | yaw_drift = shift_yaw; |
| 606 | r_drift = shift_r; |
| 607 | t_drift = shift_t; |
| 608 | } |
| 609 | } |
| 610 | } |
| 611 | } |
| 612 | |
| 613 | void PoseGraph::setLoopClosureOptimizationCallback(const EventCallback& optimization_finish_callback) { |
| 614 | loop_closure_optimization_callback_ = optimization_finish_callback; |
nothing calls this directly
no test coverage detected