Point Cloud Library (PCL)  1.10.0
board.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  *
38  */
39 
40 #ifndef PCL_FEATURES_IMPL_BOARD_H_
41 #define PCL_FEATURES_IMPL_BOARD_H_
42 
43 #include <pcl/features/board.h>
44 #include <utility>
45 #include <pcl/common/transforms.h>
46 
47 //////////////////////////////////////////////////////////////////////////////////////////////
48 template<typename PointInT, typename PointNT, typename PointOutT> void
50  Eigen::Vector3f const &axis,
51  Eigen::Vector3f const &axis_origin,
52  Eigen::Vector3f const &point,
53  Eigen::Vector3f &directed_ortho_axis)
54 {
55  Eigen::Vector3f projection;
56  projectPointOnPlane (point, axis_origin, axis, projection);
57  directed_ortho_axis = projection - axis_origin;
58 
59  directed_ortho_axis.normalize ();
60 
61  // check if the computed x axis is orthogonal to the normal
62  //assert(areEquals((float)(directed_ortho_axis.dot(axis)), 0.0f, 1E-3f));
63 }
64 
65 //////////////////////////////////////////////////////////////////////////////////////////////
66 template<typename PointInT, typename PointNT, typename PointOutT> void
68  Eigen::Vector3f const &point,
69  Eigen::Vector3f const &origin_point,
70  Eigen::Vector3f const &plane_normal,
71  Eigen::Vector3f &projected_point)
72 {
73  float t;
74  Eigen::Vector3f xo;
75 
76  xo = point - origin_point;
77  t = plane_normal.dot (xo);
78 
79  projected_point = point - (t * plane_normal);
80 }
81 
82 //////////////////////////////////////////////////////////////////////////////////////////////
83 template<typename PointInT, typename PointNT, typename PointOutT> float
85  Eigen::Vector3f const &v1,
86  Eigen::Vector3f const &v2,
87  Eigen::Vector3f const &axis)
88 {
89  Eigen::Vector3f angle_orientation;
90  angle_orientation = v1.cross (v2);
91  float angle_radians = std::acos (std::max (-1.0f, std::min (1.0f, v1.dot (v2))));
92 
93  angle_radians = angle_orientation.dot (axis) < 0.f ? (2 * static_cast<float> (M_PI) - angle_radians) : angle_radians;
94 
95  return (angle_radians);
96 }
97 
98 //////////////////////////////////////////////////////////////////////////////////////////////
99 template<typename PointInT, typename PointNT, typename PointOutT> void
101  Eigen::Vector3f const &axis,
102  Eigen::Vector3f &rand_ortho_axis)
103 {
104  if (!areEquals (axis.z (), 0.0f))
105  {
106  rand_ortho_axis.x () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
107  rand_ortho_axis.y () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
108  rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z ();
109  }
110  else if (!areEquals (axis.y (), 0.0f))
111  {
112  rand_ortho_axis.x () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
113  rand_ortho_axis.z () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
114  rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y ();
115  }
116  else if (!areEquals (axis.x (), 0.0f))
117  {
118  rand_ortho_axis.y () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
119  rand_ortho_axis.z () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
120  rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x ();
121  }
122 
123  rand_ortho_axis.normalize ();
124 
125  // check if the computed x axis is orthogonal to the normal
126  //assert(areEquals(rand_ortho_axis.dot(axis), 0.0f, 1E-6f));
127 }
128 
129 //////////////////////////////////////////////////////////////////////////////////////////////
130 template<typename PointInT, typename PointNT, typename PointOutT> void
132  Eigen::Matrix<float,
133  Eigen::Dynamic, 3> const &points,
134  Eigen::Vector3f &center,
135  Eigen::Vector3f &norm)
136 {
137  // -----------------------------------------------------
138  // Plane Fitting using Singular Value Decomposition (SVD)
139  // -----------------------------------------------------
140 
141  const auto n_points = points.rows ();
142  if (n_points == 0)
143  {
144  return;
145  }
146 
147  //find the center by averaging the points positions
148  center.setZero ();
149 
150  for (int i = 0; i < n_points; ++i)
151  {
152  center += points.row (i);
153  }
154 
155  center /= static_cast<float> (n_points);
156 
157  //copy points - average (center)
158  Eigen::Matrix<float, Eigen::Dynamic, 3> A (n_points, 3); //PointData
159  for (int i = 0; i < n_points; ++i)
160  {
161  A (i, 0) = points (i, 0) - center.x ();
162  A (i, 1) = points (i, 1) - center.y ();
163  A (i, 2) = points (i, 2) - center.z ();
164  }
165 
166  Eigen::JacobiSVD<Eigen::MatrixXf> svd (A, Eigen::ComputeFullV);
167  norm = svd.matrixV ().col (2);
168 }
169 
170 //////////////////////////////////////////////////////////////////////////////////////////////
171 template<typename PointInT, typename PointNT, typename PointOutT> void
173  pcl::PointCloud<PointNT> const &normal_cloud,
174  std::vector<int> const &normal_indices,
175  Eigen::Vector3f &normal)
176 {
177  Eigen::Vector3f normal_mean;
178  normal_mean.setZero ();
179 
180  for (const int &normal_index : normal_indices)
181  {
182  const PointNT& curPt = normal_cloud[normal_index];
183 
184  normal_mean += curPt.getNormalVector3fMap ();
185  }
186 
187  normal_mean.normalize ();
188 
189  if (normal.dot (normal_mean) < 0)
190  {
191  normal = -normal;
192  }
193 }
194 
195 //////////////////////////////////////////////////////////////////////////////////////////////
196 template<typename PointInT, typename PointNT, typename PointOutT> float
198  Eigen::Matrix3f &lrf)
199 {
200  //find Z axis
201 
202  //extract support points for Rz radius
203  std::vector<int> neighbours_indices;
204  std::vector<float> neighbours_distances;
205  int n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances);
206 
207  //check if there are enough neighbor points, otherwise compute a random X axis and use normal as Z axis
208  if (n_neighbours < 6)
209  {
210  //PCL_WARN(
211  // "[pcl::%s::computePointLRF] Warning! Neighborhood has less than 6 vertices. Aborting description of point with index %d\n",
212  // getClassName().c_str(), index);
213 
214  //setting lrf to NaN
215  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
216 
217  return (std::numeric_limits<float>::max ());
218  }
219 
220  //copy neighbours coordinates into eigen matrix
221  Eigen::Matrix<float, Eigen::Dynamic, 3> neigh_points_mat (n_neighbours, 3);
222  for (int i = 0; i < n_neighbours; ++i)
223  {
224  neigh_points_mat.row (i) = (*surface_)[neighbours_indices[i]].getVector3fMap ();
225  }
226 
227  Eigen::Vector3f x_axis, y_axis;
228  //plane fitting to find direction of Z axis
229  Eigen::Vector3f fitted_normal; //z_axis
230  Eigen::Vector3f centroid;
231  planeFitting (neigh_points_mat, centroid, fitted_normal);
232 
233  //disambiguate Z axis with normal mean
234  normalDisambiguation (*normals_, neighbours_indices, fitted_normal);
235 
236  //setting LRF Z axis
237  lrf.row (2).matrix () = fitted_normal;
238 
239  /////////////////////////////////////////////////////////////////////////////////////////
240  //find X axis
241 
242  //extract support points for Rx radius
243  if (tangent_radius_ != 0.0f && search_parameter_ != tangent_radius_)
244  {
245  n_neighbours = this->searchForNeighbors (index, tangent_radius_, neighbours_indices, neighbours_distances);
246  }
247 
248  //find point with the "most different" normal (with respect to fittedNormal)
249 
250  float min_normal_cos = std::numeric_limits<float>::max ();
251  int min_normal_index = -1;
252 
253  bool margin_point_found = false;
254  Eigen::Vector3f best_margin_point;
255  bool best_point_found_on_margins = false;
256 
257  const float radius2 = tangent_radius_ * tangent_radius_;
258  const float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2;
259 
260  float max_boundary_angle = 0;
261 
262  if (find_holes_)
263  {
264  randomOrthogonalAxis (fitted_normal, x_axis);
265 
266  lrf.row (0).matrix () = x_axis;
267 
268  for (int i = 0; i < check_margin_array_size_; i++)
269  {
270  check_margin_array_[i] = false;
271  margin_array_min_angle_[i] = std::numeric_limits<float>::max ();
272  margin_array_max_angle_[i] = -std::numeric_limits<float>::max ();
273  margin_array_min_angle_normal_[i] = -1.0;
274  margin_array_max_angle_normal_[i] = -1.0;
275  }
276  max_boundary_angle = (2 * static_cast<float> (M_PI)) / static_cast<float> (check_margin_array_size_);
277  }
278 
279  for (std::size_t curr_neigh = 0; curr_neigh < n_neighbours; ++curr_neigh)
280  {
281  const int& curr_neigh_idx = neighbours_indices[curr_neigh];
282  const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
283  if (neigh_distance_sqr <= margin_distance2)
284  {
285  continue;
286  }
287 
288  //point normalIndex is inside the ring between marginThresh and Radius
289  margin_point_found = true;
290 
291  Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
292 
293  float normal_cos = fitted_normal.dot (normal_mean);
294  if (normal_cos < min_normal_cos)
295  {
296  min_normal_index = curr_neigh_idx;
297  min_normal_cos = normal_cos;
298  best_point_found_on_margins = false;
299  }
300 
301  if (find_holes_)
302  {
303  //find angle with respect to random axis previously calculated
304  Eigen::Vector3f indicating_normal_vect;
305  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
306  surface_->at (curr_neigh_idx).getVector3fMap (), indicating_normal_vect);
307  float angle = getAngleBetweenUnitVectors (x_axis, indicating_normal_vect, fitted_normal);
308 
309  int check_margin_array_idx = std::min (static_cast<int> (std::floor (angle / max_boundary_angle)), check_margin_array_size_ - 1);
310  check_margin_array_[check_margin_array_idx] = true;
311 
312  if (angle < margin_array_min_angle_[check_margin_array_idx])
313  {
314  margin_array_min_angle_[check_margin_array_idx] = angle;
315  margin_array_min_angle_normal_[check_margin_array_idx] = normal_cos;
316  }
317  if (angle > margin_array_max_angle_[check_margin_array_idx])
318  {
319  margin_array_max_angle_[check_margin_array_idx] = angle;
320  margin_array_max_angle_normal_[check_margin_array_idx] = normal_cos;
321  }
322  }
323 
324  } //for each neighbor
325 
326  if (!margin_point_found)
327  {
328  //find among points with neighDistance <= marginThresh*radius
329  for (int curr_neigh = 0; curr_neigh < n_neighbours; curr_neigh++)
330  {
331  const int& curr_neigh_idx = neighbours_indices[curr_neigh];
332  const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
333 
334  if (neigh_distance_sqr > margin_distance2)
335  continue;
336 
337  Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
338 
339  float normal_cos = fitted_normal.dot (normal_mean);
340 
341  if (normal_cos < min_normal_cos)
342  {
343  min_normal_index = curr_neigh_idx;
344  min_normal_cos = normal_cos;
345  }
346  }//for each neighbor
347 
348  // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
349  if (min_normal_index == -1)
350  {
351  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
352  return (std::numeric_limits<float>::max ());
353  }
354  //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
355  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
356  surface_->at (min_normal_index).getVector3fMap (), x_axis);
357  y_axis = fitted_normal.cross (x_axis);
358 
359  lrf.row (0).matrix () = x_axis;
360  lrf.row (1).matrix () = y_axis;
361  //z axis already set
362 
363 
364  return (min_normal_cos);
365  }
366 
367  if (!find_holes_)
368  {
369  if (best_point_found_on_margins)
370  {
371  //if most inclined normal is on support margin
372  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
373  y_axis = fitted_normal.cross (x_axis);
374 
375  lrf.row (0).matrix () = x_axis;
376  lrf.row (1).matrix () = y_axis;
377  //z axis already set
378 
379  return (min_normal_cos);
380  }
381 
382  // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
383  if (min_normal_index == -1)
384  {
385  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
386  return (std::numeric_limits<float>::max ());
387  }
388 
389  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
390  surface_->at (min_normal_index).getVector3fMap (), x_axis);
391  y_axis = fitted_normal.cross (x_axis);
392 
393  lrf.row (0).matrix () = x_axis;
394  lrf.row (1).matrix () = y_axis;
395  //z axis already set
396 
397  return (min_normal_cos);
398  }// if(!find_holes_)
399 
400  //check if there is at least a hole
401  bool is_hole_present = false;
402  for (const auto check_margin: check_margin_array_)
403  {
404  if (!check_margin)
405  {
406  is_hole_present = true;
407  break;
408  }
409  }
410 
411  if (!is_hole_present)
412  {
413  if (best_point_found_on_margins)
414  {
415  //if most inclined normal is on support margin
416  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
417  y_axis = fitted_normal.cross (x_axis);
418 
419  lrf.row (0).matrix () = x_axis;
420  lrf.row (1).matrix () = y_axis;
421  //z axis already set
422 
423  return (min_normal_cos);
424  }
425 
426  // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
427  if (min_normal_index == -1)
428  {
429  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
430  return (std::numeric_limits<float>::max ());
431  }
432 
433  //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
434  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
435  surface_->at (min_normal_index).getVector3fMap (), x_axis);
436  y_axis = fitted_normal.cross (x_axis);
437 
438  lrf.row (0).matrix () = x_axis;
439  lrf.row (1).matrix () = y_axis;
440  //z axis already set
441 
442  return (min_normal_cos);
443  }//if (!is_hole_present)
444 
445  //case hole found
446  //find missing region
447  float angle = 0.0;
448  int hole_end;
449  int hole_first;
450 
451  //find first no border pie
452  int first_no_border = -1;
453  if (check_margin_array_[check_margin_array_size_ - 1])
454  {
455  first_no_border = 0;
456  }
457  else
458  {
459  for (int i = 0; i < check_margin_array_size_; i++)
460  {
461  if (check_margin_array_[i])
462  {
463  first_no_border = i;
464  break;
465  }
466  }
467  }
468 
469  //float steep_prob = 0.0;
470  float max_hole_prob = -std::numeric_limits<float>::max ();
471 
472  //find holes
473  for (auto ch = first_no_border; ch < check_margin_array_size_; ch++)
474  {
475  if (!check_margin_array_[ch])
476  {
477  //border beginning found
478  hole_first = ch;
479  hole_end = hole_first + 1;
480  while (!check_margin_array_[hole_end % check_margin_array_size_])
481  {
482  ++hole_end;
483  }
484  //border end found, find angle
485 
486  if ((hole_end - hole_first) > 0)
487  {
488  //check if hole can be a shapeness hole
489  int previous_hole = (((hole_first - 1) < 0) ? (hole_first - 1) + check_margin_array_size_ : (hole_first - 1))
490  % check_margin_array_size_;
491  int following_hole = (hole_end) % check_margin_array_size_;
492  float normal_begin = margin_array_max_angle_normal_[previous_hole];
493  float normal_end = margin_array_min_angle_normal_[following_hole];
494  normal_begin -= min_normal_cos;
495  normal_end -= min_normal_cos;
496  normal_begin = normal_begin / (1.0f - min_normal_cos);
497  normal_end = normal_end / (1.0f - min_normal_cos);
498  normal_begin = 1.0f - normal_begin;
499  normal_end = 1.0f - normal_end;
500 
501  //evaluate P(Hole);
502  float hole_width = 0.0f;
503  if (following_hole < previous_hole)
504  {
505  hole_width = margin_array_min_angle_[following_hole] + 2 * static_cast<float> (M_PI)
506  - margin_array_max_angle_[previous_hole];
507  }
508  else
509  {
510  hole_width = margin_array_min_angle_[following_hole] - margin_array_max_angle_[previous_hole];
511  }
512  float hole_prob = hole_width / (2 * static_cast<float> (M_PI));
513 
514  //evaluate P(zmin|Hole)
515  float steep_prob = (normal_end + normal_begin) / 2.0f;
516 
517  //check hole prob and after that, check steepThresh
518 
519  if (hole_prob > hole_size_prob_thresh_)
520  {
521  if (steep_prob > steep_thresh_)
522  {
523  if (hole_prob > max_hole_prob)
524  {
525  max_hole_prob = hole_prob;
526 
527  float angle_weight = ((normal_end - normal_begin) + 1.0f) / 2.0f;
528  if (following_hole < previous_hole)
529  {
530  angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole] + 2
531  * static_cast<float> (M_PI) - margin_array_max_angle_[previous_hole]) * angle_weight;
532  }
533  else
534  {
535  angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole]
536  - margin_array_max_angle_[previous_hole]) * angle_weight;
537  }
538  }
539  }
540  }
541  } //(hole_end-hole_first) > 0
542 
543  if (hole_end >= check_margin_array_size_)
544  {
545  break;
546  }
547  ch = hole_end - 1;
548  }
549  }
550 
551  if (max_hole_prob > -std::numeric_limits<float>::max ())
552  {
553  //hole found
554  Eigen::AngleAxisf rotation = Eigen::AngleAxisf (angle, fitted_normal);
555  x_axis = rotation * x_axis;
556 
557  min_normal_cos -= 10.0f;
558  }
559  else
560  {
561  if (best_point_found_on_margins)
562  {
563  //if most inclined normal is on support margin
564  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
565  }
566  else
567  {
568  // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
569  if (min_normal_index == -1)
570  {
571  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
572  return (std::numeric_limits<float>::max ());
573  }
574 
575  //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
576  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
577  surface_->at (min_normal_index).getVector3fMap (), x_axis);
578  }
579  }
580 
581  y_axis = fitted_normal.cross (x_axis);
582 
583  lrf.row (0).matrix () = x_axis;
584  lrf.row (1).matrix () = y_axis;
585  //z axis already set
586 
587  return (min_normal_cos);
588 }
589 
590 //////////////////////////////////////////////////////////////////////////////////////////////
591 template<typename PointInT, typename PointNT, typename PointOutT> void
593 {
594  //check whether used with search radius or search k-neighbors
595  if (this->getKSearch () != 0)
596  {
597  PCL_ERROR(
598  "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
599  getClassName().c_str());
600  return;
601  }
602 
603  this->resetData ();
604  for (std::size_t point_idx = 0; point_idx < indices_->size (); ++point_idx)
605  {
606  Eigen::Matrix3f currentLrf;
607  PointOutT &rf = output[point_idx];
608 
609  //rf.confidence = computePointLRF (*indices_[point_idx], currentLrf);
610  //if (rf.confidence == std::numeric_limits<float>::max ())
611  if (computePointLRF ((*indices_)[point_idx], currentLrf) == std::numeric_limits<float>::max ())
612  {
613  output.is_dense = false;
614  }
615 
616  for (int d = 0; d < 3; ++d)
617  {
618  rf.x_axis[d] = currentLrf (0, d);
619  rf.y_axis[d] = currentLrf (1, d);
620  rf.z_axis[d] = currentLrf (2, d);
621  }
622  }
623 }
624 
625 #define PCL_INSTANTIATE_BOARDLocalReferenceFrameEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::BOARDLocalReferenceFrameEstimation<T,NT,OutT>;
626 
627 #endif // PCL_FEATURES_IMPL_BOARD_H_
pcl::geometry::randomOrthogonalAxis
Eigen::Vector3f randomOrthogonalAxis(Eigen::Vector3f const &axis)
Define a random unit vector orthogonal to axis.
Definition: geometry.h:134
pcl::BOARDLocalReferenceFrameEstimation::directedOrthogonalAxis
void directedOrthogonalAxis(Eigen::Vector3f const &axis, Eigen::Vector3f const &axis_origin, Eigen::Vector3f const &point, Eigen::Vector3f &directed_ortho_axis)
Given an axis (with origin axis_origin), return the orthogonal axis directed to point.
Definition: board.hpp:49
pcl::PointCloud< PointNT >
pcl::BOARDLocalReferenceFrameEstimation::computePointLRF
float computePointLRF(const int &index, Eigen::Matrix3f &lrf)
Estimate the LRF descriptor for a given point based on its spatial neighborhood of 3D points with nor...
Definition: board.hpp:197
pcl::BOARDLocalReferenceFrameEstimation::normalDisambiguation
void normalDisambiguation(pcl::PointCloud< PointNT > const &normals_cloud, std::vector< int > const &normal_indices, Eigen::Vector3f &normal)
Disambiguates a normal direction using adjacent normals.
Definition: board.hpp:172
pcl::BOARDLocalReferenceFrameEstimation::planeFitting
void planeFitting(Eigen::Matrix< float, Eigen::Dynamic, 3 > const &points, Eigen::Vector3f &center, Eigen::Vector3f &norm)
Compute Least Square Plane Fitting in a set of 3D points.
Definition: board.hpp:131
pcl::BOARDLocalReferenceFrameEstimation::randomOrthogonalAxis
void randomOrthogonalAxis(Eigen::Vector3f const &axis, Eigen::Vector3f &rand_ortho_axis)
Given an axis, return a random orthogonal axis.
Definition: board.hpp:100
pcl::BOARDLocalReferenceFrameEstimation::PointCloudOut
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: board.h:234
pcl::BOARDLocalReferenceFrameEstimation::projectPointOnPlane
void projectPointOnPlane(Eigen::Vector3f const &point, Eigen::Vector3f const &origin_point, Eigen::Vector3f const &plane_normal, Eigen::Vector3f &projected_point)
Given a plane (origin and normal) and a point, return the projection of x on plane.
Definition: board.hpp:67
pcl::BOARDLocalReferenceFrameEstimation::computeFeature
void computeFeature(PointCloudOut &output) override
Abstract feature estimation method.
Definition: board.hpp:592
pcl::BOARDLocalReferenceFrameEstimation::getAngleBetweenUnitVectors
float getAngleBetweenUnitVectors(Eigen::Vector3f const &v1, Eigen::Vector3f const &v2, Eigen::Vector3f const &axis)
return the angle (in radians) that rotate v1 to v2 with respect to axis .
Definition: board.hpp:84