Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
43namespace pcl
44{
45
46template <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
85template <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
PointCloud represents the base class in PCL for storing collections of 3D points.
TrajkovicKeypoint2D implements Trajkovic and Hedley corner detector on organized point cloud using in...
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
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