| 394 | |
| 395 | template <typename PointT> |
| 396 | inline void PatchWork<PointT>::estimate_plane_(const pcl::PointCloud<PointT> &ground, |
| 397 | PCAFeature &feat) { |
| 398 | Eigen::Vector4f pc_mean; |
| 399 | Eigen::Matrix3f cov; |
| 400 | pcl::computeMeanAndCovarianceMatrix(ground, cov, pc_mean); |
| 401 | |
| 402 | // Singular Value Decomposition: SVD |
| 403 | Eigen::JacobiSVD<Eigen::MatrixXf> svd(cov, Eigen::DecompositionOptions::ComputeFullU); |
| 404 | feat.singular_values_ = svd.singularValues(); |
| 405 | |
| 406 | feat.linearity_ = |
| 407 | (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); |
| 408 | feat.planarity_ = |
| 409 | (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); |
| 410 | |
| 411 | // use the least singular vector as normal |
| 412 | feat.normal_ = (svd.matrixU().col(2)); |
| 413 | if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive |
| 414 | feat.normal_ = -feat.normal_; |
| 415 | } |
| 416 | // mean ground seeds value |
| 417 | feat.mean_ = pc_mean.head<3>(); |
| 418 | // according to normal.T*[x,y,z] = -d |
| 419 | feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); |
| 420 | feat.th_dist_d_ = th_dist_ - feat.d_; |
| 421 | } |
| 422 | |
| 423 | template <typename PointT> |
| 424 | inline void PatchWork<PointT>::extract_initial_seeds_(const pcl::PointCloud<PointT> &p_sorted, |
nothing calls this directly
no outgoing calls
no test coverage detected