Point Cloud Library (PCL)  1.8.1
brisk_2d.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (C) 2011, The Autonomous Systems Lab (ASL), ETH Zurich,
6  * Stefan Leutenegger, Simon Lynen and Margarita Chli.
7  * Copyright (c) 2012-, Open Perception, Inc.
8  *
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above
18  * copyright notice, this list of conditions and the following
19  * disclaimer in the documentation and/or other materials provided
20  * with the distribution.
21  * * Neither the name of the copyright holder(s) nor the names of its
22  * contributors may be used to endorse or promote products derived
23  * from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  *
38  */
39 
40 #ifndef PCL_KEYPOINTS_BRISK_KEYPOINT_2D_H_
41 #define PCL_KEYPOINTS_BRISK_KEYPOINT_2D_H_
42 
43 #include <pcl/keypoints/agast_2d.h>
44 
45 namespace pcl
46 {
47  /** \brief Detects BRISK interest points based on the original code and paper
48  * reference by
49  *
50  * \par
51  * Stefan Leutenegger,Margarita Chli and Roland Siegwart,
52  * BRISK: Binary Robust Invariant Scalable Keypoints,
53  * in Proceedings of the IEEE International Conference on Computer Vision (ICCV2011).
54  *
55  * Code example:
56  *
57  * \code
58  * pcl::PointCloud<pcl::PointXYZRGBA> cloud;
59  * pcl::BriskKeypoint2D<pcl::PointXYZRGBA> brisk;
60  * brisk.setThreshold (60);
61  * brisk.setOctaves (4);
62  * brisk.setInputCloud (cloud);
63  *
64  * PointCloud<pcl::PointWithScale> keypoints;
65  * brisk.compute (keypoints);
66  * \endcode
67  *
68  * \author Radu B. Rusu, Stefan Holzer
69  * \ingroup keypoints
70  */
71  template <typename PointInT, typename PointOutT = pcl::PointWithScale, typename IntensityT = pcl::common::IntensityFieldAccessor<PointInT> >
72  class BriskKeypoint2D: public Keypoint<PointInT, PointOutT>
73  {
74  public:
75  typedef boost::shared_ptr<BriskKeypoint2D<PointInT, PointOutT, IntensityT> > Ptr;
76  typedef boost::shared_ptr<const BriskKeypoint2D<PointInT, PointOutT, IntensityT> > ConstPtr;
77 
82 
87 
88  /** \brief Constructor */
89  BriskKeypoint2D (int octaves = 4, int threshold = 60)
90  : threshold_ (threshold)
91  , octaves_ (octaves)
92  , remove_invalid_3D_keypoints_ (false)
93  {
94  k_ = 1;
95  name_ = "BriskKeypoint2D";
96  }
97 
98  /** \brief Destructor. */
99  virtual ~BriskKeypoint2D ()
100  {
101  }
102 
103  /** \brief Sets the threshold for corner detection.
104  * \param[in] threshold the threshold used for corner detection.
105  */
106  inline void
107  setThreshold (const int threshold)
108  {
109  threshold_ = threshold;
110  }
111 
112  /** \brief Get the threshold for corner detection, as set by the user. */
113  inline size_t
115  {
116  return (threshold_);
117  }
118 
119  /** \brief Set the number of octaves to use
120  * \param[in] octaves the number of octaves to use
121  */
122  inline void
123  setOctaves (const int octaves)
124  {
125  octaves_ = octaves;
126  }
127 
128  /** \brief Returns the number of octaves used. */
129  inline int
131  {
132  return (octaves_);
133  }
134 
135  /** \brief Specify whether we should do a 2nd pass through the list of keypoints
136  * found, and remove the ones that do not have a valid 3D (x-y-z) position
137  * (i.e., are NaN or Inf).
138  * \param[in] remove set to true whether we want the invalid 3D keypoints removed
139  */
140  inline void
142  {
143  remove_invalid_3D_keypoints_ = remove;
144  }
145 
146  /** \brief Specify whether the keypoints that do not have a valid 3D position are
147  * kept (false) or removed (true).
148  */
149  inline bool
151  {
152  return (remove_invalid_3D_keypoints_);
153  }
154 
155  /////////////////////////////////////////////////////////////////////////
156  inline void
158  float x, float y,
159  PointOutT &pt)
160  {
161  int u = int (x);
162  int v = int (y);
163 
164  pt.x = pt.y = pt.z = 0;
165 
166  const PointInT &p1 = (*cloud)(u, v);
167  const PointInT &p2 = (*cloud)(u+1, v);
168  const PointInT &p3 = (*cloud)(u, v+1);
169  const PointInT &p4 = (*cloud)(u+1, v+1);
170 
171  float fx = x - float (u), fy = y - float (v);
172  float fx1 = 1.0f - fx, fy1 = 1.0f - fy;
173 
174  float w1 = fx1 * fy1, w2 = fx * fy1, w3 = fx1 * fy, w4 = fx * fy;
175  float weight = 0;
176 
177  if (pcl::isFinite (p1))
178  {
179  pt.x += p1.x * w1;
180  pt.y += p1.y * w1;
181  pt.z += p1.z * w1;
182  weight += w1;
183  }
184  if (pcl::isFinite (p2))
185  {
186  pt.x += p2.x * w2;
187  pt.y += p2.y * w2;
188  pt.z += p2.z * w2;
189  weight += w2;
190  }
191  if (pcl::isFinite (p3))
192  {
193  pt.x += p3.x * w3;
194  pt.y += p3.y * w3;
195  pt.z += p3.z * w3;
196  weight += w3;
197  }
198  if (pcl::isFinite (p4))
199  {
200  pt.x += p4.x * w4;
201  pt.y += p4.y * w4;
202  pt.z += p4.z * w4;
203  weight += w4;
204  }
205 
206  if (weight == 0)
207  pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
208  else
209  {
210  weight = 1.0f / weight;
211  pt.x *= weight; pt.y *= weight; pt.z *= weight;
212  }
213  }
214 
215  protected:
216  /** \brief Initializes everything and checks whether input data is fine. */
217  bool
218  initCompute ();
219 
220  /** \brief Detects the keypoints. */
221  void
222  detectKeypoints (PointCloudOut &output);
223 
224  private:
225  /** \brief Intensity field accessor. */
226  IntensityT intensity_;
227 
228  /** \brief Threshold for corner detection. */
229  int threshold_;
230 
231  int octaves_;
232 
233  /** \brief Specify whether the keypoints that do not have a valid 3D position are
234  * kept (false) or removed (true).
235  */
236  bool remove_invalid_3D_keypoints_;
237  };
238 
239  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
240  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
241  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
242  namespace keypoints
243  {
244  namespace brisk
245  {
246  /** \brief A layer in the BRISK detector pyramid. */
247  class PCL_EXPORTS Layer
248  {
249  public:
250  // constructor arguments
252  {
253  static const int HALFSAMPLE = 0;
254  static const int TWOTHIRDSAMPLE = 1;
255  };
256 
257  /** \brief Constructor.
258  * \param[in] img input image
259  * \param[in] width image width
260  * \param[in] height image height
261  * \param[in] scale scale
262  * \param[in] offset offset
263  */
264  Layer (const std::vector<unsigned char>& img,
265  int width, int height,
266  float scale = 1.0f, float offset = 0.0f);
267 
268  /** \brief Copy constructor for deriving a layer.
269  * \param[in] layer layer to derive from
270  * \param[in] mode deriving mode
271  */
272  Layer (const Layer& layer, int mode);
273 
274  /** \brief AGAST keypoints without non-max suppression.
275  * \param[in] threshold the keypoints threshold
276  * \param[out] keypoints the AGAST keypoints
277  */
278  void
279  getAgastPoints (uint8_t threshold, std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > &keypoints);
280 
281  // get scores - attention, this is in layer coordinates, not scale=1 coordinates!
282  /** \brief Get the AGAST keypoint score for a given pixel using a threshold
283  * \param[in] x the U coordinate of the pixel
284  * \param[in] y the V coordinate of the pixel
285  * \param[in] threshold the threshold to use for cutting the response
286  */
287  uint8_t
288  getAgastScore (int x, int y, uint8_t threshold);
289  /** \brief Get the AGAST keypoint score for a given pixel using a threshold
290  * \param[in] x the U coordinate of the pixel
291  * \param[in] y the V coordinate of the pixel
292  * \param[in] threshold the threshold to use for cutting the response
293  */
294  uint8_t
295  getAgastScore_5_8 (int x, int y, uint8_t threshold);
296  /** \brief Get the AGAST keypoint score for a given pixel using a threshold
297  * \param[in] xf the X coordinate of the pixel
298  * \param[in] yf the Y coordinate of the pixel
299  * \param[in] threshold the threshold to use for cutting the response
300  * \param[in] scale the scale
301  */
302  uint8_t
303  getAgastScore (float xf, float yf, uint8_t threshold, float scale = 1.0f);
304 
305  /** \brief Access gray values (smoothed/interpolated)
306  * \param[in] mat the image
307  * \param[in] width the image width
308  * \param[in] height the image height
309  * \param[in] xf the x coordinate
310  * \param[in] yf the y coordinate
311  * \param[in] scale the scale
312  */
313  uint8_t
314  getValue (const std::vector<unsigned char>& mat,
315  int width, int height, float xf, float yf, float scale);
316 
317  /** \brief Get the image used. */
318  const std::vector<unsigned char>&
319  getImage () const
320  {
321  return (img_);
322  }
323 
324  /** \brief Get the width of the image used. */
325  int
326  getImageWidth () const
327  {
328  return (img_width_);
329  }
330 
331  /** \brief Get the height of the image used. */
332  int
333  getImageHeight () const
334  {
335  return (img_height_);
336  }
337 
338  /** \brief Get the scale used. */
339  float
340  getScale () const
341  {
342  return (scale_);
343  }
344 
345  /** \brief Get the offset used. */
346  inline float
347  getOffset () const
348  {
349  return (offset_);
350  }
351 
352  /** \brief Get the scores obtained. */
353  inline const std::vector<unsigned char>&
354  getScores () const
355  {
356  return (scores_);
357  }
358 
359  private:
360  // half sampling
361  inline void
362  halfsample (const std::vector<unsigned char>& srcimg,
363  int srcwidth, int srcheight,
364  std::vector<unsigned char>& dstimg,
365  int dstwidth, int dstheight);
366 
367  // two third sampling
368  inline void
369  twothirdsample (const std::vector<unsigned char>& srcimg,
370  int srcwidth, int srcheight,
371  std::vector<unsigned char>& dstimg,
372  int dstwidth, int dstheight);
373 
374  /** the image */
375  std::vector<unsigned char> img_;
376  int img_width_;
377  int img_height_;
378 
379  /** its Fast scores */
380  std::vector<unsigned char> scores_;
381 
382  /** coordinate transformation */
383  float scale_;
384  float offset_;
385 
386  /** agast */
387  boost::shared_ptr<pcl::keypoints::agast::OastDetector9_16> oast_detector_;
388  boost::shared_ptr<pcl::keypoints::agast::AgastDetector5_8> agast_detector_5_8_;
389  };
390 
391  /** BRISK Scale Space helper. */
392  class PCL_EXPORTS ScaleSpace
393  {
394  public:
395  /** \brief Constructor. Specify the number of octaves.
396  * \param[in] octaves the number of octaves (default: 3)
397  */
398  ScaleSpace (int octaves = 3);
399  ~ScaleSpace ();
400 
401  /** \brief Construct the image pyramids.
402  * \param[in] image the image to construct pyramids for
403  * \param[in] width the image width
404  * \param[in] height the image height
405  */
406  void
407  constructPyramid (const std::vector<unsigned char>& image,
408  int width, int height);
409 
410  /** \brief Get the keypoints for the associated image and threshold.
411  * \param[in] threshold the threshold for the keypoints
412  * \param[out] keypoints the resultant list of keypoints
413  */
414  void
415  getKeypoints (const int threshold,
416  std::vector<pcl::PointWithScale, Eigen::aligned_allocator<pcl::PointWithScale> > &keypoints);
417 
418  protected:
419  /** Nonmax suppression. */
420  inline bool
421  isMax2D (const uint8_t layer, const int x_layer, const int y_layer);
422 
423  /** 1D (scale axis) refinement: around octave */
424  inline float
425  refine1D (const float s_05, const float s0, const float s05, float& max);
426 
427  /** 1D (scale axis) refinement: around intra */
428  inline float
429  refine1D_1 (const float s_05, const float s0, const float s05, float& max);
430 
431  /** 1D (scale axis) refinement: around octave 0 only */
432  inline float
433  refine1D_2 (const float s_05, const float s0, const float s05, float& max);
434 
435  /** 2D maximum refinement */
436  inline float
437  subpixel2D (const int s_0_0, const int s_0_1, const int s_0_2,
438  const int s_1_0, const int s_1_1, const int s_1_2,
439  const int s_2_0, const int s_2_1, const int s_2_2,
440  float& delta_x, float& delta_y);
441 
442  /** 3D maximum refinement centered around (x_layer,y_layer) */
443  inline float
444  refine3D (const uint8_t layer,
445  const int x_layer, const int y_layer,
446  float& x, float& y, float& scale, bool& ismax);
447 
448  /** interpolated score access with recalculation when needed */
449  inline int
450  getScoreAbove (const uint8_t layer, const int x_layer, const int y_layer);
451 
452  inline int
453  getScoreBelow (const uint8_t layer, const int x_layer, const int y_layer);
454 
455  /** return the maximum of score patches above or below */
456  inline float
457  getScoreMaxAbove (const uint8_t layer,
458  const int x_layer, const int y_layer,
459  const int threshold, bool& ismax,
460  float& dx, float& dy);
461 
462  inline float
463  getScoreMaxBelow (const uint8_t layer,
464  const int x_layer, const int y_layer,
465  const int threshold, bool& ismax,
466  float& dx, float& dy);
467 
468  // the image pyramids
469  uint8_t layers_;
470  std::vector<pcl::keypoints::brisk::Layer> pyramid_;
471 
472  // Agast
473  uint8_t threshold_;
475 
476  // some constant parameters
478  float basic_size_;
479  };
480  } // namespace brisk
481  } // namespace keypoints
482 
483 }
484 
485 #include <pcl/keypoints/impl/brisk_2d.hpp>
486 
487 #endif
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:54
BRISK Scale Space helper.
Definition: brisk_2d.h:392
A layer in the BRISK detector pyramid.
Definition: brisk_2d.h:247
std::vector< pcl::keypoints::brisk::Layer > pyramid_
Definition: brisk_2d.h:470
std::string name_
The key point detection method's name.
Definition: keypoint.h:173
void bilinearInterpolation(const PointCloudInConstPtr &cloud, float x, float y, PointOutT &pt)
Definition: brisk_2d.h:157
Keypoint< PointInT, PointOutT >::KdTree KdTree
Definition: brisk_2d.h:80
virtual ~BriskKeypoint2D()
Destructor.
Definition: brisk_2d.h:99
size_t getThreshold()
Get the threshold for corner detection, as set by the user.
Definition: brisk_2d.h:114
int getImageHeight() const
Get the height of the image used.
Definition: brisk_2d.h:333
Keypoint represents the base class for key points.
Definition: keypoint.h:49
A 2D point structure representing pixel image coordinates.
PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: brisk_2d.h:81
int k_
The number of K nearest neighbors to use for each point.
Definition: keypoint.h:194
bool initCompute()
Initializes everything and checks whether input data is fine.
Definition: brisk_2d.hpp:47
float getOffset() const
Get the offset used.
Definition: brisk_2d.h:347
boost::shared_ptr< const PointCloud< PointInT > > ConstPtr
Definition: point_cloud.h:429
float getScale() const
Get the scale used.
Definition: brisk_2d.h:340
void detectKeypoints(PointCloudOut &output)
Detects the keypoints.
Definition: brisk_2d.hpp:66
Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: brisk_2d.h:79
int getOctaves()
Returns the number of octaves used.
Definition: brisk_2d.h:130
boost::shared_ptr< const BriskKeypoint2D< PointInT, PointOutT, IntensityT > > ConstPtr
Definition: brisk_2d.h:76
const std::vector< unsigned char > & getScores() const
Get the scores obtained.
Definition: brisk_2d.h:354
bool getRemoveInvalid3DKeypoints()
Specify whether the keypoints that do not have a valid 3D position are kept (false) or removed (true)...
Definition: brisk_2d.h:150
const std::vector< unsigned char > & getImage() const
Get the image used.
Definition: brisk_2d.h:319
Keypoint< PointInT, PointOutT >::PointCloudIn PointCloudIn
Definition: brisk_2d.h:78
void setRemoveInvalid3DKeypoints(bool remove)
Specify whether we should do a 2nd pass through the list of keypoints found, and remove the ones that...
Definition: brisk_2d.h:141
Detects BRISK interest points based on the original code and paper reference by.
Definition: brisk_2d.h:72
BriskKeypoint2D(int octaves=4, int threshold=60)
Constructor.
Definition: brisk_2d.h:89
A point structure representing a 3-D position and scale.
boost::shared_ptr< BriskKeypoint2D< PointInT, PointOutT, IntensityT > > Ptr
Definition: brisk_2d.h:75
void setOctaves(const int octaves)
Set the number of octaves to use.
Definition: brisk_2d.h:123
void setThreshold(const int threshold)
Sets the threshold for corner detection.
Definition: brisk_2d.h:107
int getImageWidth() const
Get the width of the image used.
Definition: brisk_2d.h:326