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>
46 #include <pcl/common/copy_point.h>
48 #include <pcl/common/eigen.h>
49 #include <pcl/search/kdtree.h>
50 #include <pcl/search/organized.h>
52 #include <Eigen/Geometry>
60 template <
typename Po
intInT,
typename Po
intOutT>
void
71 normals_->header = input_->header;
73 normals_->width = normals_->height = 0;
74 normals_->points.clear ();
78 output.
header = input_->header;
82 if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
84 PCL_ERROR (
"[pcl::%s::process] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
89 if (upsample_method_ == DISTINCT_CLOUD && !distinct_cloud_)
91 PCL_ERROR (
"[pcl::%s::process] Upsample method was set to DISTINCT_CLOUD, but no distinct cloud was specified.\n", getClassName ().c_str ());
102 if (input_->isOrganized ())
106 setSearchMethod (tree);
110 tree_->setInputCloud (input_);
112 switch (upsample_method_)
115 case (RANDOM_UNIFORM_DENSITY):
117 std::random_device rd;
119 const double tmp = search_radius_ / 2.0;
120 rng_uniform_distribution_.reset (
new std::uniform_real_distribution<> (-tmp, tmp));
124 case (VOXEL_GRID_DILATION):
125 case (DISTINCT_CLOUD):
127 if (!cache_mls_results_)
128 PCL_WARN (
"The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n");
130 cache_mls_results_ =
true;
137 if (cache_mls_results_)
139 mls_results_.resize (input_->size ());
143 mls_results_.resize (1);
147 performProcessing (output);
149 if (compute_normals_)
151 normals_->height = 1;
152 normals_->width = normals_->size ();
154 for (std::size_t i = 0; i < output.
size (); ++i)
156 using FieldList =
typename pcl::traits::fieldList<PointOutT>::type;
173 template <
typename Po
intInT,
typename Po
intOutT>
void
184 mls_result.
computeMLSSurface<PointInT> (*input_, index, nn_indices, search_radius_, order_);
186 switch (upsample_method_)
191 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
195 case (SAMPLE_LOCAL_PLANE):
198 for (
float u_disp = -
static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
199 for (
float v_disp = -
static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
200 if (u_disp * u_disp + v_disp * v_disp < upsampling_radius_ * upsampling_radius_)
203 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
208 case (RANDOM_UNIFORM_DENSITY):
211 const int num_points_to_add =
static_cast<int> (std::floor (desired_num_points_in_radius_ / 2.0 /
static_cast<double> (nn_indices.size ())));
214 if (num_points_to_add <= 0)
218 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
223 for (
int num_added = 0; num_added < num_points_to_add;)
225 const double u = (*rng_uniform_distribution_) (rng_);
226 const double v = (*rng_uniform_distribution_) (rng_);
229 if (u * u + v * v > search_radius_ * search_radius_ / 4)
238 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
251 template <
typename Po
intInT,
typename Po
intOutT>
void
253 const Eigen::Vector3d &point,
254 const Eigen::Vector3d &normal,
261 aux.x =
static_cast<float> (point[0]);
262 aux.y =
static_cast<float> (point[1]);
263 aux.z =
static_cast<float> (point[2]);
266 copyMissingFields ((*input_)[index], aux);
269 corresponding_input_indices.
indices.push_back (index);
271 if (compute_normals_)
274 aux_normal.normal_x =
static_cast<float> (normal[0]);
275 aux_normal.normal_y =
static_cast<float> (normal[1]);
276 aux_normal.normal_z =
static_cast<float> (normal[2]);
278 projected_points_normals.
push_back (aux_normal);
283 template <
typename Po
intInT,
typename Po
intOutT>
void
287 nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
291 const unsigned int threads = threads_ == 0 ? 1 : threads_;
295 std::vector<PointIndices> corresponding_input_indices (threads);
299 #pragma omp parallel for \
301 shared(corresponding_input_indices, projected_points, projected_points_normals) \
302 schedule(dynamic,1000) \
304 for (
int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
309 std::vector<float> nn_sqr_dists;
312 if (searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
315 if (nn_indices.size () >= 3)
319 const int tn = omp_get_thread_num ();
321 std::size_t pp_size = projected_points[tn].size ();
328 const int index = (*indices_)[cp];
330 std::size_t mls_result_index = 0;
331 if (cache_mls_results_)
332 mls_result_index = index;
335 computeMLSPointNormal (index, nn_indices, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], mls_results_[mls_result_index]);
338 for (std::size_t pp = pp_size; pp < projected_points[tn].
size (); ++pp)
339 copyMissingFields ((*input_)[(*indices_)[cp]], projected_points[tn][pp]);
341 computeMLSPointNormal (index, nn_indices, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]);
344 output.
insert (output.
end (), projected_points.
begin (), projected_points.
end ());
345 if (compute_normals_)
346 normals_->insert (normals_->end (), projected_points_normals.
begin (), projected_points_normals.
end ());
354 for (
unsigned int tn = 0; tn < threads; ++tn)
356 output.
insert (output.
end (), projected_points[tn].begin (), projected_points[tn].end ());
357 corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (),
358 corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ());
359 if (compute_normals_)
360 normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ());
365 performUpsampling (output);
369 template <
typename Po
intInT,
typename Po
intOutT>
void
373 if (upsample_method_ == DISTINCT_CLOUD)
376 for (std::size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i)
379 if (!std::isfinite ((*distinct_cloud_)[dp_i].x))
385 std::vector<float> nn_dists;
386 tree_->nearestKSearch ((*distinct_cloud_)[dp_i], 1, nn_indices, nn_dists);
387 const auto input_index = nn_indices.front ();
391 if (mls_results_[input_index].valid ==
false)
394 Eigen::Vector3d add_point = (*distinct_cloud_)[dp_i].getVector3fMap ().template cast<double> ();
396 addProjectedPointNormal (input_index, proj.
point, proj.
normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
402 if (upsample_method_ == VOXEL_GRID_DILATION)
406 MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);
407 for (
int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
410 for (
typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.
voxel_grid_.begin (); m_it != voxel_grid.
voxel_grid_.end (); ++m_it)
422 std::vector<float> nn_dists;
423 tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
424 const auto input_index = nn_indices.front ();
428 if (mls_results_[input_index].valid ==
false)
431 Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> ();
433 addProjectedPointNormal (input_index, proj.
point, proj.
normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
440 const Eigen::Vector3d &a_mean,
441 const Eigen::Vector3d &a_plane_normal,
442 const Eigen::Vector3d &a_u,
443 const Eigen::Vector3d &a_v,
444 const Eigen::VectorXd &a_c_vec,
445 const int a_num_neighbors,
446 const float a_curvature,
448 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),
449 curvature (a_curvature), order (a_order), valid (true)
455 Eigen::Vector3d delta = pt - mean;
456 u = delta.dot (u_axis);
457 v = delta.dot (v_axis);
458 w = delta.dot (plane_normal);
464 Eigen::Vector3d delta = pt - mean;
465 u = delta.dot (u_axis);
466 v = delta.dot (v_axis);
477 for (
int ui = 0; ui <= order; ++ui)
480 for (
int vi = 0; vi <= order - ui; ++vi)
482 result += c_vec[j++] * u_pow * v_pow;
497 Eigen::VectorXd u_pow (order + 2), v_pow (order + 2);
500 d.z = d.z_u = d.z_v = d.z_uu = d.z_vv = d.z_uv = 0;
501 u_pow (0) = v_pow (0) = 1;
502 for (
int ui = 0; ui <= order; ++ui)
504 for (
int vi = 0; vi <= order - ui; ++vi)
507 d.z += u_pow (ui) * v_pow (vi) * c_vec[j];
511 d.z_u += c_vec[j] * ui * u_pow (ui - 1) * v_pow (vi);
514 d.z_v += c_vec[j] * vi * u_pow (ui) * v_pow (vi - 1);
516 if (ui >= 1 && vi >= 1)
517 d.z_uv += c_vec[j] * ui * u_pow (ui - 1) * vi * v_pow (vi - 1);
520 d.z_uu += c_vec[j] * ui * (ui - 1) * u_pow (ui - 2) * v_pow (vi);
523 d.z_vv += c_vec[j] * vi * (vi - 1) * u_pow (ui) * v_pow (vi - 2);
526 v_pow (vi + 1) = v_pow (vi) * v;
530 u_pow (ui + 1) = u_pow (ui) * u;
544 result.
normal = plane_normal;
545 if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
550 const double dist1 = std::abs (gw - w);
554 double e1 = (gu - u) + d.
z_u * gw - d.
z_u * w;
555 double e2 = (gv - v) + d.
z_v * gw - d.
z_v * w;
563 Eigen::MatrixXd J (2, 2);
569 Eigen::Vector2d err (e1, e2);
570 Eigen::Vector2d update = J.inverse () * err;
574 d = getPolynomialPartialDerivative (gu, gv);
576 dist2 = std::sqrt ((gu - u) * (gu - u) + (gv - v) * (gv - v) + (gw - w) * (gw - w));
578 err_total = std::sqrt (e1 * e1 + e2 * e2);
580 }
while (err_total > 1e-8 && dist2 < dist1);
586 d = getPolynomialPartialDerivative (u, v);
593 result.
normal.normalize ();
596 result.
point = mean + gu * u_axis + gv * v_axis + gw * plane_normal;
607 result.
normal = plane_normal;
608 result.
point = mean + u * u_axis + v * v_axis;
621 result.
normal = plane_normal;
623 if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
628 result.
normal.normalize ();
631 result.
point = mean + u * u_axis + v * v_axis + w * plane_normal;
640 getMLSCoordinates (pt, u, v, w);
643 if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
645 if (method == ORTHOGONAL)
646 proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
648 proj = projectPointSimpleToPolynomialSurface (u, v);
652 proj = projectPointToMLSPlane (u, v);
662 if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
664 if (method == ORTHOGONAL)
667 getMLSCoordinates (query_point, u, v, w);
668 proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
673 proj.
point = mean + (c_vec[0] * plane_normal);
676 proj.
normal = plane_normal - c_vec[order + 1] * u_axis - c_vec[1] * v_axis;
682 proj.
normal = plane_normal;
689 template <
typename Po
intT>
void
693 double search_radius,
694 int polynomial_order,
695 std::function<
double(
const double)> weight_func)
698 EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix;
699 Eigen::Vector4d xyz_centroid;
706 EIGEN_ALIGN16 Eigen::Vector3d::Scalar eigen_value;
707 EIGEN_ALIGN16 Eigen::Vector3d eigen_vector;
708 Eigen::Vector4d model_coefficients (0, 0, 0, 0);
709 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
710 model_coefficients.head<3> ().matrix () = eigen_vector;
711 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
713 query_point = cloud[index].getVector3fMap ().template cast<double> ();
715 if (!std::isfinite(eigen_vector[0]) || !std::isfinite(eigen_vector[1]) || !std::isfinite(eigen_vector[2]))
726 const double distance = query_point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
727 mean = query_point -
distance * model_coefficients.head<3> ();
729 curvature = covariance_matrix.trace ();
732 curvature = std::abs (eigen_value / curvature);
735 plane_normal = model_coefficients.head<3> ();
738 v_axis = plane_normal.unitOrthogonal ();
739 u_axis = plane_normal.cross (v_axis);
743 num_neighbors =
static_cast<int> (nn_indices.size ());
744 order = polynomial_order;
747 const int nr_coeff = (order + 1) * (order + 2) / 2;
749 if (num_neighbors >= nr_coeff)
752 weight_func = [=] (
const double sq_dist) {
return this->computeMLSWeight (sq_dist, search_radius * search_radius); };
755 Eigen::VectorXd weight_vec (num_neighbors);
756 Eigen::MatrixXd P (nr_coeff, num_neighbors);
757 Eigen::VectorXd f_vec (num_neighbors);
758 Eigen::MatrixXd P_weight_Pt (nr_coeff, nr_coeff);
762 std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > de_meaned (num_neighbors);
763 for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
765 de_meaned[ni][0] = cloud[nn_indices[ni]].x - mean[0];
766 de_meaned[ni][1] = cloud[nn_indices[ni]].y - mean[1];
767 de_meaned[ni][2] = cloud[nn_indices[ni]].z - mean[2];
768 weight_vec (ni) = weight_func (de_meaned[ni].dot (de_meaned[ni]));
773 for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
776 const double u_coord = de_meaned[ni].dot(u_axis);
777 const double v_coord = de_meaned[ni].dot(v_axis);
778 f_vec (ni) = de_meaned[ni].dot (plane_normal);
783 for (
int ui = 0; ui <= order; ++ui)
786 for (
int vi = 0; vi <= order - ui; ++vi)
788 P (j++, ni) = u_pow * v_pow;
796 const Eigen::MatrixXd P_weight = P * weight_vec.asDiagonal();
797 P_weight_Pt = P_weight * P.transpose ();
798 c_vec = P_weight * f_vec;
799 P_weight_Pt.llt ().solveInPlace (c_vec);
805 template <
typename Po
intInT,
typename Po
intOutT>
809 voxel_grid_ (), data_size_ (), voxel_size_ (voxel_size)
814 const double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
817 for (std::size_t i = 0; i < indices->size (); ++i)
818 if (std::isfinite ((*cloud)[(*indices)[i]].x))
821 getCellIndex ((*cloud)[(*indices)[i]].getVector3fMap (), pos);
823 std::uint64_t index_1d;
831 template <
typename Po
intInT,
typename Po
intOutT>
void
834 HashMap new_voxel_grid = voxel_grid_;
835 for (
typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid_.begin (); m_it != voxel_grid_.end (); ++m_it)
837 Eigen::Vector3i index;
838 getIndexIn3D (m_it->first, index);
841 for (
int x = -1; x <= 1; ++x)
842 for (
int y = -1; y <= 1; ++y)
843 for (
int z = -1; z <= 1; ++z)
844 if (x != 0 || y != 0 || z != 0)
846 Eigen::Vector3i new_index;
847 new_index = index + Eigen::Vector3i (x, y, z);
849 std::uint64_t index_1d;
850 getIndexIn1D (new_index, index_1d);
852 new_voxel_grid[index_1d] = leaf;
855 voxel_grid_ = new_voxel_grid;
860 template <
typename Po
intInT,
typename Po
intOutT>
void
862 PointOutT &point_out)
const
864 PointOutT temp = point_out;
866 point_out.x = temp.x;
867 point_out.y = temp.y;
868 point_out.z = temp.z;
871 #define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares<T,OutT>;
872 #define PCL_INSTANTIATE_MovingLeastSquaresOMP(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquaresOMP<T,OutT>;
Define methods for centroid estimation and covariance matrix calculus.
A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling.
MLSVoxelGrid(PointCloudInConstPtr &cloud, IndicesPtr &indices, float voxel_size)
void getPosition(const std::uint64_t &index_1d, Eigen::Vector3f &point) const
Eigen::Vector4f bounding_min_
void getIndexIn1D(const Eigen::Vector3i &index, std::uint64_t &index_1d) const
std::map< std::uint64_t, Leaf > HashMap
Eigen::Vector4f bounding_max_
void getCellIndex(const Eigen::Vector3f &p, Eigen::Vector3i &index) const
void performUpsampling(PointCloudOut &output)
Perform upsampling for the distinct-cloud and voxel-grid methods.
typename KdTree::Ptr KdTreePtr
typename PointCloudIn::ConstPtr PointCloudInConstPtr
void copyMissingFields(const PointInT &point_in, PointOutT &point_out) const
void performProcessing(PointCloudOut &output) override
Abstract surface reconstruction method.
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 <setInputCloud (), setIndices ()>
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.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
std::uint32_t width
The point cloud width (if organized as an image-structure).
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
pcl::PCLHeader header
The point cloud header.
std::vector< PointCloud< PointOutT >, Eigen::aligned_allocator< PointCloud< PointOutT > > > CloudVectorType
std::uint32_t height
The point cloud height (if organized as an image-structure).
void clear()
Removes all points in a cloud and sets the width and height to 0.
iterator begin() noexcept
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
Define standard C methods and C++ classes that are common to all methods.
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 copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
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.
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...
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.
float distance(const PointT &p1, const PointT &p2)
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
IndicesAllocator<> Indices
Type used for indices in PCL.
shared_ptr< Indices > IndicesPtr
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.
Eigen::Vector3d normal
The projected point's normal.
double u
The u-coordinate of the projected point in local MLS frame.
Data structure used to store the MLS polynomial partial derivatives.
double z_uv
The partial derivative d^2z/dudv.
double z_u
The partial derivative dz/du.
double z_uu
The partial derivative d^2z/du^2.
double z
The z component of the polynomial evaluated at z(u, v).
double z_vv
The partial derivative d^2z/dv^2.
double z_v
The partial derivative dz/dv.
Data structure used to store the results of the MLS fitting.
MLSProjectionResults projectPoint(const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors=0) const
Project a point using the specified method.
MLSProjectionResults projectPointOrthogonalToPolynomialSurface(const double u, const double v, const double w) const
Project a point orthogonal to the polynomial surface.
int num_neighbors
The number of neighbors used to create the mls surface.
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 neighborghood using Moving Least Squares.
void getMLSCoordinates(const Eigen::Vector3d &pt, double &u, double &v, double &w) const
Given a point calculate its 3D location in the MLS frame.
float curvature
The curvature at the query point.
PolynomialPartialDerivative getPolynomialPartialDerivative(const double u, const double v) const
Calculate the polynomial's first and second partial derivatives.
MLSProjectionResults projectPointSimpleToPolynomialSurface(const double u, const double v) const
Project a point along the MLS plane normal to the polynomial surface.
MLSProjectionResults projectPointToMLSPlane(const double u, const double v) const
Project a point onto the MLS plane.
double getPolynomialValue(const double u, const double v) const
Calculate the polynomial.
MLSProjectionResults projectQueryPoint(ProjectionMethod method, int required_neighbors=0) const
Project the query point used to generate the mls surface about using the specified method.
A point structure representing normal coordinates and the surface curvature estimate.
A helper functor that can set a specific value in a field if the field exists.