Point Cloud Library (PCL)  1.9.0-dev
trajkovic_2d.hpp
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_2D_IMPL_H_
39 #define PCL_TRAJKOVIC_KEYPOINT_2D_IMPL_H_
40 
41 template <typename PointInT, typename PointOutT, typename IntensityT> bool
43 {
45  return (false);
46 
47  keypoints_indices_.reset (new pcl::PointIndices);
48  keypoints_indices_->indices.reserve (input_->size ());
49 
50  if (!input_->isOrganized ())
51  {
52  PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
53  return (false);
54  }
55 
56  if (indices_->size () != input_->size ())
57  {
58  PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ());
59  return (false);
60  }
61 
62  if ((window_size_%2) == 0)
63  {
64  PCL_ERROR ("[pcl::%s::initCompute] Window size must be odd!\n", name_.c_str ());
65  return (false);
66  }
67 
68  if (window_size_ < 3)
69  {
70  PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ());
71  return (false);
72  }
73 
74  half_window_size_ = window_size_ / 2;
75 
76  return (true);
77 }
78 
79 /////////////////////////////////////////////////////////////////////////////////////////////
80 template <typename PointInT, typename PointOutT, typename IntensityT> void
82 {
83  response_.reset (new pcl::PointCloud<float> (input_->width, input_->height));
84  int w = static_cast<int> (input_->width) - half_window_size_;
85  int h = static_cast<int> (input_->height) - half_window_size_;
86 
88  {
89 #ifdef _OPENMP
90 #pragma omp parallel for num_threads (threads_)
91 #endif
92  for(int j = half_window_size_; j < h; ++j)
93  {
94  for(int i = half_window_size_; i < w; ++i)
95  {
96  float center = intensity_ ((*input_) (i,j));
97  float up = intensity_ ((*input_) (i, j-half_window_size_));
98  float down = intensity_ ((*input_) (i, j+half_window_size_));
99  float left = intensity_ ((*input_) (i-half_window_size_, j));
100  float right = intensity_ ((*input_) (i+half_window_size_, j));
101 
102  float up_center = up - center;
103  float r1 = up_center * up_center;
104  float down_center = down - center;
105  r1+= down_center * down_center;
106 
107  float right_center = right - center;
108  float r2 = right_center * right_center;
109  float left_center = left - center;
110  r2+= left_center * left_center;
111 
112  float d = std::min (r1, r2);
113 
114  if (d < first_threshold_)
115  continue;
116 
117  float b1 = (right - up) * up_center;
118  b1+= (left - down) * down_center;
119  float b2 = (right - down) * down_center;
120  b2+= (left - up) * up_center;
121  float B = std::min (b1, b2);
122  float A = r2 - r1 - 2*B;
123 
124  (*response_) (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d;
125  }
126  }
127  }
128  else
129  {
130 #ifdef _OPENMP
131 #pragma omp parallel for num_threads (threads_)
132 #endif
133  for(int j = half_window_size_; j < h; ++j)
134  {
135  for(int i = half_window_size_; i < w; ++i)
136  {
137  float center = intensity_ ((*input_) (i,j));
138  float up = intensity_ ((*input_) (i, j-half_window_size_));
139  float down = intensity_ ((*input_) (i, j+half_window_size_));
140  float left = intensity_ ((*input_) (i-half_window_size_, j));
141  float right = intensity_ ((*input_) (i+half_window_size_, j));
142  float upleft = intensity_ ((*input_) (i-half_window_size_, j-half_window_size_));
143  float upright = intensity_ ((*input_) (i+half_window_size_, j-half_window_size_));
144  float downleft = intensity_ ((*input_) (i-half_window_size_, j+half_window_size_));
145  float downright = intensity_ ((*input_) (i+half_window_size_, j+half_window_size_));
146  std::vector<float> r (4,0);
147 
148  float up_center = up - center;
149  r[0] = up_center * up_center;
150  float down_center = down - center;
151  r[0]+= down_center * down_center;
152 
153  float upright_center = upright - center;
154  r[1] = upright_center * upright_center;
155  float downleft_center = downleft - center;
156  r[1]+= downleft_center * downleft_center;
157 
158  float right_center = right - center;
159  r[2] = right_center * right_center;
160  float left_center = left - center;
161  r[2]+= left_center * left_center;
162 
163  float downright_center = downright - center;
164  r[3] = downright_center * downright_center;
165  float upleft_center = upleft - center;
166  r[3]+= upleft_center * upleft_center;
167 
168  float d = *(std::min_element (r.begin (), r.end ()));
169 
170  if (d < first_threshold_)
171  continue;
172 
173  std::vector<float> B (4,0);
174  std::vector<float> A (4,0);
175  std::vector<float> sumAB (4,0);
176  B[0] = (upright - up) * up_center;
177  B[0]+= (downleft - down) * down_center;
178  B[1] = (right - upright) * upright_center;
179  B[1]+= (left - downleft) * downleft_center;
180  B[2] = (downright - right) * downright_center;
181  B[2]+= (upleft - left) * upleft_center;
182  B[3] = (down - downright) * downright_center;
183  B[3]+= (up - upleft) * upleft_center;
184  A[0] = r[1] - r[0] - B[0] - B[0];
185  A[1] = r[2] - r[1] - B[1] - B[1];
186  A[2] = r[3] - r[2] - B[2] - B[2];
187  A[3] = r[0] - r[3] - B[3] - B[3];
188  sumAB[0] = A[0] + B[0];
189  sumAB[1] = A[1] + B[1];
190  sumAB[2] = A[2] + B[2];
191  sumAB[3] = A[3] + B[3];
192  if ((*std::max_element (B.begin (), B.end ()) < 0) &&
193  (*std::min_element (sumAB.begin (), sumAB.end ()) > 0))
194  {
195  std::vector<float> D (4,0);
196  D[0] = B[0] * B[0] / A[0];
197  D[1] = B[1] * B[1] / A[1];
198  D[2] = B[2] * B[2] / A[2];
199  D[3] = B[3] * B[3] / A[3];
200  (*response_) (i,j) = *(std::min (D.begin (), D.end ()));
201  }
202  else
203  (*response_) (i,j) = d;
204  }
205  }
206  }
207 
208  // Non maximas suppression
209  std::vector<int> indices = *indices_;
210  std::sort (indices.begin (), indices.end (),
211  boost::bind (&TrajkovicKeypoint2D::greaterCornernessAtIndices, this, _1, _2));
212 
213  output.clear ();
214  output.reserve (input_->size ());
215 
216  std::vector<bool> occupency_map (indices.size (), false);
217  const int width (input_->width);
218  const int height (input_->height);
219 
220 #ifdef _OPENMP
221 #pragma omp parallel for shared (output) num_threads (threads_)
222 #endif
223  for (size_t i = 0; i < indices.size (); ++i)
224  {
225  int idx = indices[i];
226  if ((response_->points[idx] < second_threshold_) || occupency_map[idx])
227  continue;
228 
229  PointOutT p;
230  p.getVector3fMap () = input_->points[idx].getVector3fMap ();
231  p.intensity = response_->points [idx];
232 
233 #ifdef _OPENMP
234 #pragma omp critical
235 #endif
236  {
237  output.push_back (p);
238  keypoints_indices_->indices.push_back (idx);
239  }
240 
241  const int x = idx % width;
242  const int y = idx / width;
243  const int u_end = std::min (width, x + half_window_size_);
244  const int v_end = std::min (height, y + half_window_size_);
245  for(int v = std::max (0, y - half_window_size_); v < v_end; ++v)
246  for(int u = std::max (0, x - half_window_size_); u < u_end; ++u)
247  occupency_map[v*width + u] = true;
248  }
249 
250  output.height = 1;
251  output.width = static_cast<uint32_t> (output.size());
252  // we don not change the denseness
253  output.is_dense = input_->is_dense;
254 }
255 
256 #define PCL_INSTANTIATE_TrajkovicKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::TrajkovicKeypoint2D<T,U,I>;
257 #endif
size_t size() const
Definition: point_cloud.h:448
void reserve(size_t n)
Definition: point_cloud.h:449
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:480
TrajkovicKeypoint2D implements Trajkovic and Hedley corner detector on organized pooint cloud using i...
Definition: trajkovic_2d.h:55
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
PCL base class.
Definition: pcl_base.h:68
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:576
PointCloud represents the base class in PCL for storing collections of 3D points. ...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:418
Definition: norms.h:55