Point Cloud Library (PCL)  1.9.1-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 #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 = acosf (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  int n_points = static_cast<int> (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  float radius2 = tangent_radius_ * tangent_radius_;
258 
259  float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2;
260 
261  float max_boundary_angle = 0;
262 
263  if (find_holes_)
264  {
265  randomOrthogonalAxis (fitted_normal, x_axis);
266 
267  lrf.row (0).matrix () = x_axis;
268 
269  for (int i = 0; i < check_margin_array_size_; i++)
270  {
271  check_margin_array_[i] = false;
272  margin_array_min_angle_[i] = std::numeric_limits<float>::max ();
273  margin_array_max_angle_[i] = -std::numeric_limits<float>::max ();
274  margin_array_min_angle_normal_[i] = -1.0;
275  margin_array_max_angle_normal_[i] = -1.0;
276  }
277  max_boundary_angle = (2 * static_cast<float> (M_PI)) / static_cast<float> (check_margin_array_size_);
278  }
279 
280  for (int curr_neigh = 0; curr_neigh < n_neighbours; ++curr_neigh)
281  {
282  const int& curr_neigh_idx = neighbours_indices[curr_neigh];
283  const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
284  if (neigh_distance_sqr <= margin_distance2)
285  {
286  continue;
287  }
288 
289  //point normalIndex is inside the ring between marginThresh and Radius
290  margin_point_found = true;
291 
292  Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
293 
294  float normal_cos = fitted_normal.dot (normal_mean);
295  if (normal_cos < min_normal_cos)
296  {
297  min_normal_index = curr_neigh_idx;
298  min_normal_cos = normal_cos;
299  best_point_found_on_margins = false;
300  }
301 
302  if (find_holes_)
303  {
304  //find angle with respect to random axis previously calculated
305  Eigen::Vector3f indicating_normal_vect;
306  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
307  surface_->at (curr_neigh_idx).getVector3fMap (), indicating_normal_vect);
308  float angle = getAngleBetweenUnitVectors (x_axis, indicating_normal_vect, fitted_normal);
309 
310  int check_margin_array_idx = std::min (static_cast<int> (std::floor (angle / max_boundary_angle)), check_margin_array_size_ - 1);
311  check_margin_array_[check_margin_array_idx] = true;
312 
313  if (angle < margin_array_min_angle_[check_margin_array_idx])
314  {
315  margin_array_min_angle_[check_margin_array_idx] = angle;
316  margin_array_min_angle_normal_[check_margin_array_idx] = normal_cos;
317  }
318  if (angle > margin_array_max_angle_[check_margin_array_idx])
319  {
320  margin_array_max_angle_[check_margin_array_idx] = angle;
321  margin_array_max_angle_normal_[check_margin_array_idx] = normal_cos;
322  }
323  }
324 
325  } //for each neighbor
326 
327  if (!margin_point_found)
328  {
329  //find among points with neighDistance <= marginThresh*radius
330  for (int curr_neigh = 0; curr_neigh < n_neighbours; curr_neigh++)
331  {
332  const int& curr_neigh_idx = neighbours_indices[curr_neigh];
333  const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
334 
335  if (neigh_distance_sqr > margin_distance2)
336  continue;
337 
338  Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
339 
340  float normal_cos = fitted_normal.dot (normal_mean);
341 
342  if (normal_cos < min_normal_cos)
343  {
344  min_normal_index = curr_neigh_idx;
345  min_normal_cos = normal_cos;
346  }
347  }//for each neighbor
348 
349  // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
350  if (min_normal_index == -1)
351  {
352  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
353  return (std::numeric_limits<float>::max ());
354  }
355  //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
356  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
357  surface_->at (min_normal_index).getVector3fMap (), 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 
365  return (min_normal_cos);
366  }
367 
368  if (!find_holes_)
369  {
370  if (best_point_found_on_margins)
371  {
372  //if most inclined normal is on support margin
373  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
374  y_axis = fitted_normal.cross (x_axis);
375 
376  lrf.row (0).matrix () = x_axis;
377  lrf.row (1).matrix () = y_axis;
378  //z axis already set
379 
380  return (min_normal_cos);
381  }
382 
383  // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
384  if (min_normal_index == -1)
385  {
386  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
387  return (std::numeric_limits<float>::max ());
388  }
389 
390  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
391  surface_->at (min_normal_index).getVector3fMap (), x_axis);
392  y_axis = fitted_normal.cross (x_axis);
393 
394  lrf.row (0).matrix () = x_axis;
395  lrf.row (1).matrix () = y_axis;
396  //z axis already set
397 
398  return (min_normal_cos);
399  }// if(!find_holes_)
400 
401  //check if there is at least a hole
402  bool is_hole_present = false;
403  for (int i = 0; i < check_margin_array_size_; i++)
404  {
405  if (!check_margin_array_[i])
406  {
407  is_hole_present = true;
408  break;
409  }
410  }
411 
412  if (!is_hole_present)
413  {
414  if (best_point_found_on_margins)
415  {
416  //if most inclined normal is on support margin
417  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
418  y_axis = fitted_normal.cross (x_axis);
419 
420  lrf.row (0).matrix () = x_axis;
421  lrf.row (1).matrix () = y_axis;
422  //z axis already set
423 
424  return (min_normal_cos);
425  }
426 
427  // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
428  if (min_normal_index == -1)
429  {
430  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
431  return (std::numeric_limits<float>::max ());
432  }
433 
434  //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
435  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
436  surface_->at (min_normal_index).getVector3fMap (), x_axis);
437  y_axis = fitted_normal.cross (x_axis);
438 
439  lrf.row (0).matrix () = x_axis;
440  lrf.row (1).matrix () = y_axis;
441  //z axis already set
442 
443  return (min_normal_cos);
444  }//if (!is_hole_present)
445 
446  //case hole found
447  //find missing region
448  float angle = 0.0;
449  int hole_end;
450  int hole_first;
451 
452  //find first no border pie
453  int first_no_border = -1;
454  if (check_margin_array_[check_margin_array_size_ - 1])
455  {
456  first_no_border = 0;
457  }
458  else
459  {
460  for (int i = 0; i < check_margin_array_size_; i++)
461  {
462  if (check_margin_array_[i])
463  {
464  first_no_border = i;
465  break;
466  }
467  }
468  }
469 
470  //float steep_prob = 0.0;
471  float max_hole_prob = -std::numeric_limits<float>::max ();
472 
473  //find holes
474  for (int ch = first_no_border; ch < check_margin_array_size_; ch++)
475  {
476  if (!check_margin_array_[ch])
477  {
478  //border beginning found
479  hole_first = ch;
480  hole_end = hole_first + 1;
481  while (!check_margin_array_[hole_end % check_margin_array_size_])
482  {
483  ++hole_end;
484  }
485  //border end found, find angle
486 
487  if ((hole_end - hole_first) > 0)
488  {
489  //check if hole can be a shapeness hole
490  int previous_hole = (((hole_first - 1) < 0) ? (hole_first - 1) + check_margin_array_size_ : (hole_first - 1))
491  % check_margin_array_size_;
492  int following_hole = (hole_end) % check_margin_array_size_;
493  float normal_begin = margin_array_max_angle_normal_[previous_hole];
494  float normal_end = margin_array_min_angle_normal_[following_hole];
495  normal_begin -= min_normal_cos;
496  normal_end -= min_normal_cos;
497  normal_begin = normal_begin / (1.0f - min_normal_cos);
498  normal_end = normal_end / (1.0f - min_normal_cos);
499  normal_begin = 1.0f - normal_begin;
500  normal_end = 1.0f - normal_end;
501 
502  //evaluate P(Hole);
503  float hole_width = 0.0f;
504  if (following_hole < previous_hole)
505  {
506  hole_width = margin_array_min_angle_[following_hole] + 2 * static_cast<float> (M_PI)
507  - margin_array_max_angle_[previous_hole];
508  }
509  else
510  {
511  hole_width = margin_array_min_angle_[following_hole] - margin_array_max_angle_[previous_hole];
512  }
513  float hole_prob = hole_width / (2 * static_cast<float> (M_PI));
514 
515  //evaluate P(zmin|Hole)
516  float steep_prob = (normal_end + normal_begin) / 2.0f;
517 
518  //check hole prob and after that, check steepThresh
519 
520  if (hole_prob > hole_size_prob_thresh_)
521  {
522  if (steep_prob > steep_thresh_)
523  {
524  if (hole_prob > max_hole_prob)
525  {
526  max_hole_prob = hole_prob;
527 
528  float angle_weight = ((normal_end - normal_begin) + 1.0f) / 2.0f;
529  if (following_hole < previous_hole)
530  {
531  angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole] + 2
532  * static_cast<float> (M_PI) - margin_array_max_angle_[previous_hole]) * angle_weight;
533  }
534  else
535  {
536  angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole]
537  - margin_array_max_angle_[previous_hole]) * angle_weight;
538  }
539  }
540  }
541  }
542  } //(hole_end-hole_first) > 0
543 
544  if (hole_end >= check_margin_array_size_)
545  {
546  break;
547  }
548  else
549  {
550  ch = hole_end - 1;
551  }
552  }
553  }
554 
555  if (max_hole_prob > -std::numeric_limits<float>::max ())
556  {
557  //hole found
558  Eigen::AngleAxisf rotation = Eigen::AngleAxisf (angle, fitted_normal);
559  x_axis = rotation * x_axis;
560 
561  min_normal_cos -= 10.0f;
562  }
563  else
564  {
565  if (best_point_found_on_margins)
566  {
567  //if most inclined normal is on support margin
568  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
569  }
570  else
571  {
572  // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
573  if (min_normal_index == -1)
574  {
575  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
576  return (std::numeric_limits<float>::max ());
577  }
578 
579  //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
580  directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
581  surface_->at (min_normal_index).getVector3fMap (), x_axis);
582  }
583  }
584 
585  y_axis = fitted_normal.cross (x_axis);
586 
587  lrf.row (0).matrix () = x_axis;
588  lrf.row (1).matrix () = y_axis;
589  //z axis already set
590 
591  return (min_normal_cos);
592 }
593 
594 //////////////////////////////////////////////////////////////////////////////////////////////
595 template<typename PointInT, typename PointNT, typename PointOutT> void
597 {
598  //check whether used with search radius or search k-neighbors
599  if (this->getKSearch () != 0)
600  {
601  PCL_ERROR(
602  "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
603  getClassName().c_str());
604  return;
605  }
606 
607  this->resetData ();
608  for (size_t point_idx = 0; point_idx < indices_->size (); ++point_idx)
609  {
610  Eigen::Matrix3f currentLrf;
611  PointOutT &rf = output[point_idx];
612 
613  //rf.confidence = computePointLRF (*indices_[point_idx], currentLrf);
614  //if (rf.confidence == std::numeric_limits<float>::max ())
615  if (computePointLRF ((*indices_)[point_idx], currentLrf) == std::numeric_limits<float>::max ())
616  {
617  output.is_dense = false;
618  }
619 
620  for (int d = 0; d < 3; ++d)
621  {
622  rf.x_axis[d] = currentLrf (0, d);
623  rf.y_axis[d] = currentLrf (1, d);
624  rf.z_axis[d] = currentLrf (2, d);
625  }
626  }
627 }
628 
629 #define PCL_INSTANTIATE_BOARDLocalReferenceFrameEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::BOARDLocalReferenceFrameEstimation<T,NT,OutT>;
630 
631 #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
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:197
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
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:417
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
void computeFeature(PointCloudOut &output) override
Abstract feature estimation method.
Definition: board.hpp:596