44 #include <pcl/pcl_base.h> 45 #include <pcl/search/pcl_search.h> 48 #include <pcl/surface/boost.h> 49 #include <pcl/surface/eigen.h> 50 #include <pcl/surface/processing.h> 52 #include <boost/function.hpp> 87 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
94 MLSResult (
const Eigen::Vector3d &a_query_point,
95 const Eigen::Vector3d &a_mean,
96 const Eigen::Vector3d &a_plane_normal,
97 const Eigen::Vector3d &a_u,
98 const Eigen::Vector3d &a_v,
99 const Eigen::VectorXd &a_c_vec,
100 const int a_num_neighbors,
101 const float a_curvature,
111 getMLSCoordinates (
const Eigen::Vector3d &pt,
double &u,
double &v,
double &w)
const;
143 inline Eigen::Vector2f
205 template <
typename Po
intT>
void 208 const std::vector<int> &nn_indices,
209 double search_radius,
210 int polynomial_order = 2,
211 boost::function<
double(
const double)> weight_func = 0);
223 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
232 double computeMLSWeight (
const double sq_dist,
const double sq_mls_radius) {
return (exp (-sq_dist / sq_mls_radius)); }
249 template <
typename Po
intInT,
typename Po
intOutT>
253 typedef boost::shared_ptr<MovingLeastSquares<PointInT, PointOutT> >
Ptr;
254 typedef boost::shared_ptr<const MovingLeastSquares<PointInT, PointOutT> >
ConstPtr;
275 typedef boost::function<int (int, double, std::vector<int> &, std::vector<float> &)>
SearchMethod;
284 RANDOM_UNIFORM_DENSITY,
301 search_radius_ (0.0),
302 sqr_gauss_param_ (0.0),
303 compute_normals_ (false),
304 upsample_method_ (
NONE),
305 upsampling_radius_ (0.0),
306 upsampling_step_ (0.0),
307 desired_num_points_in_radius_ (0),
308 cache_mls_results_ (true),
313 dilation_iteration_num_ (0),
315 corresponding_input_indices_ (),
317 rng_uniform_distribution_ ()
338 int (KdTree::*radiusSearch)(
int index,
double radius, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
unsigned int max_nn)
const = &
KdTree::radiusSearch;
339 search_method_ = boost::bind (radiusSearch, boost::ref (tree_), _1, _2, _3, _4, 0);
360 PCL_DEPRECATED (
"[pcl::surface::MovingLeastSquares::setPolynomialFit] setPolynomialFit is deprecated. Please use setPolynomialOrder instead.")
378 PCL_DEPRECATED (
"[pcl::surface::MovingLeastSquares::getPolynomialFit] getPolynomialFit is deprecated. Please use getPolynomialOrder instead.")
387 setSearchRadius (
double radius) { search_radius_ = radius; sqr_gauss_param_ = search_radius_ * search_radius_; }
412 setDistinctCloud (PointCloudInConstPtr distinct_cloud) { distinct_cloud_ = distinct_cloud; }
415 inline PointCloudInConstPtr
452 setPointDensity (
int desired_num_points_in_radius) { desired_num_points_in_radius_ = desired_num_points_in_radius; }
516 inline const std::vector<MLSResult>&
532 process (PointCloudOut &output);
618 index_1d = index[0] * data_size_ * data_size_ +
619 index[1] * data_size_ + index[2];
625 index_3d[0] =
static_cast<Eigen::Vector3i::Scalar
> (index_1d / (data_size_ * data_size_));
626 index_1d -= index_3d[0] * data_size_ * data_size_;
627 index_3d[1] =
static_cast<Eigen::Vector3i::Scalar
> (index_1d / data_size_);
628 index_1d -= index_3d[1] * data_size_;
629 index_3d[2] =
static_cast<Eigen::Vector3i::Scalar
> (index_1d);
635 for (
int i = 0; i < 3; ++i)
636 index[i] = static_cast<Eigen::Vector3i::Scalar> ((p[i] - bounding_min_ (i)) / voxel_size_);
640 getPosition (
const uint64_t &index_1d, Eigen::Vector3f &point)
const 642 Eigen::Vector3i index_3d;
643 getIndexIn3D (index_1d, index_3d);
644 for (
int i = 0; i < 3; ++i)
645 point[i] = static_cast<Eigen::Vector3f::Scalar> (index_3d[i]) * voxel_size_ + bounding_min_[i];
653 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
677 return (search_method_ (index, search_radius_, indices, sqr_distances));
692 computeMLSPointNormal (
int index,
693 const std::vector<int> &nn_indices,
694 PointCloudOut &projected_points,
695 NormalCloud &projected_points_normals,
710 addProjectedPointNormal (
int index,
711 const Eigen::Vector3d &point,
712 const Eigen::Vector3d &normal,
714 PointCloudOut &projected_points,
715 NormalCloud &projected_points_normals,
720 copyMissingFields (
const PointInT &point_in,
721 PointOutT &point_out)
const;
727 performProcessing (PointCloudOut &output);
733 performUpsampling (PointCloudOut &output);
737 boost::mt19937 rng_alg_;
742 boost::shared_ptr<boost::variate_generator<boost::mt19937&,
743 boost::uniform_real<float> >
744 > rng_uniform_distribution_;
748 getClassName ()
const {
return (
"MovingLeastSquares"); }
756 template <
typename Po
intInT,
typename Po
intOutT>
765 this->setNumberOfThreads (threads);
770 #ifdef PCL_NO_PRECOMPILE 771 #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.
virtual int radiusSearch(const PointT &p_q, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const =0
Search for all the nearest neighbors of the query point in a given radius.
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.
pcl::PointCloud< pcl::Normal > NormalCloud
double z_u
The partial derivative dz/du.
PointCloudOut::Ptr PointCloudOutPtr
NormalCloudPtr normals_
The point cloud that will hold the estimated normals, if set.
void setDilationVoxelSize(float voxel_size)
Set the voxel size for the voxel grid.
MLSResult::ProjectionMethod getProjectionMethod() const
Get the current projection method being used.
void setSearchRadius(double radius)
Set the sphere radius that is to be used for determining the k-nearest neighbors used for fitting...
PointCloudIn::Ptr PointCloudInPtr
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.
Eigen::Vector2f calculatePrincipleCurvatures(const double u, const double v) const
Calculate the principle curvatures using the polynomial surface.
void setUpsamplingMethod(UpsamplingMethod method)
Set the upsampling method to be used.
boost::shared_ptr< MovingLeastSquares< PointInT, PointOutT > > Ptr
float getDilationVoxelSize() const
Get the voxel size for the voxel grid.
boost::shared_ptr< std::vector< int > > IndicesPtr
pcl::search::Search< PointInT > KdTree
void getIndexIn1D(const Eigen::Vector3i &index, uint64_t &index_1d) const
PointCloudInConstPtr getDistinctCloud() const
Get the distinct cloud used for the DISTINCT_CLOUD upsampling method.
int getDilationIterations() const
Get the number of dilation steps of the voxel grid.
PointIndicesPtr corresponding_input_indices_
Collects for each point in output the corrseponding point in the input.
This file defines compatibility wrappers for low level I/O functions.
void setPolynomialOrder(int order)
Set the order of the polynomial to be fit.
PolynomialPartialDerivative getPolynomialPartialDerivative(const double u, const double v) const
Calculate the polynomial's first and second partial derivatives.
MovingLeastSquares()
Empty constructor.
MovingLeastSquares represent an implementation of the MLS (Moving Least Squares) algorithm for data s...
double z_vv
The partial derivative d^2z/dv^2.
SearchMethod search_method_
The search method template for indices.
double z_uv
The partial derivative d^2z/dudv.
void setCacheMLSResults(bool cache_mls_results)
Set whether the mls results should be stored for each point in the input cloud.
MovingLeastSquaresOMP implementation has been merged into MovingLeastSquares for better maintainabili...
CloudSurfaceProcessing represents the base class for algorithms that takes a point cloud as input and...
double getSearchRadius() const
Get the sphere radius used for determining the k-nearest neighbors.
pcl::PointCloud< PointInT > PointCloudIn
int getPointDensity() const
Get the parameter that specifies the desired number of points within the search radius.
boost::shared_ptr< PointIndices > PointIndicesPtr
void computeMLSSurface(const pcl::PointCloud< PointT > &cloud, int index, const std::vector< int > &nn_indices, double search_radius, int polynomial_order=2, boost::function< double(const double)> weight_func=0)
Smooth a given point and its neighborghood using Moving Least Squares.
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 setUpsamplingRadius(double radius)
Set the radius of the circle in the local point plane that will be sampled.
Project to the closest point on the polynonomial surface.
MovingLeastSquaresOMP(unsigned int threads=1)
Constructor for parallelized Moving Least Squares.
boost::shared_ptr< const MovingLeastSquares< PointInT, PointOutT > > ConstPtr
int order
The order of the polynomial.
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.
Project the points of the distinct cloud to the MLS surface.
MLSProjectionResults projectPoint(const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors=0) const
Project a point using the specified method.
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.
boost::shared_ptr< PointCloud< PointT > > Ptr
std::vector< MLSResult > mls_results_
Stores the MLS result for each point in the input cloud.
bool getPolynomialFit() const
Get the polynomial_fit value (true if the surface and normal are approximated using a polynomial)...
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.
pcl::PointCloud< PointOutT > PointCloudOut
void setProjectionMethod(MLSResult::ProjectionMethod method)
Set the method to be used when projection the point on to the MLS surface.
double getUpsamplingStepSize() const
Get the step size for the local plane sampling.
Eigen::Vector3d normal
The projected point's normal.
PointCloudOut::ConstPtr PointCloudOutConstPtr
double upsampling_radius_
Radius of the circle in the local point plane that will be sampled.
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.
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
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.
pcl::search::Search< PointInT >::Ptr KdTreePtr
int num_neighbors
The number of neighbors used to create the mls surface.
KdTreePtr getSearchMethod() const
Get a pointer to the search method used.
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.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
void setUpsamplingStepSize(double step_size)
Set the step size for the local plane sampling.
void setPolynomialFit(bool polynomial_fit)
Sets whether the surface and normal are approximated using a polynomial, or only via tangent estimati...
Eigen::Vector3d mean
The mean point of all the neighbors.
double upsampling_step_
Step size for the local plane sampling.
Eigen::Vector3d v_axis
The axis corresponding to the v-coordinates of the local plane of the query point.
unsigned int threads_
The maximum number of threads the scheduler should use.
double getUpsamplingRadius() const
Get the radius of the circle in the local point plane that will be sampled.
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...
PointCloudIn::ConstPtr PointCloudInConstPtr
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.
bool getCacheMLSResults() const
Get the cache_mls_results_ value (True if the mls results should be stored, otherwise false)...
A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling.
int order_
The order of the polynomial to be fit.
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
float voxel_size_
Voxel size for the VOXEL_GRID_DILATION upsampling method.
bool compute_normals_
Parameter that specifies whether the normals should be computed for the input cloud or not...
int desired_num_points_in_radius_
Parameter that specifies the desired number of points within the search radius.
PointIndicesPtr getCorrespondingIndices() const
Get the set of indices with each point in output having the corresponding point in input...
double getSqrGaussParam() const
Get the parameter for distance based weighting of neighbors.
void getIndexIn3D(uint64_t index_1d, Eigen::Vector3i &index_3d) const
const std::vector< MLSResult > & getMLSResults() const
Get the MLSResults for input cloud.
void getPosition(const uint64_t &index_1d, Eigen::Vector3f &point) const
void setDilationIterations(int iterations)
Set the number of dilation steps of the voxel grid.
boost::function< int(int, double, std::vector< int > &, std::vector< float > &)> SearchMethod
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.
int getPolynomialOrder() const
Get the order of the polynomial to be fit.
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.
int searchForNeighbors(int index, std::vector< int > &indices, std::vector< float > &sqr_distances) const
Search for the closest nearest neighbors of a given point using a radius search.
UpsamplingMethod upsample_method_
Parameter that specifies the upsampling method to be used.
double getPolynomialValue(const double u, const double v) const
Calculate the polynomial.
virtual ~MovingLeastSquares()
Empty destructor.
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.
pcl::PointCloud< pcl::Normal >::Ptr NormalCloudPtr
void getCellIndex(const Eigen::Vector3f &p, Eigen::Vector3i &index) 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.
std::map< uint64_t, Leaf > HashMap