40 #ifndef PCL_SURFACE_IMPL_MLS_H_ 41 #define PCL_SURFACE_IMPL_MLS_H_ 43 #include <pcl/type_traits.h> 44 #include <pcl/surface/mls.h> 45 #include <pcl/common/io.h> 47 #include <pcl/common/copy_point.h> 49 #include <pcl/common/eigen.h> 51 #include <pcl/search/kdtree.h> 52 #include <pcl/search/organized.h> 59 template <
typename Po
intInT,
typename Po
intOutT>
void 70 normals_->header = input_->header;
72 normals_->width = normals_->height = 0;
73 normals_->points.clear ();
77 output.
header = input_->header;
81 if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
83 PCL_ERROR (
"[pcl::%s::process] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
88 if (upsample_method_ == DISTINCT_CLOUD && !distinct_cloud_)
90 PCL_ERROR (
"[pcl::%s::process] Upsample method was set to DISTINCT_CLOUD, but no distinct cloud was specified.\n", getClassName ().c_str ());
101 if (input_->isOrganized ())
105 setSearchMethod (tree);
109 tree_->setInputCloud (input_);
111 switch (upsample_method_)
114 case (RANDOM_UNIFORM_DENSITY):
116 std::random_device rd;
118 const double tmp = search_radius_ / 2.0;
119 rng_uniform_distribution_.reset (
new std::uniform_real_distribution<> (-tmp, tmp));
123 case (VOXEL_GRID_DILATION):
124 case (DISTINCT_CLOUD):
126 if (!cache_mls_results_)
127 PCL_WARN (
"The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n");
129 cache_mls_results_ =
true;
136 if (cache_mls_results_)
138 mls_results_.resize (input_->size ());
142 mls_results_.resize (1);
146 performProcessing (output);
148 if (compute_normals_)
150 normals_->height = 1;
151 normals_->width = normals_->size ();
153 for (std::size_t i = 0; i < output.
size (); ++i)
155 using FieldList =
typename pcl::traits::fieldList<PointOutT>::type;
172 template <
typename Po
intInT,
typename Po
intOutT>
void 174 const std::vector<int> &nn_indices,
183 mls_result.
computeMLSSurface<PointInT> (*input_, index, nn_indices, search_radius_, order_);
185 switch (upsample_method_)
190 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
194 case (SAMPLE_LOCAL_PLANE):
197 for (
float u_disp = -static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
198 for (
float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
199 if (u_disp * u_disp + v_disp * v_disp < upsampling_radius_ * upsampling_radius_)
202 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
207 case (RANDOM_UNIFORM_DENSITY):
210 const int num_points_to_add =
static_cast<int> (std::floor (desired_num_points_in_radius_ / 2.0 / static_cast<double> (nn_indices.size ())));
213 if (num_points_to_add <= 0)
217 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
222 for (
int num_added = 0; num_added < num_points_to_add;)
224 const double u = (*rng_uniform_distribution_) (rng_);
225 const double v = (*rng_uniform_distribution_) (rng_);
228 if (u * u + v * v > search_radius_ * search_radius_ / 4)
237 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
250 template <
typename Po
intInT,
typename Po
intOutT>
void 252 const Eigen::Vector3d &point,
253 const Eigen::Vector3d &normal,
260 aux.x =
static_cast<float> (point[0]);
261 aux.y =
static_cast<float> (point[1]);
262 aux.z =
static_cast<float> (point[2]);
265 copyMissingFields ((*input_)[index], aux);
268 corresponding_input_indices.
indices.push_back (index);
270 if (compute_normals_)
273 aux_normal.normal_x =
static_cast<float> (normal[0]);
274 aux_normal.normal_y =
static_cast<float> (normal[1]);
275 aux_normal.normal_z =
static_cast<float> (normal[2]);
277 projected_points_normals.
push_back (aux_normal);
282 template <
typename Po
intInT,
typename Po
intOutT>
void 286 nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
290 const unsigned int threads = threads_ == 0 ? 1 : threads_;
294 std::vector<PointIndices> corresponding_input_indices (threads);
298 #pragma omp parallel for \ 300 shared(corresponding_input_indices, projected_points, projected_points_normals) \ 301 schedule(dynamic,1000) \ 303 for (
int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
307 std::vector<int> nn_indices;
308 std::vector<float> nn_sqr_dists;
311 if (searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
314 if (nn_indices.size () >= 3)
318 const int tn = omp_get_thread_num ();
320 std::size_t pp_size = projected_points[tn].size ();
327 const int index = (*indices_)[cp];
329 std::size_t mls_result_index = 0;
330 if (cache_mls_results_)
331 mls_result_index = index;
334 computeMLSPointNormal (index, nn_indices, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], mls_results_[mls_result_index]);
337 for (std::size_t pp = pp_size; pp < projected_points[tn].
size (); ++pp)
338 copyMissingFields ((*input_)[(*indices_)[cp]], projected_points[tn][pp]);
340 computeMLSPointNormal (index, nn_indices, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]);
343 output.
insert (output.
end (), projected_points.
begin (), projected_points.
end ());
344 if (compute_normals_)
345 normals_->insert (normals_->end (), projected_points_normals.
begin (), projected_points_normals.
end ());
353 for (
unsigned int tn = 0; tn < threads; ++tn)
355 output.
insert (output.
end (), projected_points[tn].begin (), projected_points[tn].end ());
356 corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (),
357 corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ());
358 if (compute_normals_)
359 normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ());
364 performUpsampling (output);
368 template <
typename Po
intInT,
typename Po
intOutT>
void 372 if (upsample_method_ == DISTINCT_CLOUD)
375 for (std::size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i)
378 if (!std::isfinite ((*distinct_cloud_)[dp_i].x))
383 std::vector<int> nn_indices;
384 std::vector<float> nn_dists;
385 tree_->nearestKSearch ((*distinct_cloud_)[dp_i], 1, nn_indices, nn_dists);
386 int input_index = nn_indices.front ();
390 if (mls_results_[input_index].valid ==
false)
393 Eigen::Vector3d add_point = (*distinct_cloud_)[dp_i].getVector3fMap ().template cast<double> ();
395 addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
401 if (upsample_method_ == VOXEL_GRID_DILATION)
405 MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);
406 for (
int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
409 for (
typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.
voxel_grid_.begin (); m_it != voxel_grid.
voxel_grid_.end (); ++m_it)
420 std::vector<int> nn_indices;
421 std::vector<float> nn_dists;
422 tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
423 int input_index = nn_indices.front ();
427 if (mls_results_[input_index].valid ==
false)
430 Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> ();
432 addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
439 const Eigen::Vector3d &a_mean,
440 const Eigen::Vector3d &a_plane_normal,
441 const Eigen::Vector3d &a_u,
442 const Eigen::Vector3d &a_v,
443 const Eigen::VectorXd &a_c_vec,
444 const int a_num_neighbors,
445 const float a_curvature,
447 query_point (a_query_point), mean (a_mean), plane_normal (a_plane_normal), u_axis (a_u), v_axis (a_v), c_vec (a_c_vec), num_neighbors (a_num_neighbors),
448 curvature (a_curvature), order (a_order), valid (true)
454 Eigen::Vector3d delta = pt - mean;
455 u = delta.dot (u_axis);
456 v = delta.dot (v_axis);
457 w = delta.dot (plane_normal);
463 Eigen::Vector3d delta = pt - mean;
464 u = delta.dot (u_axis);
465 v = delta.dot (v_axis);
476 for (
int ui = 0; ui <= order; ++ui)
479 for (
int vi = 0; vi <= order - ui; ++vi)
481 result += c_vec[j++] * u_pow * v_pow;
496 Eigen::VectorXd u_pow (order + 2), v_pow (order + 2);
499 d.z = d.z_u = d.z_v = d.z_uu = d.z_vv = d.z_uv = 0;
500 u_pow (0) = v_pow (0) = 1;
501 for (
int ui = 0; ui <= order; ++ui)
503 for (
int vi = 0; vi <= order - ui; ++vi)
506 d.z += u_pow (ui) * v_pow (vi) * c_vec[j];
510 d.z_u += c_vec[j] * ui * u_pow (ui - 1) * v_pow (vi);
513 d.z_v += c_vec[j] * vi * u_pow (ui) * v_pow (vi - 1);
515 if (ui >= 1 && vi >= 1)
516 d.z_uv += c_vec[j] * ui * u_pow (ui - 1) * vi * v_pow (vi - 1);
519 d.z_uu += c_vec[j] * ui * (ui - 1) * u_pow (ui - 2) * v_pow (vi);
522 d.z_vv += c_vec[j] * vi * (vi - 1) * u_pow (ui) * v_pow (vi - 2);
525 v_pow (vi + 1) = v_pow (vi) * v;
529 u_pow (ui + 1) = u_pow (ui) * u;
538 Eigen::Vector2f k (1e-5, 1e-5);
544 if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
548 const double Zlen = std::sqrt (Z);
551 const double disc2 = H * H -
K;
552 assert (disc2 >= 0.0);
553 const double disc = std::sqrt (disc2);
557 if (std::abs (k[0]) > std::abs (k[1])) std::swap (k[0], k[1]);
561 PCL_ERROR (
"No Polynomial fit data, unable to calculate the principle curvatures!\n");
575 result.
normal = plane_normal;
576 if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
581 const double dist1 = std::abs (gw - w);
585 double e1 = (gu - u) + d.
z_u * gw - d.
z_u * w;
586 double e2 = (gv - v) + d.
z_v * gw - d.
z_v * w;
594 Eigen::MatrixXd J (2, 2);
600 Eigen::Vector2d err (e1, e2);
601 Eigen::Vector2d update = J.inverse () * err;
605 d = getPolynomialPartialDerivative (gu, gv);
607 dist2 = std::sqrt ((gu - u) * (gu - u) + (gv - v) * (gv - v) + (gw - w) * (gw - w));
609 err_total = std::sqrt (e1 * e1 + e2 * e2);
611 }
while (err_total > 1e-8 && dist2 < dist1);
617 d = getPolynomialPartialDerivative (u, v);
624 result.
normal.normalize ();
627 result.
point = mean + gu * u_axis + gv * v_axis + gw * plane_normal;
638 result.
normal = plane_normal;
639 result.
point = mean + u * u_axis + v * v_axis;
652 result.
normal = plane_normal;
654 if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
659 result.
normal.normalize ();
662 result.
point = mean + u * u_axis + v * v_axis + w * plane_normal;
671 getMLSCoordinates (pt, u, v, w);
674 if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
676 if (method == ORTHOGONAL)
677 proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
679 proj = projectPointSimpleToPolynomialSurface (u, v);
683 proj = projectPointToMLSPlane (u, v);
693 if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
695 if (method == ORTHOGONAL)
698 getMLSCoordinates (query_point, u, v, w);
699 proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
704 proj.
point = mean + (c_vec[0] * plane_normal);
707 proj.
normal = plane_normal - c_vec[order + 1] * u_axis - c_vec[1] * v_axis;
713 proj.
normal = plane_normal;
720 template <
typename Po
intT>
void 723 const std::vector<int> &nn_indices,
724 double search_radius,
725 int polynomial_order,
726 std::function<
double(
const double)> weight_func)
729 EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix;
730 Eigen::Vector4d xyz_centroid;
737 EIGEN_ALIGN16 Eigen::Vector3d::Scalar eigen_value;
738 EIGEN_ALIGN16 Eigen::Vector3d eigen_vector;
739 Eigen::Vector4d model_coefficients (0, 0, 0, 0);
740 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
741 model_coefficients.head<3> ().matrix () = eigen_vector;
742 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
744 query_point = cloud[index].getVector3fMap ().template cast<double> ();
746 if (!std::isfinite(eigen_vector[0]) || !std::isfinite(eigen_vector[1]) || !std::isfinite(eigen_vector[2]))
757 const double distance = query_point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
758 mean = query_point -
distance * model_coefficients.head<3> ();
760 curvature = covariance_matrix.trace ();
763 curvature = std::abs (eigen_value / curvature);
766 plane_normal = model_coefficients.head<3> ();
769 v_axis = plane_normal.unitOrthogonal ();
770 u_axis = plane_normal.cross (v_axis);
774 num_neighbors =
static_cast<int> (nn_indices.size ());
775 order = polynomial_order;
778 const int nr_coeff = (order + 1) * (order + 2) / 2;
780 if (num_neighbors >= nr_coeff)
783 weight_func = [=] (
const double sq_dist) {
return this->computeMLSWeight (sq_dist, search_radius * search_radius); };
786 Eigen::VectorXd weight_vec (num_neighbors);
787 Eigen::MatrixXd P (nr_coeff, num_neighbors);
788 Eigen::VectorXd f_vec (num_neighbors);
789 Eigen::MatrixXd P_weight_Pt (nr_coeff, nr_coeff);
793 std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > de_meaned (num_neighbors);
794 for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
796 de_meaned[ni][0] = cloud[nn_indices[ni]].x - mean[0];
797 de_meaned[ni][1] = cloud[nn_indices[ni]].y - mean[1];
798 de_meaned[ni][2] = cloud[nn_indices[ni]].z - mean[2];
799 weight_vec (ni) = weight_func (de_meaned[ni].dot (de_meaned[ni]));
804 for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
807 const double u_coord = de_meaned[ni].dot(u_axis);
808 const double v_coord = de_meaned[ni].dot(v_axis);
809 f_vec (ni) = de_meaned[ni].dot (plane_normal);
814 for (
int ui = 0; ui <= order; ++ui)
817 for (
int vi = 0; vi <= order - ui; ++vi)
819 P (j++, ni) = u_pow * v_pow;
827 const Eigen::MatrixXd P_weight = P * weight_vec.asDiagonal();
828 P_weight_Pt = P_weight * P.transpose ();
829 c_vec = P_weight * f_vec;
830 P_weight_Pt.llt ().solveInPlace (c_vec);
836 template <
typename Po
intInT,
typename Po
intOutT>
840 voxel_grid_ (), data_size_ (), voxel_size_ (voxel_size)
845 const double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
848 for (std::size_t i = 0; i < indices->size (); ++i)
849 if (std::isfinite ((*cloud)[(*indices)[i]].x))
852 getCellIndex ((*cloud)[(*indices)[i]].getVector3fMap (), pos);
862 template <
typename Po
intInT,
typename Po
intOutT>
void 865 HashMap new_voxel_grid = voxel_grid_;
866 for (
typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid_.begin (); m_it != voxel_grid_.end (); ++m_it)
868 Eigen::Vector3i index;
869 getIndexIn3D (m_it->first, index);
872 for (
int x = -1; x <= 1; ++x)
873 for (
int y = -1; y <= 1; ++y)
874 for (
int z = -1; z <= 1; ++z)
875 if (x != 0 || y != 0 || z != 0)
877 Eigen::Vector3i new_index;
878 new_index = index + Eigen::Vector3i (x, y, z);
881 getIndexIn1D (new_index, index_1d);
883 new_voxel_grid[index_1d] = leaf;
886 voxel_grid_ = new_voxel_grid;
891 template <
typename Po
intInT,
typename Po
intOutT>
void 893 PointOutT &point_out)
const 895 PointOutT temp = point_out;
897 point_out.x = temp.x;
898 point_out.y = temp.y;
899 point_out.z = temp.z;
902 #define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares<T,OutT>; 903 #define PCL_INSTANTIATE_MovingLeastSquaresOMP(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquaresOMP<T,OutT>; 905 #endif // PCL_SURFACE_IMPL_MLS_H_ A point structure representing normal coordinates and the surface curvature estimate.
Data structure used to store the MLS polynomial partial derivatives.
void addProjectedPointNormal(int index, const Eigen::Vector3d &point, const Eigen::Vector3d &normal, double curvature, PointCloudOut &projected_points, NormalCloud &projected_points_normals, PointIndices &corresponding_input_indices) const
This is a helper function for add projected points.
double z_u
The partial derivative dz/du.
typename PointCloudIn::ConstPtr PointCloudInConstPtr
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
A helper functor that can set a specific value in a field if the field exists.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
double u
The u-coordinate of the projected point in local MLS frame.
Eigen::Vector2f calculatePrincipleCurvatures(const double u, const double v) const
Calculate the principle curvatures using the polynomial surface.
iterator begin() noexcept
PolynomialPartialDerivative getPolynomialPartialDerivative(const double u, const double v) const
Calculate the polynomial's first and second partial derivatives.
shared_ptr< Indices > IndicesPtr
double z_vv
The partial derivative d^2z/dv^2.
Defines some geometrical functions and utility functions.
Eigen::Vector4f bounding_max_
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
double z_uv
The partial derivative d^2z/dudv.
void copyMissingFields(const PointInT &point_in, PointOutT &point_out) const
MLSProjectionResults projectQueryPoint(ProjectionMethod method, int required_neighbors=0) const
Project the query point used to generate the mls surface about using the specified method...
void getPosition(const std::uint64_t &index_1d, Eigen::Vector3f &point) const
void process(PointCloudOut &output) override
Base method for surface reconstruction for all points given in <setInputCloud (), setIndices ()> ...
std::map< std::uint64_t, Leaf > HashMap
Eigen::Vector4f bounding_min_
MLSProjectionResults projectPointSimpleToPolynomialSurface(const double u, const double v) const
Project a point along the MLS plane normal to the polynomial surface.
MLSProjectionResults projectPoint(const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors=0) const
Project a point using the specified method.
double z_uu
The partial derivative d^2z/du^2.
std::uint32_t width
The point cloud width (if organized as an image-structure).
Define standard C methods and C++ classes that are common to all methods.
Data structure used to store the MLS projection results.
Eigen::Vector3d point
The projected point.
double v
The u-coordinate of the projected point in local MLS frame.
double z_v
The partial derivative dz/dv.
Eigen::Vector3d normal
The projected point's normal.
void getMLSCoordinates(const Eigen::Vector3d &pt, double &u, double &v, double &w) const
Given a point calculate it's 3D location in the MLS frame.
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.
Data structure used to store the results of the MLS fitting.
int num_neighbors
The number of neighbors used to create the mls surface.
void performUpsampling(PointCloudOut &output)
Perform upsampling for the distinct-cloud and voxel-grid methods.
std::vector< PointCloud< PointOutT >, Eigen::aligned_allocator< PointCloud< PointOutT > > > CloudVectorType
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud...
float distance(const PointT &p1, const PointT &p2)
std::uint32_t height
The point cloud height (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
void performProcessing(PointCloudOut &output) override
Abstract surface reconstruction method.
A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling.
MLSVoxelGrid(PointCloudInConstPtr &cloud, IndicesPtr &indices, float voxel_size)
void computeMLSSurface(const pcl::PointCloud< PointT > &cloud, int index, const std::vector< int > &nn_indices, double search_radius, int polynomial_order=2, std::function< double(const double)> weight_func={})
Smooth a given point and its neighborghood using Moving Least Squares.
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
float curvature
The curvature at the query point.
MLSProjectionResults projectPointOrthogonalToPolynomialSurface(const double u, const double v, const double w) const
Project a point orthogonal to the polynomial surface.
double getPolynomialValue(const double u, const double v) const
Calculate the polynomial.
void computeMLSPointNormal(int index, const std::vector< int > &nn_indices, PointCloudOut &projected_points, NormalCloud &projected_points_normals, PointIndices &corresponding_input_indices, MLSResult &mls_result) const
Smooth a given point and its neighborghood using Moving Least Squares.
double z
The z component of the polynomial evaluated at z(u, v).
MLSProjectionResults projectPointToMLSPlane(const double u, const double v) const
Project a point onto the MLS plane.
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
void getIndexIn1D(const Eigen::Vector3i &index, std::uint64_t &index_1d) const
typename KdTree::Ptr KdTreePtr
void getCellIndex(const Eigen::Vector3f &p, Eigen::Vector3i &index) const
Define methods for centroid estimation and covariance matrix calculus.