55 if (num_samples_ > indices_->size ())
57 PCL_ERROR (
"[pcl::CovarianceSampling::initCompute] The number of samples you asked for (%d) is larger than the number of input indices (%lu)\n",
58 num_samples_, indices_->size ());
64 Eigen::Vector3f centroid (0.f, 0.f, 0.f);
66 centroid += (*input_)[(*indices_)[
p_i]].getVector3fMap ();
67 centroid /=
float (indices_->size ());
69 scaled_points_.resize (indices_->size ());
73 scaled_points_[
p_i] = (*input_)[(*indices_)[
p_i]].getVector3fMap () - centroid;
77 for (std::size_t
p_i = 0;
p_i < scaled_points_.
size (); ++
p_i)
115 Eigen::Matrix<double, 6, Eigen::Dynamic>
f_mat = Eigen::Matrix<double, 6, Eigen::Dynamic> (6, indices_->size ());
116 for (std::size_t
p_i = 0;
p_i < scaled_points_.
size (); ++
p_i)
118 f_mat.block<3, 1> (0,
p_i) = scaled_points_[
p_i].cross (
119 (*input_normals_)[(*indices_)[
p_i]].getNormalVector3fMap ()).
template cast<double> ();
120 f_mat.block<3, 1> (3,
p_i) = (*input_normals_)[(*indices_)[
p_i]].getNormalVector3fMap ().template
cast<double> ();
132 Eigen::Matrix<double, 6, 6>
c_mat;
137 const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6> >
solver (
c_mat);
138 const Eigen::Matrix<double, 6, 6> x =
solver.eigenvectors ();
148 using Vector6d = Eigen::Matrix<double, 6, 1>;
149 std::vector<Vector6d, Eigen::aligned_allocator<Vector6d> > v;
153 v[
p_i].block<3, 1> (0, 0) = scaled_points_[
p_i].cross (
160 std::vector<std::list<std::pair<int, double> > > L;
163 for (std::size_t i = 0; i < 6; ++i)
166 L[i].push_back (std::make_pair (
p_i, std::abs (v[
p_i].dot (x.block<6, 1> (0, i)))));
169 L[i].sort (sort_dot_list_function);
173 std::vector<double>
t (6, 0.0);
182 for (std::size_t i = 0; i < 6; ++i)
197 for (std::size_t i = 0; i < 6; ++i)
unsigned int computeCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.