Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
mls.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009-2011, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 * $Id$
37 *
38 */
39
40#ifndef PCL_SURFACE_IMPL_MLS_H_
41#define PCL_SURFACE_IMPL_MLS_H_
42
43#include <pcl/common/centroid.h>
44#include <pcl/common/common.h> // for getMinMax3D
45#include <pcl/common/copy_point.h>
46#include <pcl/common/eigen.h>
47#include <pcl/search/kdtree.h> // for KdTree
48#include <pcl/search/organized.h> // for OrganizedNeighbor
49#include <pcl/surface/mls.h>
50#include <pcl/type_traits.h>
51
52#include <Eigen/Geometry> // for cross
53#include <Eigen/LU> // for inverse
54
55#include <memory>
56
57#ifdef _OPENMP
58#include <omp.h>
59#endif
60
61//////////////////////////////////////////////////////////////////////////////////////////////
62template <typename PointInT, typename PointOutT> void
64{
65 // Reset or initialize the collection of indices
66 corresponding_input_indices_.reset (new PointIndices);
67
68 normals_.reset (new NormalCloud); // always init this since it is dereferenced in performUpsampling
69 // Check if normals have to be computed/saved
70 if (compute_normals_)
71 {
72 // Copy the header
73 normals_->header = input_->header;
74 // Clear the fields in case the method exits before computation
75 normals_->width = normals_->height = 0;
76 normals_->points.clear ();
77 }
78
79 // Copy the header
80 output.header = input_->header;
81 output.width = output.height = 0;
82 output.clear ();
83
84 if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
85 {
86 PCL_ERROR ("[pcl::%s::process] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
87 return;
88 }
89
90 // Check if distinct_cloud_ was set
91 if (upsample_method_ == DISTINCT_CLOUD && !distinct_cloud_)
92 {
93 PCL_ERROR ("[pcl::%s::process] Upsample method was set to DISTINCT_CLOUD, but no distinct cloud was specified.\n", getClassName ().c_str ());
94 return;
95 }
96
97 if (!initCompute ())
98 return;
99
100 // Initialize the spatial locator
101 if (!tree_)
102 {
103 KdTreePtr tree;
104 if (input_->isOrganized ())
106 else
107 tree.reset (new pcl::search::KdTree<PointInT> (false));
108 setSearchMethod (tree);
109 }
110
111 // Send the surface dataset to the spatial locator
112 tree_->setInputCloud (input_);
113
114 switch (upsample_method_)
115 {
116 // Initialize random number generator if necessary
117 case (RANDOM_UNIFORM_DENSITY):
118 {
119 std::random_device rd;
120 rng_.seed (rd());
121 const double tmp = search_radius_ / 2.0;
122 rng_uniform_distribution_ = std::make_unique<std::uniform_real_distribution<>> (-tmp, tmp);
123
124 break;
125 }
126 case (VOXEL_GRID_DILATION):
127 case (DISTINCT_CLOUD):
128 {
129 if (!cache_mls_results_)
130 PCL_WARN ("The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n");
131
132 cache_mls_results_ = true;
133 break;
134 }
135 default:
136 break;
137 }
138
139 if (cache_mls_results_)
140 {
141 mls_results_.resize (input_->size ());
142 }
143 else
144 {
145 mls_results_.resize (1); // Need to have a reference to a single dummy result.
146 }
147
148 // Perform the actual surface reconstruction
149 performProcessing (output);
150
151 if (compute_normals_)
152 {
153 normals_->height = 1;
154 normals_->width = normals_->size ();
155
156 for (std::size_t i = 0; i < output.size (); ++i)
157 {
158 using FieldList = typename pcl::traits::fieldList<PointOutT>::type;
159 pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output[i], "normal_x", (*normals_)[i].normal_x));
160 pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output[i], "normal_y", (*normals_)[i].normal_y));
161 pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output[i], "normal_z", (*normals_)[i].normal_z));
162 pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output[i], "curvature", (*normals_)[i].curvature));
163 }
164
165 }
166
167 // Set proper widths and heights for the clouds
168 output.height = 1;
169 output.width = output.size ();
170
171 deinitCompute ();
172}
173
174//////////////////////////////////////////////////////////////////////////////////////////////
175template <typename PointInT, typename PointOutT> void
177 const pcl::Indices &nn_indices,
178 PointCloudOut &projected_points,
179 NormalCloud &projected_points_normals,
180 PointIndices &corresponding_input_indices,
181 MLSResult &mls_result) const
182{
183 // Note: this method is const because it needs to be thread-safe
184 // (MovingLeastSquaresOMP calls it from multiple threads)
185
186 mls_result.computeMLSSurface<PointInT> (*input_, index, nn_indices, search_radius_, order_);
187
188 switch (upsample_method_)
189 {
190 case (NONE):
191 {
192 const MLSResult::MLSProjectionResults proj = mls_result.projectQueryPoint (projection_method_, nr_coeff_);
193 addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices);
194 break;
195 }
196
197 case (SAMPLE_LOCAL_PLANE):
198 {
199 // Uniformly sample a circle around the query point using the radius and step parameters
200 for (float u_disp = -static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
201 for (float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
202 if (u_disp * u_disp + v_disp * v_disp < upsampling_radius_ * upsampling_radius_)
203 {
205 addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices);
206 }
207 break;
208 }
209
210 case (RANDOM_UNIFORM_DENSITY):
211 {
212 // Compute the local point density and add more samples if necessary
213 const int num_points_to_add = static_cast<int> (std::floor (desired_num_points_in_radius_ / 2.0 / static_cast<double> (nn_indices.size ())));
214
215 // Just add the query point, because the density is good
216 if (num_points_to_add <= 0)
217 {
218 // Just add the current point
219 const MLSResult::MLSProjectionResults proj = mls_result.projectQueryPoint (projection_method_, nr_coeff_);
220 addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices);
221 }
222 else
223 {
224 // Sample the local plane
225 for (int num_added = 0; num_added < num_points_to_add;)
226 {
227 const double u = (*rng_uniform_distribution_) (rng_);
228 const double v = (*rng_uniform_distribution_) (rng_);
229
230 // Check if inside circle; if not, try another coin flip
231 if (u * u + v * v > search_radius_ * search_radius_ / 4)
232 continue;
233
235 if (order_ > 1 && mls_result.num_neighbors >= 5 * nr_coeff_)
236 proj = mls_result.projectPointSimpleToPolynomialSurface (u, v);
237 else
238 proj = mls_result.projectPointToMLSPlane (u, v);
239
240 addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices);
241
242 num_added++;
243 }
244 }
245 break;
246 }
247
248 default:
249 break;
250 }
251}
252
253template <typename PointInT, typename PointOutT> void
255 const Eigen::Vector3d &point,
256 const Eigen::Vector3d &normal,
257 double curvature,
258 PointCloudOut &projected_points,
259 NormalCloud &projected_points_normals,
260 PointIndices &corresponding_input_indices) const
261{
262 PointOutT aux;
263 aux.x = static_cast<float> (point[0]);
264 aux.y = static_cast<float> (point[1]);
265 aux.z = static_cast<float> (point[2]);
266
267 // Copy additional point information if available
268 copyMissingFields ((*input_)[index], aux);
269
270 projected_points.push_back (aux);
271 corresponding_input_indices.indices.push_back (index);
272
273 if (compute_normals_)
274 {
275 pcl::Normal aux_normal;
276 aux_normal.normal_x = static_cast<float> (normal[0]);
277 aux_normal.normal_y = static_cast<float> (normal[1]);
278 aux_normal.normal_z = static_cast<float> (normal[2]);
279 aux_normal.curvature = curvature;
280 projected_points_normals.push_back (aux_normal);
281 }
282}
283
284//////////////////////////////////////////////////////////////////////////////////////////////
285template <typename PointInT, typename PointOutT> void
287{
288 // Compute the number of coefficients
289 nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
290
291#ifdef _OPENMP
292 // (Maximum) number of threads
293 const unsigned int threads = threads_ == 0 ? 1 : threads_;
294 // Create temporaries for each thread in order to avoid synchronization
295 typename PointCloudOut::CloudVectorType projected_points (threads);
296 typename NormalCloud::CloudVectorType projected_points_normals (threads);
297 std::vector<PointIndices> corresponding_input_indices (threads);
298#endif
299
300 // For all points
301#pragma omp parallel for \
302 default(none) \
303 shared(corresponding_input_indices, projected_points, projected_points_normals) \
304 schedule(dynamic,1000) \
305 num_threads(threads)
306 for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
307 {
308 // Allocate enough space to hold the results of nearest neighbor searches
309 // \note resize is irrelevant for a radiusSearch ().
310 pcl::Indices nn_indices;
311 std::vector<float> nn_sqr_dists;
312
313 // Get the initial estimates of point positions and their neighborhoods
314 if (searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
315 {
316 // Check the number of nearest neighbors for normal estimation (and later for polynomial fit as well)
317 if (nn_indices.size () >= 3)
318 {
319 // This thread's ID (range 0 to threads-1)
320#ifdef _OPENMP
321 const int tn = omp_get_thread_num ();
322 // Size of projected points before computeMLSPointNormal () adds points
323 std::size_t pp_size = projected_points[tn].size ();
324#else
325 PointCloudOut projected_points;
326 NormalCloud projected_points_normals;
327#endif
328
329 // Get a plane approximating the local surface's tangent and project point onto it
330 const int index = (*indices_)[cp];
331
332 std::size_t mls_result_index = 0;
333 if (cache_mls_results_)
334 mls_result_index = index; // otherwise we give it a dummy location.
335
336#ifdef _OPENMP
337 computeMLSPointNormal (index, nn_indices, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], mls_results_[mls_result_index]);
338
339 // Copy all information from the input cloud to the output points (not doing any interpolation)
340 for (std::size_t pp = pp_size; pp < projected_points[tn].size (); ++pp)
341 copyMissingFields ((*input_)[(*indices_)[cp]], projected_points[tn][pp]);
342#else
343 computeMLSPointNormal (index, nn_indices, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]);
344
345 // Append projected points to output
346 output.insert (output.end (), projected_points.begin (), projected_points.end ());
347 if (compute_normals_)
348 normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ());
349#endif
350 }
351 }
352 }
353
354#ifdef _OPENMP
355 // Combine all threads' results into the output vectors
356 for (unsigned int tn = 0; tn < threads; ++tn)
357 {
358 output.insert (output.end (), projected_points[tn].begin (), projected_points[tn].end ());
359 corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (),
360 corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ());
361 if (compute_normals_)
362 normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ());
363 }
364#endif
365
366 // Perform the distinct-cloud or voxel-grid upsampling
367 performUpsampling (output);
368}
369
370//////////////////////////////////////////////////////////////////////////////////////////////
371template <typename PointInT, typename PointOutT> void
373{
374
375 if (upsample_method_ == DISTINCT_CLOUD)
376 {
377 corresponding_input_indices_.reset (new PointIndices);
378 for (std::size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i
379 {
380 // Distinct cloud may have nan points, skip them
381 if (!std::isfinite ((*distinct_cloud_)[dp_i].x))
382 continue;
383
384 // Get 3D position of point
385 //Eigen::Vector3f pos = (*distinct_cloud_)[dp_i].getVector3fMap ();
386 pcl::Indices nn_indices;
387 std::vector<float> nn_dists;
388 tree_->nearestKSearch ((*distinct_cloud_)[dp_i], 1, nn_indices, nn_dists);
389 const auto input_index = nn_indices.front ();
390
391 // If the closest point did not have a valid MLS fitting result
392 // OR if it is too far away from the sampled point
393 if (!mls_results_[input_index].valid)
394 continue;
395
396 Eigen::Vector3d add_point = (*distinct_cloud_)[dp_i].getVector3fMap ().template cast<double> ();
397 MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint (add_point, projection_method_, 5 * nr_coeff_);
398 addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
399 }
400 }
401
402 // For the voxel grid upsampling method, generate the voxel grid and dilate it
403 // Then, project the newly obtained points to the MLS surface
404 if (upsample_method_ == VOXEL_GRID_DILATION)
405 {
406 corresponding_input_indices_.reset (new PointIndices);
407
408 MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_, dilation_iteration_num_);
409 for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
410 voxel_grid.dilate ();
411
412 for (auto m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it)
413 {
414 // Get 3D position of point
415 Eigen::Vector3f pos;
416 voxel_grid.getPosition (m_it->first, pos);
417
418 PointInT p;
419 p.x = pos[0];
420 p.y = pos[1];
421 p.z = pos[2];
422
423 pcl::Indices nn_indices;
424 std::vector<float> nn_dists;
425 tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
426 const auto input_index = nn_indices.front ();
427
428 // If the closest point did not have a valid MLS fitting result
429 // OR if it is too far away from the sampled point
430 if (!mls_results_[input_index].valid)
431 continue;
432
433 Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> ();
434 MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint (add_point, projection_method_, 5 * nr_coeff_);
435 addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
436 }
437 }
438}
439
440//////////////////////////////////////////////////////////////////////////////////////////////
441pcl::MLSResult::MLSResult (const Eigen::Vector3d &a_query_point,
442 const Eigen::Vector3d &a_mean,
443 const Eigen::Vector3d &a_plane_normal,
444 const Eigen::Vector3d &a_u,
445 const Eigen::Vector3d &a_v,
446 const Eigen::VectorXd &a_c_vec,
447 const int a_num_neighbors,
448 const float a_curvature,
449 const int a_order) :
450 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),
451 curvature (a_curvature), order (a_order), valid (true)
452{}
453
454void
455pcl::MLSResult::getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v, double &w) const
456{
457 Eigen::Vector3d delta = pt - mean;
458 u = delta.dot (u_axis);
459 v = delta.dot (v_axis);
460 w = delta.dot (plane_normal);
461}
462
463void
464pcl::MLSResult::getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v) const
465{
466 Eigen::Vector3d delta = pt - mean;
467 u = delta.dot (u_axis);
468 v = delta.dot (v_axis);
469}
470
471double
472pcl::MLSResult::getPolynomialValue (const double u, const double v) const
473{
474 // Compute the polynomial's terms at the current point
475 // Example for second order: z = a + b*y + c*y^2 + d*x + e*x*y + f*x^2
476 int j = 0;
477 double u_pow = 1;
478 double result = 0;
479 for (int ui = 0; ui <= order; ++ui)
480 {
481 double v_pow = 1;
482 for (int vi = 0; vi <= order - ui; ++vi)
483 {
484 result += c_vec[j++] * u_pow * v_pow;
485 v_pow *= v;
486 }
487 u_pow *= u;
488 }
489
490 return (result);
491}
492
494pcl::MLSResult::getPolynomialPartialDerivative (const double u, const double v) const
495{
496 // Compute the displacement along the normal using the fitted polynomial
497 // and compute the partial derivatives needed for estimating the normal
499 Eigen::VectorXd u_pow (order + 2), v_pow (order + 2);
500 int j = 0;
501
502 d.z = d.z_u = d.z_v = d.z_uu = d.z_vv = d.z_uv = 0;
503 u_pow (0) = v_pow (0) = 1;
504 for (int ui = 0; ui <= order; ++ui)
505 {
506 for (int vi = 0; vi <= order - ui; ++vi)
507 {
508 // Compute displacement along normal
509 d.z += u_pow (ui) * v_pow (vi) * c_vec[j];
510
511 // Compute partial derivatives
512 if (ui >= 1)
513 d.z_u += c_vec[j] * ui * u_pow (ui - 1) * v_pow (vi);
514
515 if (vi >= 1)
516 d.z_v += c_vec[j] * vi * u_pow (ui) * v_pow (vi - 1);
517
518 if (ui >= 1 && vi >= 1)
519 d.z_uv += c_vec[j] * ui * u_pow (ui - 1) * vi * v_pow (vi - 1);
520
521 if (ui >= 2)
522 d.z_uu += c_vec[j] * ui * (ui - 1) * u_pow (ui - 2) * v_pow (vi);
523
524 if (vi >= 2)
525 d.z_vv += c_vec[j] * vi * (vi - 1) * u_pow (ui) * v_pow (vi - 2);
526
527 if (ui == 0)
528 v_pow (vi + 1) = v_pow (vi) * v;
529
530 ++j;
531 }
532 u_pow (ui + 1) = u_pow (ui) * u;
533 }
534
535 return (d);
536}
537
539pcl::MLSResult::projectPointOrthogonalToPolynomialSurface (const double u, const double v, const double w) const
540{
541 double gu = u;
542 double gv = v;
543 double gw = 0;
544
546 result.normal = plane_normal;
547 if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
548 {
549 PolynomialPartialDerivative d = getPolynomialPartialDerivative (gu, gv);
550 gw = d.z;
551 double err_total;
552 const double dist1 = std::abs (gw - w);
553 double dist2;
554 do
555 {
556 double e1 = (gu - u) + d.z_u * gw - d.z_u * w;
557 double e2 = (gv - v) + d.z_v * gw - d.z_v * w;
558
559 const double F1u = 1 + d.z_uu * gw + d.z_u * d.z_u - d.z_uu * w;
560 const double F1v = d.z_uv * gw + d.z_u * d.z_v - d.z_uv * w;
561
562 const double F2u = d.z_uv * gw + d.z_v * d.z_u - d.z_uv * w;
563 const double F2v = 1 + d.z_vv * gw + d.z_v * d.z_v - d.z_vv * w;
564
565 Eigen::MatrixXd J (2, 2);
566 J (0, 0) = F1u;
567 J (0, 1) = F1v;
568 J (1, 0) = F2u;
569 J (1, 1) = F2v;
570
571 Eigen::Vector2d err (e1, e2);
572 Eigen::Vector2d update = J.inverse () * err;
573 gu -= update (0);
574 gv -= update (1);
575
576 d = getPolynomialPartialDerivative (gu, gv);
577 gw = d.z;
578 dist2 = std::sqrt ((gu - u) * (gu - u) + (gv - v) * (gv - v) + (gw - w) * (gw - w));
579
580 err_total = std::sqrt (e1 * e1 + e2 * e2);
581
582 } while (err_total > 1e-8 && dist2 < dist1);
583
584 if (dist2 > dist1) // the optimization was diverging reset the coordinates for simple projection
585 {
586 gu = u;
587 gv = v;
588 d = getPolynomialPartialDerivative (u, v);
589 gw = d.z;
590 }
591
592 result.u = gu;
593 result.v = gv;
594 result.normal -= (d.z_u * u_axis + d.z_v * v_axis);
595 result.normal.normalize ();
596 }
597
598 result.point = mean + gu * u_axis + gv * v_axis + gw * plane_normal;
599
600 return (result);
601}
602
604pcl::MLSResult::projectPointToMLSPlane (const double u, const double v) const
605{
607 result.u = u;
608 result.v = v;
609 result.normal = plane_normal;
610 result.point = mean + u * u_axis + v * v_axis;
611
612 return (result);
613}
614
616pcl::MLSResult::projectPointSimpleToPolynomialSurface (const double u, const double v) const
617{
619 double w = 0;
620
621 result.u = u;
622 result.v = v;
623 result.normal = plane_normal;
624
625 if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
626 {
627 const PolynomialPartialDerivative d = getPolynomialPartialDerivative (u, v);
628 w = d.z;
629 result.normal -= (d.z_u * u_axis + d.z_v * v_axis);
630 result.normal.normalize ();
631 }
632
633 result.point = mean + u * u_axis + v * v_axis + w * plane_normal;
634
635 return (result);
636}
637
639pcl::MLSResult::projectPoint (const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors) const
640{
641 double u, v, w;
642 getMLSCoordinates (pt, u, v, w);
643
645 if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
646 {
647 if (method == ORTHOGONAL)
648 proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
649 else // SIMPLE
650 proj = projectPointSimpleToPolynomialSurface (u, v);
651 }
652 else
653 {
654 proj = projectPointToMLSPlane (u, v);
655 }
656
657 return (proj);
658}
659
661pcl::MLSResult::projectQueryPoint (ProjectionMethod method, int required_neighbors) const
662{
664 if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
665 {
666 if (method == ORTHOGONAL)
667 {
668 double u, v, w;
669 getMLSCoordinates (query_point, u, v, w);
670 proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
671 }
672 else // SIMPLE
673 {
674 // Projection onto MLS surface along Darboux normal to the height at (0,0)
675 proj.point = mean + (c_vec[0] * plane_normal);
676
677 // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
678 proj.normal = plane_normal - c_vec[order + 1] * u_axis - c_vec[1] * v_axis;
679 proj.normal.normalize ();
680 }
681 }
682 else
683 {
684 proj.normal = plane_normal;
685 proj.point = mean;
686 }
687
688 return (proj);
689}
690
691template <typename PointT> void
693 pcl::index_t index,
694 const pcl::Indices &nn_indices,
695 double search_radius,
696 int polynomial_order,
697 std::function<double(const double)> weight_func)
698{
699 // Compute the plane coefficients
700 EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix;
701 Eigen::Vector4d xyz_centroid;
702
703 // Estimate the XYZ centroid
704 pcl::compute3DCentroid (cloud, nn_indices, xyz_centroid);
705
706 // Compute the 3x3 covariance matrix
707 pcl::computeCovarianceMatrix (cloud, nn_indices, xyz_centroid, covariance_matrix);
708 EIGEN_ALIGN16 Eigen::Vector3d::Scalar eigen_value;
709 EIGEN_ALIGN16 Eigen::Vector3d eigen_vector;
710 Eigen::Vector4d model_coefficients (0, 0, 0, 0);
711 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
712 model_coefficients.head<3> ().matrix () = eigen_vector;
713 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
714
715 query_point = cloud[index].getVector3fMap ().template cast<double> ();
716
717 if (!std::isfinite(eigen_vector[0]) || !std::isfinite(eigen_vector[1]) || !std::isfinite(eigen_vector[2]))
718 {
719 // Invalid plane coefficients, this may happen if the input cloud is non-dense (it contains invalid points).
720 // Keep the input point and stop here.
721 valid = false;
722 mean = query_point;
723 return;
724 }
725
726 // Projected query point
727 valid = true;
728 const double distance = query_point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
729 mean = query_point - distance * model_coefficients.head<3> ();
730
731 curvature = covariance_matrix.trace ();
732 // Compute the curvature surface change
733 if (curvature != 0)
734 curvature = std::abs (eigen_value / curvature);
735
736 // Get a copy of the plane normal easy access
737 plane_normal = model_coefficients.head<3> ();
738
739 // Local coordinate system (Darboux frame)
740 v_axis = plane_normal.unitOrthogonal ();
741 u_axis = plane_normal.cross (v_axis);
742
743 // Perform polynomial fit to update point and normal
744 ////////////////////////////////////////////////////
745 num_neighbors = static_cast<int> (nn_indices.size ());
746 order = polynomial_order;
747 if (order > 1)
748 {
749 const int nr_coeff = (order + 1) * (order + 2) / 2;
750
751 if (num_neighbors >= nr_coeff)
752 {
753 if (!weight_func)
754 weight_func = [this, search_radius] (const double sq_dist) { return this->computeMLSWeight (sq_dist, search_radius * search_radius); };
755
756 // Allocate matrices and vectors to hold the data used for the polynomial fit
757 Eigen::VectorXd weight_vec (num_neighbors);
758 Eigen::MatrixXd P (nr_coeff, num_neighbors);
759 Eigen::VectorXd f_vec (num_neighbors);
760 Eigen::MatrixXd P_weight_Pt (nr_coeff, nr_coeff);
761
762 // Update neighborhood, since point was projected, and computing relative
763 // positions. Note updating only distances for the weights for speed
764 std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > de_meaned (num_neighbors);
765 for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
766 {
767 de_meaned[ni][0] = cloud[nn_indices[ni]].x - mean[0];
768 de_meaned[ni][1] = cloud[nn_indices[ni]].y - mean[1];
769 de_meaned[ni][2] = cloud[nn_indices[ni]].z - mean[2];
770 weight_vec (ni) = weight_func (de_meaned[ni].dot (de_meaned[ni]));
771 }
772
773 // Go through neighbors, transform them in the local coordinate system,
774 // save height and the evaluation of the polynomial's terms
775 for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
776 {
777 // Transforming coordinates
778 const double u_coord = de_meaned[ni].dot(u_axis);
779 const double v_coord = de_meaned[ni].dot(v_axis);
780 f_vec (ni) = de_meaned[ni].dot (plane_normal);
781
782 // Compute the polynomial's terms at the current point
783 int j = 0;
784 double u_pow = 1;
785 for (int ui = 0; ui <= order; ++ui)
786 {
787 double v_pow = 1;
788 for (int vi = 0; vi <= order - ui; ++vi)
789 {
790 P (j++, ni) = u_pow * v_pow;
791 v_pow *= v_coord;
792 }
793 u_pow *= u_coord;
794 }
795 }
796
797 // Computing coefficients
798 const Eigen::MatrixXd P_weight = P * weight_vec.asDiagonal(); // size will be (nr_coeff_, nn_indices.size ());
799 P_weight_Pt = P_weight * P.transpose ();
800 c_vec = P_weight * f_vec;
801 P_weight_Pt.llt ().solveInPlace (c_vec);
802 }
803 }
804}
805
806//////////////////////////////////////////////////////////////////////////////////////////////
807template <typename PointInT, typename PointOutT>
809 IndicesPtr &indices,
810 float voxel_size,
811 int dilation_iteration_num) :
812 voxel_grid_ (), voxel_size_ (voxel_size)
813{
814 pcl::getMinMax3D (*cloud, *indices, bounding_min_, bounding_max_);
815 bounding_min_ -= Eigen::Vector4f::Constant(voxel_size_ * (dilation_iteration_num + 1));
816 bounding_max_ += Eigen::Vector4f::Constant(voxel_size_ * (dilation_iteration_num + 1));
817
818 Eigen::Vector4f bounding_box_size = bounding_max_ - bounding_min_;
819 const double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
820 // Put initial cloud in voxel grid
821 data_size_ = static_cast<std::uint64_t> (std::ceil(max_size / voxel_size_));
822 for (std::size_t i = 0; i < indices->size (); ++i)
823 if (std::isfinite ((*cloud)[(*indices)[i]].x))
824 {
825 Eigen::Vector3i pos;
826 getCellIndex ((*cloud)[(*indices)[i]].getVector3fMap (), pos);
827
828 std::uint64_t index_1d;
829 getIndexIn1D (pos, index_1d);
830 Leaf leaf;
831 voxel_grid_[index_1d] = leaf;
832 }
833}
834
835//////////////////////////////////////////////////////////////////////////////////////////////
836template <typename PointInT, typename PointOutT> void
838{
839 HashMap new_voxel_grid = voxel_grid_;
840 for (auto m_it = voxel_grid_.begin (); m_it != voxel_grid_.end (); ++m_it)
841 {
842 Eigen::Vector3i index;
843 getIndexIn3D (m_it->first, index);
844
845 // Now dilate all of its voxels
846 for (int x = -1; x <= 1; ++x)
847 for (int y = -1; y <= 1; ++y)
848 for (int z = -1; z <= 1; ++z)
849 if (x != 0 || y != 0 || z != 0)
850 {
851 Eigen::Vector3i new_index;
852 new_index = index + Eigen::Vector3i (x, y, z);
853
854 std::uint64_t index_1d;
855 getIndexIn1D (new_index, index_1d);
856 Leaf leaf;
857 new_voxel_grid[index_1d] = leaf;
858 }
859 }
860 voxel_grid_ = new_voxel_grid;
861}
862
863
864/////////////////////////////////////////////////////////////////////////////////////////////
865template <typename PointInT, typename PointOutT> void
867 PointOutT &point_out) const
868{
869 PointOutT temp = point_out;
870 copyPoint (point_in, point_out);
871 point_out.x = temp.x;
872 point_out.y = temp.y;
873 point_out.z = temp.z;
874}
875
876#define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares<T,OutT>;
877#define PCL_INSTANTIATE_MovingLeastSquaresOMP(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquaresOMP<T,OutT>;
878
879#endif // PCL_SURFACE_IMPL_MLS_H_
Define methods for centroid estimation and covariance matrix calculus.
A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling.
Definition mls.h:577
void getPosition(const std::uint64_t &index_1d, Eigen::Vector3f &point) const
Definition mls.h:614
void getIndexIn1D(const Eigen::Vector3i &index, std::uint64_t &index_1d) const
Definition mls.h:590
void getCellIndex(const Eigen::Vector3f &p, Eigen::Vector3i &index) const
Definition mls.h:607
MLSVoxelGrid(PointCloudInConstPtr &cloud, IndicesPtr &indices, float voxel_size, int dilation_iteration_num)
Definition mls.hpp:808
std::map< std::uint64_t, Leaf > HashMap
Definition mls.h:622
void performUpsampling(PointCloudOut &output)
Perform upsampling for the distinct-cloud and voxel-grid methods.
Definition mls.hpp:372
typename KdTree::Ptr KdTreePtr
Definition mls.h:275
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition mls.h:285
void copyMissingFields(const PointInT &point_in, PointOutT &point_out) const
Definition mls.hpp:866
void performProcessing(PointCloudOut &output) override
Abstract surface reconstruction method.
Definition mls.hpp:286
void computeMLSPointNormal(pcl::index_t index, const pcl::Indices &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.
Definition mls.hpp:176
void process(PointCloudOut &output) override
Base method for surface reconstruction for all points given in <setInputCloud (), setIndices ()>
Definition mls.hpp:63
void addProjectedPointNormal(pcl::index_t 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 adding projected points.
Definition mls.hpp:254
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
std::uint32_t width
The point cloud width (if organized as an image-structure).
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
pcl::PCLHeader header
The point cloud header.
std::vector< PointCloud< PointOutT >, Eigen::aligned_allocator< PointCloud< PointOutT > > > CloudVectorType
iterator end() noexcept
std::uint32_t height
The point cloud height (if organized as an image-structure).
void clear()
Removes all points in a cloud and sets the width and height to 0.
std::size_t size() const
iterator begin() noexcept
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition kdtree.h:62
OrganizedNeighbor is a class for optimized nearest neighbor search in organized projectable point clo...
Definition organized.h:65
Define standard C methods and C++ classes that are common to all 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.
Definition common.hpp:295
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
unsigned int computeCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.
Definition centroid.hpp:192
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...
Definition eigen.hpp:295
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition centroid.hpp:57
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
Data structure used to store the MLS projection results.
Definition mls.h:81
Eigen::Vector3d point
The projected point.
Definition mls.h:86
double v
The v-coordinate of the projected point in local MLS frame.
Definition mls.h:85
Eigen::Vector3d normal
The projected point's normal.
Definition mls.h:87
double u
The u-coordinate of the projected point in local MLS frame.
Definition mls.h:84
Data structure used to store the MLS polynomial partial derivatives.
Definition mls.h:70
double z_uv
The partial derivative d^2z/dudv.
Definition mls.h:76
double z_u
The partial derivative dz/du.
Definition mls.h:72
double z_uu
The partial derivative d^2z/du^2.
Definition mls.h:74
double z
The z component of the polynomial evaluated at z(u, v).
Definition mls.h:71
double z_vv
The partial derivative d^2z/dv^2.
Definition mls.h:75
double z_v
The partial derivative dz/dv.
Definition mls.h:73
Data structure used to store the results of the MLS fitting.
Definition mls.h:60
MLSProjectionResults projectPoint(const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors=0) const
Project a point using the specified method.
Definition mls.hpp:639
MLSResult()
Definition mls.h:92
MLSProjectionResults projectPointOrthogonalToPolynomialSurface(const double u, const double v, const double w) const
Project a point orthogonal to the polynomial surface.
Definition mls.hpp:539
ProjectionMethod
Definition mls.h:62
int num_neighbors
The number of neighbors used to create the mls surface.
Definition mls.h:231
void computeMLSSurface(const pcl::PointCloud< PointT > &cloud, pcl::index_t index, const pcl::Indices &nn_indices, double search_radius, int polynomial_order=2, std::function< double(const double)> weight_func={})
Smooth a given point and its neighborhood using Moving Least Squares.
Definition mls.hpp:692
void getMLSCoordinates(const Eigen::Vector3d &pt, double &u, double &v, double &w) const
Given a point calculate its 3D location in the MLS frame.
Definition mls.hpp:455
float curvature
The curvature at the query point.
Definition mls.h:232
PolynomialPartialDerivative getPolynomialPartialDerivative(const double u, const double v) const
Calculate the polynomial's first and second partial derivatives.
Definition mls.hpp:494
MLSProjectionResults projectPointSimpleToPolynomialSurface(const double u, const double v) const
Project a point along the MLS plane normal to the polynomial surface.
Definition mls.hpp:616
MLSProjectionResults projectPointToMLSPlane(const double u, const double v) const
Project a point onto the MLS plane.
Definition mls.hpp:604
double getPolynomialValue(const double u, const double v) const
Calculate the polynomial.
Definition mls.hpp:472
MLSProjectionResults projectQueryPoint(ProjectionMethod method, int required_neighbors=0) const
Project the query point used to generate the mls surface about using the specified method.
Definition mls.hpp:661
A point structure representing normal coordinates and the surface curvature estimate.
A helper functor that can set a specific value in a field if the field exists.