| 2105 | }; |
| 2106 | |
| 2107 | void STDescManager::semantic_selector(const std::vector<STDesc> &curr_stds_vec, |
| 2108 | const std::vector<STDesc> &map_stds_vec) { |
| 2109 | // double match_array[MAX_FRAME_N] = {0}; |
| 2110 | std::vector<std::pair<STDesc, STDesc>> match_vec; |
| 2111 | std::vector<int> match_index_vec; |
| 2112 | |
| 2113 | std::vector<bool> useful_match(curr_stds_vec.size()); |
| 2114 | std::vector<std::vector<size_t>> useful_match_index(curr_stds_vec.size()); |
| 2115 | std::vector<std::vector<STDesc_LOC>> useful_match_position( |
| 2116 | curr_stds_vec.size()); |
| 2117 | std::vector<size_t> index(curr_stds_vec.size()); |
| 2118 | std::vector<Eigen::Vector3i> voxel_round; |
| 2119 | for (int x = -1; x <= 1; x++) { |
| 2120 | for (int y = -1; y <= 1; y++) { |
| 2121 | for (int z = -1; z <= 1; z++) { |
| 2122 | Eigen::Vector3i voxel_inc(x, y, z); |
| 2123 | voxel_round.push_back(voxel_inc); |
| 2124 | } |
| 2125 | } |
| 2126 | } |
| 2127 | |
| 2128 | // speed up matching |
| 2129 | int dis_match_cnt = 0; |
| 2130 | int final_match_cnt = 0; |
| 2131 | #ifdef MP_EN |
| 2132 | omp_set_num_threads(MP_PROC_NUM); |
| 2133 | #pragma omp parallel for |
| 2134 | #endif |
| 2135 | for (size_t i = 0; i < curr_stds_vec.size(); i++) { |
| 2136 | STDesc src_std = curr_stds_vec[i]; |
| 2137 | STDesc_LOC position; |
| 2138 | int best_index = 0; |
| 2139 | STDesc_LOC best_position; |
| 2140 | double dis_threshold = |
| 2141 | src_std.side_length_.norm() * config_setting_.rough_dis_threshold_; |
| 2142 | for (auto voxel_inc : voxel_round) { |
| 2143 | position.x = (int)(src_std.side_length_[0] + voxel_inc[0]); |
| 2144 | position.y = (int)(src_std.side_length_[1] + voxel_inc[1]); |
| 2145 | position.z = (int)(src_std.side_length_[2] + voxel_inc[2]); |
| 2146 | position.a = (int)(src_std.angle_[0]); |
| 2147 | position.b = (int)(src_std.angle_[1]); |
| 2148 | position.c = (int)(src_std.angle_[2]); |
| 2149 | Eigen::Vector3d voxel_center((double)position.x, (double)position.y, |
| 2150 | (double)position.z); |
| 2151 | if ((src_std.side_length_ - voxel_center).norm() < 1.5) { |
| 2152 | auto iter = data_base_.find(position); |
| 2153 | if (iter != data_base_.end()) { |
| 2154 | for (size_t j = 0; j < data_base_[position].size(); j++) { |
| 2155 | double dis = |
| 2156 | (src_std.side_length_ - data_base_[position][j].side_length_) |
| 2157 | .norm(); |
| 2158 | // rough filter with side lengths |
| 2159 | if (dis < dis_threshold) { |
| 2160 | dis_match_cnt++; |
| 2161 | // rough filter with vertex attached info |
| 2162 | double vertex_attach_diff = |
| 2163 | (src_std.vertex_attached_ - |
| 2164 | data_base_[position][j].vertex_attached_) |