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>
60template <
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;
173template <
typename Po
intInT,
typename Po
intOutT>
void
186 switch (upsample_method_)
195 case (SAMPLE_LOCAL_PLANE):
208 case (RANDOM_UNIFORM_DENSITY):
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)
233 if (order_ > 1 &&
mls_result.num_neighbors >= 5 * nr_coeff_)
251template <
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);
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]);
283template <
typename Po
intInT,
typename Po
intOutT>
void
287 nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
291 const unsigned int threads = threads_ == 0 ? 1 : threads_;
299#pragma omp parallel for \
301 shared(corresponding_input_indices, projected_points, projected_points_normals) \
302 schedule(dynamic,1000) \
328 const int index = (*indices_)[cp];
331 if (cache_mls_results_)
345 if (compute_normals_)
354 for (
unsigned int tn = 0;
tn < threads; ++
tn)
357 corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (),
359 if (compute_normals_)
365 performUpsampling (
output);
369template <
typename Po
intInT,
typename Po
intOutT>
void
373 if (upsample_method_ == DISTINCT_CLOUD)
379 if (!std::isfinite ((*distinct_cloud_)[
dp_i].x))
402 if (upsample_method_ == VOXEL_GRID_DILATION)
440 const Eigen::Vector3d &
a_mean,
442 const Eigen::Vector3d &
a_u,
443 const Eigen::Vector3d &
a_v,
444 const Eigen::VectorXd &
a_c_vec,
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)
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;
502 for (
int ui = 0; ui <= order; ++ui)
504 for (
int vi = 0;
vi <= order - ui; ++
vi)
516 if (ui >= 1 &&
vi >= 1)
520 d.z_uu += c_vec[j] * ui * (ui - 1) *
u_pow (ui - 2) *
v_pow (
vi);
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);
563 Eigen::MatrixXd
J (2, 2);
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));
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);
646 proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
648 proj = projectPointSimpleToPolynomialSurface (u, v);
652 proj = projectPointToMLSPlane (u, v);
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;
677 proj.normal.normalize ();
682 proj.normal = plane_normal;
689template <
typename Po
intT>
void
699 Eigen::Vector4d xyz_centroid;
713 query_point = cloud[index].getVector3fMap ().template
cast<double> ();
738 v_axis = plane_normal.unitOrthogonal ();
739 u_axis = plane_normal.cross (v_axis);
747 const int nr_coeff = (order + 1) * (order + 2) / 2;
756 Eigen::MatrixXd
P (
nr_coeff, num_neighbors);
757 Eigen::VectorXd
f_vec (num_neighbors);
762 std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >
de_meaned (num_neighbors);
783 for (
int ui = 0; ui <= order; ++ui)
786 for (
int vi = 0;
vi <= order - ui; ++
vi)
805template <
typename Po
intInT,
typename Po
intOutT>
809 voxel_grid_ (), data_size_ (), voxel_size_ (
voxel_size)
817 for (std::size_t i = 0; i < indices->size (); ++i)
818 if (std::isfinite ((*cloud)[(*indices)[i]].x))
831template <
typename Po
intInT,
typename Po
intOutT>
void
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)
847 new_index = index + Eigen::Vector3i (x, y, z);
860template <
typename Po
intInT,
typename Po
intOutT>
void
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.
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
std::size_t size() const
Size of the range the iterator is going through.
A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling.
MLSVoxelGrid(PointCloudInConstPtr &cloud, IndicesPtr &indices, float voxel_size)
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.
std::vector< PointCloud< PointOutT >, Eigen::aligned_allocator< PointCloud< PointOutT > > > CloudVectorType
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.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
shared_ptr< Indices > IndicesPtr
Data structure used to store the MLS projection results.
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.
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.
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.