Point Cloud Library (PCL)  1.9.1
trajkovic_3d.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2013-, Open Perception Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_TRAJKOVIC_KEYPOINT_3D_H_
39 #define PCL_TRAJKOVIC_KEYPOINT_3D_H_
40 
41 #include <pcl/keypoints/keypoint.h>
42 #include <pcl/common/intensity.h>
43 
44 namespace pcl
45 {
46  /** \brief TrajkovicKeypoint3D implements Trajkovic and Hedley corner detector on
47  * point cloud using geometric information.
48  * It uses first order statistics to find variation of normals.
49  * This work is part of Nizar Sallem PhD thesis.
50  *
51  * \author Nizar Sallem
52  * \ingroup keypoints
53  */
54  template <typename PointInT, typename PointOutT, typename NormalT = pcl::Normal>
55  class TrajkovicKeypoint3D : public Keypoint<PointInT, PointOutT>
56  {
57  public:
58  typedef boost::shared_ptr<TrajkovicKeypoint3D<PointInT, PointOutT, NormalT> > Ptr;
59  typedef boost::shared_ptr<const TrajkovicKeypoint3D<PointInT, PointOutT, NormalT> > ConstPtr;
64  typedef typename Normals::Ptr NormalsPtr;
66 
72 
74 
75  /** \brief Constructor
76  * \param[in] method the method to be used to determine the corner responses
77  * \param[in] window_size
78  * \param[in] first_threshold the threshold used in the simple cornerness test.
79  * \param[in] second_threshold the threshold used to reject weak corners.
80  */
82  int window_size = 3,
83  float first_threshold = 0.00046,
84  float second_threshold = 0.03589)
85  : method_ (method)
86  , window_size_ (window_size)
87  , first_threshold_ (first_threshold)
88  , second_threshold_ (second_threshold)
89  , threads_ (1)
90  {
91  name_ = "TrajkovicKeypoint3D";
92  }
93 
94  /** \brief set the method of the response to be calculated.
95  * \param[in] method either 4 corners or 8 corners
96  */
97  inline void
98  setMethod (ComputationMethod method) { method_ = method; }
99 
100  /// \brief \return the computation method
101  inline ComputationMethod
102  getMethod () const { return (method_); }
103 
104  /// \brief Set window size
105  inline void
106  setWindowSize (int window_size) { window_size_= window_size; }
107 
108  /// \brief \return window size i.e. window width or height
109  inline int
110  getWindowSize () const { return (window_size_); }
111 
112  /** \brief set the first_threshold to reject corners in the simple cornerness
113  * computation stage.
114  * \param[in] threshold
115  */
116  inline void
117  setFirstThreshold (float threshold) { first_threshold_= threshold; }
118 
119  /// \brief \return first threshold
120  inline float
121  getFirstThreshold () const { return (first_threshold_); }
122 
123  /** \brief set the second threshold to reject corners in the final cornerness
124  * computation stage.
125  * \param[in] threshold
126  */
127  inline void
128  setSecondThreshold (float threshold) { second_threshold_= threshold; }
129 
130  /// \brief \return second threshold
131  inline float
132  getSecondThreshold () const { return (second_threshold_); }
133 
134  /** \brief Set normals if precalculated normals are available.
135  * \param normals
136  */
137  inline void
138  setNormals (const NormalsConstPtr &normals) { normals_ = normals; }
139 
140  /// \brief \return points normals as calculated or given
141  inline void
142  getNormals () const { return (normals_); }
143 
144  /** \brief Initialize the scheduler and set the number of threads to use.
145  * \param nr_threads the number of hardware threads to use, 0 for automatic.
146  */
147  inline void
148  setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; }
149 
150  /// \brief \return the number of threads
151  inline unsigned int
152  getNumberOfThreads () const { return (threads_); }
153 
154  protected:
155  bool
156  initCompute ();
157 
158  void
159  detectKeypoints (PointCloudOut &output);
160 
161  private:
162  /** Return a const reference to the normal at (i,j) if it is finite else return
163  * a reference to a null normal.
164  * If the returned normal is valid \a counter is incremented.
165  */
166  inline const NormalT&
167  getNormalOrNull (int i, int j, int& counter) const
168  {
169  static const NormalT null;
170  if (!isFinite ((*normals_) (i,j))) return (null);
171  ++counter;
172  return ((*normals_) (i,j));
173  }
174  /// \return difference of two normals vectors
175  inline float
176  normalsDiff (const NormalT& a, const NormalT& b) const
177  {
178  double nx = a.normal_x; double ny = a.normal_y; double nz = a.normal_z;
179  double mx = b.normal_x; double my = b.normal_y; double mz = b.normal_z;
180  return (static_cast<float> (1.0 - (nx*mx + ny*my + nz*mz)));
181  }
182  /// \return squared difference of two normals vectors
183  inline float
184  squaredNormalsDiff (const NormalT& a, const NormalT& b) const
185  {
186  float diff = normalsDiff (a,b);
187  return (diff * diff);
188  }
189  /** Comparator for responses intensity
190  * \return true if \a response_ at index \aa is greater than response at index \ab
191  */
192  inline bool
193  greaterCornernessAtIndices (int a, int b) const
194  {
195  return (response_->points [a] > response_->points [b]);
196  }
197  /// computation method
198  ComputationMethod method_;
199  /// window size
200  int window_size_;
201  /// half window size
202  int half_window_size_;
203  /// first threshold for quick rejection
204  float first_threshold_;
205  /// second threshold for corner evaluation
206  float second_threshold_;
207  /// number of threads to be used
208  unsigned int threads_;
209  /// point cloud normals
210  NormalsConstPtr normals_;
211  /// point cloud response
212  pcl::PointCloud<float>::Ptr response_;
213  };
214 }
215 
216 #include <pcl/keypoints/impl/trajkovic_3d.hpp>
217 
218 #endif // #ifndef PCL_TRAJKOVIC_KEYPOINT_3D_H_
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::Normal
A point structure representing normal coordinates and the surface curvature estimate.
Definition: point_types.hpp:791
pcl::PointCloud< NormalT >::Ptr
boost::shared_ptr< PointCloud< NormalT > > Ptr
Definition: point_cloud.h:428
pcl::TrajkovicKeypoint3D::getNormals
void getNormals() const
Definition: trajkovic_3d.h:142
pcl::TrajkovicKeypoint3D::ComputationMethod
ComputationMethod
Definition: trajkovic_3d.h:73
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
pcl::TrajkovicKeypoint3D::setSecondThreshold
void setSecondThreshold(float threshold)
set the second threshold to reject corners in the final cornerness computation stage.
Definition: trajkovic_3d.h:128
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:54
pcl::TrajkovicKeypoint3D::PointCloudInConstPtr
PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: trajkovic_3d.h:62
pcl::TrajkovicKeypoint3D::setMethod
void setMethod(ComputationMethod method)
set the method of the response to be calculated.
Definition: trajkovic_3d.h:98
pcl::TrajkovicKeypoint3D::TrajkovicKeypoint3D
TrajkovicKeypoint3D(ComputationMethod method=FOUR_CORNERS, int window_size=3, float first_threshold=0.00046, float second_threshold=0.03589)
Constructor.
Definition: trajkovic_3d.h:81
pcl::TrajkovicKeypoint3D::getFirstThreshold
float getFirstThreshold() const
Definition: trajkovic_3d.h:121
pcl::PointCloud< PointInT >
pcl::TrajkovicKeypoint3D::setNormals
void setNormals(const NormalsConstPtr &normals)
Set normals if precalculated normals are available.
Definition: trajkovic_3d.h:138
pcl::TrajkovicKeypoint3D::FOUR_CORNERS
@ FOUR_CORNERS
Definition: trajkovic_3d.h:73
pcl::TrajkovicKeypoint3D::getSecondThreshold
float getSecondThreshold() const
Definition: trajkovic_3d.h:132
pcl::TrajkovicKeypoint3D::NormalsConstPtr
Normals::ConstPtr NormalsConstPtr
Definition: trajkovic_3d.h:65
pcl::TrajkovicKeypoint3D::ConstPtr
boost::shared_ptr< const TrajkovicKeypoint3D< PointInT, PointOutT, NormalT > > ConstPtr
Definition: trajkovic_3d.h:59
pcl::TrajkovicKeypoint3D
TrajkovicKeypoint3D implements Trajkovic and Hedley corner detector on point cloud using geometric in...
Definition: trajkovic_3d.h:55
pcl::TrajkovicKeypoint3D::EIGHT_CORNERS
@ EIGHT_CORNERS
Definition: trajkovic_3d.h:73
pcl::TrajkovicKeypoint3D::setFirstThreshold
void setFirstThreshold(float threshold)
set the first_threshold to reject corners in the simple cornerness computation stage.
Definition: trajkovic_3d.h:117
pcl::Keypoint::name_
std::string name_
The key point detection method's name.
Definition: keypoint.h:173
pcl::TrajkovicKeypoint3D::PointCloudIn
Keypoint< PointInT, PointOutT >::PointCloudIn PointCloudIn
Definition: trajkovic_3d.h:60
pcl::TrajkovicKeypoint3D::NormalsPtr
Normals::Ptr NormalsPtr
Definition: trajkovic_3d.h:64
pcl::TrajkovicKeypoint3D::Normals
pcl::PointCloud< NormalT > Normals
Definition: trajkovic_3d.h:63
pcl::TrajkovicKeypoint3D::PointCloudOut
Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: trajkovic_3d.h:61
pcl::PointCloud< PointInT >::ConstPtr
boost::shared_ptr< const PointCloud< PointInT > > ConstPtr
Definition: point_cloud.h:429
pcl::TrajkovicKeypoint3D::initCompute
bool initCompute()
Definition: trajkovic_3d.hpp:44
pcl::Keypoint
Keypoint represents the base class for key points.
Definition: keypoint.h:49
pcl::TrajkovicKeypoint3D::getWindowSize
int getWindowSize() const
Definition: trajkovic_3d.h:110
pcl::TrajkovicKeypoint3D::getNumberOfThreads
unsigned int getNumberOfThreads() const
Definition: trajkovic_3d.h:152
pcl::TrajkovicKeypoint3D::detectKeypoints
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
Definition: trajkovic_3d.hpp:94
pcl::TrajkovicKeypoint3D::setWindowSize
void setWindowSize(int window_size)
Set window size.
Definition: trajkovic_3d.h:106
pcl::TrajkovicKeypoint3D::setNumberOfThreads
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
Definition: trajkovic_3d.h:148
pcl::TrajkovicKeypoint3D::getMethod
ComputationMethod getMethod() const
Definition: trajkovic_3d.h:102
pcl::TrajkovicKeypoint3D::Ptr
boost::shared_ptr< TrajkovicKeypoint3D< PointInT, PointOutT, NormalT > > Ptr
Definition: trajkovic_3d.h:58