| 2287 | } |
| 2288 | |
| 2289 | void STDescManager::naive_selector(const std::vector<STDesc> &curr_stds_vec, |
| 2290 | const std::vector<STDesc> &map_stds_vec) { |
| 2291 | // double match_array[MAX_FRAME_N] = {0}; |
| 2292 | std::vector<std::pair<STDesc, STDesc>> match_vec; |
| 2293 | std::vector<int> match_index_vec; |
| 2294 | |
| 2295 | std::vector<bool> useful_match(curr_stds_vec.size()); |
| 2296 | std::vector<std::vector<size_t>> useful_match_index(curr_stds_vec.size()); |
| 2297 | std::vector<std::vector<STDesc_LOC>> useful_match_position( |
| 2298 | curr_stds_vec.size()); |
| 2299 | std::vector<size_t> index(curr_stds_vec.size()); |
| 2300 | std::vector<Eigen::Vector3i> voxel_round; |
| 2301 | for (int x = -1; x <= 1; x++) { |
| 2302 | for (int y = -1; y <= 1; y++) { |
| 2303 | for (int z = -1; z <= 1; z++) { |
| 2304 | Eigen::Vector3i voxel_inc(x, y, z); |
| 2305 | voxel_round.push_back(voxel_inc); |
| 2306 | } |
| 2307 | } |
| 2308 | } |
| 2309 | |
| 2310 | // speed up matching |
| 2311 | int dis_match_cnt = 0; |
| 2312 | int final_match_cnt = 0; |
| 2313 | #ifdef MP_EN |
| 2314 | omp_set_num_threads(MP_PROC_NUM); |
| 2315 | #pragma omp parallel for |
| 2316 | #endif |
| 2317 | for (size_t i = 0; i < curr_stds_vec.size(); i++) { |
| 2318 | STDesc src_std = curr_stds_vec[i]; |
| 2319 | STDesc_LOC position; |
| 2320 | int best_index = 0; |
| 2321 | STDesc_LOC best_position; |
| 2322 | double dis_threshold = |
| 2323 | src_std.side_length_.norm() * config_setting_.rough_dis_threshold_; |
| 2324 | for (auto voxel_inc : voxel_round) { |
| 2325 | position.x = (int)(src_std.side_length_[0] + voxel_inc[0]); |
| 2326 | position.y = (int)(src_std.side_length_[1] + voxel_inc[1]); |
| 2327 | position.z = (int)(src_std.side_length_[2] + voxel_inc[2]); |
| 2328 | position.a = (int)(src_std.angle_[0]); |
| 2329 | position.b = (int)(src_std.angle_[1]); |
| 2330 | position.c = (int)(src_std.angle_[2]); |
| 2331 | Eigen::Vector3d voxel_center((double)position.x, (double)position.y, |
| 2332 | (double)position.z); |
| 2333 | if ((src_std.side_length_ - voxel_center).norm() < 1.5) { |
| 2334 | auto iter = data_base_.find(position); |
| 2335 | if (iter != data_base_.end()) { |
| 2336 | for (size_t j = 0; j < data_base_[position].size(); j++) { |
| 2337 | double dis = |
| 2338 | (src_std.side_length_ - data_base_[position][j].side_length_) |
| 2339 | .norm(); |
| 2340 | // rough filter with side lengths |
| 2341 | if (dis < dis_threshold) { |
| 2342 | dis_match_cnt++; |
| 2343 | // rough filter with vertex attached info |
| 2344 | double vertex_attach_diff = |
| 2345 | (src_std.vertex_attached_ - |
| 2346 | data_base_[position][j].vertex_attached_) |