Point Cloud Library (PCL)  1.14.1-dev
board.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, 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 #pragma once
41 
42 #include <pcl/point_types.h>
43 #include <pcl/features/feature.h>
44 
45 namespace pcl
46 {
47  /** \brief BOARDLocalReferenceFrameEstimation implements the BOrder Aware Repeatable Directions algorithm
48  * for local reference frame estimation as described here:
49  *
50  * - A. Petrelli, L. Di Stefano,
51  * "On the repeatability of the local reference frame for partial shape matching",
52  * 13th International Conference on Computer Vision (ICCV), 2011
53  *
54  * \author Alioscia Petrelli (original), Tommaso Cavallari (PCL port)
55  * \ingroup features
56  */
57  template<typename PointInT, typename PointNT, typename PointOutT = ReferenceFrame>
58  class BOARDLocalReferenceFrameEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>
59  {
60  public:
61  using Ptr = shared_ptr<BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT> >;
62  using ConstPtr = shared_ptr<const BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT> >;
63 
64  /** \brief Constructor. */
66  {
67  feature_name_ = "BOARDLocalReferenceFrameEstimation";
68  setCheckMarginArraySize (check_margin_array_size_);
69  }
70 
71  /** \brief Empty destructor */
72  ~BOARDLocalReferenceFrameEstimation () override = default;
73 
74  //Getters/Setters
75 
76  /** \brief Set the maximum distance of the points used to estimate the x_axis and y_axis of the BOARD Reference Frame for a given point.
77  *
78  * \param[in] radius The search radius for x and y axes. If not set or set to 0 the parameter given with setRadiusSearch is used.
79  */
80  inline void
81  setTangentRadius (float radius)
82  {
83  tangent_radius_ = radius;
84  }
85 
86  /** \brief Get the maximum distance of the points used to estimate the x_axis and y_axis of the BOARD Reference Frame for a given point.
87  *
88  * \return The search radius for x and y axes. If set to 0 the parameter given with setRadiusSearch is used.
89  */
90  inline float
92  {
93  return (tangent_radius_);
94  }
95 
96  /** \brief Sets whether holes in the margin of the support, for each point, are searched and accounted for in the estimation of the
97  * Reference Frame or not.
98  *
99  * \param[in] find_holes Enable/Disable the search for holes in the support.
100  */
101  inline void
102  setFindHoles (bool find_holes)
103  {
104  find_holes_ = find_holes;
105  }
106 
107  /** \brief Gets whether holes in the margin of the support, for each point, are searched and accounted for in the estimation of the
108  * Reference Frame or not.
109  *
110  * \return The search for holes in the support is enabled/disabled.
111  */
112  inline bool
113  getFindHoles () const
114  {
115  return (find_holes_);
116  }
117 
118  /** \brief Sets the percentage of the search radius (or tangent radius if set) after which a point is considered part of the support margin.
119  *
120  * \param[in] margin_thresh the percentage of the search radius after which a point is considered a margin point.
121  */
122  inline void
123  setMarginThresh (float margin_thresh)
124  {
125  margin_thresh_ = margin_thresh;
126  }
127 
128  /** \brief Gets the percentage of the search radius (or tangent radius if set) after which a point is considered part of the support margin.
129  *
130  * \return The percentage of the search radius after which a point is considered a margin point.
131  */
132  inline float
134  {
135  return (margin_thresh_);
136  }
137 
138  /** \brief Sets the number of slices in which is divided the margin for the search of missing regions.
139  *
140  * \param[in] size the number of margin slices.
141  */
142  void
144  {
145  check_margin_array_size_ = size;
146 
147  check_margin_array_.clear ();
148  check_margin_array_.resize (check_margin_array_size_);
149 
150  margin_array_min_angle_.clear ();
151  margin_array_min_angle_.resize (check_margin_array_size_);
152 
153  margin_array_max_angle_.clear ();
154  margin_array_max_angle_.resize (check_margin_array_size_);
155 
156  margin_array_min_angle_normal_.clear ();
157  margin_array_min_angle_normal_.resize (check_margin_array_size_);
158 
159  margin_array_max_angle_normal_.clear ();
160  margin_array_max_angle_normal_.resize (check_margin_array_size_);
161  }
162 
163  /** \brief Gets the number of slices in which is divided the margin for the search of missing regions.
164  *
165  * \return the number of margin slices.
166  */
167  inline int
169  {
170  return (check_margin_array_size_);
171  }
172 
173  /** \brief Given the angle width of a hole in the support margin, sets the minimum percentage of a circumference this angle
174  * must cover to be considered a missing region in the support and hence used for the estimation of the Reference Frame.
175  *
176  * \param[in] prob_thresh the minimum percentage of a circumference after which a hole is considered in the calculation
177  */
178  inline void
179  setHoleSizeProbThresh (float prob_thresh)
180  {
181  hole_size_prob_thresh_ = prob_thresh;
182  }
183 
184  /** \brief Given the angle width of a hole in the support margin, gets the minimum percentage of a circumference this angle
185  * must cover to be considered a missing region in the support and hence used for the estimation of the Reference Frame.
186  *
187  * \return the minimum percentage of a circumference after which a hole is considered in the calculation
188  */
189  inline float
191  {
192  return (hole_size_prob_thresh_);
193  }
194 
195  /** \brief Sets the minimum steepness that the normals of the points situated on the borders of the hole, with reference
196  * to the normal of the best point found by the algorithm, must have in order to be considered in the calculation of the Reference Frame.
197  *
198  * \param[in] steep_thresh threshold that defines if a missing region contains a point with the most different normal.
199  */
200  inline void
201  setSteepThresh (float steep_thresh)
202  {
203  steep_thresh_ = steep_thresh;
204  }
205 
206  /** \brief Gets the minimum steepness that the normals of the points situated on the borders of the hole, with reference
207  * to the normal of the best point found by the algorithm, must have in order to be considered in the calculation of the Reference Frame.
208  *
209  * \return threshold that defines if a missing region contains a point with the most different normal.
210  */
211  inline float
212  getSteepThresh () const
213  {
214  return (steep_thresh_);
215  }
216 
217  protected:
226 
229 
230  void
232  {
233  setCheckMarginArraySize (check_margin_array_size_);
234  }
235 
236  /** \brief Estimate the LRF descriptor for a given point based on its spatial neighborhood of 3D points with normals
237  * \param[in] index the index of the point in input_
238  * \param[out] lrf the resultant local reference frame
239  */
240  float
241  computePointLRF (const int &index, Eigen::Matrix3f &lrf);
242 
243  /** \brief Abstract feature estimation method.
244  * \param[out] output the resultant features
245  */
246  void
247  computeFeature (PointCloudOut &output) override;
248 
249  /** \brief Given an axis (with origin axis_origin), return the orthogonal axis directed to point.
250  *
251  * \note axis must be normalized.
252  *
253  * \param[in] axis the axis
254  * \param[in] axis_origin the axis_origin
255  * \param[in] point the point towards which the resulting axis is directed
256  * \param[out] directed_ortho_axis the directed orthogonal axis calculated
257  */
258  void
259  directedOrthogonalAxis (Eigen::Vector3f const &axis, Eigen::Vector3f const &axis_origin,
260  Eigen::Vector3f const &point, Eigen::Vector3f &directed_ortho_axis);
261 
262  /** \brief return the angle (in radians) that rotate v1 to v2 with respect to axis .
263  *
264  * \param[in] v1 the first vector
265  * \param[in] v2 the second vector
266  * \param[in] axis the rotation axis. Axis must be normalized and orthogonal to plane defined by v1 and v2.
267  * \return angle
268  */
269  float
270  getAngleBetweenUnitVectors (Eigen::Vector3f const &v1, Eigen::Vector3f const &v2, Eigen::Vector3f const &axis);
271 
272  /** \brief Disambiguates a normal direction using adjacent normals
273  *
274  * \param[in] normals_cloud a cloud of normals used for the calculation
275  * \param[in] normal_indices the indices of the normals in the cloud that should to be used for the calculation
276  * \param[in,out] normal the normal to disambiguate, the calculation is performed in place
277  */
278  void
279  normalDisambiguation (pcl::PointCloud<PointNT> const &normals_cloud, pcl::Indices const &normal_indices,
280  Eigen::Vector3f &normal);
281 
282  /** \brief Compute Least Square Plane Fitting in a set of 3D points
283  *
284  * \param[in] points Matrix(nPoints,3) of 3D points coordinates
285  * \param[out] center centroid of the distribution of points that belongs to the fitted plane
286  * \param[out] norm normal to the fitted plane
287  */
288  void
289  planeFitting (Eigen::Matrix<float, Eigen::Dynamic, 3> const &points, Eigen::Vector3f &center,
290  Eigen::Vector3f &norm);
291 
292  /** \brief Given a plane (origin and normal) and a point, return the projection of x on plane
293  *
294  * Equivalent to vtkPlane::ProjectPoint()
295  *
296  * \param[in] point the point to project
297  * \param[in] origin_point a point belonging to the plane
298  * \param[in] plane_normal normal of the plane
299  * \param[out] projected_point the projection of the point on the plane
300  */
301  void
302  projectPointOnPlane (Eigen::Vector3f const &point, Eigen::Vector3f const &origin_point,
303  Eigen::Vector3f const &plane_normal, Eigen::Vector3f &projected_point);
304 
305  /** \brief Given an axis, return a random orthogonal axis
306  *
307  * \param[in] axis input axis
308  * \param[out] rand_ortho_axis an axis orthogonal to the input axis and whose direction is random
309  */
310  void
311  randomOrthogonalAxis (Eigen::Vector3f const &axis, Eigen::Vector3f &rand_ortho_axis);
312 
313  /** \brief Check if val1 and val2 are equals.
314  *
315  * \param[in] val1 first number to check.
316  * \param[in] val2 second number to check.
317  * \param[in] zero_float_eps epsilon
318  * \return true if val1 is equal to val2, false otherwise.
319  */
320  inline bool
321  areEquals (float val1, float val2, float zero_float_eps = 1E-8f) const
322  {
323  return (std::abs (val1 - val2) < zero_float_eps);
324  }
325 
326  private:
327  /** \brief Radius used to find tangent axis. */
328  float tangent_radius_{0.0f};
329 
330  /** \brief If true, check if support is complete or has missing regions because it is too near to mesh borders. */
331  bool find_holes_{false};
332 
333  /** \brief Threshold that define if a support point is near the margins. */
334  float margin_thresh_{0.85f};
335 
336  /** \brief Number of slices that divide the support in order to determine if a missing region is present. */
337  int check_margin_array_size_{24};
338 
339  /** \brief Threshold used to determine a missing region */
340  float hole_size_prob_thresh_{0.2f};
341 
342  /** \brief Threshold that defines if a missing region contains a point with the most different normal. */
343  float steep_thresh_{0.1f};
344 
345  std::vector<bool> check_margin_array_;
346  std::vector<float> margin_array_min_angle_;
347  std::vector<float> margin_array_max_angle_;
348  std::vector<float> margin_array_min_angle_normal_;
349  std::vector<float> margin_array_max_angle_normal_;
350  };
351 }
352 
353 #ifdef PCL_NO_PRECOMPILE
354 #include <pcl/features/impl/board.hpp>
355 #endif
BOARDLocalReferenceFrameEstimation implements the BOrder Aware Repeatable Directions algorithm for lo...
Definition: board.h:59
void setSteepThresh(float steep_thresh)
Sets the minimum steepness that the normals of the points situated on the borders of the hole,...
Definition: board.h:201
bool getFindHoles() const
Gets whether holes in the margin of the support, for each point, are searched and accounted for in th...
Definition: board.h:113
float getTangentRadius() const
Get the maximum distance of the points used to estimate the x_axis and y_axis of the BOARD Reference ...
Definition: board.h:91
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
void setHoleSizeProbThresh(float prob_thresh)
Given the angle width of a hole in the support margin, sets the minimum percentage of a circumference...
Definition: board.h:179
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:228
void setCheckMarginArraySize(int size)
Sets the number of slices in which is divided the margin for the search of missing regions.
Definition: board.h:143
void setTangentRadius(float radius)
Set the maximum distance of the points used to estimate the x_axis and y_axis of the BOARD Reference ...
Definition: board.h:81
float getMarginThresh() const
Gets the percentage of the search radius (or tangent radius if set) after which a point is considered...
Definition: board.h:133
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
~BOARDLocalReferenceFrameEstimation() override=default
Empty destructor.
BOARDLocalReferenceFrameEstimation()
Constructor.
Definition: board.h:65
void setMarginThresh(float margin_thresh)
Sets the percentage of the search radius (or tangent radius if set) after which a point is considered...
Definition: board.h:123
typename Feature< PointInT, PointOutT >::PointCloudIn PointCloudIn
Definition: board.h:227
float getSteepThresh() const
Gets the minimum steepness that the normals of the points situated on the borders of the hole,...
Definition: board.h:212
int getCheckMarginArraySize() const
Gets the number of slices in which is divided the margin for the search of missing regions.
Definition: board.h:168
shared_ptr< BOARDLocalReferenceFrameEstimation< PointInT, PointNT, PointOutT > > Ptr
Definition: board.h:61
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
bool areEquals(float val1, float val2, float zero_float_eps=1E-8f) const
Check if val1 and val2 are equals.
Definition: board.h:321
float getHoleSizeProbThresh() const
Given the angle width of a hole in the support margin, gets the minimum percentage of a circumference...
Definition: board.h:190
void setFindHoles(bool find_holes)
Sets whether holes in the margin of the support, for each point, are searched and accounted for in th...
Definition: board.h:102
shared_ptr< const BOARDLocalReferenceFrameEstimation< PointInT, PointNT, PointOutT > > ConstPtr
Definition: board.h:62
Feature represents the base feature class.
Definition: feature.h:107
std::string feature_name_
The feature name.
Definition: feature.h:220
Defines all the PCL implemented PointT point type structures.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133