48 #include <pcl/memory.h>
49 #include <pcl/pcl_base.h>
50 #include <pcl/pcl_macros.h>
51 #include <pcl/search/search.h>
53 #include <pcl/surface/processing.h>
95 MLSResult (
const Eigen::Vector3d &a_query_point,
96 const Eigen::Vector3d &a_mean,
97 const Eigen::Vector3d &a_plane_normal,
98 const Eigen::Vector3d &a_u,
99 const Eigen::Vector3d &a_v,
100 const Eigen::VectorXd &a_c_vec,
101 const int a_num_neighbors,
102 const float a_curvature,
112 getMLSCoordinates (
const Eigen::Vector3d &pt,
double &u,
double &v,
double &w)
const;
135 inline PolynomialPartialDerivative
153 PCL_DEPRECATED(1, 15,
"use calculatePrincipalCurvatures() instead")
154 inline
Eigen::Vector2f
166 inline MLSProjectionResults
174 inline MLSProjectionResults
183 inline MLSProjectionResults
195 inline MLSProjectionResults
206 inline MLSProjectionResults
217 template <
typename Po
intT>
void
221 double search_radius,
222 int polynomial_order = 2,
223 std::function<
double(
const double)> weight_func = {});
244 double computeMLSWeight (
const double sq_dist,
const double sq_mls_radius) {
return (std::exp (-sq_dist / sq_mls_radius)); }
261 template <
typename Po
intInT,
typename Po
intOutT>
265 using Ptr = shared_ptr<MovingLeastSquares<PointInT, PointOutT> >;
266 using ConstPtr = shared_ptr<const MovingLeastSquares<PointInT, PointOutT> >;
287 using SearchMethod = std::function<int (pcl::index_t, double, pcl::Indices &, std::vector<float> &)>;
313 rng_uniform_distribution_ ()
336 return tree_->radiusSearch (index, radius, k_indices, k_sqr_distances, 0);
489 inline const std::vector<MLSResult>&
584 int dilation_iteration_num);
590 getIndexIn1D (
const Eigen::Vector3i &index, std::uint64_t &index_1d)
const
601 index_3d[1] =
static_cast<Eigen::Vector3i::Scalar
> (index_1d /
data_size_);
603 index_3d[2] =
static_cast<Eigen::Vector3i::Scalar
> (index_1d);
609 for (
int i = 0; i < 3; ++i)
614 getPosition (
const std::uint64_t &index_1d, Eigen::Vector3f &point)
const
616 Eigen::Vector3i index_3d;
618 for (
int i = 0; i < 3; ++i)
622 using HashMap = std::map<std::uint64_t, Leaf>;
651 return (search_method_ (index,
search_radius_, indices, sqr_distances));
685 const Eigen::Vector3d &point,
686 const Eigen::Vector3d &normal,
695 PointOutT &point_out)
const;
711 mutable std::mt19937 rng_;
716 std::unique_ptr<std::uniform_real_distribution<>> rng_uniform_distribution_;
720 getClassName ()
const {
return (
"MovingLeastSquares"); }
724 #ifdef PCL_NO_PRECOMPILE
725 #include <pcl/surface/impl/mls.hpp>
Data structure used to store the MLS polynomial partial derivatives.
bool valid
If True, the mls results data is valid, otherwise False.
MLSProjectionResults()=default
int nr_coeff_
Number of coefficients, to be computed from the requested order.
Eigen::Vector3d plane_normal
The normal of the local plane of the query point.
PointCloudInConstPtr getDistinctCloud() const
Get the distinct cloud used for the DISTINCT_CLOUD upsampling method.
int getPolynomialOrder() const
Get the order of the polynomial to be fit.
Eigen::Vector2f calculatePrincipalCurvatures(const double u, const double v) const
Calculate the principal curvatures using the polynomial surface.
double getSqrGaussParam() const
Get the parameter for distance based weighting of neighbors.
double z_u
The partial derivative dz/du.
const std::vector< MLSResult > & getMLSResults() const
Get the MLSResults for input cloud.
double getSearchRadius() const
Get the sphere radius used for determining the k-nearest neighbors.
typename PointCloudIn::ConstPtr PointCloudInConstPtr
NormalCloudPtr normals_
The point cloud that will hold the estimated normals, if set.
shared_ptr< PointCloud< pcl::Normal > > Ptr
void setDilationVoxelSize(float voxel_size)
Set the voxel size for the voxel grid.
void setSearchRadius(double radius)
Set the sphere radius that is to be used for determining the k-nearest neighbors used for fitting...
The input cloud will be inserted into a voxel grid with voxels of size voxel_size_; this voxel grid w...
Eigen::VectorXd c_vec
The polynomial coefficients Example: z = c_vec[0] + c_vec[1]*v + c_vec[2]*v^2 + c_vec[3]*u + c_vec[4]...
int dilation_iteration_num_
Number of dilation steps for the VOXEL_GRID_DILATION upsampling method.
double u
The u-coordinate of the projected point in local MLS frame.
void setUpsamplingMethod(UpsamplingMethod method)
Set the upsampling method to be used.
PointIndicesPtr getCorrespondingIndices() const
Get the set of indices with each point in output having the corresponding point in input...
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.
PointIndicesPtr corresponding_input_indices_
Collects for each point in output the corresponding point in the input.
PointIndices::Ptr PointIndicesPtr
MLSProjectionResults projectPointToMLSPlane(const double u, const double v) const
Project a point onto the MLS plane.
void setPolynomialOrder(int order)
Set the order of the polynomial to be fit.
typename PointCloudOut::ConstPtr PointCloudOutConstPtr
~MovingLeastSquares() override=default
Empty destructor.
MovingLeastSquares()
Empty constructor.
MovingLeastSquares represent an implementation of the MLS (Moving Least Squares) algorithm for data s...
shared_ptr< Indices > IndicesPtr
double z_vv
The partial derivative d^2z/dv^2.
shared_ptr< const MovingLeastSquares< PointInT, PointOutT > > ConstPtr
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.
float getDilationVoxelSize() const
Get the voxel size for the voxel grid.
SearchMethod search_method_
The search method template for indices.
double z_uv
The partial derivative d^2z/dudv.
int getPointDensity() const
Get the parameter that specifies the desired number of points within the search radius.
void setCacheMLSResults(bool cache_mls_results)
Set whether the mls results should be stored for each point in the input cloud.
CloudSurfaceProcessing represents the base class for algorithms that takes a point cloud as input and...
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
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.
double getUpsamplingStepSize() const
Get the step size for the local plane sampling.
void setUpsamplingRadius(double radius)
Set the radius of the circle in the local point plane that will be sampled.
void process(PointCloudOut &output) override
Base method for surface reconstruction for all points given in ...
Project to the closest point on the polynonomial surface.
int order
The order of the polynomial.
Eigen::Vector4f bounding_min_
Project the points of the distinct cloud to the MLS surface.
bool cache_mls_results_
True if the mls results for the input cloud should be stored.
double z_uu
The partial derivative d^2z/du^2.
std::vector< MLSResult > mls_results_
Stores the MLS result for each point in the input cloud.
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.
void setProjectionMethod(MLSResult::ProjectionMethod method)
Set the method to be used when projection the point on to the MLS surface.
Eigen::Vector3d normal
The projected point's normal.
double upsampling_radius_
Radius of the circle in the local point plane that will be sampled.
void getIndexIn1D(const Eigen::Vector3i &index, std::uint64_t &index_1d) const
typename PointCloudOut::Ptr PointCloudOutPtr
The local plane of each input point will be sampled in a circular fashion using the upsampling_radius...
pcl::PointCloud< pcl::Normal > NormalCloud
MLSProjectionResults projectPoint(const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors=0) const
Project a point using the specified method.
KdTreePtr getSearchMethod() const
Get a pointer to the search method used.
shared_ptr< MovingLeastSquares< PointInT, PointOutT > > Ptr
PolynomialPartialDerivative getPolynomialPartialDerivative(const double u, const double v) const
Calculate the polynomial's first and second partial derivatives.
No upsampling will be done, only the input points will be projected to their own MLS surfaces...
Data structure used to store the results of the MLS fitting.
PointCloudInConstPtr distinct_cloud_
The distinct point cloud that will be projected to the MLS surface.
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.
typename PointCloudIn::Ptr PointCloudInPtr
void setDistinctCloud(PointCloudInConstPtr distinct_cloud)
Set the distinct cloud used for the DISTINCT_CLOUD upsampling method.
double search_radius_
The nearest neighbors search radius for each point.
Eigen::Vector3d u_axis
The axis corresponding to the u-coordinates of the local plane of the query point.
void setUpsamplingStepSize(double step_size)
Set the step size for the local plane sampling.
Eigen::Vector3d mean
The mean point of all the neighbors.
double upsampling_step_
Step size for the local plane sampling.
std::function< int(pcl::index_t, double, pcl::Indices &, std::vector< float > &)> SearchMethod
Eigen::Vector3d v_axis
The axis corresponding to the v-coordinates of the local plane of the query point.
IndicesAllocator<> Indices
Type used for indices in PCL.
unsigned int threads_
The maximum number of threads the scheduler should use.
Project to the mls plane.
void setSqrGaussParam(double sqr_gauss_param)
Set the parameter used for distance based weighting of neighbors (the square of the search radius wor...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Eigen::Vector3d query_point
The query point about which the mls surface was generated.
KdTreePtr tree_
A pointer to the spatial search object.
void performProcessing(PointCloudOut &output) override
Abstract surface reconstruction method.
int getDilationIterations() const
Get the number of dilation steps of the voxel grid.
A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling.
std::map< std::uint64_t, Leaf > HashMap
int order_
The order of the polynomial to be fit.
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
bool getCacheMLSResults() const
Get the cache_mls_results_ value (True if the mls results should be stored, otherwise false)...
float voxel_size_
Voxel size for the VOXEL_GRID_DILATION upsampling method.
shared_ptr< const PointCloud< PointOutT > > ConstPtr
bool compute_normals_
Parameter that specifies whether the normals should be computed for the input cloud or not...
void getMLSCoordinates(const Eigen::Vector3d &pt, double &u, double &v, double &w) const
Given a point calculate its 3D location in the MLS frame.
int desired_num_points_in_radius_
Parameter that specifies the desired number of points within the search radius.
int searchForNeighbors(pcl::index_t index, pcl::Indices &indices, std::vector< float > &sqr_distances) const
Search for the nearest neighbors of a given point using a radius search.
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.
void setDilationIterations(int iterations)
Set the number of dilation steps of the voxel grid.
float curvature
The curvature at the query point.
shared_ptr< pcl::search::Search< PointInT > > Ptr
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...
MLSResult::ProjectionMethod projection_method_
Parameter that specifies the projection method to be used.
void setPointDensity(int desired_num_points_in_radius)
Set the parameter that specifies the desired number of points within the search radius.
void setComputeNormals(bool compute_normals)
Set whether the algorithm should also store the normals computed.
UpsamplingMethod upsample_method_
Parameter that specifies the upsampling method to be used.
NormalCloud::Ptr NormalCloudPtr
double z
The z component of the polynomial evaluated at z(u, v).
void getCellIndex(const Eigen::Vector3f &p, Eigen::Vector3i &index) const
MLSResult::ProjectionMethod getProjectionMethod() const
Get the current projection method being used.
void copyMissingFields(const PointInT &point_in, PointOutT &point_out) const
void getPosition(const std::uint64_t &index_1d, Eigen::Vector3f &point) const
typename KdTree::Ptr KdTreePtr
pcl::PointCloud< PointOutT > PointCloudOut
Eigen::Vector2f calculatePrincipleCurvatures(const double u, const double v) const
Calculate the principal curvatures using the polynomial surface.
double getUpsamplingRadius() const
Get the radius of the circle in the local point plane that will be sampled.
void getIndexIn3D(std::uint64_t index_1d, Eigen::Vector3i &index_3d) const
double sqr_gauss_param_
Parameter for distance based weighting of neighbors (search_radius_ * search_radius_ works fine) ...
void setNumberOfThreads(unsigned int threads=1)
Set the maximum number of threads to use.
Project along the mls plane normal to the polynomial surface.
The local plane of each input point will be sampled using an uniform random distribution such that th...
PointIndices::Ptr PointIndicesPtr