Point Cloud Library (PCL) 1.13.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
46//////////////////////////////////////////////////////////////////////////////////////////////
47template<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//////////////////////////////////////////////////////////////////////////////////////////////
65template<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//////////////////////////////////////////////////////////////////////////////////////////////
82template<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//////////////////////////////////////////////////////////////////////////////////////////////
98template<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//////////////////////////////////////////////////////////////////////////////////////////////
129template<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//////////////////////////////////////////////////////////////////////////////////////////////
157template<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//////////////////////////////////////////////////////////////////////////////////////////////
182template<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//////////////////////////////////////////////////////////////////////////////////////////////
568template<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:234
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