| 2465 | } |
| 2466 | |
| 2467 | void STDescManager::candidate_selector( |
| 2468 | const std::vector<STDesc> &stds_vec, |
| 2469 | std::vector<STDMatchList> &candidate_matcher_vec) { |
| 2470 | double match_array[MAX_FRAME_N] = {0}; |
| 2471 | std::vector<std::pair<STDesc, STDesc>> match_vec; |
| 2472 | std::vector<int> match_index_vec; |
| 2473 | std::vector<Eigen::Vector3i> voxel_round; |
| 2474 | for (int x = -1; x <= 1; x++) { |
| 2475 | for (int y = -1; y <= 1; y++) { |
| 2476 | for (int z = -1; z <= 1; z++) { |
| 2477 | Eigen::Vector3i voxel_inc(x, y, z); |
| 2478 | voxel_round.push_back(voxel_inc); |
| 2479 | } |
| 2480 | } |
| 2481 | } |
| 2482 | |
| 2483 | std::vector<bool> useful_match(stds_vec.size()); |
| 2484 | std::vector<std::vector<size_t>> useful_match_index(stds_vec.size()); |
| 2485 | std::vector<std::vector<STDesc_LOC>> useful_match_position(stds_vec.size()); |
| 2486 | std::vector<size_t> index(stds_vec.size()); |
| 2487 | for (size_t i = 0; i < index.size(); ++i) { |
| 2488 | index[i] = i; |
| 2489 | useful_match[i] = false; |
| 2490 | } |
| 2491 | // speed up matching |
| 2492 | int dis_match_cnt = 0; |
| 2493 | int final_match_cnt = 0; |
| 2494 | #ifdef MP_EN |
| 2495 | omp_set_num_threads(MP_PROC_NUM); |
| 2496 | #pragma omp parallel for |
| 2497 | #endif |
| 2498 | for (size_t i = 0; i < stds_vec.size(); i++) { |
| 2499 | STDesc src_std = stds_vec[i]; |
| 2500 | STDesc_LOC position; |
| 2501 | int best_index = 0; |
| 2502 | STDesc_LOC best_position; |
| 2503 | double dis_threshold = |
| 2504 | src_std.side_length_.norm() * config_setting_.rough_dis_threshold_; |
| 2505 | for (auto voxel_inc : voxel_round) { |
| 2506 | position.x = (int)(src_std.side_length_[0] + voxel_inc[0]); |
| 2507 | position.y = (int)(src_std.side_length_[1] + voxel_inc[1]); |
| 2508 | position.z = (int)(src_std.side_length_[2] + voxel_inc[2]); |
| 2509 | Eigen::Vector3d voxel_center((double)position.x + 0.5, |
| 2510 | (double)position.y + 0.5, |
| 2511 | (double)position.z + 0.5); |
| 2512 | if ((src_std.side_length_ - voxel_center).norm() < 1.5) { |
| 2513 | auto iter = data_base_.find(position); |
| 2514 | if (iter != data_base_.end()) { |
| 2515 | for (size_t j = 0; j < data_base_[position].size(); j++) { |
| 2516 | if ((src_std.frame_id_ - data_base_[position][j].frame_id_) > |
| 2517 | config_setting_.skip_near_num_) { |
| 2518 | double dis = |
| 2519 | (src_std.side_length_ - data_base_[position][j].side_length_) |
| 2520 | .norm(); |
| 2521 | // rough filter with side lengths |
| 2522 | if (dis < dis_threshold) { |
| 2523 | dis_match_cnt++; |
| 2524 | // rough filter with vertex attached info |