Get the best candidate frame by geometry check
| 2603 | |
| 2604 | // Get the best candidate frame by geometry check |
| 2605 | void STDescManager::candidate_verify( |
| 2606 | const STDMatchList &candidate_matcher, double &verify_score, |
| 2607 | std::pair<Eigen::Vector3d, Eigen::Matrix3d> &relative_pose, |
| 2608 | std::vector<std::pair<STDesc, STDesc>> &sucess_match_vec) { |
| 2609 | sucess_match_vec.clear(); |
| 2610 | int skip_len = (int)(candidate_matcher.match_list_.size() / 50) + 1; |
| 2611 | int use_size = candidate_matcher.match_list_.size() / skip_len; |
| 2612 | double dis_threshold = 3.0; |
| 2613 | std::vector<size_t> index(use_size); |
| 2614 | std::vector<int> vote_list(use_size); |
| 2615 | for (size_t i = 0; i < index.size(); i++) { |
| 2616 | index[i] = i; |
| 2617 | } |
| 2618 | std::mutex mylock; |
| 2619 | |
| 2620 | #ifdef MP_EN |
| 2621 | omp_set_num_threads(MP_PROC_NUM); |
| 2622 | #pragma omp parallel for |
| 2623 | #endif |
| 2624 | for (size_t i = 0; i < use_size; i++) { |
| 2625 | auto single_pair = candidate_matcher.match_list_[i * skip_len]; |
| 2626 | int vote = 0; |
| 2627 | Eigen::Matrix3d test_rot; |
| 2628 | Eigen::Vector3d test_t; |
| 2629 | triangle_solver(single_pair, test_t, test_rot); |
| 2630 | for (size_t j = 0; j < candidate_matcher.match_list_.size(); j++) { |
| 2631 | auto verify_pair = candidate_matcher.match_list_[j]; |
| 2632 | Eigen::Vector3d A = verify_pair.first.vertex_A_; |
| 2633 | Eigen::Vector3d A_transform = test_rot * A + test_t; |
| 2634 | Eigen::Vector3d B = verify_pair.first.vertex_B_; |
| 2635 | Eigen::Vector3d B_transform = test_rot * B + test_t; |
| 2636 | Eigen::Vector3d C = verify_pair.first.vertex_C_; |
| 2637 | Eigen::Vector3d C_transform = test_rot * C + test_t; |
| 2638 | double dis_A = (A_transform - verify_pair.second.vertex_A_).norm(); |
| 2639 | double dis_B = (B_transform - verify_pair.second.vertex_B_).norm(); |
| 2640 | double dis_C = (C_transform - verify_pair.second.vertex_C_).norm(); |
| 2641 | if (dis_A < dis_threshold && dis_B < dis_threshold && |
| 2642 | dis_C < dis_threshold) { |
| 2643 | vote++; |
| 2644 | } |
| 2645 | } |
| 2646 | mylock.lock(); |
| 2647 | vote_list[i] = vote; |
| 2648 | mylock.unlock(); |
| 2649 | } |
| 2650 | int max_vote_index = 0; |
| 2651 | int max_vote = 0; |
| 2652 | for (size_t i = 0; i < vote_list.size(); i++) { |
| 2653 | if (max_vote < vote_list[i]) { |
| 2654 | max_vote_index = i; |
| 2655 | max_vote = vote_list[i]; |
| 2656 | } |
| 2657 | } |
| 2658 | if (max_vote >= 4) { |
| 2659 | auto best_pair = candidate_matcher.match_list_[max_vote_index * skip_len]; |
| 2660 | int vote = 0; |
| 2661 | Eigen::Matrix3d best_rot; |
| 2662 | Eigen::Vector3d best_t; |