| 610 | } |
| 611 | |
| 612 | void STDescManager::GenerateSTDescsOneTime( |
| 613 | pcl::PointCloud<pcl::PointXYZI>::Ptr &input_cloud, |
| 614 | std::vector<STDesc> &stds_vec) { |
| 615 | // step1, voxelization and plane dection |
| 616 | std::unordered_map<VOXEL_LOC, OctoTree *> voxel_map; |
| 617 | init_voxel_map(input_cloud, voxel_map); |
| 618 | pcl::PointCloud<pcl::PointXYZINormal>::Ptr plane_cloud( |
| 619 | new pcl::PointCloud<pcl::PointXYZINormal>); |
| 620 | getPlane(voxel_map, plane_cloud); |
| 621 | // std::cout << "[Description] planes size:" << plane_cloud->size() << |
| 622 | // std::endl; |
| 623 | // plane_cloud_vec_.push_back(plane_cloud); |
| 624 | current_plane_cloud_.reset(new pcl::PointCloud<pcl::PointXYZINormal>); |
| 625 | current_plane_cloud_ = plane_cloud; |
| 626 | |
| 627 | // step2, build connection for planes in the voxel map |
| 628 | build_connection(voxel_map); |
| 629 | |
| 630 | // step3, extraction corner points |
| 631 | pcl::PointCloud<pcl::PointXYZINormal>::Ptr corner_points( |
| 632 | new pcl::PointCloud<pcl::PointXYZINormal>); |
| 633 | corner_extractor(voxel_map, input_cloud, corner_points); |
| 634 | corner_cloud_vec_.push_back(corner_points); |
| 635 | // std::cout << "[Description] corners size:" << corner_points->size() |
| 636 | // << std::endl; |
| 637 | |
| 638 | // step4, generate stable triangle descriptors |
| 639 | stds_vec.clear(); |
| 640 | build_stdesc(corner_points, stds_vec); |
| 641 | // std::cout << "[Description] stds size:" << stds_vec.size() << std::endl; |
| 642 | |
| 643 | // step5, clear memory |
| 644 | for (auto iter = voxel_map.begin(); iter != voxel_map.end(); iter++) { |
| 645 | delete (iter->second); |
| 646 | } |
| 647 | return; |
| 648 | } |
| 649 | |
| 650 | void STDescManager::SearchCorresSemSTD( |
| 651 | const std::vector<STDesc> &curr_stds_vec, |