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