Point Cloud Library (PCL)  1.14.1-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 
39 #ifndef PCL_TRAJKOVIC_KEYPOINT_2D_IMPL_H_
40 #define PCL_TRAJKOVIC_KEYPOINT_2D_IMPL_H_
41 
42 
43 namespace pcl
44 {
45 
46 template <typename PointInT, typename PointOutT, typename IntensityT> bool
48 {
50  return (false);
51 
52  keypoints_indices_.reset (new pcl::PointIndices);
53  keypoints_indices_->indices.reserve (input_->size ());
54 
55  if (!input_->isOrganized ())
56  {
57  PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
58  return (false);
59  }
60 
61  if (indices_->size () != input_->size ())
62  {
63  PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ());
64  return (false);
65  }
66 
67  if ((window_size_%2) == 0)
68  {
69  PCL_ERROR ("[pcl::%s::initCompute] Window size must be odd!\n", name_.c_str ());
70  return (false);
71  }
72 
73  if (window_size_ < 3)
74  {
75  PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ());
76  return (false);
77  }
78 
79  half_window_size_ = window_size_ / 2;
80 
81  return (true);
82 }
83 
84 
85 template <typename PointInT, typename PointOutT, typename IntensityT> void
87 {
88  response_.reset (new pcl::PointCloud<float> (input_->width, input_->height));
89  const int w = static_cast<int> (input_->width) - half_window_size_;
90  const int h = static_cast<int> (input_->height) - half_window_size_;
91 
93  {
94 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
95 #pragma omp parallel for \
96  default(none) \
97  num_threads(threads_)
98 #else
99 #pragma omp parallel for \
100  default(none) \
101  shared(h, w) \
102  num_threads(threads_)
103 #endif
104  for(int j = half_window_size_; j < h; ++j)
105  {
106  for(int i = half_window_size_; i < w; ++i)
107  {
108  float center = intensity_ ((*input_) (i,j));
109  float up = intensity_ ((*input_) (i, j-half_window_size_));
110  float down = intensity_ ((*input_) (i, j+half_window_size_));
111  float left = intensity_ ((*input_) (i-half_window_size_, j));
112  float right = intensity_ ((*input_) (i+half_window_size_, j));
113 
114  float up_center = up - center;
115  float r1 = up_center * up_center;
116  float down_center = down - center;
117  r1+= down_center * down_center;
118 
119  float right_center = right - center;
120  float r2 = right_center * right_center;
121  float left_center = left - center;
122  r2+= left_center * left_center;
123 
124  float d = std::min (r1, r2);
125 
126  if (d < first_threshold_)
127  continue;
128 
129  float b1 = (right - up) * up_center;
130  b1+= (left - down) * down_center;
131  float b2 = (right - down) * down_center;
132  b2+= (left - up) * up_center;
133  float B = std::min (b1, b2);
134  float A = r2 - r1 - 2*B;
135 
136  (*response_) (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d;
137  }
138  }
139  }
140  else
141  {
142 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
143 #pragma omp parallel for \
144  default(none) \
145  num_threads(threads_)
146 #else
147 #pragma omp parallel for \
148  default(none) \
149  shared(h, w) \
150  num_threads(threads_)
151 #endif
152  for(int j = half_window_size_; j < h; ++j)
153  {
154  for(int i = half_window_size_; i < w; ++i)
155  {
156  float center = intensity_ ((*input_) (i,j));
157  float up = intensity_ ((*input_) (i, j-half_window_size_));
158  float down = intensity_ ((*input_) (i, j+half_window_size_));
159  float left = intensity_ ((*input_) (i-half_window_size_, j));
160  float right = intensity_ ((*input_) (i+half_window_size_, j));
161  float upleft = intensity_ ((*input_) (i-half_window_size_, j-half_window_size_));
162  float upright = intensity_ ((*input_) (i+half_window_size_, j-half_window_size_));
163  float downleft = intensity_ ((*input_) (i-half_window_size_, j+half_window_size_));
164  float downright = intensity_ ((*input_) (i+half_window_size_, j+half_window_size_));
165  std::vector<float> r (4,0);
166 
167  float up_center = up - center;
168  r[0] = up_center * up_center;
169  float down_center = down - center;
170  r[0]+= down_center * down_center;
171 
172  float upright_center = upright - center;
173  r[1] = upright_center * upright_center;
174  float downleft_center = downleft - center;
175  r[1]+= downleft_center * downleft_center;
176 
177  float right_center = right - center;
178  r[2] = right_center * right_center;
179  float left_center = left - center;
180  r[2]+= left_center * left_center;
181 
182  float downright_center = downright - center;
183  r[3] = downright_center * downright_center;
184  float upleft_center = upleft - center;
185  r[3]+= upleft_center * upleft_center;
186 
187  float d = *(std::min_element (r.begin (), r.end ()));
188 
189  if (d < first_threshold_)
190  continue;
191 
192  std::vector<float> B (4,0);
193  std::vector<float> A (4,0);
194  std::vector<float> sumAB (4,0);
195  B[0] = (upright - up) * up_center;
196  B[0]+= (downleft - down) * down_center;
197  B[1] = (right - upright) * upright_center;
198  B[1]+= (left - downleft) * downleft_center;
199  B[2] = (downright - right) * downright_center;
200  B[2]+= (upleft - left) * upleft_center;
201  B[3] = (down - downright) * downright_center;
202  B[3]+= (up - upleft) * upleft_center;
203  A[0] = r[1] - r[0] - B[0] - B[0];
204  A[1] = r[2] - r[1] - B[1] - B[1];
205  A[2] = r[3] - r[2] - B[2] - B[2];
206  A[3] = r[0] - r[3] - B[3] - B[3];
207  sumAB[0] = A[0] + B[0];
208  sumAB[1] = A[1] + B[1];
209  sumAB[2] = A[2] + B[2];
210  sumAB[3] = A[3] + B[3];
211  if ((*std::max_element (B.begin (), B.end ()) < 0) &&
212  (*std::min_element (sumAB.begin (), sumAB.end ()) > 0))
213  {
214  std::vector<float> D (4,0);
215  D[0] = B[0] * B[0] / A[0];
216  D[1] = B[1] * B[1] / A[1];
217  D[2] = B[2] * B[2] / A[2];
218  D[3] = B[3] * B[3] / A[3];
219  (*response_) (i,j) = *(std::min (D.begin (), D.end ()));
220  }
221  else
222  (*response_) (i,j) = d;
223  }
224  }
225  }
226 
227  // Non maximas suppression
228  pcl::Indices indices = *indices_;
229  std::sort (indices.begin (), indices.end (), [this] (int p1, int p2) { return greaterCornernessAtIndices (p1, p2); });
230 
231  output.clear ();
232  output.reserve (input_->size ());
233 
234  std::vector<bool> occupency_map (indices.size (), false);
235  const int width (input_->width);
236  const int height (input_->height);
237 
238 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
239 #pragma omp parallel for \
240  default(none) \
241  shared(indices, occupency_map, output) \
242  num_threads(threads_)
243 #else
244 #pragma omp parallel for \
245  default(none) \
246  shared(height, indices, occupency_map, output, width) \
247  num_threads(threads_)
248 #endif
249  // Disable lint since this 'for' is part of the pragma
250  // NOLINTNEXTLINE(modernize-loop-convert)
251  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t> (indices.size ()); ++i)
252  {
253  int idx = indices[i];
254  if (((*response_)[idx] < second_threshold_) || occupency_map[idx])
255  continue;
256 
257  PointOutT p;
258  p.getVector3fMap () = (*input_)[idx].getVector3fMap ();
259  p.intensity = response_->points [idx];
260 
261 #pragma omp critical
262  {
263  output.push_back (p);
264  keypoints_indices_->indices.push_back (idx);
265  }
266 
267  const int x = idx % width;
268  const int y = idx / width;
269  const int u_end = std::min (width, x + half_window_size_);
270  const int v_end = std::min (height, y + half_window_size_);
271  for(int v = std::max (0, y - half_window_size_); v < v_end; ++v)
272  for(int u = std::max (0, x - half_window_size_); u < u_end; ++u)
273  occupency_map[v*width + u] = true;
274  }
275 
276  output.height = 1;
277  output.width = output.size();
278  // we don not change the denseness
279  output.is_dense = input_->is_dense;
280 }
281 
282 } // namespace pcl
283 
284 #define PCL_INSTANTIATE_TrajkovicKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::TrajkovicKeypoint2D<T,U,I>;
285 
286 #endif
287 
PCL base class.
Definition: pcl_base.h:70
TrajkovicKeypoint2D implements Trajkovic and Hedley corner detector on organized point cloud using in...
Definition: trajkovic_2d.h:55
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: trajkovic_2d.h:60
void detectKeypoints(PointCloudOut &output) override
bool initCompute() override
@ B
Definition: norms.h:54
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133