| 861 | } |
| 862 | |
| 863 | void STDescManager::getPlane( |
| 864 | const std::unordered_map<VOXEL_LOC, OctoTree *> &voxel_map, |
| 865 | pcl::PointCloud<pcl::PointXYZINormal>::Ptr &plane_cloud) { |
| 866 | for (auto iter = voxel_map.begin(); iter != voxel_map.end(); iter++) { |
| 867 | if (iter->second->plane_ptr_->is_plane_) { |
| 868 | pcl::PointXYZINormal pi; |
| 869 | pi.x = iter->second->plane_ptr_->center_[0]; |
| 870 | pi.y = iter->second->plane_ptr_->center_[1]; |
| 871 | pi.z = iter->second->plane_ptr_->center_[2]; |
| 872 | pi.normal_x = iter->second->plane_ptr_->normal_[0]; |
| 873 | pi.normal_y = iter->second->plane_ptr_->normal_[1]; |
| 874 | pi.normal_z = iter->second->plane_ptr_->normal_[2]; |
| 875 | plane_cloud->push_back(pi); |
| 876 | } |
| 877 | } |
| 878 | } |
| 879 | |
| 880 | void STDescManager::corner_extractor( |
| 881 | std::unordered_map<VOXEL_LOC, OctoTree *> &voxel_map, |