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 ();
148 center = points.colwise().mean().transpose();
151 const Eigen::Matrix<float, Eigen::Dynamic, 3> A = points.rowwise() - center.transpose();
153 Eigen::JacobiSVD<Eigen::MatrixXf> svd (A, Eigen::ComputeFullV);
154 norm = svd.matrixV ().col (2);
158 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
161 std::vector<int>
const &normal_indices,
162 Eigen::Vector3f &normal)
164 Eigen::Vector3f normal_mean;
165 normal_mean.setZero ();
167 for (
const int &normal_index : normal_indices)
169 const PointNT& curPt = normal_cloud[normal_index];
171 normal_mean += curPt.getNormalVector3fMap ();
174 normal_mean.normalize ();
176 if (normal.dot (normal_mean) < 0)
183 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
float
185 Eigen::Matrix3f &lrf)
190 std::vector<int> neighbours_indices;
191 std::vector<float> neighbours_distances;
192 int n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances);
195 if (n_neighbours < 6)
202 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
204 return (std::numeric_limits<float>::max ());
208 Eigen::Matrix<float, Eigen::Dynamic, 3> neigh_points_mat (n_neighbours, 3);
209 for (
int i = 0; i < n_neighbours; ++i)
211 neigh_points_mat.row (i) = (*surface_)[neighbours_indices[i]].getVector3fMap ();
214 Eigen::Vector3f x_axis, y_axis;
216 Eigen::Vector3f fitted_normal;
217 Eigen::Vector3f centroid;
218 planeFitting (neigh_points_mat, centroid, fitted_normal);
221 normalDisambiguation (*normals_, neighbours_indices, fitted_normal);
224 lrf.row (2).matrix () = fitted_normal;
230 if (tangent_radius_ != 0.0f && search_parameter_ != tangent_radius_)
232 n_neighbours = this->searchForNeighbors (index, tangent_radius_, neighbours_indices, neighbours_distances);
237 float min_normal_cos = std::numeric_limits<float>::max ();
238 int min_normal_index = -1;
240 bool margin_point_found =
false;
241 Eigen::Vector3f best_margin_point;
242 bool best_point_found_on_margins =
false;
244 const float radius2 = tangent_radius_ * tangent_radius_;
245 const float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2;
247 float max_boundary_angle = 0;
253 lrf.row (0).matrix () = x_axis;
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);
261 max_boundary_angle = (2 *
static_cast<float> (M_PI)) /
static_cast<float> (check_margin_array_size_);
264 for (std::size_t curr_neigh = 0; curr_neigh < n_neighbours; ++curr_neigh)
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)
274 margin_point_found =
true;
276 Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
278 float normal_cos = fitted_normal.dot (normal_mean);
279 if (normal_cos < min_normal_cos)
281 min_normal_index = curr_neigh_idx;
282 min_normal_cos = normal_cos;
283 best_point_found_on_margins =
false;
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);
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;
297 if (angle < margin_array_min_angle_[check_margin_array_idx])
299 margin_array_min_angle_[check_margin_array_idx] = angle;
300 margin_array_min_angle_normal_[check_margin_array_idx] = normal_cos;
302 if (angle > margin_array_max_angle_[check_margin_array_idx])
304 margin_array_max_angle_[check_margin_array_idx] = angle;
305 margin_array_max_angle_normal_[check_margin_array_idx] = normal_cos;
311 if (!margin_point_found)
314 for (
int curr_neigh = 0; curr_neigh < n_neighbours; curr_neigh++)
316 const int& curr_neigh_idx = neighbours_indices[curr_neigh];
317 const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
319 if (neigh_distance_sqr > margin_distance2)
322 Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
324 float normal_cos = fitted_normal.dot (normal_mean);
326 if (normal_cos < min_normal_cos)
328 min_normal_index = curr_neigh_idx;
329 min_normal_cos = normal_cos;
334 if (min_normal_index == -1)
336 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
337 return (std::numeric_limits<float>::max ());
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);
344 lrf.row (0).matrix () = x_axis;
345 lrf.row (1).matrix () = y_axis;
349 return (min_normal_cos);
354 if (best_point_found_on_margins)
357 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
358 y_axis = fitted_normal.cross (x_axis);
360 lrf.row (0).matrix () = x_axis;
361 lrf.row (1).matrix () = y_axis;
364 return (min_normal_cos);
368 if (min_normal_index == -1)
370 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
371 return (std::numeric_limits<float>::max ());
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);
378 lrf.row (0).matrix () = x_axis;
379 lrf.row (1).matrix () = y_axis;
382 return (min_normal_cos);
386 bool is_hole_present =
false;
387 for (
const auto check_margin: check_margin_array_)
391 is_hole_present =
true;
396 if (!is_hole_present)
398 if (best_point_found_on_margins)
401 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
402 y_axis = fitted_normal.cross (x_axis);
404 lrf.row (0).matrix () = x_axis;
405 lrf.row (1).matrix () = y_axis;
408 return (min_normal_cos);
412 if (min_normal_index == -1)
414 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
415 return (std::numeric_limits<float>::max ());
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);
423 lrf.row (0).matrix () = x_axis;
424 lrf.row (1).matrix () = y_axis;
427 return (min_normal_cos);
436 const auto find_first_no_border_pie = [](
const auto& array) -> std::size_t {
441 const auto result = std::find_if(array.cbegin (), array.cend (),
442 [](
const auto& x) ->
bool { return x;});
445 const auto first_no_border = find_first_no_border_pie(check_margin_array_);
448 float max_hole_prob = -std::numeric_limits<float>::max ();
451 for (
auto ch = first_no_border; ch < check_margin_array_size_; ch++)
453 if (!check_margin_array_[ch])
457 hole_end = hole_first + 1;
458 while (!check_margin_array_[hole_end % check_margin_array_size_])
464 if ((hole_end - hole_first) > 0)
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;
480 float hole_width = 0.0f;
481 if (following_hole < previous_hole)
483 hole_width = margin_array_min_angle_[following_hole] + 2 *
static_cast<float> (M_PI)
484 - margin_array_max_angle_[previous_hole];
488 hole_width = margin_array_min_angle_[following_hole] - margin_array_max_angle_[previous_hole];
490 float hole_prob = hole_width / (2 *
static_cast<float> (M_PI));
493 float steep_prob = (normal_end + normal_begin) / 2.0f;
497 if (hole_prob > hole_size_prob_thresh_)
499 if (steep_prob > steep_thresh_)
501 if (hole_prob > max_hole_prob)
503 max_hole_prob = hole_prob;
505 float angle_weight = ((normal_end - normal_begin) + 1.0f) / 2.0f;
506 if (following_hole < previous_hole)
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;
513 angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole]
514 - margin_array_max_angle_[previous_hole]) * angle_weight;
521 if (hole_end >= check_margin_array_size_)
529 if (max_hole_prob > -std::numeric_limits<float>::max ())
532 Eigen::AngleAxisf rotation = Eigen::AngleAxisf (angle, fitted_normal);
533 x_axis = rotation * x_axis;
535 min_normal_cos -= 10.0f;
539 if (best_point_found_on_margins)
542 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
547 if (min_normal_index == -1)
549 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
550 return (std::numeric_limits<float>::max ());
554 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
555 surface_->at (min_normal_index).getVector3fMap (), x_axis);
559 y_axis = fitted_normal.cross (x_axis);
561 lrf.row (0).matrix () = x_axis;
562 lrf.row (1).matrix () = y_axis;
565 return (min_normal_cos);
569 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
573 if (this->getKSearch () != 0)
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());
582 for (std::size_t point_idx = 0; point_idx < indices_->size (); ++point_idx)
584 Eigen::Matrix3f currentLrf;
585 PointOutT &rf = output[point_idx];
589 if (computePointLRF ((*indices_)[point_idx], currentLrf) == std::numeric_limits<float>::max ())
591 output.is_dense =
false;
594 for (
int d = 0; d < 3; ++d)
596 rf.x_axis[d] = currentLrf (0, d);
597 rf.y_axis[d] = currentLrf (1, d);
598 rf.z_axis[d] = currentLrf (2, d);
603 #define PCL_INSTANTIATE_BOARDLocalReferenceFrameEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::BOARDLocalReferenceFrameEstimation<T,NT,OutT>;
605 #endif // PCL_FEATURES_IMPL_BOARD_H_