| 284 | |
| 285 | template<typename PointT> |
| 286 | inline |
| 287 | void PatchWork<PointT>::estimate_plane_(const pcl::PointCloud<PointT> &ground) { |
| 288 | pcl::computeMeanAndCovarianceMatrix(ground, cov_, pc_mean_); |
| 289 | // Singular Value Decomposition: SVD |
| 290 | Eigen::JacobiSVD<Eigen::MatrixXf> svd(cov_, Eigen::DecompositionOptions::ComputeFullU); |
| 291 | singular_values_ = svd.singularValues(); |
| 292 | |
| 293 | // use the least singular vector as normal |
| 294 | normal_ = (svd.matrixU().col(2)); |
| 295 | // mean ground seeds value |
| 296 | Eigen::Vector3f seeds_mean = pc_mean_.head<3>(); |
| 297 | |
| 298 | // according to normal.T*[x,y,z] = -d |
| 299 | d_ = -(normal_.transpose() * seeds_mean)(0, 0); |
| 300 | // set distance threhold to `th_dist - d` |
| 301 | th_dist_d_ = th_dist_ - d_; |
| 302 | } |
| 303 | |
| 304 | template<typename PointT> |
| 305 | inline |
nothing calls this directly
no outgoing calls
no test coverage detected