40 #ifndef PCL_SURFACE_IMPL_MLS_H_ 41 #define PCL_SURFACE_IMPL_MLS_H_ 43 #include <pcl/point_traits.h> 44 #include <pcl/surface/mls.h> 45 #include <pcl/common/io.h> 46 #include <pcl/common/copy_point.h> 48 #include <pcl/common/eigen.h> 50 #include <boost/bind.hpp> 57 template <
typename Po
intInT,
typename Po
intOutT>
void 68 normals_->header = input_->header;
70 normals_->width = normals_->height = 0;
71 normals_->points.clear ();
75 output.
header = input_->header;
79 if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
81 PCL_ERROR (
"[pcl::%s::process] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
86 if (upsample_method_ == DISTINCT_CLOUD && !distinct_cloud_)
88 PCL_ERROR (
"[pcl::%s::process] Upsample method was set to DISTINCT_CLOUD, but no distinct cloud was specified.\n", getClassName ().c_str ());
99 if (input_->isOrganized ())
103 setSearchMethod (tree);
107 tree_->setInputCloud (input_);
109 switch (upsample_method_)
112 case (RANDOM_UNIFORM_DENSITY):
114 rng_alg_.seed (static_cast<unsigned> (std::time (0)));
115 float tmp =
static_cast<float> (search_radius_ / 2.0f);
116 boost::uniform_real<float> uniform_distrib (-tmp, tmp);
117 rng_uniform_distribution_.reset (
new boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > (rng_alg_, uniform_distrib));
121 case (VOXEL_GRID_DILATION):
122 case (DISTINCT_CLOUD):
124 if (!cache_mls_results_)
125 PCL_WARN (
"The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n");
127 cache_mls_results_ =
true;
134 if (cache_mls_results_)
136 mls_results_.resize (input_->size ());
140 mls_results_.resize (1);
144 performProcessing (output);
146 if (compute_normals_)
148 normals_->height = 1;
149 normals_->width =
static_cast<uint32_t
> (normals_->size ());
151 for (
unsigned int i = 0; i < output.
size (); ++i)
164 output.
width =
static_cast<uint32_t
> (output.
size ());
170 template <
typename Po
intInT,
typename Po
intOutT>
void 172 const std::vector<int> &nn_indices,
181 mls_result.
computeMLSSurface<PointInT> (*input_, index, nn_indices, search_radius_, order_);
183 switch (upsample_method_)
188 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
192 case (SAMPLE_LOCAL_PLANE):
195 for (
float u_disp = -static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
196 for (
float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
197 if (u_disp * u_disp + v_disp * v_disp < upsampling_radius_ * upsampling_radius_)
200 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
205 case (RANDOM_UNIFORM_DENSITY):
208 int num_points_to_add =
static_cast<int> (floor (desired_num_points_in_radius_ / 2.0 / static_cast<double> (nn_indices.size ())));
211 if (num_points_to_add <= 0)
215 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
220 for (
int num_added = 0; num_added < num_points_to_add;)
222 double u = (*rng_uniform_distribution_) ();
223 double v = (*rng_uniform_distribution_) ();
226 if (u * u + v * v > search_radius_ * search_radius_ / 4)
235 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
248 template <
typename Po
intInT,
typename Po
intOutT>
void 250 const Eigen::Vector3d &point,
251 const Eigen::Vector3d &normal,
258 aux.x =
static_cast<float> (point[0]);
259 aux.y =
static_cast<float> (point[1]);
260 aux.z =
static_cast<float> (point[2]);
263 copyMissingFields (input_->points[index], aux);
266 corresponding_input_indices.
indices.push_back (index);
268 if (compute_normals_)
271 aux_normal.normal_x =
static_cast<float> (normal[0]);
272 aux_normal.normal_y =
static_cast<float> (normal[1]);
273 aux_normal.normal_z =
static_cast<float> (normal[2]);
275 projected_points_normals.
push_back (aux_normal);
280 template <
typename Po
intInT,
typename Po
intOutT>
void 284 nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
288 const unsigned int threads = threads_ == 0 ? 1 : threads_;
292 std::vector<PointIndices> corresponding_input_indices (threads);
297 #pragma omp parallel for schedule (dynamic,1000) num_threads (threads) 299 for (
int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
303 std::vector<int> nn_indices;
304 std::vector<float> nn_sqr_dists;
307 if (searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
310 if (nn_indices.size () >= 3)
314 const int tn = omp_get_thread_num ();
316 size_t pp_size = projected_points[tn].size ();
323 const int index = (*indices_)[cp];
325 size_t mls_result_index = 0;
326 if (cache_mls_results_)
327 mls_result_index = index;
330 computeMLSPointNormal (index, nn_indices, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], mls_results_[mls_result_index]);
333 for (
size_t pp = pp_size; pp < projected_points[tn].
size (); ++pp)
334 copyMissingFields (input_->points[(*indices_)[cp]], projected_points[tn][pp]);
336 computeMLSPointNormal (index, nn_indices, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]);
339 output.
insert (output.
end (), projected_points.
begin (), projected_points.
end ());
340 if (compute_normals_)
341 normals_->insert (normals_->end (), projected_points_normals.
begin (), projected_points_normals.
end ());
349 for (
unsigned int tn = 0; tn < threads; ++tn)
351 output.
insert (output.
end (), projected_points[tn].begin (), projected_points[tn].end ());
352 corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (),
353 corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ());
354 if (compute_normals_)
355 normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ());
360 performUpsampling (output);
364 template <
typename Po
intInT,
typename Po
intOutT>
void 368 if (upsample_method_ == DISTINCT_CLOUD)
371 for (
size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i)
374 if (!pcl_isfinite (distinct_cloud_->points[dp_i].x))
379 std::vector<int> nn_indices;
380 std::vector<float> nn_dists;
381 tree_->nearestKSearch (distinct_cloud_->points[dp_i], 1, nn_indices, nn_dists);
382 int input_index = nn_indices.front ();
386 if (mls_results_[input_index].valid ==
false)
389 Eigen::Vector3d add_point = distinct_cloud_->points[dp_i].getVector3fMap ().template cast<double> ();
391 addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
397 if (upsample_method_ == VOXEL_GRID_DILATION)
401 MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);
402 for (
int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
405 for (
typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.
voxel_grid_.begin (); m_it != voxel_grid.
voxel_grid_.end (); ++m_it)
416 std::vector<int> nn_indices;
417 std::vector<float> nn_dists;
418 tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
419 int input_index = nn_indices.front ();
423 if (mls_results_[input_index].valid ==
false)
426 Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> ();
428 addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
435 const Eigen::Vector3d &a_mean,
436 const Eigen::Vector3d &a_plane_normal,
437 const Eigen::Vector3d &a_u,
438 const Eigen::Vector3d &a_v,
439 const Eigen::VectorXd &a_c_vec,
440 const int a_num_neighbors,
441 const float a_curvature,
443 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),
444 curvature (a_curvature), order (a_order), valid (true)
450 Eigen::Vector3d delta = pt -
mean;
459 Eigen::Vector3d delta = pt -
mean;
469 double u_pow, v_pow, result;
473 for (
int ui = 0; ui <=
order; ++ui)
476 for (
int vi = 0; vi <=
order - ui; ++vi)
478 result +=
c_vec[j++] * u_pow * v_pow;
493 Eigen::VectorXd u_pow (
order + 2), v_pow (
order + 2);
497 u_pow (0) = v_pow (0) = 1;
498 for (
int ui = 0; ui <=
order; ++ui)
500 for (
int vi = 0; vi <=
order - ui; ++vi)
503 d.
z += u_pow (ui) * v_pow (vi) *
c_vec[j];
507 d.
z_u +=
c_vec[j] * ui * u_pow (ui - 1) * v_pow (vi);
510 d.
z_v +=
c_vec[j] * vi * u_pow (ui) * v_pow (vi - 1);
512 if (ui >= 1 && vi >= 1)
513 d.
z_uv +=
c_vec[j] * ui * u_pow (ui - 1) * vi * v_pow (vi - 1);
516 d.
z_uu +=
c_vec[j] * ui * (ui - 1) * u_pow (ui - 2) * v_pow (vi);
519 d.
z_vv +=
c_vec[j] * vi * (vi - 1) * u_pow (ui) * v_pow (vi - 2);
522 v_pow (vi + 1) = v_pow (vi) * v;
526 u_pow (ui + 1) = u_pow (ui) * u;
535 Eigen::Vector2f k (1e-5, 1e-5);
545 double Zlen = std::sqrt (Z);
548 double disc2 = H * H -
K;
549 assert (disc2 >= 0.0);
550 double disc = std::sqrt (disc2);
554 if (std::abs (k[0]) > std::abs (k[1])) std::swap (k[0], k[1]);
558 PCL_ERROR (
"No Polynomial fit data, unable to calculate the principle curvatures!\n");
578 double dist1 = std::abs (gw - w);
582 double e1 = (gu - u) + d.
z_u * gw - d.
z_u * w;
583 double e2 = (gv - v) + d.
z_v * gw - d.
z_v * w;
591 Eigen::MatrixXd J (2, 2);
597 Eigen::Vector2d err (e1, e2);
598 Eigen::Vector2d update = J.inverse () * err;
604 dist2 = std::sqrt ((gu - u) * (gu - u) + (gv - v) * (gv - v) + (gw - w) * (gw - w));
606 err_total = std::sqrt (e1 * e1 + e2 * e2);
608 }
while (err_total > 1e-8 && dist2 < dist1);
621 result.
normal.normalize ();
656 result.
normal.normalize ();
717 template <
typename Po
intT>
void 720 const std::vector<int> &nn_indices,
721 double search_radius,
722 int polynomial_order,
723 boost::function<
double(
const double)> weight_func)
727 Eigen::Vector4d xyz_centroid;
736 Eigen::Vector4d model_coefficients (0, 0, 0, 0);
737 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
738 model_coefficients.head<3> ().matrix () = eigen_vector;
739 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
762 order = polynomial_order;
770 double max_sq_radius = 1;
771 if (weight_func == 0)
773 max_sq_radius = search_radius * search_radius;
774 weight_func = boost::bind (&pcl::MLSResult::computeMLSWeight,
this, _1, max_sq_radius);
781 Eigen::MatrixXd P_weight;
782 Eigen::MatrixXd P_weight_Pt (nr_coeff, nr_coeff);
786 std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > de_meaned (
num_neighbors);
789 de_meaned[ni][0] = cloud.
points[nn_indices[ni]].x -
mean[0];
790 de_meaned[ni][1] = cloud.
points[nn_indices[ni]].y -
mean[1];
791 de_meaned[ni][2] = cloud.
points[nn_indices[ni]].z -
mean[2];
792 weight_vec (ni) = weight_func (de_meaned[ni].dot (de_meaned[ni]));
797 double u_coord, v_coord, u_pow, v_pow;
801 u_coord = de_meaned[ni].dot (
u_axis);
802 v_coord = de_meaned[ni].dot (
v_axis);
808 for (
int ui = 0; ui <=
order; ++ui)
811 for (
int vi = 0; vi <=
order - ui; ++vi)
813 P (j++, ni) = u_pow * v_pow;
821 P_weight = P * weight_vec.asDiagonal ();
822 P_weight_Pt = P_weight * P.transpose ();
823 c_vec = P_weight * f_vec;
824 P_weight_Pt.llt ().solveInPlace (
c_vec);
830 template <
typename Po
intInT,
typename Po
intOutT>
834 voxel_grid_ (), bounding_min_ (), bounding_max_ (), data_size_ (), voxel_size_ (voxel_size)
839 double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
842 for (
unsigned int i = 0; i < indices->size (); ++i)
843 if (pcl_isfinite (cloud->points[(*indices)[i]].x))
846 getCellIndex (cloud->points[(*indices)[i]].getVector3fMap (), pos);
856 template <
typename Po
intInT,
typename Po
intOutT>
void 860 for (
typename MLSVoxelGrid::HashMap::iterator m_it =
voxel_grid_.begin (); m_it !=
voxel_grid_.end (); ++m_it)
862 Eigen::Vector3i index;
866 for (
int x = -1; x <= 1; ++x)
867 for (
int y = -1; y <= 1; ++y)
868 for (
int z = -1; z <= 1; ++z)
869 if (x != 0 || y != 0 || z != 0)
871 Eigen::Vector3i new_index;
872 new_index = index + Eigen::Vector3i (x, y, z);
877 new_voxel_grid[index_1d] = leaf;
885 template <
typename Po
intInT,
typename Po
intOutT>
void 887 PointOutT &point_out)
const 889 PointOutT temp = point_out;
891 point_out.x = temp.x;
892 point_out.y = temp.y;
893 point_out.z = temp.z;
896 #define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares<T,OutT>; 897 #define PCL_INSTANTIATE_MovingLeastSquaresOMP(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquaresOMP<T,OutT>; 899 #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.
bool valid
If True, the mls results data is valid, otherwise False.
Eigen::Vector3d plane_normal
The normal of the local plane of the query point.
void addProjectedPointNormal(int 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 add projected points.
double z_u
The partial derivative dz/du.
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.
std::vector< PointCloud< PointT >, Eigen::aligned_allocator< PointCloud< PointT > > > CloudVectorType
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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]...
struct pcl::PointXYZIEdge EIGEN_ALIGN16
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.
virtual void performProcessing(PointCloudOut &output)
Abstract surface reconstruction method.
boost::shared_ptr< std::vector< int > > IndicesPtr
void getIndexIn1D(const Eigen::Vector3i &index, uint64_t &index_1d) const
PolynomialPartialDerivative getPolynomialPartialDerivative(const double u, const double v) const
Calculate the polynomial's first and second partial derivatives.
double z_vv
The partial derivative d^2z/dv^2.
Defines some geometrical functions and utility functions.
Eigen::Vector4f bounding_max_
std::vector< int > indices
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 copyMissingFields(const PointInT &point_in, PointOutT &point_out) const
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...
Project to the closest point on the polynonomial surface.
uint32_t height
The point cloud height (if organized as an image-structure).
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.
MLSProjectionResults projectPoint(const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors=0) const
Project a point using the specified method.
double z_uu
The partial derivative d^2z/du^2.
Data structure used to store the MLS projection results.
Eigen::Vector3d point
The projected point.
uint32_t width
The point cloud width (if organized as an image-structure).
double v
The u-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 getMLSCoordinates(const Eigen::Vector3d &pt, double &u, double &v, double &w) const
Given a point calculate it's 3D location in the MLS frame.
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.
pcl::search::Search< PointInT >::Ptr KdTreePtr
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.
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...
float distance(const PointT &p1, const PointT &p2)
Eigen::Vector3d u_axis
The axis corresponding to the u-coordinates of the local plane of the query point.
Eigen::Vector3d mean
The mean point of all the neighbors.
void process(PointCloudOut &output)
Base method for surface reconstruction for all points given in <setInputCloud (), setIndices ()> ...
Eigen::Vector3d v_axis
The axis corresponding to the v-coordinates of the local plane of the query point.
Project to the mls plane.
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.
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...
A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling.
MLSVoxelGrid(PointCloudInConstPtr &cloud, IndicesPtr &indices, float voxel_size)
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
void getIndexIn3D(uint64_t index_1d, Eigen::Vector3i &index_3d) const
void getPosition(const uint64_t &index_1d, Eigen::Vector3f &point) const
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.
double getPolynomialValue(const double u, const double v) const
Calculate the polynomial.
void computeMLSPointNormal(int index, const std::vector< int > &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 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.
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.
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
void getCellIndex(const Eigen::Vector3f &p, Eigen::Vector3i &index) const
Define methods for centroid estimation and covariance matrix calculus.
std::map< uint64_t, Leaf > HashMap