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 = acosf (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 int n_points =
static_cast<int> (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 (
size_t i = 0; i < normal_indices.size (); ++i)
182 const PointNT& curPt = normal_cloud[normal_indices[i]];
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 float radius2 = tangent_radius_ * tangent_radius_;
259 float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2;
261 float max_boundary_angle = 0;
267 lrf.row (0).matrix () = x_axis;
269 for (
int i = 0; i < check_margin_array_size_; i++)
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;
277 max_boundary_angle = (2 *
static_cast<float> (M_PI)) /
static_cast<float> (check_margin_array_size_);
280 for (
int curr_neigh = 0; curr_neigh < n_neighbours; ++curr_neigh)
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)
290 margin_point_found =
true;
292 Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
294 float normal_cos = fitted_normal.dot (normal_mean);
295 if (normal_cos < min_normal_cos)
297 min_normal_index = curr_neigh_idx;
298 min_normal_cos = normal_cos;
299 best_point_found_on_margins =
false;
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);
310 int check_margin_array_idx = std::min (static_cast<int> (floor (angle / max_boundary_angle)), check_margin_array_size_ - 1);
311 check_margin_array_[check_margin_array_idx] =
true;
313 if (angle < margin_array_min_angle_[check_margin_array_idx])
315 margin_array_min_angle_[check_margin_array_idx] = angle;
316 margin_array_min_angle_normal_[check_margin_array_idx] = normal_cos;
318 if (angle > margin_array_max_angle_[check_margin_array_idx])
320 margin_array_max_angle_[check_margin_array_idx] = angle;
321 margin_array_max_angle_normal_[check_margin_array_idx] = normal_cos;
327 if (!margin_point_found)
330 for (
int curr_neigh = 0; curr_neigh < n_neighbours; curr_neigh++)
332 const int& curr_neigh_idx = neighbours_indices[curr_neigh];
333 const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
335 if (neigh_distance_sqr > margin_distance2)
338 Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
340 float normal_cos = fitted_normal.dot (normal_mean);
342 if (normal_cos < min_normal_cos)
344 min_normal_index = curr_neigh_idx;
345 min_normal_cos = normal_cos;
350 if (min_normal_index == -1)
352 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
353 return (std::numeric_limits<float>::max ());
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);
360 lrf.row (0).matrix () = x_axis;
361 lrf.row (1).matrix () = y_axis;
365 return (min_normal_cos);
370 if (best_point_found_on_margins)
373 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
374 y_axis = fitted_normal.cross (x_axis);
376 lrf.row (0).matrix () = x_axis;
377 lrf.row (1).matrix () = y_axis;
380 return (min_normal_cos);
384 if (min_normal_index == -1)
386 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
387 return (std::numeric_limits<float>::max ());
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);
394 lrf.row (0).matrix () = x_axis;
395 lrf.row (1).matrix () = y_axis;
398 return (min_normal_cos);
402 bool is_hole_present =
false;
403 for (
int i = 0; i < check_margin_array_size_; i++)
405 if (!check_margin_array_[i])
407 is_hole_present =
true;
412 if (!is_hole_present)
414 if (best_point_found_on_margins)
417 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
418 y_axis = fitted_normal.cross (x_axis);
420 lrf.row (0).matrix () = x_axis;
421 lrf.row (1).matrix () = y_axis;
424 return (min_normal_cos);
428 if (min_normal_index == -1)
430 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
431 return (std::numeric_limits<float>::max ());
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);
439 lrf.row (0).matrix () = x_axis;
440 lrf.row (1).matrix () = y_axis;
443 return (min_normal_cos);
453 int first_no_border = -1;
454 if (check_margin_array_[check_margin_array_size_ - 1])
460 for (
int i = 0; i < check_margin_array_size_; i++)
462 if (check_margin_array_[i])
471 float max_hole_prob = -std::numeric_limits<float>::max ();
474 for (
int ch = first_no_border; ch < check_margin_array_size_; ch++)
476 if (!check_margin_array_[ch])
480 hole_end = hole_first + 1;
481 while (!check_margin_array_[hole_end % check_margin_array_size_])
487 if ((hole_end - hole_first) > 0)
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;
503 float hole_width = 0.0f;
504 if (following_hole < previous_hole)
506 hole_width = margin_array_min_angle_[following_hole] + 2 *
static_cast<float> (M_PI)
507 - margin_array_max_angle_[previous_hole];
511 hole_width = margin_array_min_angle_[following_hole] - margin_array_max_angle_[previous_hole];
513 float hole_prob = hole_width / (2 *
static_cast<float> (M_PI));
516 float steep_prob = (normal_end + normal_begin) / 2.0f;
520 if (hole_prob > hole_size_prob_thresh_)
522 if (steep_prob > steep_thresh_)
524 if (hole_prob > max_hole_prob)
526 max_hole_prob = hole_prob;
528 float angle_weight = ((normal_end - normal_begin) + 1.0f) / 2.0f;
529 if (following_hole < previous_hole)
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;
536 angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole]
537 - margin_array_max_angle_[previous_hole]) * angle_weight;
544 if (hole_end >= check_margin_array_size_)
555 if (max_hole_prob > -std::numeric_limits<float>::max ())
558 Eigen::AngleAxisf rotation = Eigen::AngleAxisf (angle, fitted_normal);
559 x_axis = rotation * x_axis;
561 min_normal_cos -= 10.0f;
565 if (best_point_found_on_margins)
568 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
573 if (min_normal_index == -1)
575 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
576 return (std::numeric_limits<float>::max ());
580 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
581 surface_->at (min_normal_index).getVector3fMap (), x_axis);
585 y_axis = fitted_normal.cross (x_axis);
587 lrf.row (0).matrix () = x_axis;
588 lrf.row (1).matrix () = y_axis;
591 return (min_normal_cos);
595 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 599 if (this->getKSearch () != 0)
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());
608 for (
size_t point_idx = 0; point_idx < indices_->size (); ++point_idx)
610 Eigen::Matrix3f currentLrf;
611 PointOutT &rf = output[point_idx];
615 if (computePointLRF ((*indices_)[point_idx], currentLrf) == std::numeric_limits<float>::max ())
620 for (
int d = 0; d < 3; ++d)
622 rf.x_axis[d] = currentLrf (0, d);
623 rf.y_axis[d] = currentLrf (1, d);
624 rf.z_axis[d] = currentLrf (2, d);
629 #define PCL_INSTANTIATE_BOARDLocalReferenceFrameEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::BOARDLocalReferenceFrameEstimation<T,NT,OutT>; 631 #endif // PCL_FEATURES_IMPL_BOARD_H_ Eigen::Vector3f randomOrthogonalAxis(Eigen::Vector3f const &axis)
Define a random unit vector orthogonal to axis.
void planeFitting(Eigen::Matrix< float, Eigen::Dynamic, 3 > const &points, Eigen::Vector3f ¢er, Eigen::Vector3f &norm)
Compute Least Square Plane Fitting in a set of 3D points.
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.
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 .
void randomOrthogonalAxis(Eigen::Vector3f const &axis, Eigen::Vector3f &rand_ortho_axis)
Given an axis, return a random orthogonal axis.
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...
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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
void normalDisambiguation(pcl::PointCloud< PointNT > const &normals_cloud, std::vector< int > const &normal_indices, Eigen::Vector3f &normal)
Disambiguates a normal direction using adjacent normals.
virtual void computeFeature(PointCloudOut &output)
Abstract feature estimation method.