| 672 | } |
| 673 | |
| 674 | void STDescManager::SearchLoop( |
| 675 | const std::vector<STDesc> &stds_vec, std::pair<int, double> &loop_result, |
| 676 | std::pair<Eigen::Vector3d, Eigen::Matrix3d> &loop_transform, |
| 677 | std::vector<std::pair<STDesc, STDesc>> &loop_std_pair) { |
| 678 | if (stds_vec.size() == 0) { |
| 679 | ROS_ERROR_STREAM("No STDescs!"); |
| 680 | loop_result = std::pair<int, double>(-1, 0); |
| 681 | return; |
| 682 | } |
| 683 | // step1, select candidates, default number 50 |
| 684 | auto t1 = std::chrono::high_resolution_clock::now(); |
| 685 | std::vector<STDMatchList> candidate_matcher_vec; |
| 686 | candidate_selector(stds_vec, candidate_matcher_vec); |
| 687 | |
| 688 | auto t2 = std::chrono::high_resolution_clock::now(); |
| 689 | // step2, select best candidates from rough candidates |
| 690 | double best_score = 0; |
| 691 | unsigned int best_candidate_id = -1; |
| 692 | unsigned int triggle_candidate = -1; |
| 693 | std::pair<Eigen::Vector3d, Eigen::Matrix3d> best_transform; |
| 694 | std::vector<std::pair<STDesc, STDesc>> best_sucess_match_vec; |
| 695 | for (size_t i = 0; i < candidate_matcher_vec.size(); i++) { |
| 696 | double verify_score = -1; |
| 697 | std::pair<Eigen::Vector3d, Eigen::Matrix3d> relative_pose; |
| 698 | std::vector<std::pair<STDesc, STDesc>> sucess_match_vec; |
| 699 | candidate_verify(candidate_matcher_vec[i], verify_score, relative_pose, |
| 700 | sucess_match_vec); |
| 701 | if (verify_score > best_score) { |
| 702 | best_score = verify_score; |
| 703 | best_candidate_id = candidate_matcher_vec[i].match_id_.second; |
| 704 | best_transform = relative_pose; |
| 705 | best_sucess_match_vec = sucess_match_vec; |
| 706 | triggle_candidate = i; |
| 707 | } |
| 708 | } |
| 709 | auto t3 = std::chrono::high_resolution_clock::now(); |
| 710 | |
| 711 | // std::cout << "[Time] candidate selector: " << time_inc(t2, t1) |
| 712 | // << " ms, candidate verify: " << time_inc(t3, t2) << "ms" |
| 713 | // << std::endl; |
| 714 | |
| 715 | if (best_score > config_setting_.icp_threshold_) { |
| 716 | loop_result = std::pair<int, double>(best_candidate_id, best_score); |
| 717 | loop_transform = best_transform; |
| 718 | loop_std_pair = best_sucess_match_vec; |
| 719 | return; |
| 720 | } else { |
| 721 | loop_result = std::pair<int, double>(-1, 0); |
| 722 | return; |
| 723 | } |
| 724 | } |
| 725 | |
| 726 | void STDescManager::AddSTDescs(const std::vector<STDesc> &stds_vec) { |
| 727 | // update frame id |