40 #ifndef PCL_SURFACE_IMPL_MLS_H_
41 #define PCL_SURFACE_IMPL_MLS_H_
43 #include <pcl/common/centroid.h>
44 #include <pcl/common/common.h>
45 #include <pcl/common/copy_point.h>
46 #include <pcl/common/eigen.h>
47 #include <pcl/search/kdtree.h>
48 #include <pcl/search/organized.h>
49 #include <pcl/surface/mls.h>
50 #include <pcl/type_traits.h>
52 #include <Eigen/Geometry>
62 template <
typename Po
intInT,
typename Po
intOutT>
void
73 normals_->header = input_->header;
75 normals_->width = normals_->height = 0;
76 normals_->points.clear ();
80 output.
header = input_->header;
84 if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
86 PCL_ERROR (
"[pcl::%s::process] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
91 if (upsample_method_ == DISTINCT_CLOUD && !distinct_cloud_)
93 PCL_ERROR (
"[pcl::%s::process] Upsample method was set to DISTINCT_CLOUD, but no distinct cloud was specified.\n", getClassName ().c_str ());
104 if (input_->isOrganized ())
108 setSearchMethod (tree);
112 tree_->setInputCloud (input_);
114 switch (upsample_method_)
117 case (RANDOM_UNIFORM_DENSITY):
119 std::random_device rd;
121 const double tmp = search_radius_ / 2.0;
122 rng_uniform_distribution_ = std::make_unique<std::uniform_real_distribution<>> (-tmp, tmp);
126 case (VOXEL_GRID_DILATION):
127 case (DISTINCT_CLOUD):
129 if (!cache_mls_results_)
130 PCL_WARN (
"The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n");
132 cache_mls_results_ =
true;
139 if (cache_mls_results_)
141 mls_results_.resize (input_->size ());
145 mls_results_.resize (1);
149 performProcessing (output);
151 if (compute_normals_)
153 normals_->height = 1;
154 normals_->width = normals_->size ();
156 for (std::size_t i = 0; i < output.
size (); ++i)
158 using FieldList =
typename pcl::traits::fieldList<PointOutT>::type;
175 template <
typename Po
intInT,
typename Po
intOutT>
void
186 mls_result.
computeMLSSurface<PointInT> (*input_, index, nn_indices, search_radius_, order_);
188 switch (upsample_method_)
193 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
197 case (SAMPLE_LOCAL_PLANE):
200 for (
float u_disp = -static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
201 for (
float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
202 if (u_disp * u_disp + v_disp * v_disp < upsampling_radius_ * upsampling_radius_)
205 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
210 case (RANDOM_UNIFORM_DENSITY):
213 const int num_points_to_add =
static_cast<int> (std::floor (desired_num_points_in_radius_ / 2.0 / static_cast<double> (nn_indices.size ())));
216 if (num_points_to_add <= 0)
220 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
225 for (
int num_added = 0; num_added < num_points_to_add;)
227 const double u = (*rng_uniform_distribution_) (rng_);
228 const double v = (*rng_uniform_distribution_) (rng_);
231 if (u * u + v * v > search_radius_ * search_radius_ / 4)
240 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
253 template <
typename Po
intInT,
typename Po
intOutT>
void
255 const Eigen::Vector3d &point,
256 const Eigen::Vector3d &normal,
263 aux.x =
static_cast<float> (point[0]);
264 aux.y =
static_cast<float> (point[1]);
265 aux.z =
static_cast<float> (point[2]);
268 copyMissingFields ((*input_)[index], aux);
271 corresponding_input_indices.
indices.push_back (index);
273 if (compute_normals_)
276 aux_normal.normal_x =
static_cast<float> (normal[0]);
277 aux_normal.normal_y =
static_cast<float> (normal[1]);
278 aux_normal.normal_z =
static_cast<float> (normal[2]);
280 projected_points_normals.
push_back (aux_normal);
285 template <
typename Po
intInT,
typename Po
intOutT>
void
289 nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
293 const unsigned int threads = threads_ == 0 ? 1 : threads_;
297 std::vector<PointIndices> corresponding_input_indices (threads);
301 #pragma omp parallel for \
303 shared(corresponding_input_indices, projected_points, projected_points_normals) \
304 schedule(dynamic,1000) \
306 for (
int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
311 std::vector<float> nn_sqr_dists;
314 if (searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
317 if (nn_indices.size () >= 3)
321 const int tn = omp_get_thread_num ();
323 std::size_t pp_size = projected_points[tn].size ();
330 const int index = (*indices_)[cp];
332 std::size_t mls_result_index = 0;
333 if (cache_mls_results_)
334 mls_result_index = index;
337 computeMLSPointNormal (index, nn_indices, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], mls_results_[mls_result_index]);
340 for (std::size_t pp = pp_size; pp < projected_points[tn].
size (); ++pp)
341 copyMissingFields ((*input_)[(*indices_)[cp]], projected_points[tn][pp]);
343 computeMLSPointNormal (index, nn_indices, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]);
346 output.
insert (output.
end (), projected_points.
begin (), projected_points.
end ());
347 if (compute_normals_)
348 normals_->insert (normals_->end (), projected_points_normals.
begin (), projected_points_normals.
end ());
356 for (
unsigned int tn = 0; tn < threads; ++tn)
358 output.
insert (output.
end (), projected_points[tn].begin (), projected_points[tn].end ());
359 corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (),
360 corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ());
361 if (compute_normals_)
362 normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ());
367 performUpsampling (output);
371 template <
typename Po
intInT,
typename Po
intOutT>
void
375 if (upsample_method_ == DISTINCT_CLOUD)
378 for (std::size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i)
381 if (!std::isfinite ((*distinct_cloud_)[dp_i].x))
387 std::vector<float> nn_dists;
388 tree_->nearestKSearch ((*distinct_cloud_)[dp_i], 1, nn_indices, nn_dists);
389 const auto input_index = nn_indices.front ();
393 if (!mls_results_[input_index].valid)
396 Eigen::Vector3d add_point = (*distinct_cloud_)[dp_i].getVector3fMap ().template cast<double> ();
398 addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
404 if (upsample_method_ == VOXEL_GRID_DILATION)
408 MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_, dilation_iteration_num_);
409 for (
int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
424 std::vector<float> nn_dists;
425 tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
426 const auto input_index = nn_indices.front ();
430 if (!mls_results_[input_index].valid)
433 Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> ();
435 addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
442 const Eigen::Vector3d &a_mean,
443 const Eigen::Vector3d &a_plane_normal,
444 const Eigen::Vector3d &a_u,
445 const Eigen::Vector3d &a_v,
446 const Eigen::VectorXd &a_c_vec,
447 const int a_num_neighbors,
448 const float a_curvature,
450 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),
451 curvature (a_curvature), order (a_order), valid (true)
457 Eigen::Vector3d delta = pt - mean;
458 u = delta.dot (u_axis);
459 v = delta.dot (v_axis);
460 w = delta.dot (plane_normal);
466 Eigen::Vector3d delta = pt - mean;
467 u = delta.dot (u_axis);
468 v = delta.dot (v_axis);
479 for (
int ui = 0; ui <= order; ++ui)
482 for (
int vi = 0; vi <= order - ui; ++vi)
484 result += c_vec[j++] * u_pow * v_pow;
499 Eigen::VectorXd u_pow (order + 2), v_pow (order + 2);
502 d.z = d.z_u = d.z_v = d.z_uu = d.z_vv = d.z_uv = 0;
503 u_pow (0) = v_pow (0) = 1;
504 for (
int ui = 0; ui <= order; ++ui)
506 for (
int vi = 0; vi <= order - ui; ++vi)
509 d.z += u_pow (ui) * v_pow (vi) * c_vec[j];
513 d.z_u += c_vec[j] * ui * u_pow (ui - 1) * v_pow (vi);
516 d.z_v += c_vec[j] * vi * u_pow (ui) * v_pow (vi - 1);
518 if (ui >= 1 && vi >= 1)
519 d.z_uv += c_vec[j] * ui * u_pow (ui - 1) * vi * v_pow (vi - 1);
522 d.z_uu += c_vec[j] * ui * (ui - 1) * u_pow (ui - 2) * v_pow (vi);
525 d.z_vv += c_vec[j] * vi * (vi - 1) * u_pow (ui) * v_pow (vi - 2);
528 v_pow (vi + 1) = v_pow (vi) * v;
532 u_pow (ui + 1) = u_pow (ui) * u;
546 result.
normal = plane_normal;
547 if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
552 const double dist1 = std::abs (gw - w);
556 double e1 = (gu - u) + d.
z_u * gw - d.
z_u * w;
557 double e2 = (gv - v) + d.
z_v * gw - d.
z_v * w;
565 Eigen::MatrixXd J (2, 2);
571 Eigen::Vector2d err (e1, e2);
572 Eigen::Vector2d update = J.inverse () * err;
576 d = getPolynomialPartialDerivative (gu, gv);
578 dist2 = std::sqrt ((gu - u) * (gu - u) + (gv - v) * (gv - v) + (gw - w) * (gw - w));
580 err_total = std::sqrt (e1 * e1 + e2 * e2);
582 }
while (err_total > 1e-8 && dist2 < dist1);
588 d = getPolynomialPartialDerivative (u, v);
595 result.
normal.normalize ();
598 result.
point = mean + gu * u_axis + gv * v_axis + gw * plane_normal;
609 result.
normal = plane_normal;
610 result.
point = mean + u * u_axis + v * v_axis;
623 result.
normal = plane_normal;
625 if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
630 result.
normal.normalize ();
633 result.
point = mean + u * u_axis + v * v_axis + w * plane_normal;
642 getMLSCoordinates (pt, u, v, w);
645 if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
647 if (method == ORTHOGONAL)
648 proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
650 proj = projectPointSimpleToPolynomialSurface (u, v);
654 proj = projectPointToMLSPlane (u, v);
664 if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
666 if (method == ORTHOGONAL)
669 getMLSCoordinates (query_point, u, v, w);
670 proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
675 proj.
point = mean + (c_vec[0] * plane_normal);
678 proj.
normal = plane_normal - c_vec[order + 1] * u_axis - c_vec[1] * v_axis;
684 proj.
normal = plane_normal;
691 template <
typename Po
intT>
void
695 double search_radius,
696 int polynomial_order,
697 std::function<
double(
const double)> weight_func)
700 EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix;
701 Eigen::Vector4d xyz_centroid;
708 EIGEN_ALIGN16 Eigen::Vector3d::Scalar eigen_value;
709 EIGEN_ALIGN16 Eigen::Vector3d eigen_vector;
710 Eigen::Vector4d model_coefficients (0, 0, 0, 0);
711 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
712 model_coefficients.head<3> ().matrix () = eigen_vector;
713 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
715 query_point = cloud[index].getVector3fMap ().template cast<double> ();
717 if (!std::isfinite(eigen_vector[0]) || !std::isfinite(eigen_vector[1]) || !std::isfinite(eigen_vector[2]))
728 const double distance = query_point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
729 mean = query_point - distance * model_coefficients.head<3> ();
731 curvature = covariance_matrix.trace ();
734 curvature = std::abs (eigen_value / curvature);
737 plane_normal = model_coefficients.head<3> ();
740 v_axis = plane_normal.unitOrthogonal ();
741 u_axis = plane_normal.cross (v_axis);
745 num_neighbors =
static_cast<int> (nn_indices.size ());
746 order = polynomial_order;
749 const int nr_coeff = (order + 1) * (order + 2) / 2;
751 if (num_neighbors >= nr_coeff)
754 weight_func = [
this, search_radius] (
const double sq_dist) {
return this->computeMLSWeight (sq_dist, search_radius * search_radius); };
757 Eigen::VectorXd weight_vec (num_neighbors);
758 Eigen::MatrixXd P (nr_coeff, num_neighbors);
759 Eigen::VectorXd f_vec (num_neighbors);
760 Eigen::MatrixXd P_weight_Pt (nr_coeff, nr_coeff);
764 std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > de_meaned (num_neighbors);
765 for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
767 de_meaned[ni][0] = cloud[nn_indices[ni]].x - mean[0];
768 de_meaned[ni][1] = cloud[nn_indices[ni]].y - mean[1];
769 de_meaned[ni][2] = cloud[nn_indices[ni]].z - mean[2];
770 weight_vec (ni) = weight_func (de_meaned[ni].dot (de_meaned[ni]));
775 for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
778 const double u_coord = de_meaned[ni].dot(u_axis);
779 const double v_coord = de_meaned[ni].dot(v_axis);
780 f_vec (ni) = de_meaned[ni].dot (plane_normal);
785 for (
int ui = 0; ui <= order; ++ui)
788 for (
int vi = 0; vi <= order - ui; ++vi)
790 P (j++, ni) = u_pow * v_pow;
798 const Eigen::MatrixXd P_weight = P * weight_vec.asDiagonal();
799 P_weight_Pt = P_weight * P.transpose ();
800 c_vec = P_weight * f_vec;
801 P_weight_Pt.llt ().solveInPlace (c_vec);
807 template <
typename Po
intInT,
typename Po
intOutT>
811 int dilation_iteration_num) :
812 voxel_grid_ (), voxel_size_ (voxel_size)
819 const double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
822 for (std::size_t i = 0; i < indices->size (); ++i)
823 if (std::isfinite ((*cloud)[(*indices)[i]].x))
826 getCellIndex ((*cloud)[(*indices)[i]].getVector3fMap (), pos);
828 std::uint64_t index_1d;
836 template <
typename Po
intInT,
typename Po
intOutT>
void
839 HashMap new_voxel_grid = voxel_grid_;
840 for (
auto m_it = voxel_grid_.begin (); m_it != voxel_grid_.end (); ++m_it)
842 Eigen::Vector3i index;
843 getIndexIn3D (m_it->first, index);
846 for (
int x = -1; x <= 1; ++x)
847 for (
int y = -1; y <= 1; ++y)
848 for (
int z = -1; z <= 1; ++z)
849 if (x != 0 || y != 0 || z != 0)
851 Eigen::Vector3i new_index;
852 new_index = index + Eigen::Vector3i (x, y, z);
854 std::uint64_t index_1d;
855 getIndexIn1D (new_index, index_1d);
857 new_voxel_grid[index_1d] = leaf;
860 voxel_grid_ = new_voxel_grid;
865 template <
typename Po
intInT,
typename Po
intOutT>
void
867 PointOutT &point_out)
const
869 PointOutT temp = point_out;
871 point_out.x = temp.x;
872 point_out.y = temp.y;
873 point_out.z = temp.z;
876 #define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares<T,OutT>;
877 #define PCL_INSTANTIATE_MovingLeastSquaresOMP(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquaresOMP<T,OutT>;
879 #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.
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.
double u
The u-coordinate of the projected point in local MLS frame.
MLSProjectionResults projectPointOrthogonalToPolynomialSurface(const double u, const double v, const double w) const
Project a point orthogonal to the polynomial surface.
void addProjectedPointNormal(pcl::index_t 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 adding projected points.
MLSProjectionResults projectPointToMLSPlane(const double u, const double v) const
Project a point onto the MLS plane.
iterator begin() noexcept
shared_ptr< Indices > IndicesPtr
double z_vv
The partial derivative d^2z/dv^2.
Eigen::Vector4f bounding_max_
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
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 computeMLSPointNormal(pcl::index_t index, const pcl::Indices &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.
void process(PointCloudOut &output) override
Base method for surface reconstruction for all points given in ...
Eigen::Vector4f bounding_min_
double z_uu
The partial derivative d^2z/du^2.
std::uint32_t width
The point cloud width (if organized as an image-structure).
double getPolynomialValue(const double u, const double v) const
Calculate the polynomial.
MLSVoxelGrid(PointCloudInConstPtr &cloud, IndicesPtr &indices, float voxel_size, int dilation_iteration_num)
Data structure used to store the MLS projection results.
Eigen::Vector3d point
The projected point.
double v
The v-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 getIndexIn1D(const Eigen::Vector3i &index, std::uint64_t &index_1d) const
MLSProjectionResults projectPoint(const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors=0) const
Project a point using the specified method.
PolynomialPartialDerivative getPolynomialPartialDerivative(const double u, const double v) const
Calculate the polynomial's first and second partial derivatives.
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...
void clear()
Removes all points in a cloud and sets the width and height to 0.
std::uint32_t height
The point cloud height (if organized as an image-structure).
IndicesAllocator<> Indices
Type used for indices in PCL.
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.
std::map< std::uint64_t, Leaf > HashMap
void getMLSCoordinates(const Eigen::Vector3d &pt, double &u, double &v, double &w) const
Given a point calculate its 3D location in the MLS frame.
OrganizedNeighbor is a class for optimized nearest neighbor search in organized projectable point clo...
void computeMLSSurface(const pcl::PointCloud< PointT > &cloud, pcl::index_t index, const pcl::Indices &nn_indices, double search_radius, int polynomial_order=2, std::function< double(const double)> weight_func={})
Smooth a given point and its neighborhood using Moving Least Squares.
float curvature
The curvature at the query point.
MLSProjectionResults projectPointSimpleToPolynomialSurface(const double u, const double v) const
Project a point along the MLS plane normal to the polynomial surface.
MLSProjectionResults projectQueryPoint(ProjectionMethod method, int required_neighbors=0) const
Project the query point used to generate the mls surface about using the specified method...
double z
The z component of the polynomial evaluated at z(u, v).
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.
void getCellIndex(const Eigen::Vector3f &p, Eigen::Vector3i &index) const
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
void copyMissingFields(const PointInT &point_in, PointOutT &point_out) const
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
void getPosition(const std::uint64_t &index_1d, Eigen::Vector3f &point) const
typename KdTree::Ptr KdTreePtr