| 724 | } |
| 725 | |
| 726 | void STDescManager::AddSTDescs(const std::vector<STDesc> &stds_vec) { |
| 727 | // update frame id |
| 728 | current_frame_id_++; |
| 729 | for (auto single_std : stds_vec) { |
| 730 | // calculate the position of single std |
| 731 | STDesc_LOC position; |
| 732 | position.x = (int)(single_std.side_length_[0] + 0.5); |
| 733 | position.y = (int)(single_std.side_length_[1] + 0.5); |
| 734 | position.z = (int)(single_std.side_length_[2] + 0.5); |
| 735 | position.a = (int)(single_std.angle_[0]); |
| 736 | position.b = (int)(single_std.angle_[1]); |
| 737 | position.c = (int)(single_std.angle_[2]); |
| 738 | auto iter = data_base_.find(position); |
| 739 | if (iter != data_base_.end()) { |
| 740 | data_base_[position].push_back(single_std); |
| 741 | } else { |
| 742 | std::vector<STDesc> descriptor_vec; |
| 743 | descriptor_vec.push_back(single_std); |
| 744 | data_base_[position] = descriptor_vec; |
| 745 | } |
| 746 | } |
| 747 | return; |
| 748 | } |
| 749 | |
| 750 | void STDescManager::init_voxel_map( |
| 751 | const pcl::PointCloud<pcl::PointXYZI>::Ptr &input_cloud, |