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> 47 #include <pcl/common/centroid.h> 48 #include <pcl/common/eigen.h> 49 #include <pcl/common/geometry.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 const float tmp =
static_cast<float> (search_radius_ / 2.0f);
116 const 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 (
size_t 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 const 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 const double u = (*rng_uniform_distribution_) ();
223 const 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 (!std::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;
472 for (
int ui = 0; ui <=
order; ++ui)
475 for (
int vi = 0; vi <=
order - ui; ++vi)
477 result +=
c_vec[j++] * u_pow * v_pow;
492 Eigen::VectorXd u_pow (
order + 2), v_pow (
order + 2);
495 d.z = d.z_u = d.z_v = d.z_uu = d.z_vv = d.z_uv = 0;
496 u_pow (0) = v_pow (0) = 1;
497 for (
int ui = 0; ui <=
order; ++ui)
499 for (
int vi = 0; vi <=
order - ui; ++vi)
502 d.z += u_pow (ui) * v_pow (vi) *
c_vec[j];
506 d.z_u +=
c_vec[j] * ui * u_pow (ui - 1) * v_pow (vi);
509 d.z_v +=
c_vec[j] * vi * u_pow (ui) * v_pow (vi - 1);
511 if (ui >= 1 && vi >= 1)
512 d.z_uv +=
c_vec[j] * ui * u_pow (ui - 1) * vi * v_pow (vi - 1);
515 d.z_uu +=
c_vec[j] * ui * (ui - 1) * u_pow (ui - 2) * v_pow (vi);
518 d.z_vv +=
c_vec[j] * vi * (vi - 1) * u_pow (ui) * v_pow (vi - 2);
521 v_pow (vi + 1) = v_pow (vi) * v;
525 u_pow (ui + 1) = u_pow (ui) * u;
534 Eigen::Vector2f k (1e-5, 1e-5);
544 const double Zlen = std::sqrt (Z);
547 const double disc2 = H * H -
K;
548 assert (disc2 >= 0.0);
549 const double disc = std::sqrt (disc2);
553 if (std::abs (k[0]) > std::abs (k[1])) std::swap (k[0], k[1]);
557 PCL_ERROR (
"No Polynomial fit data, unable to calculate the principle curvatures!\n");
577 const double dist1 = std::abs (gw - w);
581 double e1 = (gu - u) + d.
z_u * gw - d.
z_u * w;
582 double e2 = (gv - v) + d.
z_v * gw - d.
z_v * w;
590 Eigen::MatrixXd J (2, 2);
596 Eigen::Vector2d err (e1, e2);
597 Eigen::Vector2d update = J.inverse () * err;
603 dist2 = std::sqrt ((gu - u) * (gu - u) + (gv - v) * (gv - v) + (gw - w) * (gw - w));
605 err_total = std::sqrt (e1 * e1 + e2 * e2);
607 }
while (err_total > 1e-8 && dist2 < dist1);
620 result.
normal.normalize ();
655 result.
normal.normalize ();
716 template <
typename Po
intT>
void 719 const std::vector<int> &nn_indices,
720 double search_radius,
721 int polynomial_order,
722 boost::function<
double(
const double)> weight_func)
726 Eigen::Vector4d xyz_centroid;
735 Eigen::Vector4d model_coefficients (0, 0, 0, 0);
736 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
737 model_coefficients.head<3> ().matrix () = eigen_vector;
738 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
742 if (!std::isfinite(eigen_vector[0]) || !std::isfinite(eigen_vector[1]) || !std::isfinite(eigen_vector[2]))
753 const double distance =
query_point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
771 order = polynomial_order;
774 const int nr_coeff = (
order + 1) * (
order + 2) / 2;
779 double max_sq_radius = 1;
780 if (weight_func == 0)
782 max_sq_radius = search_radius * search_radius;
783 weight_func = boost::bind (&pcl::MLSResult::computeMLSWeight,
this, _1, max_sq_radius);
790 Eigen::MatrixXd P_weight_Pt (nr_coeff, nr_coeff);
794 std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > de_meaned (
num_neighbors);
795 for (
size_t ni = 0; ni < static_cast<size_t>(
num_neighbors); ++ni)
797 de_meaned[ni][0] = cloud.
points[nn_indices[ni]].x -
mean[0];
798 de_meaned[ni][1] = cloud.
points[nn_indices[ni]].y -
mean[1];
799 de_meaned[ni][2] = cloud.
points[nn_indices[ni]].z -
mean[2];
800 weight_vec (ni) = weight_func (de_meaned[ni].dot (de_meaned[ni]));
805 for (
size_t ni = 0; ni < static_cast<size_t>(
num_neighbors); ++ni)
808 const double u_coord = de_meaned[ni].dot(
u_axis);
809 const double v_coord = de_meaned[ni].dot(
v_axis);
815 for (
int ui = 0; ui <=
order; ++ui)
818 for (
int vi = 0; vi <=
order - ui; ++vi)
820 P (j++, ni) = u_pow * v_pow;
828 const Eigen::MatrixXd P_weight = P * weight_vec.asDiagonal();
829 P_weight_Pt = P_weight * P.transpose ();
830 c_vec = P_weight * f_vec;
831 P_weight_Pt.llt ().solveInPlace (
c_vec);
837 template <
typename Po
intInT,
typename Po
intOutT>
841 voxel_grid_ (), bounding_min_ (), bounding_max_ (), data_size_ (), voxel_size_ (voxel_size)
846 const double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
849 for (
size_t i = 0; i < indices->size (); ++i)
850 if (std::isfinite (cloud->points[(*indices)[i]].x))
853 getCellIndex (cloud->points[(*indices)[i]].getVector3fMap (), pos);
863 template <
typename Po
intInT,
typename Po
intOutT>
void 867 for (
typename MLSVoxelGrid::HashMap::iterator m_it =
voxel_grid_.begin (); m_it !=
voxel_grid_.end (); ++m_it)
869 Eigen::Vector3i index;
873 for (
int x = -1; x <= 1; ++x)
874 for (
int y = -1; y <= 1; ++y)
875 for (
int z = -1; z <= 1; ++z)
876 if (x != 0 || y != 0 || z != 0)
878 Eigen::Vector3i new_index;
879 new_index = index + Eigen::Vector3i (x, y, z);
884 new_voxel_grid[index_1d] = leaf;
892 template <
typename Po
intInT,
typename Po
intOutT>
void 894 PointOutT &point_out)
const 896 PointOutT temp = point_out;
898 point_out.x = temp.x;
899 point_out.y = temp.y;
900 point_out.z = temp.z;
903 #define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares<T,OutT>; 904 #define PCL_INSTANTIATE_MovingLeastSquaresOMP(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquaresOMP<T,OutT>; 906 #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.
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< PointOutT >, Eigen::aligned_allocator< PointCloud< PointOutT > > > 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.
MLSProjectionResults projectPointOrthogonalToPolynomialSurface(const double u, const double v, const double w) const
Project a point orthogonal to the polynomial surface.
boost::shared_ptr< std::vector< int > > IndicesPtr
MLSProjectionResults projectPointToMLSPlane(const double u, const double v) const
Project a point onto the MLS plane.
double z_vv
The partial derivative d^2z/dv^2.
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 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.
Project to the closest point on the polynonomial surface.
void process(PointCloudOut &output) override
Base method for surface reconstruction for all points given in <setInputCloud (), setIndices ()> ...
uint32_t height
The point cloud height (if organized as an image-structure).
int order
The order of the polynomial.
Eigen::Vector4f bounding_min_
double z_uu
The partial derivative d^2z/du^2.
double getPolynomialValue(const double u, const double v) const
Calculate the polynomial.
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.
MLSProjectionResults projectPoint(const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors=0) const
Project a point using the specified method.
void getIndexIn3D(uint64_t index_1d, Eigen::Vector3i &index_3d) const
PolynomialPartialDerivative getPolynomialPartialDerivative(const double u, const double v) const
Calculate the polynomial's first and second partial derivatives.
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...
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.
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
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...
void performProcessing(PointCloudOut &output) override
Abstract surface reconstruction method.
A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling.
MLSVoxelGrid(PointCloudInConstPtr &cloud, IndicesPtr &indices, float voxel_size)
void getIndexIn1D(const Eigen::Vector3i &index, uint64_t &index_1d) const
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.
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
void getPosition(const uint64_t &index_1d, Eigen::Vector3f &point) const
float curvature
The curvature at the query point.
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...
double z
The z component of the polynomial evaluated at z(u, v).
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.
void getCellIndex(const Eigen::Vector3f &p, Eigen::Vector3i &index) const
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
void copyMissingFields(const PointInT &point_in, PointOutT &point_out) const
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.
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
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.
Eigen::Vector2f calculatePrincipleCurvatures(const double u, const double v) const
Calculate the principle curvatures using the polynomial surface.
std::map< uint64_t, Leaf > HashMap