| 796 | } |
| 797 | |
| 798 | void STDescManager::build_connection( |
| 799 | std::unordered_map<VOXEL_LOC, OctoTree *> &voxel_map) { |
| 800 | for (auto iter = voxel_map.begin(); iter != voxel_map.end(); iter++) { |
| 801 | if (iter->second->plane_ptr_->is_plane_) { |
| 802 | OctoTree *current_octo = iter->second; |
| 803 | for (int i = 0; i < 6; i++) { |
| 804 | VOXEL_LOC neighbor = iter->first; |
| 805 | if (i == 0) { |
| 806 | neighbor.x = neighbor.x + 1; |
| 807 | } else if (i == 1) { |
| 808 | neighbor.y = neighbor.y + 1; |
| 809 | } else if (i == 2) { |
| 810 | neighbor.z = neighbor.z + 1; |
| 811 | } else if (i == 3) { |
| 812 | neighbor.x = neighbor.x - 1; |
| 813 | } else if (i == 4) { |
| 814 | neighbor.y = neighbor.y - 1; |
| 815 | } else if (i == 5) { |
| 816 | neighbor.z = neighbor.z - 1; |
| 817 | } |
| 818 | auto near = voxel_map.find(neighbor); |
| 819 | if (near == voxel_map.end()) { |
| 820 | current_octo->is_check_connect_[i] = true; |
| 821 | current_octo->connect_[i] = false; |
| 822 | } else { |
| 823 | if (!current_octo->is_check_connect_[i]) { |
| 824 | OctoTree *near_octo = near->second; |
| 825 | current_octo->is_check_connect_[i] = true; |
| 826 | int j; |
| 827 | if (i >= 3) { |
| 828 | j = i - 3; |
| 829 | } else { |
| 830 | j = i + 3; |
| 831 | } |
| 832 | near_octo->is_check_connect_[j] = true; |
| 833 | if (near_octo->plane_ptr_->is_plane_) { |
| 834 | // merge near octo |
| 835 | Eigen::Vector3d normal_diff = current_octo->plane_ptr_->normal_ - |
| 836 | near_octo->plane_ptr_->normal_; |
| 837 | Eigen::Vector3d normal_add = current_octo->plane_ptr_->normal_ + |
| 838 | near_octo->plane_ptr_->normal_; |
| 839 | if (normal_diff.norm() < |
| 840 | config_setting_.plane_merge_normal_thre_ || |
| 841 | normal_add.norm() < |
| 842 | config_setting_.plane_merge_normal_thre_) { |
| 843 | current_octo->connect_[i] = true; |
| 844 | near_octo->connect_[j] = true; |
| 845 | current_octo->connect_tree_[i] = near_octo; |
| 846 | near_octo->connect_tree_[j] = current_octo; |
| 847 | } else { |
| 848 | current_octo->connect_[i] = false; |
| 849 | near_octo->connect_[j] = false; |
| 850 | } |
| 851 | } else { |
| 852 | current_octo->connect_[i] = false; |
| 853 | near_octo->connect_[j] = true; |
| 854 | near_octo->connect_tree_[j] = current_octo; |
| 855 | } |