40 #ifndef PCL_SURFACE_IMPL_MLS_H_
41 #define PCL_SURFACE_IMPL_MLS_H_
43 #include <pcl/type_traits.h>
44 #include <pcl/surface/mls.h>
45 #include <pcl/common/io.h>
47 #include <pcl/common/copy_point.h>
49 #include <pcl/common/eigen.h>
51 #include <pcl/search/kdtree.h>
52 #include <pcl/search/organized.h>
59 template <
typename Po
intInT,
typename Po
intOutT>
void
70 normals_->header = input_->header;
72 normals_->width = normals_->height = 0;
73 normals_->points.clear ();
77 output.
header = input_->header;
81 if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
83 PCL_ERROR (
"[pcl::%s::process] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
88 if (upsample_method_ == DISTINCT_CLOUD && !distinct_cloud_)
90 PCL_ERROR (
"[pcl::%s::process] Upsample method was set to DISTINCT_CLOUD, but no distinct cloud was specified.\n", getClassName ().c_str ());
101 if (input_->isOrganized ())
105 setSearchMethod (tree);
109 tree_->setInputCloud (input_);
111 switch (upsample_method_)
114 case (RANDOM_UNIFORM_DENSITY):
116 std::random_device rd;
118 const double tmp = search_radius_ / 2.0;
119 rng_uniform_distribution_.reset (
new std::uniform_real_distribution<> (-tmp, tmp));
123 case (VOXEL_GRID_DILATION):
124 case (DISTINCT_CLOUD):
126 if (!cache_mls_results_)
127 PCL_WARN (
"The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n");
129 cache_mls_results_ =
true;
136 if (cache_mls_results_)
138 mls_results_.resize (input_->size ());
142 mls_results_.resize (1);
146 performProcessing (output);
148 if (compute_normals_)
150 normals_->height = 1;
151 normals_->width = normals_->size ();
153 for (std::size_t i = 0; i < output.
size (); ++i)
155 using FieldList =
typename pcl::traits::fieldList<PointOutT>::type;
172 template <
typename Po
intInT,
typename Po
intOutT>
void
174 const std::vector<int> &nn_indices,
183 mls_result.
computeMLSSurface<PointInT> (*input_, index, nn_indices, search_radius_, order_);
185 switch (upsample_method_)
190 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
194 case (SAMPLE_LOCAL_PLANE):
197 for (
float u_disp = -
static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
198 for (
float v_disp = -
static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
199 if (u_disp * u_disp + v_disp * v_disp < upsampling_radius_ * upsampling_radius_)
202 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
207 case (RANDOM_UNIFORM_DENSITY):
210 const int num_points_to_add =
static_cast<int> (std::floor (desired_num_points_in_radius_ / 2.0 /
static_cast<double> (nn_indices.size ())));
213 if (num_points_to_add <= 0)
217 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
222 for (
int num_added = 0; num_added < num_points_to_add;)
224 const double u = (*rng_uniform_distribution_) (rng_);
225 const double v = (*rng_uniform_distribution_) (rng_);
228 if (u * u + v * v > search_radius_ * search_radius_ / 4)
237 addProjectedPointNormal (index, proj.
point, proj.
normal, mls_result.
curvature, projected_points, projected_points_normals, corresponding_input_indices);
250 template <
typename Po
intInT,
typename Po
intOutT>
void
252 const Eigen::Vector3d &point,
253 const Eigen::Vector3d &normal,
260 aux.x =
static_cast<float> (point[0]);
261 aux.y =
static_cast<float> (point[1]);
262 aux.z =
static_cast<float> (point[2]);
265 copyMissingFields ((*input_)[index], aux);
268 corresponding_input_indices.
indices.push_back (index);
270 if (compute_normals_)
273 aux_normal.normal_x =
static_cast<float> (normal[0]);
274 aux_normal.normal_y =
static_cast<float> (normal[1]);
275 aux_normal.normal_z =
static_cast<float> (normal[2]);
277 projected_points_normals.
push_back (aux_normal);
282 template <
typename Po
intInT,
typename Po
intOutT>
void
286 nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
290 const unsigned int threads = threads_ == 0 ? 1 : threads_;
294 std::vector<PointIndices> corresponding_input_indices (threads);
298 #pragma omp parallel for \
300 shared(corresponding_input_indices, projected_points, projected_points_normals) \
301 schedule(dynamic,1000) \
303 for (
int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
307 std::vector<int> nn_indices;
308 std::vector<float> nn_sqr_dists;
311 if (searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
314 if (nn_indices.size () >= 3)
318 const int tn = omp_get_thread_num ();
320 std::size_t pp_size = projected_points[tn].size ();
327 const int index = (*indices_)[cp];
329 std::size_t mls_result_index = 0;
330 if (cache_mls_results_)
331 mls_result_index = index;
334 computeMLSPointNormal (index, nn_indices, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], mls_results_[mls_result_index]);
337 for (std::size_t pp = pp_size; pp < projected_points[tn].
size (); ++pp)
338 copyMissingFields ((*input_)[(*indices_)[cp]], projected_points[tn][pp]);
340 computeMLSPointNormal (index, nn_indices, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]);
343 output.
insert (output.
end (), projected_points.
begin (), projected_points.
end ());
344 if (compute_normals_)
345 normals_->insert (normals_->end (), projected_points_normals.
begin (), projected_points_normals.
end ());
353 for (
unsigned int tn = 0; tn < threads; ++tn)
355 output.
insert (output.
end (), projected_points[tn].begin (), projected_points[tn].end ());
356 corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (),
357 corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ());
358 if (compute_normals_)
359 normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ());
364 performUpsampling (output);
368 template <
typename Po
intInT,
typename Po
intOutT>
void
372 if (upsample_method_ == DISTINCT_CLOUD)
375 for (std::size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i)
378 if (!std::isfinite ((*distinct_cloud_)[dp_i].x))
383 std::vector<int> nn_indices;
384 std::vector<float> nn_dists;
385 tree_->nearestKSearch ((*distinct_cloud_)[dp_i], 1, nn_indices, nn_dists);
386 int input_index = nn_indices.front ();
390 if (mls_results_[input_index].valid ==
false)
393 Eigen::Vector3d add_point = (*distinct_cloud_)[dp_i].getVector3fMap ().template cast<double> ();
395 addProjectedPointNormal (input_index, proj.
point, proj.
normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
401 if (upsample_method_ == VOXEL_GRID_DILATION)
405 MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);
406 for (
int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
409 for (
typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.
voxel_grid_.begin (); m_it != voxel_grid.
voxel_grid_.end (); ++m_it)
420 std::vector<int> nn_indices;
421 std::vector<float> nn_dists;
422 tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
423 int input_index = nn_indices.front ();
427 if (mls_results_[input_index].valid ==
false)
430 Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> ();
432 addProjectedPointNormal (input_index, proj.
point, proj.
normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
439 const Eigen::Vector3d &a_mean,
440 const Eigen::Vector3d &a_plane_normal,
441 const Eigen::Vector3d &a_u,
442 const Eigen::Vector3d &a_v,
443 const Eigen::VectorXd &a_c_vec,
444 const int a_num_neighbors,
445 const float a_curvature,
447 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),
448 curvature (a_curvature), order (a_order), valid (true)
454 Eigen::Vector3d delta = pt - mean;
455 u = delta.dot (u_axis);
456 v = delta.dot (v_axis);
457 w = delta.dot (plane_normal);
463 Eigen::Vector3d delta = pt - mean;
464 u = delta.dot (u_axis);
465 v = delta.dot (v_axis);
476 for (
int ui = 0; ui <= order; ++ui)
479 for (
int vi = 0; vi <= order - ui; ++vi)
481 result += c_vec[j++] * u_pow * v_pow;
496 Eigen::VectorXd u_pow (order + 2), v_pow (order + 2);
499 d.z = d.z_u = d.z_v = d.z_uu = d.z_vv = d.z_uv = 0;
500 u_pow (0) = v_pow (0) = 1;
501 for (
int ui = 0; ui <= order; ++ui)
503 for (
int vi = 0; vi <= order - ui; ++vi)
506 d.z += u_pow (ui) * v_pow (vi) * c_vec[j];
510 d.z_u += c_vec[j] * ui * u_pow (ui - 1) * v_pow (vi);
513 d.z_v += c_vec[j] * vi * u_pow (ui) * v_pow (vi - 1);
515 if (ui >= 1 && vi >= 1)
516 d.z_uv += c_vec[j] * ui * u_pow (ui - 1) * vi * v_pow (vi - 1);
519 d.z_uu += c_vec[j] * ui * (ui - 1) * u_pow (ui - 2) * v_pow (vi);
522 d.z_vv += c_vec[j] * vi * (vi - 1) * u_pow (ui) * v_pow (vi - 2);
525 v_pow (vi + 1) = v_pow (vi) * v;
529 u_pow (ui + 1) = u_pow (ui) * u;
538 Eigen::Vector2f k (1e-5, 1e-5);
544 if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
548 const double Zlen = std::sqrt (Z);
551 const double disc2 = H * H -
K;
552 assert (disc2 >= 0.0);
553 const double disc = std::sqrt (disc2);
557 if (std::abs (k[0]) > std::abs (k[1])) std::swap (k[0], k[1]);
561 PCL_ERROR (
"No Polynomial fit data, unable to calculate the principle curvatures!\n");
575 result.
normal = plane_normal;
576 if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
581 const double dist1 = std::abs (gw - w);
585 double e1 = (gu - u) + d.
z_u * gw - d.
z_u * w;
586 double e2 = (gv - v) + d.
z_v * gw - d.
z_v * w;
594 Eigen::MatrixXd J (2, 2);
600 Eigen::Vector2d err (e1, e2);
601 Eigen::Vector2d update = J.inverse () * err;
605 d = getPolynomialPartialDerivative (gu, gv);
607 dist2 = std::sqrt ((gu - u) * (gu - u) + (gv - v) * (gv - v) + (gw - w) * (gw - w));
609 err_total = std::sqrt (e1 * e1 + e2 * e2);
611 }
while (err_total > 1e-8 && dist2 < dist1);
617 d = getPolynomialPartialDerivative (u, v);
624 result.
normal.normalize ();
627 result.
point = mean + gu * u_axis + gv * v_axis + gw * plane_normal;
638 result.
normal = plane_normal;
639 result.
point = mean + u * u_axis + v * v_axis;
652 result.
normal = plane_normal;
654 if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
659 result.
normal.normalize ();
662 result.
point = mean + u * u_axis + v * v_axis + w * plane_normal;
671 getMLSCoordinates (pt, u, v, w);
674 if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
676 if (method == ORTHOGONAL)
677 proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
679 proj = projectPointSimpleToPolynomialSurface (u, v);
683 proj = projectPointToMLSPlane (u, v);
693 if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
695 if (method == ORTHOGONAL)
698 getMLSCoordinates (query_point, u, v, w);
699 proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
704 proj.
point = mean + (c_vec[0] * plane_normal);
707 proj.
normal = plane_normal - c_vec[order + 1] * u_axis - c_vec[1] * v_axis;
713 proj.
normal = plane_normal;
720 template <
typename Po
intT>
void
723 const std::vector<int> &nn_indices,
724 double search_radius,
725 int polynomial_order,
726 std::function<
double(
const double)> weight_func)
729 EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix;
730 Eigen::Vector4d xyz_centroid;
737 EIGEN_ALIGN16 Eigen::Vector3d::Scalar eigen_value;
738 EIGEN_ALIGN16 Eigen::Vector3d eigen_vector;
739 Eigen::Vector4d model_coefficients (0, 0, 0, 0);
740 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
741 model_coefficients.head<3> ().matrix () = eigen_vector;
742 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
744 query_point = cloud[index].getVector3fMap ().template cast<double> ();
746 if (!std::isfinite(eigen_vector[0]) || !std::isfinite(eigen_vector[1]) || !std::isfinite(eigen_vector[2]))
757 const double distance = query_point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
758 mean = query_point -
distance * model_coefficients.head<3> ();
760 curvature = covariance_matrix.trace ();
763 curvature = std::abs (eigen_value / curvature);
766 plane_normal = model_coefficients.head<3> ();
769 v_axis = plane_normal.unitOrthogonal ();
770 u_axis = plane_normal.cross (v_axis);
774 num_neighbors =
static_cast<int> (nn_indices.size ());
775 order = polynomial_order;
778 const int nr_coeff = (order + 1) * (order + 2) / 2;
780 if (num_neighbors >= nr_coeff)
783 weight_func = [=] (
const double sq_dist) {
return this->computeMLSWeight (sq_dist, search_radius * search_radius); };
786 Eigen::VectorXd weight_vec (num_neighbors);
787 Eigen::MatrixXd P (nr_coeff, num_neighbors);
788 Eigen::VectorXd f_vec (num_neighbors);
789 Eigen::MatrixXd P_weight_Pt (nr_coeff, nr_coeff);
793 std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > de_meaned (num_neighbors);
794 for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
796 de_meaned[ni][0] = cloud[nn_indices[ni]].x - mean[0];
797 de_meaned[ni][1] = cloud[nn_indices[ni]].y - mean[1];
798 de_meaned[ni][2] = cloud[nn_indices[ni]].z - mean[2];
799 weight_vec (ni) = weight_func (de_meaned[ni].dot (de_meaned[ni]));
804 for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
807 const double u_coord = de_meaned[ni].dot(u_axis);
808 const double v_coord = de_meaned[ni].dot(v_axis);
809 f_vec (ni) = de_meaned[ni].dot (plane_normal);
814 for (
int ui = 0; ui <= order; ++ui)
817 for (
int vi = 0; vi <= order - ui; ++vi)
819 P (j++, ni) = u_pow * v_pow;
827 const Eigen::MatrixXd P_weight = P * weight_vec.asDiagonal();
828 P_weight_Pt = P_weight * P.transpose ();
829 c_vec = P_weight * f_vec;
830 P_weight_Pt.llt ().solveInPlace (c_vec);
836 template <
typename Po
intInT,
typename Po
intOutT>
840 voxel_grid_ (), data_size_ (), voxel_size_ (voxel_size)
845 const double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
848 for (std::size_t i = 0; i < indices->size (); ++i)
849 if (std::isfinite ((*cloud)[(*indices)[i]].x))
852 getCellIndex ((*cloud)[(*indices)[i]].getVector3fMap (), pos);
862 template <
typename Po
intInT,
typename Po
intOutT>
void
865 HashMap new_voxel_grid = voxel_grid_;
866 for (
typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid_.begin (); m_it != voxel_grid_.end (); ++m_it)
868 Eigen::Vector3i index;
869 getIndexIn3D (m_it->first, index);
872 for (
int x = -1; x <= 1; ++x)
873 for (
int y = -1; y <= 1; ++y)
874 for (
int z = -1; z <= 1; ++z)
875 if (x != 0 || y != 0 || z != 0)
877 Eigen::Vector3i new_index;
878 new_index = index + Eigen::Vector3i (x, y, z);
881 getIndexIn1D (new_index, index_1d);
883 new_voxel_grid[index_1d] = leaf;
886 voxel_grid_ = new_voxel_grid;
891 template <
typename Po
intInT,
typename Po
intOutT>
void
893 PointOutT &point_out)
const
895 PointOutT temp = point_out;
897 point_out.x = temp.x;
898 point_out.y = temp.y;
899 point_out.z = temp.z;
902 #define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares<T,OutT>;
903 #define PCL_INSTANTIATE_MovingLeastSquaresOMP(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquaresOMP<T,OutT>;
905 #endif // PCL_SURFACE_IMPL_MLS_H_