| 2943 | } |
| 2944 | |
| 2945 | void OctoTree::init_plane() { |
| 2946 | plane_ptr_->covariance_ = Eigen::Matrix3d::Zero(); |
| 2947 | plane_ptr_->center_ = Eigen::Vector3d::Zero(); |
| 2948 | plane_ptr_->normal_ = Eigen::Vector3d::Zero(); |
| 2949 | plane_ptr_->points_size_ = voxel_points_.size(); |
| 2950 | plane_ptr_->radius_ = 0; |
| 2951 | for (auto pi : voxel_points_) { |
| 2952 | plane_ptr_->covariance_ += pi * pi.transpose(); |
| 2953 | plane_ptr_->center_ += pi; |
| 2954 | } |
| 2955 | plane_ptr_->center_ = plane_ptr_->center_ / plane_ptr_->points_size_; |
| 2956 | plane_ptr_->covariance_ = |
| 2957 | plane_ptr_->covariance_ / plane_ptr_->points_size_ - |
| 2958 | plane_ptr_->center_ * plane_ptr_->center_.transpose(); |
| 2959 | Eigen::EigenSolver<Eigen::Matrix3d> es(plane_ptr_->covariance_); |
| 2960 | Eigen::Matrix3cd evecs = es.eigenvectors(); |
| 2961 | Eigen::Vector3cd evals = es.eigenvalues(); |
| 2962 | Eigen::Vector3d evalsReal; |
| 2963 | evalsReal = evals.real(); |
| 2964 | Eigen::Matrix3d::Index evalsMin, evalsMax; |
| 2965 | evalsReal.rowwise().sum().minCoeff(&evalsMin); |
| 2966 | evalsReal.rowwise().sum().maxCoeff(&evalsMax); |
| 2967 | int evalsMid = 3 - evalsMin - evalsMax; |
| 2968 | if (evalsReal(evalsMin) < config_setting_.plane_detection_thre_) { |
| 2969 | plane_ptr_->normal_ << evecs.real()(0, evalsMin), evecs.real()(1, evalsMin), |
| 2970 | evecs.real()(2, evalsMin); |
| 2971 | plane_ptr_->min_eigen_value_ = evalsReal(evalsMin); |
| 2972 | plane_ptr_->radius_ = sqrt(evalsReal(evalsMax)); |
| 2973 | plane_ptr_->is_plane_ = true; |
| 2974 | |
| 2975 | plane_ptr_->intercept_ = -(plane_ptr_->normal_(0) * plane_ptr_->center_(0) + |
| 2976 | plane_ptr_->normal_(1) * plane_ptr_->center_(1) + |
| 2977 | plane_ptr_->normal_(2) * plane_ptr_->center_(2)); |
| 2978 | plane_ptr_->p_center_.x = plane_ptr_->center_(0); |
| 2979 | plane_ptr_->p_center_.y = plane_ptr_->center_(1); |
| 2980 | plane_ptr_->p_center_.z = plane_ptr_->center_(2); |
| 2981 | plane_ptr_->p_center_.normal_x = plane_ptr_->normal_(0); |
| 2982 | plane_ptr_->p_center_.normal_y = plane_ptr_->normal_(1); |
| 2983 | plane_ptr_->p_center_.normal_z = plane_ptr_->normal_(2); |
| 2984 | } else { |
| 2985 | plane_ptr_->is_plane_ = false; |
| 2986 | } |
| 2987 | } |
| 2988 | |
| 2989 | void OctoTree::init_octo_tree() { |
| 2990 | if (voxel_points_.size() > config_setting_.voxel_init_num_) { |