Point Cloud Library (PCL)  1.9.1-dev
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 #pragma once
41 
42 #include <pcl/keypoints/agast_2d.h>
43 
44 namespace pcl
45 {
46  /** \brief Detects BRISK interest points based on the original code and paper
47  * reference by
48  *
49  * \par
50  * Stefan Leutenegger,Margarita Chli and Roland Siegwart,
51  * BRISK: Binary Robust Invariant Scalable Keypoints,
52  * in Proceedings of the IEEE International Conference on Computer Vision (ICCV2011).
53  *
54  * Code example:
55  *
56  * \code
57  * pcl::PointCloud<pcl::PointXYZRGBA> cloud;
58  * pcl::BriskKeypoint2D<pcl::PointXYZRGBA> brisk;
59  * brisk.setThreshold (60);
60  * brisk.setOctaves (4);
61  * brisk.setInputCloud (cloud);
62  *
63  * PointCloud<pcl::PointWithScale> keypoints;
64  * brisk.compute (keypoints);
65  * \endcode
66  *
67  * \author Radu B. Rusu, Stefan Holzer
68  * \ingroup keypoints
69  */
70  template <typename PointInT, typename PointOutT = pcl::PointWithScale, typename IntensityT = pcl::common::IntensityFieldAccessor<PointInT> >
71  class BriskKeypoint2D: public Keypoint<PointInT, PointOutT>
72  {
73  public:
74  typedef boost::shared_ptr<BriskKeypoint2D<PointInT, PointOutT, IntensityT> > Ptr;
75  typedef boost::shared_ptr<const BriskKeypoint2D<PointInT, PointOutT, IntensityT> > ConstPtr;
76 
81 
86 
87  /** \brief Constructor */
88  BriskKeypoint2D (int octaves = 4, int threshold = 60)
89  : threshold_ (threshold)
90  , octaves_ (octaves)
91  , remove_invalid_3D_keypoints_ (false)
92  {
93  k_ = 1;
94  name_ = "BriskKeypoint2D";
95  }
96 
97  /** \brief Destructor. */
99  {
100  }
101 
102  /** \brief Sets the threshold for corner detection.
103  * \param[in] threshold the threshold used for corner detection.
104  */
105  inline void
106  setThreshold (const int threshold)
107  {
108  threshold_ = threshold;
109  }
110 
111  /** \brief Get the threshold for corner detection, as set by the user. */
112  inline size_t
114  {
115  return (threshold_);
116  }
117 
118  /** \brief Set the number of octaves to use
119  * \param[in] octaves the number of octaves to use
120  */
121  inline void
122  setOctaves (const int octaves)
123  {
124  octaves_ = octaves;
125  }
126 
127  /** \brief Returns the number of octaves used. */
128  inline int
130  {
131  return (octaves_);
132  }
133 
134  /** \brief Specify whether we should do a 2nd pass through the list of keypoints
135  * found, and remove the ones that do not have a valid 3D (x-y-z) position
136  * (i.e., are NaN or Inf).
137  * \param[in] remove set to true whether we want the invalid 3D keypoints removed
138  */
139  inline void
141  {
142  remove_invalid_3D_keypoints_ = remove;
143  }
144 
145  /** \brief Specify whether the keypoints that do not have a valid 3D position are
146  * kept (false) or removed (true).
147  */
148  inline bool
150  {
151  return (remove_invalid_3D_keypoints_);
152  }
153 
154  /////////////////////////////////////////////////////////////////////////
155  inline void
156  bilinearInterpolation (const PointCloudInConstPtr &cloud,
157  float x, float y,
158  PointOutT &pt)
159  {
160  int u = int (x);
161  int v = int (y);
162 
163  pt.x = pt.y = pt.z = 0;
164 
165  const PointInT &p1 = (*cloud)(u, v);
166  const PointInT &p2 = (*cloud)(u+1, v);
167  const PointInT &p3 = (*cloud)(u, v+1);
168  const PointInT &p4 = (*cloud)(u+1, v+1);
169 
170  float fx = x - float (u), fy = y - float (v);
171  float fx1 = 1.0f - fx, fy1 = 1.0f - fy;
172 
173  float w1 = fx1 * fy1, w2 = fx * fy1, w3 = fx1 * fy, w4 = fx * fy;
174  float weight = 0;
175 
176  if (pcl::isFinite (p1))
177  {
178  pt.x += p1.x * w1;
179  pt.y += p1.y * w1;
180  pt.z += p1.z * w1;
181  weight += w1;
182  }
183  if (pcl::isFinite (p2))
184  {
185  pt.x += p2.x * w2;
186  pt.y += p2.y * w2;
187  pt.z += p2.z * w2;
188  weight += w2;
189  }
190  if (pcl::isFinite (p3))
191  {
192  pt.x += p3.x * w3;
193  pt.y += p3.y * w3;
194  pt.z += p3.z * w3;
195  weight += w3;
196  }
197  if (pcl::isFinite (p4))
198  {
199  pt.x += p4.x * w4;
200  pt.y += p4.y * w4;
201  pt.z += p4.z * w4;
202  weight += w4;
203  }
204 
205  if (weight == 0)
206  pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
207  else
208  {
209  weight = 1.0f / weight;
210  pt.x *= weight; pt.y *= weight; pt.z *= weight;
211  }
212  }
213 
214  protected:
215  /** \brief Initializes everything and checks whether input data is fine. */
216  bool
217  initCompute () override;
218 
219  /** \brief Detects the keypoints. */
220  void
221  detectKeypoints (PointCloudOut &output) override;
222 
223  private:
224  /** \brief Intensity field accessor. */
225  IntensityT intensity_;
226 
227  /** \brief Threshold for corner detection. */
228  int threshold_;
229 
230  int octaves_;
231 
232  /** \brief Specify whether the keypoints that do not have a valid 3D position are
233  * kept (false) or removed (true).
234  */
235  bool remove_invalid_3D_keypoints_;
236  };
237 
238  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
239  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
240  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
241  namespace keypoints
242  {
243  namespace brisk
244  {
245  /** \brief A layer in the BRISK detector pyramid. */
246  class PCL_EXPORTS Layer
247  {
248  public:
249  // constructor arguments
251  {
252  static const int HALFSAMPLE;
253  static const int TWOTHIRDSAMPLE;
254  };
255 
256  /** \brief Constructor.
257  * \param[in] img input image
258  * \param[in] width image width
259  * \param[in] height image height
260  * \param[in] scale scale
261  * \param[in] offset offset
262  */
263  Layer (const std::vector<unsigned char>& img,
264  int width, int height,
265  float scale = 1.0f, float offset = 0.0f);
266 
267  /** \brief Copy constructor for deriving a layer.
268  * \param[in] layer layer to derive from
269  * \param[in] mode deriving mode
270  */
271  Layer (const Layer& layer, int mode);
272 
273  /** \brief AGAST keypoints without non-max suppression.
274  * \param[in] threshold the keypoints threshold
275  * \param[out] keypoints the AGAST keypoints
276  */
277  void
278  getAgastPoints (uint8_t threshold, std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > &keypoints);
279 
280  // get scores - attention, this is in layer coordinates, not scale=1 coordinates!
281  /** \brief Get the AGAST keypoint score for a given pixel using a threshold
282  * \param[in] x the U coordinate of the pixel
283  * \param[in] y the V coordinate of the pixel
284  * \param[in] threshold the threshold to use for cutting the response
285  */
286  uint8_t
287  getAgastScore (int x, int y, uint8_t threshold);
288  /** \brief Get the AGAST keypoint score for a given pixel using a threshold
289  * \param[in] x the U coordinate of the pixel
290  * \param[in] y the V coordinate of the pixel
291  * \param[in] threshold the threshold to use for cutting the response
292  */
293  uint8_t
294  getAgastScore_5_8 (int x, int y, uint8_t threshold);
295  /** \brief Get the AGAST keypoint score for a given pixel using a threshold
296  * \param[in] xf the X coordinate of the pixel
297  * \param[in] yf the Y coordinate of the pixel
298  * \param[in] threshold the threshold to use for cutting the response
299  * \param[in] scale the scale
300  */
301  uint8_t
302  getAgastScore (float xf, float yf, uint8_t threshold, float scale = 1.0f);
303 
304  /** \brief Access gray values (smoothed/interpolated)
305  * \param[in] mat the image
306  * \param[in] width the image width
307  * \param[in] height the image height
308  * \param[in] xf the x coordinate
309  * \param[in] yf the y coordinate
310  * \param[in] scale the scale
311  */
312  uint8_t
313  getValue (const std::vector<unsigned char>& mat,
314  int width, int height, float xf, float yf, float scale);
315 
316  /** \brief Get the image used. */
317  const std::vector<unsigned char>&
318  getImage () const
319  {
320  return (img_);
321  }
322 
323  /** \brief Get the width of the image used. */
324  int
325  getImageWidth () const
326  {
327  return (img_width_);
328  }
329 
330  /** \brief Get the height of the image used. */
331  int
332  getImageHeight () const
333  {
334  return (img_height_);
335  }
336 
337  /** \brief Get the scale used. */
338  float
339  getScale () const
340  {
341  return (scale_);
342  }
343 
344  /** \brief Get the offset used. */
345  inline float
346  getOffset () const
347  {
348  return (offset_);
349  }
350 
351  /** \brief Get the scores obtained. */
352  inline const std::vector<unsigned char>&
353  getScores () const
354  {
355  return (scores_);
356  }
357 
358  private:
359  // half sampling
360  inline void
361  halfsample (const std::vector<unsigned char>& srcimg,
362  int srcwidth, int srcheight,
363  std::vector<unsigned char>& dstimg,
364  int dstwidth, int dstheight);
365 
366  // two third sampling
367  inline void
368  twothirdsample (const std::vector<unsigned char>& srcimg,
369  int srcwidth, int srcheight,
370  std::vector<unsigned char>& dstimg,
371  int dstwidth, int dstheight);
372 
373  /** the image */
374  std::vector<unsigned char> img_;
375  int img_width_;
376  int img_height_;
377 
378  /** its Fast scores */
379  std::vector<unsigned char> scores_;
380 
381  /** coordinate transformation */
382  float scale_;
383  float offset_;
384 
385  /** agast */
388  };
389 
390  /** BRISK Scale Space helper. */
391  class PCL_EXPORTS ScaleSpace
392  {
393  public:
394  /** \brief Constructor. Specify the number of octaves.
395  * \param[in] octaves the number of octaves (default: 3)
396  */
397  ScaleSpace (int octaves = 3);
398  ~ScaleSpace ();
399 
400  /** \brief Construct the image pyramids.
401  * \param[in] image the image to construct pyramids for
402  * \param[in] width the image width
403  * \param[in] height the image height
404  */
405  void
406  constructPyramid (const std::vector<unsigned char>& image,
407  int width, int height);
408 
409  /** \brief Get the keypoints for the associated image and threshold.
410  * \param[in] threshold the threshold for the keypoints
411  * \param[out] keypoints the resultant list of keypoints
412  */
413  void
414  getKeypoints (const int threshold,
415  std::vector<pcl::PointWithScale, Eigen::aligned_allocator<pcl::PointWithScale> > &keypoints);
416 
417  protected:
418  /** Nonmax suppression. */
419  inline bool
420  isMax2D (const uint8_t layer, const int x_layer, const int y_layer);
421 
422  /** 1D (scale axis) refinement: around octave */
423  inline float
424  refine1D (const float s_05, const float s0, const float s05, float& max);
425 
426  /** 1D (scale axis) refinement: around intra */
427  inline float
428  refine1D_1 (const float s_05, const float s0, const float s05, float& max);
429 
430  /** 1D (scale axis) refinement: around octave 0 only */
431  inline float
432  refine1D_2 (const float s_05, const float s0, const float s05, float& max);
433 
434  /** 2D maximum refinement */
435  inline float
436  subpixel2D (const int s_0_0, const int s_0_1, const int s_0_2,
437  const int s_1_0, const int s_1_1, const int s_1_2,
438  const int s_2_0, const int s_2_1, const int s_2_2,
439  float& delta_x, float& delta_y);
440 
441  /** 3D maximum refinement centered around (x_layer,y_layer) */
442  inline float
443  refine3D (const uint8_t layer,
444  const int x_layer, const int y_layer,
445  float& x, float& y, float& scale, bool& ismax);
446 
447  /** interpolated score access with recalculation when needed */
448  inline int
449  getScoreAbove (const uint8_t layer, const int x_layer, const int y_layer);
450 
451  inline int
452  getScoreBelow (const uint8_t layer, const int x_layer, const int y_layer);
453 
454  /** return the maximum of score patches above or below */
455  inline float
456  getScoreMaxAbove (const uint8_t layer,
457  const int x_layer, const int y_layer,
458  const int threshold, bool& ismax,
459  float& dx, float& dy);
460 
461  inline float
462  getScoreMaxBelow (const uint8_t layer,
463  const int x_layer, const int y_layer,
464  const int threshold, bool& ismax,
465  float& dx, float& dy);
466 
467  // the image pyramids
468  uint8_t layers_;
469  std::vector<pcl::keypoints::brisk::Layer> pyramid_;
470 
471  // Agast
472  uint8_t threshold_;
474 
475  // some constant parameters
477  float basic_size_;
478  };
479  } // namespace brisk
480  } // namespace keypoints
481 
482 }
483 
484 #include <pcl/keypoints/impl/brisk_2d.hpp>
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:53
BRISK Scale Space helper.
Definition: brisk_2d.h:391
A layer in the BRISK detector pyramid.
Definition: brisk_2d.h:246
std::vector< pcl::keypoints::brisk::Layer > pyramid_
Definition: brisk_2d.h:469
std::string name_
The key point detection method&#39;s name.
Definition: keypoint.h:170
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
void bilinearInterpolation(const PointCloudInConstPtr &cloud, float x, float y, PointOutT &pt)
Definition: brisk_2d.h:156
int getImageWidth() const
Get the width of the image used.
Definition: brisk_2d.h:325
Keypoint< PointInT, PointOutT >::KdTree KdTree
Definition: brisk_2d.h:79
float getOffset() const
Get the offset used.
Definition: brisk_2d.h:346
size_t getThreshold()
Get the threshold for corner detection, as set by the user.
Definition: brisk_2d.h:113
Keypoint represents the base class for key points.
Definition: keypoint.h:48
A 2D point structure representing pixel image coordinates.
PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: brisk_2d.h:80
int k_
The number of K nearest neighbors to use for each point.
Definition: keypoint.h:191
boost::shared_ptr< OastDetector9_16 > Ptr
Definition: agast_2d.h:410
boost::shared_ptr< const PointCloud< PointInT > > ConstPtr
Definition: point_cloud.h:428
Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: brisk_2d.h:78
int getOctaves()
Returns the number of octaves used.
Definition: brisk_2d.h:129
bool initCompute() override
Initializes everything and checks whether input data is fine.
Definition: brisk_2d.hpp:47
boost::shared_ptr< AgastDetector5_8 > Ptr
Definition: agast_2d.h:339
boost::shared_ptr< const BriskKeypoint2D< PointInT, PointOutT, IntensityT > > ConstPtr
Definition: brisk_2d.h:75
int getImageHeight() const
Get the height of the image used.
Definition: brisk_2d.h:332
float getScale() const
Get the scale used.
Definition: brisk_2d.h:339
bool getRemoveInvalid3DKeypoints()
Specify whether the keypoints that do not have a valid 3D position are kept (false) or removed (true)...
Definition: brisk_2d.h:149
Keypoint< PointInT, PointOutT >::PointCloudIn PointCloudIn
Definition: brisk_2d.h:77
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:140
Detects BRISK interest points based on the original code and paper reference by.
Definition: brisk_2d.h:71
BriskKeypoint2D(int octaves=4, int threshold=60)
Constructor.
Definition: brisk_2d.h:88
A point structure representing a 3-D position and scale.
~BriskKeypoint2D()
Destructor.
Definition: brisk_2d.h:98
boost::shared_ptr< BriskKeypoint2D< PointInT, PointOutT, IntensityT > > Ptr
Definition: brisk_2d.h:74
void setOctaves(const int octaves)
Set the number of octaves to use.
Definition: brisk_2d.h:122
void setThreshold(const int threshold)
Sets the threshold for corner detection.
Definition: brisk_2d.h:106
const std::vector< unsigned char > & getScores() const
Get the scores obtained.
Definition: brisk_2d.h:353
const std::vector< unsigned char > & getImage() const
Get the image used.
Definition: brisk_2d.h:318
void detectKeypoints(PointCloudOut &output) override
Detects the keypoints.
Definition: brisk_2d.hpp:66