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>
56 template <
typename Po
intInT,
typename Po
intOutT>
void
67 normals_->header = input_->header;
69 normals_->width = normals_->height = 0;
70 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 ());
100 if (input_->isOrganized ())
104 setSearchMethod (tree);
108 tree_->setInputCloud (input_);
110 switch (upsample_method_)
113 case (RANDOM_UNIFORM_DENSITY):
115 rng_alg_.seed (static_cast<unsigned> (std::time (0)));
116 float tmp =
static_cast<float> (search_radius_ / 2.0f);
117 boost::uniform_real<float> uniform_distrib (-tmp, tmp);
118 rng_uniform_distribution_.reset (
new boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > (rng_alg_, uniform_distrib));
120 mls_results_.resize (1);
124 case (VOXEL_GRID_DILATION):
125 case (DISTINCT_CLOUD):
127 mls_results_.resize (input_->size ());
132 mls_results_.resize (1);
138 performProcessing (output);
140 if (compute_normals_)
142 normals_->height = 1;
143 normals_->width =
static_cast<uint32_t
> (normals_->size ());
145 for (
unsigned int i = 0; i < output.
size (); ++i)
158 output.
width =
static_cast<uint32_t
> (output.
size ());
164 template <
typename Po
intInT,
typename Po
intOutT>
void
166 const std::vector<int> &nn_indices,
167 std::vector<float> &nn_sqr_dists,
178 Eigen::Vector4d xyz_centroid;
187 Eigen::Vector4d model_coefficients;
188 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
189 model_coefficients.head<3> ().matrix () = eigen_vector;
190 model_coefficients[3] = 0;
191 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
194 Eigen::Vector3d point = input_->points[index].getVector3fMap ().template cast<double> ();
195 double distance = point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
196 point -= distance * model_coefficients.head<3> ();
198 float curvature =
static_cast<float> (covariance_matrix.trace ());
201 curvature = fabsf (
float (eigen_value /
double (curvature)));
205 Eigen::Vector3d plane_normal = model_coefficients.head<3> ();
207 Eigen::VectorXd c_vec;
209 Eigen::Vector3d v_axis (0.0f, 0.0f, 0.0f), u_axis (0.0f, 0.0f, 0.0f);
215 if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_)
219 std::vector<Eigen::Vector3d> de_meaned (nn_indices.size ());
220 for (
size_t ni = 0; ni < nn_indices.size (); ++ni)
222 de_meaned[ni][0] = input_->points[nn_indices[ni]].x - point[0];
223 de_meaned[ni][1] = input_->points[nn_indices[ni]].y - point[1];
224 de_meaned[ni][2] = input_->points[nn_indices[ni]].z - point[2];
225 nn_sqr_dists[ni] =
static_cast<float> (de_meaned[ni].dot (de_meaned[ni]));
229 Eigen::VectorXd weight_vec (nn_indices.size ());
230 Eigen::MatrixXd P (nr_coeff_, nn_indices.size ());
231 Eigen::VectorXd f_vec (nn_indices.size ());
232 Eigen::MatrixXd P_weight;
233 Eigen::MatrixXd P_weight_Pt (nr_coeff_, nr_coeff_);
236 v_axis = plane_normal.unitOrthogonal ();
237 u_axis = plane_normal.cross (v_axis);
241 double u_coord, v_coord, u_pow, v_pow;
242 for (
size_t ni = 0; ni < nn_indices.size (); ++ni)
245 weight_vec (ni) = exp (-nn_sqr_dists[ni] / sqr_gauss_param_);
248 u_coord = de_meaned[ni].dot (u_axis);
249 v_coord = de_meaned[ni].dot (v_axis);
250 f_vec (ni) = de_meaned[ni].dot (plane_normal);
255 for (
int ui = 0; ui <= order_; ++ui)
258 for (
int vi = 0; vi <= order_ - ui; ++vi)
260 P (j++, ni) = u_pow * v_pow;
268 P_weight = P * weight_vec.asDiagonal ();
269 P_weight_Pt = P_weight * P.transpose ();
270 c_vec = P_weight * f_vec;
271 P_weight_Pt.llt ().solveInPlace (c_vec);
274 switch (upsample_method_)
278 Eigen::Vector3d normal = plane_normal;
280 if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0]))
282 point += (c_vec[0] * plane_normal);
285 if (compute_normals_)
286 normal = plane_normal - c_vec[order_ + 1] * u_axis - c_vec[1] * v_axis;
290 aux.x =
static_cast<float> (point[0]);
291 aux.y =
static_cast<float> (point[1]);
292 aux.z =
static_cast<float> (point[2]);
295 if (compute_normals_)
298 aux_normal.normal_x =
static_cast<float> (normal[0]);
299 aux_normal.normal_y =
static_cast<float> (normal[1]);
300 aux_normal.normal_z =
static_cast<float> (normal[2]);
302 projected_points_normals.
push_back (aux_normal);
303 corresponding_input_indices.
indices.push_back (index);
309 case (SAMPLE_LOCAL_PLANE):
312 for (
float u_disp = -static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
313 for (
float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
314 if (u_disp*u_disp + v_disp*v_disp < upsampling_radius_*upsampling_radius_)
316 PointOutT projected_point;
318 projectPointToMLSSurface (u_disp, v_disp, u_axis, v_axis, plane_normal, point,
320 static_cast<int> (nn_indices.size ()),
321 projected_point, projected_normal);
323 projected_points.
push_back (projected_point);
324 corresponding_input_indices.
indices.push_back (index);
325 if (compute_normals_)
326 projected_points_normals.
push_back (projected_normal);
331 case (RANDOM_UNIFORM_DENSITY):
334 int num_points_to_add =
static_cast<int> (floor (desired_num_points_in_radius_ / 2.0 / static_cast<double> (nn_indices.size ())));
337 if (num_points_to_add <= 0)
340 Eigen::Vector3d normal = plane_normal;
341 if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0]))
344 point += (c_vec[0] * plane_normal);
346 if (compute_normals_)
347 normal = plane_normal - c_vec[order_ + 1] * u_axis - c_vec[1] * v_axis;
350 aux.x =
static_cast<float> (point[0]);
351 aux.y =
static_cast<float> (point[1]);
352 aux.z =
static_cast<float> (point[2]);
354 corresponding_input_indices.
indices.push_back (index);
356 if (compute_normals_)
359 aux_normal.normal_x =
static_cast<float> (normal[0]);
360 aux_normal.normal_y =
static_cast<float> (normal[1]);
361 aux_normal.normal_z =
static_cast<float> (normal[2]);
363 projected_points_normals.
push_back (aux_normal);
369 for (
int num_added = 0; num_added < num_points_to_add;)
371 float u_disp = (*rng_uniform_distribution_) (),
372 v_disp = (*rng_uniform_distribution_) ();
374 if (u_disp * u_disp + v_disp * v_disp > search_radius_ * search_radius_/4)
378 PointOutT projected_point;
380 projectPointToMLSSurface (u_disp, v_disp, u_axis, v_axis, plane_normal, point,
382 static_cast<int> (nn_indices.size ()),
383 projected_point, projected_normal);
385 projected_points.
push_back (projected_point);
386 corresponding_input_indices.
indices.push_back (index);
387 if (compute_normals_)
388 projected_points_normals.
push_back (projected_normal);
396 case (VOXEL_GRID_DILATION):
397 case (DISTINCT_CLOUD):
401 mls_result =
MLSResult (point, plane_normal, u_axis, v_axis, c_vec, static_cast<int> (nn_indices.size ()), curvature);
408 template <
typename Po
intInT,
typename Po
intOutT>
void
410 Eigen::Vector3d &u, Eigen::Vector3d &v,
411 Eigen::Vector3d &plane_normal,
412 Eigen::Vector3d &mean,
414 Eigen::VectorXd &c_vec,
416 PointOutT &result_point,
419 double n_disp = 0.0f;
420 double d_u = 0.0f, d_v = 0.0f;
423 if (polynomial_fit_ && num_neighbors >= 5*nr_coeff_ && pcl_isfinite (c_vec[0]))
428 float u_pow = 1.0f, v_pow = 1.0f, u_pow_prev = 1.0f, v_pow_prev = 1.0f;
429 for (
int ui = 0; ui <= order_; ++ui)
432 for (
int vi = 0; vi <= order_ - ui; ++vi)
435 n_disp += u_pow * v_pow * c_vec[j++];
439 d_u += c_vec[j-1] * ui * u_pow_prev * v_pow;
441 d_v += c_vec[j-1] * vi * u_pow * v_pow_prev;
451 result_point.x =
static_cast<float> (mean[0] + u[0] * u_disp + v[0] * v_disp + plane_normal[0] * n_disp);
452 result_point.y =
static_cast<float> (mean[1] + u[1] * u_disp + v[1] * v_disp + plane_normal[1] * n_disp);
453 result_point.z =
static_cast<float> (mean[2] + u[2] * u_disp + v[2] * v_disp + plane_normal[2] * n_disp);
455 Eigen::Vector3d normal = plane_normal - d_u * u - d_v * v;
458 result_normal.normal_x =
static_cast<float> (normal[0]);
459 result_normal.normal_y =
static_cast<float> (normal[1]);
460 result_normal.normal_z =
static_cast<float> (normal[2]);
465 template <
typename Po
intInT,
typename Po
intOutT>
void
469 nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
473 std::vector<int> nn_indices;
474 std::vector<float> nn_sqr_dists;
476 size_t mls_result_index = 0;
479 for (
size_t cp = 0; cp < indices_->size (); ++cp)
482 if (!searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
488 if (nn_indices.size () < 3)
495 int index = (*indices_)[cp];
497 if (upsample_method_ == VOXEL_GRID_DILATION || upsample_method_ == DISTINCT_CLOUD)
498 mls_result_index = index;
500 computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]);
504 for (
size_t pp = 0; pp < projected_points.
size (); ++pp)
505 copyMissingFields (input_->points[(*indices_)[cp]], projected_points[pp]);
509 output.
insert (output.
end (), projected_points.
begin (), projected_points.
end ());
510 if (compute_normals_)
511 normals_->insert (normals_->end (), projected_points_normals.
begin (), projected_points_normals.
end ());
515 performUpsampling (output);
520 template <
typename Po
intInT,
typename Po
intOutT>
void
521 pcl::MovingLeastSquaresOMP<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
524 nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
527 unsigned int threads = threads_ == 0 ? 1 : threads_;
530 typename PointCloudOut::CloudVectorType projected_points (threads);
531 typename NormalCloud::CloudVectorType projected_points_normals (threads);
532 std::vector<PointIndices> corresponding_input_indices (threads);
535 #pragma omp parallel for schedule (dynamic,1000) num_threads (threads)
536 for (
int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
540 std::vector<int> nn_indices;
541 std::vector<float> nn_sqr_dists;
544 if (this->searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
548 if (nn_indices.size () >= 3)
551 int tn = omp_get_thread_num ();
554 size_t pp_size = projected_points[tn].size ();
557 int index = (*indices_)[cp];
558 size_t mls_result_index = 0;
560 if (upsample_method_ == VOXEL_GRID_DILATION || upsample_method_ == DISTINCT_CLOUD)
561 mls_result_index = index;
563 this->computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], this->mls_results_[mls_result_index]);
566 for (
size_t pp = pp_size; pp < projected_points[tn].size (); ++pp)
567 this->copyMissingFields (input_->points[(*indices_)[cp]], projected_points[tn][pp]);
574 for (
unsigned int tn = 0; tn < threads; ++tn)
576 output.insert (output.end (), projected_points[tn].begin (), projected_points[tn].end ());
577 corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (),
578 corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ());
579 if (compute_normals_)
580 normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ());
584 this->performUpsampling (output);
589 template <
typename Po
intInT,
typename Po
intOutT>
void
592 if (upsample_method_ == DISTINCT_CLOUD)
594 for (
size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i)
597 if (!pcl_isfinite (distinct_cloud_->points[dp_i].x))
602 std::vector<int> nn_indices;
603 std::vector<float> nn_dists;
604 tree_->nearestKSearch (distinct_cloud_->points[dp_i], 1, nn_indices, nn_dists);
605 int input_index = nn_indices.front ();
609 if (mls_results_[input_index].valid ==
false)
612 Eigen::Vector3d add_point = distinct_cloud_->points[dp_i].getVector3fMap ().template cast<double> ();
614 float u_disp =
static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].u_axis)),
615 v_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].v_axis));
617 PointOutT result_point;
619 projectPointToMLSSurface (u_disp, v_disp,
620 mls_results_[input_index].u_axis, mls_results_[input_index].v_axis,
621 mls_results_[input_index].plane_normal,
622 mls_results_[input_index].mean,
623 mls_results_[input_index].curvature,
624 mls_results_[input_index].c_vec,
625 mls_results_[input_index].num_neighbors,
626 result_point, result_normal);
629 copyMissingFields (input_->points[input_index], result_point);
632 corresponding_input_indices_->indices.push_back (input_index);
635 if (compute_normals_)
636 normals_->push_back (result_normal);
642 if (upsample_method_ == VOXEL_GRID_DILATION)
644 MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);
645 for (
int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
648 for (
typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.
voxel_grid_.begin (); m_it != voxel_grid.
voxel_grid_.end (); ++m_it)
659 std::vector<int> nn_indices;
660 std::vector<float> nn_dists;
661 tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
662 int input_index = nn_indices.front ();
666 if (mls_results_[input_index].valid ==
false)
669 Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> ();
670 float u_disp =
static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].u_axis)),
671 v_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].v_axis));
673 PointOutT result_point;
675 projectPointToMLSSurface (u_disp, v_disp,
676 mls_results_[input_index].u_axis, mls_results_[input_index].v_axis,
677 mls_results_[input_index].plane_normal,
678 mls_results_[input_index].mean,
679 mls_results_[input_index].curvature,
680 mls_results_[input_index].c_vec,
681 mls_results_[input_index].num_neighbors,
682 result_point, result_normal);
685 copyMissingFields (input_->points[input_index], result_point);
688 corresponding_input_indices_->indices.push_back (input_index);
692 if (compute_normals_)
693 normals_->push_back (result_normal);
699 template <
typename Po
intInT,
typename Po
intOutT>
701 const Eigen::Vector3d &a_plane_normal,
702 const Eigen::Vector3d &a_u,
703 const Eigen::Vector3d &a_v,
704 const Eigen::VectorXd a_c_vec,
705 const int a_num_neighbors,
706 const float &a_curvature) :
707 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),
708 curvature (a_curvature), valid (true)
713 template <
typename Po
intInT,
typename Po
intOutT>
717 voxel_grid_ (), bounding_min_ (), bounding_max_ (), data_size_ (),
voxel_size_ (voxel_size)
722 double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
725 for (
unsigned int i = 0; i < indices->size (); ++i)
726 if (pcl_isfinite (cloud->points[(*indices)[i]].x))
729 getCellIndex (cloud->points[(*indices)[i]].getVector3fMap (), pos);
739 template <
typename Po
intInT,
typename Po
intOutT>
void
742 HashMap new_voxel_grid = voxel_grid_;
743 for (
typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid_.begin (); m_it != voxel_grid_.end (); ++m_it)
745 Eigen::Vector3i index;
746 getIndexIn3D (m_it->first, index);
749 for (
int x = -1; x <= 1; ++x)
750 for (
int y = -1; y <= 1; ++y)
751 for (
int z = -1; z <= 1; ++z)
752 if (x != 0 || y != 0 || z != 0)
754 Eigen::Vector3i new_index;
755 new_index = index + Eigen::Vector3i (x, y, z);
758 getIndexIn1D (new_index, index_1d);
760 new_voxel_grid[index_1d] = leaf;
763 voxel_grid_ = new_voxel_grid;
768 template <
typename Po
intInT,
typename Po
intOutT>
void
770 PointOutT &point_out)
const
772 PointOutT temp = point_out;
774 point_out.x = temp.x;
775 point_out.y = temp.y;
776 point_out.z = temp.z;
780 #define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares<T,OutT>;
783 #define PCL_INSTANTIATE_MovingLeastSquaresOMP(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquaresOMP<T,OutT>;
786 #endif // PCL_SURFACE_IMPL_MLS_H_
A point structure representing normal coordinates and the surface curvature estimate.
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.
struct pcl::_PointXYZHSV EIGEN_ALIGN16
virtual void performProcessing(PointCloudOut &output)
Abstract surface reconstruction method.
boost::shared_ptr< std::vector< int > > IndicesPtr
Eigen::Vector4f bounding_max_
std::vector< int > indices
uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
void projectPointToMLSSurface(float &u_disp, float &v_disp, Eigen::Vector3d &u_axis, Eigen::Vector3d &v_axis, Eigen::Vector3d &n_axis, Eigen::Vector3d &mean, float &curvature, Eigen::VectorXd &c_vec, int num_neighbors, PointOutT &result_point, pcl::Normal &result_normal) const
Fits a point (sample point) given in the local plane coordinates of an input point (query point) to t...
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Eigen::Vector4f bounding_min_
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
void computeMLSPointNormal(int index, const std::vector< int > &nn_indices, std::vector< float > &nn_sqr_dists, 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.
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.
pcl::search::Search< PointInT >::Ptr KdTreePtr
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...
void process(PointCloudOut &output)
Base method for surface reconstruction for all points given in <setInputCloud (), setIndices ()> ...
PointCloudIn::ConstPtr PointCloudInConstPtr
PointCloud represents the base class in PCL for storing collections of 3D 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...
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
float voxel_size_
Voxel size for the VOXEL_GRID_DILATION upsampling method.
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
void getPosition(const uint64_t &index_1d, Eigen::Vector3f &point) const
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
void copyMissingFields(const PointInT &point_in, PointOutT &point_out) const
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
Data structure used to store the results of the MLS fitting.
std::map< uint64_t, Leaf > HashMap
uint32_t height
The point cloud height (if organized as an image-structure).