Point Cloud Library (PCL)  1.14.0-dev
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 
46 //////////////////////////////////////////////////////////////////////////////////////////////
47 template<typename PointInT, typename PointNT, typename PointOutT> void
49  Eigen::Vector3f const &axis,
50  Eigen::Vector3f const &axis_origin,
51  Eigen::Vector3f const &point,
52  Eigen::Vector3f &directed_ortho_axis)
53 {
54  Eigen::Vector3f projection;
55  projectPointOnPlane (point, axis_origin, axis, projection);
56  directed_ortho_axis = projection - axis_origin;
57 
58  directed_ortho_axis.normalize ();
59 
60  // check if the computed x axis is orthogonal to the normal
61  //assert(areEquals((float)(directed_ortho_axis.dot(axis)), 0.0f, 1E-3f));
62 }
63 
64 //////////////////////////////////////////////////////////////////////////////////////////////
65 template<typename PointInT, typename PointNT, typename PointOutT> void
67  Eigen::Vector3f const &point,
68  Eigen::Vector3f const &origin_point,
69  Eigen::Vector3f const &plane_normal,
70  Eigen::Vector3f &projected_point)
71 {
72  float t;
73  Eigen::Vector3f xo;
74 
75  xo = point - origin_point;
76  t = plane_normal.dot (xo);
77 
78  projected_point = point - (t * plane_normal);
79 }
80 
81 //////////////////////////////////////////////////////////////////////////////////////////////
82 template<typename PointInT, typename PointNT, typename PointOutT> float
84  Eigen::Vector3f const &v1,
85  Eigen::Vector3f const &v2,
86  Eigen::Vector3f const &axis)
87 {
88  Eigen::Vector3f angle_orientation;
89  angle_orientation = v1.cross (v2);
90  float angle_radians = std::acos (std::max (-1.0f, std::min (1.0f, v1.dot (v2))));
91 
92  angle_radians = angle_orientation.dot (axis) < 0.f ? (2 * static_cast<float> (M_PI) - angle_radians) : angle_radians;
93 
94  return (angle_radians);
95 }
96 
97 //////////////////////////////////////////////////////////////////////////////////////////////
98 template<typename PointInT, typename PointNT, typename PointOutT> void
100  Eigen::Vector3f const &axis,
101  Eigen::Vector3f &rand_ortho_axis)
102 {
103  if (!areEquals (axis.z (), 0.0f))
104  {
105  rand_ortho_axis.x () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
106  rand_ortho_axis.y () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
107  rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z ();
108  }
109  else if (!areEquals (axis.y (), 0.0f))
110  {
111  rand_ortho_axis.x () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
112  rand_ortho_axis.z () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
113  rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y ();
114  }
115  else if (!areEquals (axis.x (), 0.0f))
116  {
117  rand_ortho_axis.y () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
118  rand_ortho_axis.z () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
119  rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x ();
120  }
121 
122  rand_ortho_axis.normalize ();
123 
124  // check if the computed x axis is orthogonal to the normal
125  //assert(areEquals(rand_ortho_axis.dot(axis), 0.0f, 1E-6f));
126 }
127 
128 //////////////////////////////////////////////////////////////////////////////////////////////
129 template<typename PointInT, typename PointNT, typename PointOutT> void
131  Eigen::Matrix<float,
132  Eigen::Dynamic, 3> const &points,
133  Eigen::Vector3f &center,
134  Eigen::Vector3f &norm)
135 {
136  // -----------------------------------------------------
137  // Plane Fitting using Singular Value Decomposition (SVD)
138  // -----------------------------------------------------
139 
140  const auto n_points = points.rows ();
141  if (n_points == 0)
142  {
143  return;
144  }
145 
146  //find the center by averaging the points positions
147  center = points.colwise().mean().transpose();
148 
149  //copy points - average (center)
150  const Eigen::Matrix<float, Eigen::Dynamic, 3> A = points.rowwise() - center.transpose();
151 
152  Eigen::JacobiSVD<Eigen::MatrixXf> svd (A, Eigen::ComputeFullV);
153  norm = svd.matrixV ().col (2);
154 }
155 
156 //////////////////////////////////////////////////////////////////////////////////////////////
157 template<typename PointInT, typename PointNT, typename PointOutT> void
159  pcl::PointCloud<PointNT> const &normal_cloud,
160  pcl::Indices const &normal_indices,
161  Eigen::Vector3f &normal)
162 {
163  Eigen::Vector3f normal_mean;
164  normal_mean.setZero ();
165 
166  for (const auto &normal_index : normal_indices)
167  {
168  const PointNT& curPt = normal_cloud[normal_index];
169 
170  normal_mean += curPt.getNormalVector3fMap ();
171  }
172 
173  normal_mean.normalize ();
174 
175  if (normal.dot (normal_mean) < 0)
176  {
177  normal = -normal;
178  }
179 }
180 
181 //////////////////////////////////////////////////////////////////////////////////////////////
182 template<typename PointInT, typename PointNT, typename PointOutT> float
184  Eigen::Matrix3f &lrf)
185 {
186  //find Z axis
187 
188  //extract support points for Rz radius
189  pcl::Indices neighbours_indices;
190  std::vector<float> neighbours_distances;
191  std::size_t n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances);
192 
193  //check if there are enough neighbor points, otherwise compute a random X axis and use normal as Z axis
194  if (n_neighbours < 6)
195  {
196  //PCL_WARN(
197  // "[pcl::%s::computePointLRF] Warning! Neighborhood has less than 6 vertices. Aborting description of point with index %d\n",
198  // getClassName().c_str(), index);
199 
200  //setting lrf to NaN
201  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
202 
203  return (std::numeric_limits<float>::max ());
204  }
205 
206  //copy neighbours coordinates into eigen matrix
207  Eigen::Matrix<float, Eigen::Dynamic, 3> neigh_points_mat (n_neighbours, 3);
208  for (std::size_t i = 0; i < n_neighbours; ++i)
209  {
210  neigh_points_mat.row (i) = (*surface_)[neighbours_indices[i]].getVector3fMap ();
211  }
212 
213  Eigen::Vector3f x_axis, y_axis;
214  //plane fitting to find direction of Z axis
215  Eigen::Vector3f fitted_normal; //z_axis
216  Eigen::Vector3f centroid;
217  planeFitting (neigh_points_mat, centroid, fitted_normal);
218 
219  //disambiguate Z axis with normal mean
220  normalDisambiguation (*normals_, neighbours_indices, fitted_normal);
221 
222  //setting LRF Z axis
223  lrf.row (2).matrix () = fitted_normal;
224 
225  /////////////////////////////////////////////////////////////////////////////////////////
226  //find X axis
227 
228  //extract support points for Rx radius
229  if (tangent_radius_ != 0.0f && search_parameter_ != tangent_radius_)
230  {
231  n_neighbours = this->searchForNeighbors (index, tangent_radius_, neighbours_indices, neighbours_distances);
232  }
233 
234  //find point with the "most different" normal (with respect to fittedNormal)
235 
236  float min_normal_cos = std::numeric_limits<float>::max ();
237  int min_normal_index = -1;
238 
239  bool margin_point_found = false;
240  Eigen::Vector3f best_margin_point;
241  bool best_point_found_on_margins = false;
242 
243  const float radius2 = tangent_radius_ * tangent_radius_;
244  const float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2;
245 
246  float max_boundary_angle = 0;
247 
248  if (find_holes_)
249  {
250  randomOrthogonalAxis (fitted_normal, x_axis);
251 
252  lrf.row (0).matrix () = x_axis;
253 
254  check_margin_array_.assign(check_margin_array_size_, false);
255  margin_array_min_angle_.assign(check_margin_array_size_, std::numeric_limits<float>::max ());
256  margin_array_max_angle_.assign(check_margin_array_size_, -std::numeric_limits<float>::max ());
257  margin_array_min_angle_normal_.assign(check_margin_array_size_, -1.0);
258  margin_array_max_angle_normal_.assign(check_margin_array_size_, -1.0);
259 
260  max_boundary_angle = (2 * static_cast<float> (M_PI)) / static_cast<float> (check_margin_array_size_);
261  }
262 
263  for (std::size_t curr_neigh = 0; curr_neigh < n_neighbours; ++curr_neigh)
264  {
265  const int& curr_neigh_idx = neighbours_indices[curr_neigh];
266  const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
267  if (neigh_distance_sqr <= margin_distance2)
268  {
269  continue;
270  }
271 
272  //point normalIndex is inside the ring between marginThresh and Radius
273  margin_point_found = true;
274 
275  Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
276 
277  float normal_cos = fitted_normal.dot (normal_mean);
278  if (normal_cos < min_normal_cos)
279  {
280  min_normal_index = curr_neigh_idx;
281  min_normal_cos = normal_cos;
282  best_point_found_on_margins = false;
283  }
284 
285  if (find_holes_)
286  {
287  //find angle with respect to random axis previously calculated
288  Eigen::Vector3f indicating_normal_vect;
289  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
290  surface_->at (curr_neigh_idx).getVector3fMap (), indicating_normal_vect);
291  float angle = getAngleBetweenUnitVectors (x_axis, indicating_normal_vect, fitted_normal);
292 
293  int check_margin_array_idx = std::min (static_cast<int> (std::floor (angle / max_boundary_angle)), check_margin_array_size_ - 1);
294  check_margin_array_[check_margin_array_idx] = true;
295 
296  if (angle < margin_array_min_angle_[check_margin_array_idx])
297  {
298  margin_array_min_angle_[check_margin_array_idx] = angle;
299  margin_array_min_angle_normal_[check_margin_array_idx] = normal_cos;
300  }
301  if (angle > margin_array_max_angle_[check_margin_array_idx])
302  {
303  margin_array_max_angle_[check_margin_array_idx] = angle;
304  margin_array_max_angle_normal_[check_margin_array_idx] = normal_cos;
305  }
306  }
307 
308  } //for each neighbor
309 
310  if (!margin_point_found)
311  {
312  //find among points with neighDistance <= marginThresh*radius
313  for (std::size_t curr_neigh = 0; curr_neigh < n_neighbours; curr_neigh++)
314  {
315  const int& curr_neigh_idx = neighbours_indices[curr_neigh];
316  const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
317 
318  if (neigh_distance_sqr > margin_distance2)
319  continue;
320 
321  Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
322 
323  float normal_cos = fitted_normal.dot (normal_mean);
324 
325  if (normal_cos < min_normal_cos)
326  {
327  min_normal_index = curr_neigh_idx;
328  min_normal_cos = normal_cos;
329  }
330  }//for each neighbor
331 
332  // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
333  if (min_normal_index == -1)
334  {
335  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
336  return (std::numeric_limits<float>::max ());
337  }
338  //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
339  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
340  surface_->at (min_normal_index).getVector3fMap (), x_axis);
341  y_axis = fitted_normal.cross (x_axis);
342 
343  lrf.row (0).matrix () = x_axis;
344  lrf.row (1).matrix () = y_axis;
345  //z axis already set
346 
347 
348  return (min_normal_cos);
349  }
350 
351  if (!find_holes_)
352  {
353  if (best_point_found_on_margins)
354  {
355  //if most inclined normal is on support margin
356  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, 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  return (min_normal_cos);
364  }
365 
366  // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
367  if (min_normal_index == -1)
368  {
369  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
370  return (std::numeric_limits<float>::max ());
371  }
372 
373  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
374  surface_->at (min_normal_index).getVector3fMap (), x_axis);
375  y_axis = fitted_normal.cross (x_axis);
376 
377  lrf.row (0).matrix () = x_axis;
378  lrf.row (1).matrix () = y_axis;
379  //z axis already set
380 
381  return (min_normal_cos);
382  }// if(!find_holes_)
383 
384  //check if there is at least a hole
385  bool is_hole_present = false;
386  for (const auto check_margin: check_margin_array_)
387  {
388  if (!check_margin)
389  {
390  is_hole_present = true;
391  break;
392  }
393  }
394 
395  if (!is_hole_present)
396  {
397  if (best_point_found_on_margins)
398  {
399  //if most inclined normal is on support margin
400  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
401  y_axis = fitted_normal.cross (x_axis);
402 
403  lrf.row (0).matrix () = x_axis;
404  lrf.row (1).matrix () = y_axis;
405  //z axis already set
406 
407  return (min_normal_cos);
408  }
409 
410  // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
411  if (min_normal_index == -1)
412  {
413  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
414  return (std::numeric_limits<float>::max ());
415  }
416 
417  //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
418  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
419  surface_->at (min_normal_index).getVector3fMap (), x_axis);
420  y_axis = fitted_normal.cross (x_axis);
421 
422  lrf.row (0).matrix () = x_axis;
423  lrf.row (1).matrix () = y_axis;
424  //z axis already set
425 
426  return (min_normal_cos);
427  }//if (!is_hole_present)
428 
429  //case hole found
430  //find missing region
431  float angle = 0.0;
432  int hole_end;
433  int hole_first;
434 
435  const auto find_first_no_border_pie = [](const auto& array) -> std::size_t {
436  if (array.back())
437  {
438  return 0;
439  }
440  const auto result = std::find_if(array.cbegin (), array.cend (),
441  [](const auto& x) -> bool { return x;});
442  return std::distance(array.cbegin (), result);
443  };
444  const auto first_no_border = find_first_no_border_pie(check_margin_array_);
445 
446  //float steep_prob = 0.0;
447  float max_hole_prob = -std::numeric_limits<float>::max ();
448 
449  //find holes
450  for (auto ch = first_no_border; ch < static_cast<std::size_t>(check_margin_array_size_); ch++)
451  {
452  if (!check_margin_array_[ch])
453  {
454  //border beginning found
455  hole_first = ch;
456  hole_end = hole_first + 1;
457  while (!check_margin_array_[hole_end % check_margin_array_size_])
458  {
459  ++hole_end;
460  }
461  //border end found, find angle
462 
463  if ((hole_end - hole_first) > 0)
464  {
465  //check if hole can be a shapeness hole
466  int previous_hole = (((hole_first - 1) < 0) ? (hole_first - 1) + check_margin_array_size_ : (hole_first - 1))
467  % check_margin_array_size_;
468  int following_hole = (hole_end) % check_margin_array_size_;
469  float normal_begin = margin_array_max_angle_normal_[previous_hole];
470  float normal_end = margin_array_min_angle_normal_[following_hole];
471  normal_begin -= min_normal_cos;
472  normal_end -= min_normal_cos;
473  normal_begin = normal_begin / (1.0f - min_normal_cos);
474  normal_end = normal_end / (1.0f - min_normal_cos);
475  normal_begin = 1.0f - normal_begin;
476  normal_end = 1.0f - normal_end;
477 
478  //evaluate P(Hole);
479  float hole_width = 0.0f;
480  if (following_hole < previous_hole)
481  {
482  hole_width = margin_array_min_angle_[following_hole] + 2 * static_cast<float> (M_PI)
483  - margin_array_max_angle_[previous_hole];
484  }
485  else
486  {
487  hole_width = margin_array_min_angle_[following_hole] - margin_array_max_angle_[previous_hole];
488  }
489  float hole_prob = hole_width / (2 * static_cast<float> (M_PI));
490 
491  //evaluate P(zmin|Hole)
492  float steep_prob = (normal_end + normal_begin) / 2.0f;
493 
494  //check hole prob and after that, check steepThresh
495 
496  if (hole_prob > hole_size_prob_thresh_)
497  {
498  if (steep_prob > steep_thresh_)
499  {
500  if (hole_prob > max_hole_prob)
501  {
502  max_hole_prob = hole_prob;
503 
504  float angle_weight = ((normal_end - normal_begin) + 1.0f) / 2.0f;
505  if (following_hole < previous_hole)
506  {
507  angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole] + 2
508  * static_cast<float> (M_PI) - margin_array_max_angle_[previous_hole]) * angle_weight;
509  }
510  else
511  {
512  angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole]
513  - margin_array_max_angle_[previous_hole]) * angle_weight;
514  }
515  }
516  }
517  }
518  } //(hole_end-hole_first) > 0
519 
520  if (hole_end >= check_margin_array_size_)
521  {
522  break;
523  }
524  ch = hole_end - 1;
525  }
526  }
527 
528  if (max_hole_prob > -std::numeric_limits<float>::max ())
529  {
530  //hole found
531  Eigen::AngleAxisf rotation = Eigen::AngleAxisf (angle, fitted_normal);
532  x_axis = rotation * x_axis;
533 
534  min_normal_cos -= 10.0f;
535  }
536  else
537  {
538  if (best_point_found_on_margins)
539  {
540  //if most inclined normal is on support margin
541  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
542  }
543  else
544  {
545  // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
546  if (min_normal_index == -1)
547  {
548  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
549  return (std::numeric_limits<float>::max ());
550  }
551 
552  //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
553  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
554  surface_->at (min_normal_index).getVector3fMap (), x_axis);
555  }
556  }
557 
558  y_axis = fitted_normal.cross (x_axis);
559 
560  lrf.row (0).matrix () = x_axis;
561  lrf.row (1).matrix () = y_axis;
562  //z axis already set
563 
564  return (min_normal_cos);
565 }
566 
567 //////////////////////////////////////////////////////////////////////////////////////////////
568 template<typename PointInT, typename PointNT, typename PointOutT> void
570 {
571  //check whether used with search radius or search k-neighbors
572  if (this->getKSearch () != 0)
573  {
574  PCL_ERROR(
575  "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
576  getClassName().c_str());
577  return;
578  }
579 
580  this->resetData ();
581  for (std::size_t point_idx = 0; point_idx < indices_->size (); ++point_idx)
582  {
583  Eigen::Matrix3f currentLrf;
584  PointOutT &rf = output[point_idx];
585 
586  //rf.confidence = computePointLRF (*indices_[point_idx], currentLrf);
587  //if (rf.confidence == std::numeric_limits<float>::max ())
588  if (computePointLRF ((*indices_)[point_idx], currentLrf) == std::numeric_limits<float>::max ())
589  {
590  output.is_dense = false;
591  }
592 
593  for (int d = 0; d < 3; ++d)
594  {
595  rf.x_axis[d] = currentLrf (0, d);
596  rf.y_axis[d] = currentLrf (1, d);
597  rf.z_axis[d] = currentLrf (2, d);
598  }
599  }
600 }
601 
602 #define PCL_INSTANTIATE_BOARDLocalReferenceFrameEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::BOARDLocalReferenceFrameEstimation<T,NT,OutT>;
603 
604 #endif // PCL_FEATURES_IMPL_BOARD_H_
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:66
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:48
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:183
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:130
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: board.h:228
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:83
void normalDisambiguation(pcl::PointCloud< PointNT > const &normals_cloud, pcl::Indices const &normal_indices, Eigen::Vector3f &normal)
Disambiguates a normal direction using adjacent normals.
Definition: board.hpp:158
void computeFeature(PointCloudOut &output) override
Abstract feature estimation method.
Definition: board.hpp:569
void randomOrthogonalAxis(Eigen::Vector3f const &axis, Eigen::Vector3f &rand_ortho_axis)
Given an axis, return a random orthogonal axis.
Definition: board.hpp:99
Eigen::Vector3f randomOrthogonalAxis(Eigen::Vector3f const &axis)
Define a random unit vector orthogonal to axis.
Definition: geometry.h:134
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define M_PI
Definition: pcl_macros.h:201