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/type_traits.h>
44 #include <pcl/surface/mls.h>
45 #include <pcl/common/common.h> // for getMinMax3D
46 #include <pcl/common/copy_point.h>
47 #include <pcl/common/centroid.h>
48 #include <pcl/common/eigen.h>
49 #include <pcl/search/kdtree.h> // for KdTree
50 #include <pcl/search/organized.h> // for OrganizedNeighbor
51
52 #include <Eigen/Geometry> // for cross
53 #include <Eigen/LU> // for inverse
54
55 #ifdef _OPENMP
56 #include <omp.h>
57 #endif
58
59 //////////////////////////////////////////////////////////////////////////////////////////////
60 template <typename PointInT, typename PointOutT> void
process(PointCloudOut & output)61 pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output)
62 {
63 // Reset or initialize the collection of indices
64 corresponding_input_indices_.reset (new PointIndices);
65
66 // Check if normals have to be computed/saved
67 if (compute_normals_)
68 {
69 normals_.reset (new NormalCloud);
70 // Copy the header
71 normals_->header = input_->header;
72 // Clear the fields in case the method exits before computation
73 normals_->width = normals_->height = 0;
74 normals_->points.clear ();
75 }
76
77 // Copy the header
78 output.header = input_->header;
79 output.width = output.height = 0;
80 output.clear ();
81
82 if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
83 {
84 PCL_ERROR ("[pcl::%s::process] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
85 return;
86 }
87
88 // Check if distinct_cloud_ was set
89 if (upsample_method_ == DISTINCT_CLOUD && !distinct_cloud_)
90 {
91 PCL_ERROR ("[pcl::%s::process] Upsample method was set to DISTINCT_CLOUD, but no distinct cloud was specified.\n", getClassName ().c_str ());
92 return;
93 }
94
95 if (!initCompute ())
96 return;
97
98 // Initialize the spatial locator
99 if (!tree_)
100 {
101 KdTreePtr tree;
102 if (input_->isOrganized ())
103 tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
104 else
105 tree.reset (new pcl::search::KdTree<PointInT> (false));
106 setSearchMethod (tree);
107 }
108
109 // Send the surface dataset to the spatial locator
110 tree_->setInputCloud (input_);
111
112 switch (upsample_method_)
113 {
114 // Initialize random number generator if necessary
115 case (RANDOM_UNIFORM_DENSITY):
116 {
117 std::random_device rd;
118 rng_.seed (rd());
119 const double tmp = search_radius_ / 2.0;
120 rng_uniform_distribution_.reset (new std::uniform_real_distribution<> (-tmp, tmp));
121
122 break;
123 }
124 case (VOXEL_GRID_DILATION):
125 case (DISTINCT_CLOUD):
126 {
127 if (!cache_mls_results_)
128 PCL_WARN ("The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n");
129
130 cache_mls_results_ = true;
131 break;
132 }
133 default:
134 break;
135 }
136
137 if (cache_mls_results_)
138 {
139 mls_results_.resize (input_->size ());
140 }
141 else
142 {
143 mls_results_.resize (1); // Need to have a reference to a single dummy result.
144 }
145
146 // Perform the actual surface reconstruction
147 performProcessing (output);
148
149 if (compute_normals_)
150 {
151 normals_->height = 1;
152 normals_->width = normals_->size ();
153
154 for (std::size_t i = 0; i < output.size (); ++i)
155 {
156 using FieldList = typename pcl::traits::fieldList<PointOutT>::type;
157 pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output[i], "normal_x", (*normals_)[i].normal_x));
158 pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output[i], "normal_y", (*normals_)[i].normal_y));
159 pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output[i], "normal_z", (*normals_)[i].normal_z));
160 pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output[i], "curvature", (*normals_)[i].curvature));
161 }
162
163 }
164
165 // Set proper widths and heights for the clouds
166 output.height = 1;
167 output.width = output.size ();
168
169 deinitCompute ();
170 }
171
172 //////////////////////////////////////////////////////////////////////////////////////////////
173 template <typename PointInT, typename PointOutT> 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) const174 pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (pcl::index_t index,
175 const pcl::Indices &nn_indices,
176 PointCloudOut &projected_points,
177 NormalCloud &projected_points_normals,
178 PointIndices &corresponding_input_indices,
179 MLSResult &mls_result) const
180 {
181 // Note: this method is const because it needs to be thread-safe
182 // (MovingLeastSquaresOMP calls it from multiple threads)
183
184 mls_result.computeMLSSurface<PointInT> (*input_, index, nn_indices, search_radius_, order_);
185
186 switch (upsample_method_)
187 {
188 case (NONE):
189 {
190 const MLSResult::MLSProjectionResults proj = mls_result.projectQueryPoint (projection_method_, nr_coeff_);
191 addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices);
192 break;
193 }
194
195 case (SAMPLE_LOCAL_PLANE):
196 {
197 // Uniformly sample a circle around the query point using the radius and step parameters
198 for (float u_disp = -static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
199 for (float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
200 if (u_disp * u_disp + v_disp * v_disp < upsampling_radius_ * upsampling_radius_)
201 {
202 MLSResult::MLSProjectionResults proj = mls_result.projectPointSimpleToPolynomialSurface (u_disp, v_disp);
203 addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices);
204 }
205 break;
206 }
207
208 case (RANDOM_UNIFORM_DENSITY):
209 {
210 // Compute the local point density and add more samples if necessary
211 const int num_points_to_add = static_cast<int> (std::floor (desired_num_points_in_radius_ / 2.0 / static_cast<double> (nn_indices.size ())));
212
213 // Just add the query point, because the density is good
214 if (num_points_to_add <= 0)
215 {
216 // Just add the current point
217 const MLSResult::MLSProjectionResults proj = mls_result.projectQueryPoint (projection_method_, nr_coeff_);
218 addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices);
219 }
220 else
221 {
222 // Sample the local plane
223 for (int num_added = 0; num_added < num_points_to_add;)
224 {
225 const double u = (*rng_uniform_distribution_) (rng_);
226 const double v = (*rng_uniform_distribution_) (rng_);
227
228 // Check if inside circle; if not, try another coin flip
229 if (u * u + v * v > search_radius_ * search_radius_ / 4)
230 continue;
231
232 MLSResult::MLSProjectionResults proj;
233 if (order_ > 1 && mls_result.num_neighbors >= 5 * nr_coeff_)
234 proj = mls_result.projectPointSimpleToPolynomialSurface (u, v);
235 else
236 proj = mls_result.projectPointToMLSPlane (u, v);
237
238 addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices);
239
240 num_added++;
241 }
242 }
243 break;
244 }
245
246 default:
247 break;
248 }
249 }
250
251 template <typename PointInT, typename PointOutT> 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) const252 pcl::MovingLeastSquares<PointInT, PointOutT>::addProjectedPointNormal (pcl::index_t index,
253 const Eigen::Vector3d &point,
254 const Eigen::Vector3d &normal,
255 double curvature,
256 PointCloudOut &projected_points,
257 NormalCloud &projected_points_normals,
258 PointIndices &corresponding_input_indices) const
259 {
260 PointOutT aux;
261 aux.x = static_cast<float> (point[0]);
262 aux.y = static_cast<float> (point[1]);
263 aux.z = static_cast<float> (point[2]);
264
265 // Copy additional point information if available
266 copyMissingFields ((*input_)[index], aux);
267
268 projected_points.push_back (aux);
269 corresponding_input_indices.indices.push_back (index);
270
271 if (compute_normals_)
272 {
273 pcl::Normal aux_normal;
274 aux_normal.normal_x = static_cast<float> (normal[0]);
275 aux_normal.normal_y = static_cast<float> (normal[1]);
276 aux_normal.normal_z = static_cast<float> (normal[2]);
277 aux_normal.curvature = curvature;
278 projected_points_normals.push_back (aux_normal);
279 }
280 }
281
282 //////////////////////////////////////////////////////////////////////////////////////////////
283 template <typename PointInT, typename PointOutT> void
performProcessing(PointCloudOut & output)284 pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
285 {
286 // Compute the number of coefficients
287 nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
288
289 #ifdef _OPENMP
290 // (Maximum) number of threads
291 const unsigned int threads = threads_ == 0 ? 1 : threads_;
292 // Create temporaries for each thread in order to avoid synchronization
293 typename PointCloudOut::CloudVectorType projected_points (threads);
294 typename NormalCloud::CloudVectorType projected_points_normals (threads);
295 std::vector<PointIndices> corresponding_input_indices (threads);
296 #endif
297
298 // For all points
299 #pragma omp parallel for \
300 default(none) \
301 shared(corresponding_input_indices, projected_points, projected_points_normals) \
302 schedule(dynamic,1000) \
303 num_threads(threads)
304 for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
305 {
306 // Allocate enough space to hold the results of nearest neighbor searches
307 // \note resize is irrelevant for a radiusSearch ().
308 pcl::Indices nn_indices;
309 std::vector<float> nn_sqr_dists;
310
311 // Get the initial estimates of point positions and their neighborhoods
312 if (searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
313 {
314 // Check the number of nearest neighbors for normal estimation (and later for polynomial fit as well)
315 if (nn_indices.size () >= 3)
316 {
317 // This thread's ID (range 0 to threads-1)
318 #ifdef _OPENMP
319 const int tn = omp_get_thread_num ();
320 // Size of projected points before computeMLSPointNormal () adds points
321 std::size_t pp_size = projected_points[tn].size ();
322 #else
323 PointCloudOut projected_points;
324 NormalCloud projected_points_normals;
325 #endif
326
327 // Get a plane approximating the local surface's tangent and project point onto it
328 const int index = (*indices_)[cp];
329
330 std::size_t mls_result_index = 0;
331 if (cache_mls_results_)
332 mls_result_index = index; // otherwise we give it a dummy location.
333
334 #ifdef _OPENMP
335 computeMLSPointNormal (index, nn_indices, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], mls_results_[mls_result_index]);
336
337 // Copy all information from the input cloud to the output points (not doing any interpolation)
338 for (std::size_t pp = pp_size; pp < projected_points[tn].size (); ++pp)
339 copyMissingFields ((*input_)[(*indices_)[cp]], projected_points[tn][pp]);
340 #else
341 computeMLSPointNormal (index, nn_indices, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]);
342
343 // Append projected points to output
344 output.insert (output.end (), projected_points.begin (), projected_points.end ());
345 if (compute_normals_)
346 normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ());
347 #endif
348 }
349 }
350 }
351
352 #ifdef _OPENMP
353 // Combine all threads' results into the output vectors
354 for (unsigned int tn = 0; tn < threads; ++tn)
355 {
356 output.insert (output.end (), projected_points[tn].begin (), projected_points[tn].end ());
357 corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (),
358 corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ());
359 if (compute_normals_)
360 normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ());
361 }
362 #endif
363
364 // Perform the distinct-cloud or voxel-grid upsampling
365 performUpsampling (output);
366 }
367
368 //////////////////////////////////////////////////////////////////////////////////////////////
369 template <typename PointInT, typename PointOutT> void
performUpsampling(PointCloudOut & output)370 pcl::MovingLeastSquares<PointInT, PointOutT>::performUpsampling (PointCloudOut &output)
371 {
372
373 if (upsample_method_ == DISTINCT_CLOUD)
374 {
375 corresponding_input_indices_.reset (new PointIndices);
376 for (std::size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i
377 {
378 // Distinct cloud may have nan points, skip them
379 if (!std::isfinite ((*distinct_cloud_)[dp_i].x))
380 continue;
381
382 // Get 3D position of point
383 //Eigen::Vector3f pos = (*distinct_cloud_)[dp_i].getVector3fMap ();
384 pcl::Indices nn_indices;
385 std::vector<float> nn_dists;
386 tree_->nearestKSearch ((*distinct_cloud_)[dp_i], 1, nn_indices, nn_dists);
387 const auto input_index = nn_indices.front ();
388
389 // If the closest point did not have a valid MLS fitting result
390 // OR if it is too far away from the sampled point
391 if (mls_results_[input_index].valid == false)
392 continue;
393
394 Eigen::Vector3d add_point = (*distinct_cloud_)[dp_i].getVector3fMap ().template cast<double> ();
395 MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint (add_point, projection_method_, 5 * nr_coeff_);
396 addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
397 }
398 }
399
400 // For the voxel grid upsampling method, generate the voxel grid and dilate it
401 // Then, project the newly obtained points to the MLS surface
402 if (upsample_method_ == VOXEL_GRID_DILATION)
403 {
404 corresponding_input_indices_.reset (new PointIndices);
405
406 MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);
407 for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
408 voxel_grid.dilate ();
409
410 for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it)
411 {
412 // Get 3D position of point
413 Eigen::Vector3f pos;
414 voxel_grid.getPosition (m_it->first, pos);
415
416 PointInT p;
417 p.x = pos[0];
418 p.y = pos[1];
419 p.z = pos[2];
420
421 pcl::Indices nn_indices;
422 std::vector<float> nn_dists;
423 tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
424 const auto input_index = nn_indices.front ();
425
426 // If the closest point did not have a valid MLS fitting result
427 // OR if it is too far away from the sampled point
428 if (mls_results_[input_index].valid == false)
429 continue;
430
431 Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> ();
432 MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint (add_point, projection_method_, 5 * nr_coeff_);
433 addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
434 }
435 }
436 }
437
438 //////////////////////////////////////////////////////////////////////////////////////////////
MLSResult(const Eigen::Vector3d & a_query_point,const Eigen::Vector3d & a_mean,const Eigen::Vector3d & a_plane_normal,const Eigen::Vector3d & a_u,const Eigen::Vector3d & a_v,const Eigen::VectorXd & a_c_vec,const int a_num_neighbors,const float a_curvature,const int a_order)439 pcl::MLSResult::MLSResult (const Eigen::Vector3d &a_query_point,
440 const Eigen::Vector3d &a_mean,
441 const Eigen::Vector3d &a_plane_normal,
442 const Eigen::Vector3d &a_u,
443 const Eigen::Vector3d &a_v,
444 const Eigen::VectorXd &a_c_vec,
445 const int a_num_neighbors,
446 const float a_curvature,
447 const int a_order) :
448 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),
449 curvature (a_curvature), order (a_order), valid (true)
450 {}
451
452 void
getMLSCoordinates(const Eigen::Vector3d & pt,double & u,double & v,double & w) const453 pcl::MLSResult::getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v, double &w) const
454 {
455 Eigen::Vector3d delta = pt - mean;
456 u = delta.dot (u_axis);
457 v = delta.dot (v_axis);
458 w = delta.dot (plane_normal);
459 }
460
461 void
getMLSCoordinates(const Eigen::Vector3d & pt,double & u,double & v) const462 pcl::MLSResult::getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v) const
463 {
464 Eigen::Vector3d delta = pt - mean;
465 u = delta.dot (u_axis);
466 v = delta.dot (v_axis);
467 }
468
469 double
getPolynomialValue(const double u,const double v) const470 pcl::MLSResult::getPolynomialValue (const double u, const double v) const
471 {
472 // Compute the polynomial's terms at the current point
473 // Example for second order: z = a + b*y + c*y^2 + d*x + e*x*y + f*x^2
474 int j = 0;
475 double u_pow = 1;
476 double result = 0;
477 for (int ui = 0; ui <= order; ++ui)
478 {
479 double v_pow = 1;
480 for (int vi = 0; vi <= order - ui; ++vi)
481 {
482 result += c_vec[j++] * u_pow * v_pow;
483 v_pow *= v;
484 }
485 u_pow *= u;
486 }
487
488 return (result);
489 }
490
491 pcl::MLSResult::PolynomialPartialDerivative
getPolynomialPartialDerivative(const double u,const double v) const492 pcl::MLSResult::getPolynomialPartialDerivative (const double u, const double v) const
493 {
494 // Compute the displacement along the normal using the fitted polynomial
495 // and compute the partial derivatives needed for estimating the normal
496 PolynomialPartialDerivative d{};
497 Eigen::VectorXd u_pow (order + 2), v_pow (order + 2);
498 int j = 0;
499
500 d.z = d.z_u = d.z_v = d.z_uu = d.z_vv = d.z_uv = 0;
501 u_pow (0) = v_pow (0) = 1;
502 for (int ui = 0; ui <= order; ++ui)
503 {
504 for (int vi = 0; vi <= order - ui; ++vi)
505 {
506 // Compute displacement along normal
507 d.z += u_pow (ui) * v_pow (vi) * c_vec[j];
508
509 // Compute partial derivatives
510 if (ui >= 1)
511 d.z_u += c_vec[j] * ui * u_pow (ui - 1) * v_pow (vi);
512
513 if (vi >= 1)
514 d.z_v += c_vec[j] * vi * u_pow (ui) * v_pow (vi - 1);
515
516 if (ui >= 1 && vi >= 1)
517 d.z_uv += c_vec[j] * ui * u_pow (ui - 1) * vi * v_pow (vi - 1);
518
519 if (ui >= 2)
520 d.z_uu += c_vec[j] * ui * (ui - 1) * u_pow (ui - 2) * v_pow (vi);
521
522 if (vi >= 2)
523 d.z_vv += c_vec[j] * vi * (vi - 1) * u_pow (ui) * v_pow (vi - 2);
524
525 if (ui == 0)
526 v_pow (vi + 1) = v_pow (vi) * v;
527
528 ++j;
529 }
530 u_pow (ui + 1) = u_pow (ui) * u;
531 }
532
533 return (d);
534 }
535
536 Eigen::Vector2f
calculatePrincipalCurvatures(const double u,const double v) const537 pcl::MLSResult::calculatePrincipalCurvatures (const double u, const double v) const
538 {
539 Eigen::Vector2f k (1e-5, 1e-5);
540
541 // Note: this use the Monge Patch to derive the Gaussian curvature and Mean Curvature found here http://mathworld.wolfram.com/MongePatch.html
542 // Then:
543 // k1 = H + sqrt(H^2 - K)
544 // k2 = H - sqrt(H^2 - K)
545 if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
546 {
547 const PolynomialPartialDerivative d = getPolynomialPartialDerivative (u, v);
548 const double Z = 1 + d.z_u * d.z_u + d.z_v * d.z_v;
549 const double Zlen = ::sqrt (Z);
550 const double K = (d.z_uu * d.z_vv - d.z_uv * d.z_uv) / (Z * Z);
551 const double H = ((1.0 + d.z_v * d.z_v) * d.z_uu - 2.0 * d.z_u * d.z_v * d.z_uv + (1.0 + d.z_u * d.z_u) * d.z_vv) / (2.0 * Zlen * Zlen * Zlen);
552 const double disc2 = H * H - K;
553 assert (disc2 >= 0.0);
554 const double disc = ::sqrt (disc2);
555 k[0] = H + disc;
556 k[1] = H - disc;
557
558 if (std::abs (k[0]) > std::abs (k[1])) std::swap (k[0], k[1]);
559 }
560 else
561 {
562 PCL_ERROR ("No Polynomial fit data, unable to calculate the principal curvatures!\n");
563 }
564
565 return (k);
566 }
567
568 pcl::MLSResult::MLSProjectionResults
projectPointOrthogonalToPolynomialSurface(const double u,const double v,const double w) const569 pcl::MLSResult::projectPointOrthogonalToPolynomialSurface (const double u, const double v, const double w) const
570 {
571 double gu = u;
572 double gv = v;
573 double gw = 0;
574
575 MLSProjectionResults result;
576 result.normal = plane_normal;
577 if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
578 {
579 PolynomialPartialDerivative d = getPolynomialPartialDerivative (gu, gv);
580 gw = d.z;
581 double err_total;
582 const double dist1 = std::abs (gw - w);
583 double dist2;
584 do
585 {
586 double e1 = (gu - u) + d.z_u * gw - d.z_u * w;
587 double e2 = (gv - v) + d.z_v * gw - d.z_v * w;
588
589 const double F1u = 1 + d.z_uu * gw + d.z_u * d.z_u - d.z_uu * w;
590 const double F1v = d.z_uv * gw + d.z_u * d.z_v - d.z_uv * w;
591
592 const double F2u = d.z_uv * gw + d.z_v * d.z_u - d.z_uv * w;
593 const double F2v = 1 + d.z_vv * gw + d.z_v * d.z_v - d.z_vv * w;
594
595 Eigen::MatrixXd J (2, 2);
596 J (0, 0) = F1u;
597 J (0, 1) = F1v;
598 J (1, 0) = F2u;
599 J (1, 1) = F2v;
600
601 Eigen::Vector2d err (e1, e2);
602 Eigen::Vector2d update = J.inverse () * err;
603 gu -= update (0);
604 gv -= update (1);
605
606 d = getPolynomialPartialDerivative (gu, gv);
607 gw = d.z;
608 dist2 = ::sqrt ((gu - u) * (gu - u) + (gv - v) * (gv - v) + (gw - w) * (gw - w));
609
610 err_total = ::sqrt (e1 * e1 + e2 * e2);
611
612 } while (err_total > 1e-8 && dist2 < dist1);
613
614 if (dist2 > dist1) // the optimization was diverging reset the coordinates for simple projection
615 {
616 gu = u;
617 gv = v;
618 d = getPolynomialPartialDerivative (u, v);
619 gw = d.z;
620 }
621
622 result.u = gu;
623 result.v = gv;
624 result.normal -= (d.z_u * u_axis + d.z_v * v_axis);
625 result.normal.normalize ();
626 }
627
628 result.point = mean + gu * u_axis + gv * v_axis + gw * plane_normal;
629
630 return (result);
631 }
632
633 pcl::MLSResult::MLSProjectionResults
projectPointToMLSPlane(const double u,const double v) const634 pcl::MLSResult::projectPointToMLSPlane (const double u, const double v) const
635 {
636 MLSProjectionResults result;
637 result.u = u;
638 result.v = v;
639 result.normal = plane_normal;
640 result.point = mean + u * u_axis + v * v_axis;
641
642 return (result);
643 }
644
645 pcl::MLSResult::MLSProjectionResults
projectPointSimpleToPolynomialSurface(const double u,const double v) const646 pcl::MLSResult::projectPointSimpleToPolynomialSurface (const double u, const double v) const
647 {
648 MLSProjectionResults result;
649 double w = 0;
650
651 result.u = u;
652 result.v = v;
653 result.normal = plane_normal;
654
655 if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
656 {
657 const PolynomialPartialDerivative d = getPolynomialPartialDerivative (u, v);
658 w = d.z;
659 result.normal -= (d.z_u * u_axis + d.z_v * v_axis);
660 result.normal.normalize ();
661 }
662
663 result.point = mean + u * u_axis + v * v_axis + w * plane_normal;
664
665 return (result);
666 }
667
668 pcl::MLSResult::MLSProjectionResults
projectPoint(const Eigen::Vector3d & pt,ProjectionMethod method,int required_neighbors) const669 pcl::MLSResult::projectPoint (const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors) const
670 {
671 double u, v, w;
672 getMLSCoordinates (pt, u, v, w);
673
674 MLSResult::MLSProjectionResults proj;
675 if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
676 {
677 if (method == ORTHOGONAL)
678 proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
679 else // SIMPLE
680 proj = projectPointSimpleToPolynomialSurface (u, v);
681 }
682 else
683 {
684 proj = projectPointToMLSPlane (u, v);
685 }
686
687 return (proj);
688 }
689
690 pcl::MLSResult::MLSProjectionResults
projectQueryPoint(ProjectionMethod method,int required_neighbors) const691 pcl::MLSResult::projectQueryPoint (ProjectionMethod method, int required_neighbors) const
692 {
693 MLSResult::MLSProjectionResults proj;
694 if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
695 {
696 if (method == ORTHOGONAL)
697 {
698 double u, v, w;
699 getMLSCoordinates (query_point, u, v, w);
700 proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
701 }
702 else // SIMPLE
703 {
704 // Projection onto MLS surface along Darboux normal to the height at (0,0)
705 proj.point = mean + (c_vec[0] * plane_normal);
706
707 // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
708 proj.normal = plane_normal - c_vec[order + 1] * u_axis - c_vec[1] * v_axis;
709 proj.normal.normalize ();
710 }
711 }
712 else
713 {
714 proj.normal = plane_normal;
715 proj.point = mean;
716 }
717
718 return (proj);
719 }
720
721 template <typename PointT> void
computeMLSSurface(const pcl::PointCloud<PointT> & cloud,pcl::index_t index,const pcl::Indices & nn_indices,double search_radius,int polynomial_order,std::function<double (const double)> weight_func)722 pcl::MLSResult::computeMLSSurface (const pcl::PointCloud<PointT> &cloud,
723 pcl::index_t index,
724 const pcl::Indices &nn_indices,
725 double search_radius,
726 int polynomial_order,
727 std::function<double(const double)> weight_func)
728 {
729 // Compute the plane coefficients
730 EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix;
731 Eigen::Vector4d xyz_centroid;
732
733 // Estimate the XYZ centroid
734 pcl::compute3DCentroid (cloud, nn_indices, xyz_centroid);
735
736 // Compute the 3x3 covariance matrix
737 pcl::computeCovarianceMatrix (cloud, nn_indices, xyz_centroid, covariance_matrix);
738 EIGEN_ALIGN16 Eigen::Vector3d::Scalar eigen_value;
739 EIGEN_ALIGN16 Eigen::Vector3d eigen_vector;
740 Eigen::Vector4d model_coefficients (0, 0, 0, 0);
741 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
742 model_coefficients.head<3> ().matrix () = eigen_vector;
743 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
744
745 query_point = cloud[index].getVector3fMap ().template cast<double> ();
746
747 if (!std::isfinite(eigen_vector[0]) || !std::isfinite(eigen_vector[1]) || !std::isfinite(eigen_vector[2]))
748 {
749 // Invalid plane coefficients, this may happen if the input cloud is non-dense (it contains invalid points).
750 // Keep the input point and stop here.
751 valid = false;
752 mean = query_point;
753 return;
754 }
755
756 // Projected query point
757 valid = true;
758 const double distance = query_point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
759 mean = query_point - distance * model_coefficients.head<3> ();
760
761 curvature = covariance_matrix.trace ();
762 // Compute the curvature surface change
763 if (curvature != 0)
764 curvature = std::abs (eigen_value / curvature);
765
766 // Get a copy of the plane normal easy access
767 plane_normal = model_coefficients.head<3> ();
768
769 // Local coordinate system (Darboux frame)
770 v_axis = plane_normal.unitOrthogonal ();
771 u_axis = plane_normal.cross (v_axis);
772
773 // Perform polynomial fit to update point and normal
774 ////////////////////////////////////////////////////
775 num_neighbors = static_cast<int> (nn_indices.size ());
776 order = polynomial_order;
777 if (order > 1)
778 {
779 const int nr_coeff = (order + 1) * (order + 2) / 2;
780
781 if (num_neighbors >= nr_coeff)
782 {
783 if (!weight_func)
784 weight_func = [=] (const double sq_dist) { return this->computeMLSWeight (sq_dist, search_radius * search_radius); };
785
786 // Allocate matrices and vectors to hold the data used for the polynomial fit
787 Eigen::VectorXd weight_vec (num_neighbors);
788 Eigen::MatrixXd P (nr_coeff, num_neighbors);
789 Eigen::VectorXd f_vec (num_neighbors);
790 Eigen::MatrixXd P_weight_Pt (nr_coeff, nr_coeff);
791
792 // Update neighborhood, since point was projected, and computing relative
793 // positions. Note updating only distances for the weights for speed
794 std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > de_meaned (num_neighbors);
795 for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
796 {
797 de_meaned[ni][0] = cloud[nn_indices[ni]].x - mean[0];
798 de_meaned[ni][1] = cloud[nn_indices[ni]].y - mean[1];
799 de_meaned[ni][2] = cloud[nn_indices[ni]].z - mean[2];
800 weight_vec (ni) = weight_func (de_meaned[ni].dot (de_meaned[ni]));
801 }
802
803 // Go through neighbors, transform them in the local coordinate system,
804 // save height and the evaluation of the polynomial's terms
805 for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
806 {
807 // Transforming coordinates
808 const double u_coord = de_meaned[ni].dot(u_axis);
809 const double v_coord = de_meaned[ni].dot(v_axis);
810 f_vec (ni) = de_meaned[ni].dot (plane_normal);
811
812 // Compute the polynomial's terms at the current point
813 int j = 0;
814 double u_pow = 1;
815 for (int ui = 0; ui <= order; ++ui)
816 {
817 double v_pow = 1;
818 for (int vi = 0; vi <= order - ui; ++vi)
819 {
820 P (j++, ni) = u_pow * v_pow;
821 v_pow *= v_coord;
822 }
823 u_pow *= u_coord;
824 }
825 }
826
827 // Computing coefficients
828 const Eigen::MatrixXd P_weight = P * weight_vec.asDiagonal(); // size will be (nr_coeff_, nn_indices.size ());
829 P_weight_Pt = P_weight * P.transpose ();
830 c_vec = P_weight * f_vec;
831 P_weight_Pt.llt ().solveInPlace (c_vec);
832 }
833 }
834 }
835
836 //////////////////////////////////////////////////////////////////////////////////////////////
837 template <typename PointInT, typename PointOutT>
MLSVoxelGrid(PointCloudInConstPtr & cloud,IndicesPtr & indices,float voxel_size)838 pcl::MovingLeastSquares<PointInT, PointOutT>::MLSVoxelGrid::MLSVoxelGrid (PointCloudInConstPtr& cloud,
839 IndicesPtr &indices,
840 float voxel_size) :
841 voxel_grid_ (), data_size_ (), voxel_size_ (voxel_size)
842 {
843 pcl::getMinMax3D (*cloud, *indices, bounding_min_, bounding_max_);
844
845 Eigen::Vector4f bounding_box_size = bounding_max_ - bounding_min_;
846 const double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
847 // Put initial cloud in voxel grid
848 data_size_ = static_cast<std::uint64_t> (1.5 * max_size / voxel_size_);
849 for (std::size_t i = 0; i < indices->size (); ++i)
850 if (std::isfinite ((*cloud)[(*indices)[i]].x))
851 {
852 Eigen::Vector3i pos;
853 getCellIndex ((*cloud)[(*indices)[i]].getVector3fMap (), pos);
854
855 std::uint64_t index_1d;
856 getIndexIn1D (pos, index_1d);
857 Leaf leaf;
858 voxel_grid_[index_1d] = leaf;
859 }
860 }
861
862 //////////////////////////////////////////////////////////////////////////////////////////////
863 template <typename PointInT, typename PointOutT> void
dilate()864 pcl::MovingLeastSquares<PointInT, PointOutT>::MLSVoxelGrid::dilate ()
865 {
866 HashMap new_voxel_grid = voxel_grid_;
867 for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid_.begin (); m_it != voxel_grid_.end (); ++m_it)
868 {
869 Eigen::Vector3i index;
870 getIndexIn3D (m_it->first, index);
871
872 // Now dilate all of its voxels
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)
877 {
878 Eigen::Vector3i new_index;
879 new_index = index + Eigen::Vector3i (x, y, z);
880
881 std::uint64_t index_1d;
882 getIndexIn1D (new_index, index_1d);
883 Leaf leaf;
884 new_voxel_grid[index_1d] = leaf;
885 }
886 }
887 voxel_grid_ = new_voxel_grid;
888 }
889
890
891 /////////////////////////////////////////////////////////////////////////////////////////////
892 template <typename PointInT, typename PointOutT> void
copyMissingFields(const PointInT & point_in,PointOutT & point_out) const893 pcl::MovingLeastSquares<PointInT, PointOutT>::copyMissingFields (const PointInT &point_in,
894 PointOutT &point_out) const
895 {
896 PointOutT temp = point_out;
897 copyPoint (point_in, point_out);
898 point_out.x = temp.x;
899 point_out.y = temp.y;
900 point_out.z = temp.z;
901 }
902
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>;
905
906 #endif // PCL_SURFACE_IMPL_MLS_H_
907