| 878 | } |
| 879 | |
| 880 | void STDescManager::corner_extractor( |
| 881 | std::unordered_map<VOXEL_LOC, OctoTree *> &voxel_map, |
| 882 | const pcl::PointCloud<pcl::PointXYZI>::Ptr &input_cloud, |
| 883 | pcl::PointCloud<pcl::PointXYZINormal>::Ptr &corner_points) { |
| 884 | pcl::PointCloud<pcl::PointXYZINormal>::Ptr prepare_corner_points( |
| 885 | new pcl::PointCloud<pcl::PointXYZINormal>); |
| 886 | |
| 887 | // Avoid inconsistent voxel cutting caused by different view point |
| 888 | std::vector<Eigen::Vector3i> voxel_round; |
| 889 | for (int x = -1; x <= 1; x++) { |
| 890 | for (int y = -1; y <= 1; y++) { |
| 891 | for (int z = -1; z <= 1; z++) { |
| 892 | Eigen::Vector3i voxel_inc(x, y, z); |
| 893 | voxel_round.push_back(voxel_inc); |
| 894 | } |
| 895 | } |
| 896 | } |
| 897 | for (auto iter = voxel_map.begin(); iter != voxel_map.end(); iter++) { |
| 898 | if (!iter->second->plane_ptr_->is_plane_) { |
| 899 | VOXEL_LOC current_position = iter->first; |
| 900 | OctoTree *current_octo = iter->second; |
| 901 | int connect_index = -1; |
| 902 | for (int i = 0; i < 6; i++) { |
| 903 | if (current_octo->connect_[i]) { |
| 904 | connect_index = i; |
| 905 | OctoTree *connect_octo = current_octo->connect_tree_[connect_index]; |
| 906 | bool use = false; |
| 907 | for (int j = 0; j < 6; j++) { |
| 908 | if (connect_octo->is_check_connect_[j]) { |
| 909 | if (connect_octo->connect_[j]) { |
| 910 | use = true; |
| 911 | } |
| 912 | } |
| 913 | } |
| 914 | // if no plane near the voxel, skip |
| 915 | if (use == false) { |
| 916 | continue; |
| 917 | } |
| 918 | // only project voxels with points num > 10 |
| 919 | if (current_octo->voxel_points_.size() > 10) { |
| 920 | Eigen::Vector3d projection_normal = |
| 921 | current_octo->connect_tree_[connect_index]->plane_ptr_->normal_; |
| 922 | Eigen::Vector3d projection_center = |
| 923 | current_octo->connect_tree_[connect_index]->plane_ptr_->center_; |
| 924 | std::vector<Eigen::Vector3d> proj_points; |
| 925 | // proj the boundary voxel and nearby voxel onto |
| 926 | // adjacent plane |
| 927 | for (auto voxel_inc : voxel_round) { |
| 928 | VOXEL_LOC connect_project_position = current_position; |
| 929 | connect_project_position.x += voxel_inc[0]; |
| 930 | connect_project_position.y += voxel_inc[1]; |
| 931 | connect_project_position.z += voxel_inc[2]; |
| 932 | auto iter_near = voxel_map.find(connect_project_position); |
| 933 | if (iter_near != voxel_map.end()) { |
| 934 | bool skip_flag = false; |
| 935 | if (!voxel_map[connect_project_position] |
| 936 | ->plane_ptr_->is_plane_) { |
| 937 | if (voxel_map[connect_project_position]->is_project_) { |