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