40 #ifndef PCL_FEATURES_IMPL_BOARD_H_
41 #define PCL_FEATURES_IMPL_BOARD_H_
43 #include <pcl/features/board.h>
45 #include <pcl/common/transforms.h>
48 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
50 Eigen::Vector3f
const &axis,
51 Eigen::Vector3f
const &axis_origin,
52 Eigen::Vector3f
const &point,
53 Eigen::Vector3f &directed_ortho_axis)
55 Eigen::Vector3f projection;
56 projectPointOnPlane (point, axis_origin, axis, projection);
57 directed_ortho_axis = projection - axis_origin;
59 directed_ortho_axis.normalize ();
66 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
68 Eigen::Vector3f
const &point,
69 Eigen::Vector3f
const &origin_point,
70 Eigen::Vector3f
const &plane_normal,
71 Eigen::Vector3f &projected_point)
76 xo = point - origin_point;
77 t = plane_normal.dot (xo);
79 projected_point = point - (t * plane_normal);
83 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
float
85 Eigen::Vector3f
const &v1,
86 Eigen::Vector3f
const &v2,
87 Eigen::Vector3f
const &axis)
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))));
93 angle_radians = angle_orientation.dot (axis) < 0.f ? (2 *
static_cast<float> (M_PI) - angle_radians) : angle_radians;
95 return (angle_radians);
99 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
101 Eigen::Vector3f
const &axis,
102 Eigen::Vector3f &rand_ortho_axis)
104 if (!areEquals (axis.z (), 0.0f))
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 ();
110 else if (!areEquals (axis.y (), 0.0f))
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 ();
116 else if (!areEquals (axis.x (), 0.0f))
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 ();
123 rand_ortho_axis.normalize ();
130 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
133 Eigen::Dynamic, 3>
const &points,
134 Eigen::Vector3f ¢er,
135 Eigen::Vector3f &norm)
141 const auto n_points = points.rows ();
150 for (
int i = 0; i < n_points; ++i)
152 center += points.row (i);
155 center /=
static_cast<float> (n_points);
158 Eigen::Matrix<float, Eigen::Dynamic, 3> A (n_points, 3);
159 for (
int i = 0; i < n_points; ++i)
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 ();
166 Eigen::JacobiSVD<Eigen::MatrixXf> svd (A, Eigen::ComputeFullV);
167 norm = svd.matrixV ().col (2);
171 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
174 std::vector<int>
const &normal_indices,
175 Eigen::Vector3f &normal)
177 Eigen::Vector3f normal_mean;
178 normal_mean.setZero ();
180 for (
const int &normal_index : normal_indices)
182 const PointNT& curPt = normal_cloud[normal_index];
184 normal_mean += curPt.getNormalVector3fMap ();
187 normal_mean.normalize ();
189 if (normal.dot (normal_mean) < 0)
196 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
float
198 Eigen::Matrix3f &lrf)
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);
208 if (n_neighbours < 6)
215 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
217 return (std::numeric_limits<float>::max ());
221 Eigen::Matrix<float, Eigen::Dynamic, 3> neigh_points_mat (n_neighbours, 3);
222 for (
int i = 0; i < n_neighbours; ++i)
224 neigh_points_mat.row (i) = (*surface_)[neighbours_indices[i]].getVector3fMap ();
227 Eigen::Vector3f x_axis, y_axis;
229 Eigen::Vector3f fitted_normal;
230 Eigen::Vector3f centroid;
231 planeFitting (neigh_points_mat, centroid, fitted_normal);
234 normalDisambiguation (*normals_, neighbours_indices, fitted_normal);
237 lrf.row (2).matrix () = fitted_normal;
243 if (tangent_radius_ != 0.0f && search_parameter_ != tangent_radius_)
245 n_neighbours = this->searchForNeighbors (index, tangent_radius_, neighbours_indices, neighbours_distances);
250 float min_normal_cos = std::numeric_limits<float>::max ();
251 int min_normal_index = -1;
253 bool margin_point_found =
false;
254 Eigen::Vector3f best_margin_point;
255 bool best_point_found_on_margins =
false;
257 const float radius2 = tangent_radius_ * tangent_radius_;
258 const float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2;
260 float max_boundary_angle = 0;
266 lrf.row (0).matrix () = x_axis;
268 for (
int i = 0; i < check_margin_array_size_; i++)
270 check_margin_array_[i] =
false;
271 margin_array_min_angle_[i] = std::numeric_limits<float>::max ();
272 margin_array_max_angle_[i] = -std::numeric_limits<float>::max ();
273 margin_array_min_angle_normal_[i] = -1.0;
274 margin_array_max_angle_normal_[i] = -1.0;
276 max_boundary_angle = (2 *
static_cast<float> (M_PI)) /
static_cast<float> (check_margin_array_size_);
279 for (std::size_t curr_neigh = 0; curr_neigh < n_neighbours; ++curr_neigh)
281 const int& curr_neigh_idx = neighbours_indices[curr_neigh];
282 const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
283 if (neigh_distance_sqr <= margin_distance2)
289 margin_point_found =
true;
291 Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
293 float normal_cos = fitted_normal.dot (normal_mean);
294 if (normal_cos < min_normal_cos)
296 min_normal_index = curr_neigh_idx;
297 min_normal_cos = normal_cos;
298 best_point_found_on_margins =
false;
304 Eigen::Vector3f indicating_normal_vect;
305 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
306 surface_->at (curr_neigh_idx).getVector3fMap (), indicating_normal_vect);
307 float angle = getAngleBetweenUnitVectors (x_axis, indicating_normal_vect, fitted_normal);
309 int check_margin_array_idx = std::min (
static_cast<int> (std::floor (angle / max_boundary_angle)), check_margin_array_size_ - 1);
310 check_margin_array_[check_margin_array_idx] =
true;
312 if (angle < margin_array_min_angle_[check_margin_array_idx])
314 margin_array_min_angle_[check_margin_array_idx] = angle;
315 margin_array_min_angle_normal_[check_margin_array_idx] = normal_cos;
317 if (angle > margin_array_max_angle_[check_margin_array_idx])
319 margin_array_max_angle_[check_margin_array_idx] = angle;
320 margin_array_max_angle_normal_[check_margin_array_idx] = normal_cos;
326 if (!margin_point_found)
329 for (
int curr_neigh = 0; curr_neigh < n_neighbours; curr_neigh++)
331 const int& curr_neigh_idx = neighbours_indices[curr_neigh];
332 const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
334 if (neigh_distance_sqr > margin_distance2)
337 Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
339 float normal_cos = fitted_normal.dot (normal_mean);
341 if (normal_cos < min_normal_cos)
343 min_normal_index = curr_neigh_idx;
344 min_normal_cos = normal_cos;
349 if (min_normal_index == -1)
351 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
352 return (std::numeric_limits<float>::max ());
355 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
356 surface_->at (min_normal_index).getVector3fMap (), x_axis);
357 y_axis = fitted_normal.cross (x_axis);
359 lrf.row (0).matrix () = x_axis;
360 lrf.row (1).matrix () = y_axis;
364 return (min_normal_cos);
369 if (best_point_found_on_margins)
372 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
373 y_axis = fitted_normal.cross (x_axis);
375 lrf.row (0).matrix () = x_axis;
376 lrf.row (1).matrix () = y_axis;
379 return (min_normal_cos);
383 if (min_normal_index == -1)
385 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
386 return (std::numeric_limits<float>::max ());
389 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
390 surface_->at (min_normal_index).getVector3fMap (), x_axis);
391 y_axis = fitted_normal.cross (x_axis);
393 lrf.row (0).matrix () = x_axis;
394 lrf.row (1).matrix () = y_axis;
397 return (min_normal_cos);
401 bool is_hole_present =
false;
402 for (
const auto check_margin: check_margin_array_)
406 is_hole_present =
true;
411 if (!is_hole_present)
413 if (best_point_found_on_margins)
416 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
417 y_axis = fitted_normal.cross (x_axis);
419 lrf.row (0).matrix () = x_axis;
420 lrf.row (1).matrix () = y_axis;
423 return (min_normal_cos);
427 if (min_normal_index == -1)
429 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
430 return (std::numeric_limits<float>::max ());
434 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
435 surface_->at (min_normal_index).getVector3fMap (), x_axis);
436 y_axis = fitted_normal.cross (x_axis);
438 lrf.row (0).matrix () = x_axis;
439 lrf.row (1).matrix () = y_axis;
442 return (min_normal_cos);
452 int first_no_border = -1;
453 if (check_margin_array_[check_margin_array_size_ - 1])
459 for (
int i = 0; i < check_margin_array_size_; i++)
461 if (check_margin_array_[i])
470 float max_hole_prob = -std::numeric_limits<float>::max ();
473 for (
auto ch = first_no_border; ch < check_margin_array_size_; ch++)
475 if (!check_margin_array_[ch])
479 hole_end = hole_first + 1;
480 while (!check_margin_array_[hole_end % check_margin_array_size_])
486 if ((hole_end - hole_first) > 0)
489 int previous_hole = (((hole_first - 1) < 0) ? (hole_first - 1) + check_margin_array_size_ : (hole_first - 1))
490 % check_margin_array_size_;
491 int following_hole = (hole_end) % check_margin_array_size_;
492 float normal_begin = margin_array_max_angle_normal_[previous_hole];
493 float normal_end = margin_array_min_angle_normal_[following_hole];
494 normal_begin -= min_normal_cos;
495 normal_end -= min_normal_cos;
496 normal_begin = normal_begin / (1.0f - min_normal_cos);
497 normal_end = normal_end / (1.0f - min_normal_cos);
498 normal_begin = 1.0f - normal_begin;
499 normal_end = 1.0f - normal_end;
502 float hole_width = 0.0f;
503 if (following_hole < previous_hole)
505 hole_width = margin_array_min_angle_[following_hole] + 2 *
static_cast<float> (M_PI)
506 - margin_array_max_angle_[previous_hole];
510 hole_width = margin_array_min_angle_[following_hole] - margin_array_max_angle_[previous_hole];
512 float hole_prob = hole_width / (2 *
static_cast<float> (M_PI));
515 float steep_prob = (normal_end + normal_begin) / 2.0f;
519 if (hole_prob > hole_size_prob_thresh_)
521 if (steep_prob > steep_thresh_)
523 if (hole_prob > max_hole_prob)
525 max_hole_prob = hole_prob;
527 float angle_weight = ((normal_end - normal_begin) + 1.0f) / 2.0f;
528 if (following_hole < previous_hole)
530 angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole] + 2
531 *
static_cast<float> (M_PI) - margin_array_max_angle_[previous_hole]) * angle_weight;
535 angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole]
536 - margin_array_max_angle_[previous_hole]) * angle_weight;
543 if (hole_end >= check_margin_array_size_)
551 if (max_hole_prob > -std::numeric_limits<float>::max ())
554 Eigen::AngleAxisf rotation = Eigen::AngleAxisf (angle, fitted_normal);
555 x_axis = rotation * x_axis;
557 min_normal_cos -= 10.0f;
561 if (best_point_found_on_margins)
564 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
569 if (min_normal_index == -1)
571 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
572 return (std::numeric_limits<float>::max ());
576 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
577 surface_->at (min_normal_index).getVector3fMap (), x_axis);
581 y_axis = fitted_normal.cross (x_axis);
583 lrf.row (0).matrix () = x_axis;
584 lrf.row (1).matrix () = y_axis;
587 return (min_normal_cos);
591 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
595 if (this->getKSearch () != 0)
598 "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
599 getClassName().c_str());
604 for (std::size_t point_idx = 0; point_idx < indices_->size (); ++point_idx)
606 Eigen::Matrix3f currentLrf;
607 PointOutT &rf = output[point_idx];
611 if (computePointLRF ((*indices_)[point_idx], currentLrf) == std::numeric_limits<float>::max ())
613 output.is_dense =
false;
616 for (
int d = 0; d < 3; ++d)
618 rf.x_axis[d] = currentLrf (0, d);
619 rf.y_axis[d] = currentLrf (1, d);
620 rf.z_axis[d] = currentLrf (2, d);
625 #define PCL_INSTANTIATE_BOARDLocalReferenceFrameEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::BOARDLocalReferenceFrameEstimation<T,NT,OutT>;
627 #endif // PCL_FEATURES_IMPL_BOARD_H_