| 543 | } |
| 544 | |
| 545 | void PoseGraph::updatePath() { |
| 546 | std::lock_guard<std::mutex> l(kflistMutex_); |
| 547 | |
| 548 | std::list<Keyframe*>::iterator it; |
| 549 | |
| 550 | std::vector<std::pair<Timestamp, Eigen::Matrix4d>> loop_closure_path; |
| 551 | std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> loop_closure_edges; |
| 552 | |
| 553 | for (it = keyframelist.begin(); it != keyframelist.end(); it++) { |
| 554 | Eigen::Vector3d P; |
| 555 | Eigen::Matrix3d R; |
| 556 | (*it)->getPose(P, R); |
| 557 | Eigen::Quaterniond Q{R}; |
| 558 | |
| 559 | std::pair<Timestamp, Eigen::Matrix4d> pose; |
| 560 | pose.first = (*it)->time_stamp; |
| 561 | pose.second.block<3, 3>(0, 0) = R; |
| 562 | pose.second.block<3, 1>(0, 3) = P; |
| 563 | loop_closure_path.push_back(pose); |
| 564 | |
| 565 | if ((*it)->has_loop) { |
| 566 | Keyframe* connected_KF = getKFPtr((*it)->loop_index); |
| 567 | Eigen::Vector3d connected_P; |
| 568 | Eigen::Matrix3d connected_R; |
| 569 | connected_KF->getPose(connected_P, connected_R); |
| 570 | (*it)->getPose(P, R); |
| 571 | loop_closure_edges.push_back({P, connected_P}); |
| 572 | } |
| 573 | } |
| 574 | |
| 575 | CHECK(loop_closure_callback_); |
| 576 | loop_closure_callback_(loop_closure_path, loop_closure_edges); |
| 577 | } |
| 578 | |
| 579 | void PoseGraph::updateKeyFrameLoop(int index, Eigen::Matrix<double, 8, 1>& _loop_info) { |
| 580 | Keyframe* kf = getKFPtr(index); |