Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
trajkovic_3d.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_3D_IMPL_H_
39#define PCL_TRAJKOVIC_KEYPOINT_3D_IMPL_H_
40
41#include <pcl/features/integral_image_normal.h>
42
43
44namespace pcl
45{
46
47template <typename PointInT, typename PointOutT, typename NormalT> bool
49{
51 return (false);
52
53 keypoints_indices_.reset (new pcl::PointIndices);
54 keypoints_indices_->indices.reserve (input_->size ());
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 if (!normals_)
77 {
78 NormalsPtr normals (new Normals ());
81 normal_estimation.setInputCloud (input_);
82 normal_estimation.setNormalSmoothingSize (5.0);
83 normal_estimation.compute (*normals);
84 normals_ = normals;
85 }
86
87 if (normals_->size () != input_->size ())
88 {
89 PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ());
90 return (false);
91 }
92
93 return (true);
94}
95
96
97template <typename PointInT, typename PointOutT, typename NormalT> void
99{
100 response_.reset (new pcl::PointCloud<float> (input_->width, input_->height));
101 const Normals &normals = *normals_;
102 const PointCloudIn &input = *input_;
103 pcl::PointCloud<float>& response = *response_;
104 const int w = static_cast<int> (input_->width) - half_window_size_;
105 const int h = static_cast<int> (input_->height) - half_window_size_;
106
107 if (method_ == FOUR_CORNERS)
108 {
109#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
110#pragma omp parallel for \
111 default(none) \
112 shared(input, normals, response) \
113 num_threads(threads_)
114#else
115#pragma omp parallel for \
116 default(none) \
117 shared(h, input, normals, response, w) \
118 num_threads(threads_)
119#endif
120 for(int j = half_window_size_; j < h; ++j)
121 {
122 for(int i = half_window_size_; i < w; ++i)
123 {
124 if (!isFinite (input (i,j))) continue;
125 const NormalT &center = normals (i,j);
126 if (!isFinite (center)) continue;
127
128 int count = 0;
129 const NormalT &up = getNormalOrNull (i, j-half_window_size_, count);
130 const NormalT &down = getNormalOrNull (i, j+half_window_size_, count);
131 const NormalT &left = getNormalOrNull (i-half_window_size_, j, count);
132 const NormalT &right = getNormalOrNull (i+half_window_size_, j, count);
133 // Get rid of isolated points
134 if (!count) continue;
135
136 float sn1 = squaredNormalsDiff (up, center);
137 float sn2 = squaredNormalsDiff (down, center);
138 float r1 = sn1 + sn2;
139 float r2 = squaredNormalsDiff (right, center) + squaredNormalsDiff (left, center);
140
141 float d = std::min (r1, r2);
142 if (d < first_threshold_) continue;
143
144 sn1 = std::sqrt (sn1);
145 sn2 = std::sqrt (sn2);
146 float b1 = normalsDiff (right, up) * sn1;
147 b1+= normalsDiff (left, down) * sn2;
148 float b2 = normalsDiff (right, down) * sn2;
149 b2+= normalsDiff (left, up) * sn1;
150 float B = std::min (b1, b2);
151 float A = r2 - r1 - 2*B;
152
153 response (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d;
154 }
155 }
156 }
157 else
158 {
159#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
160#pragma omp parallel for \
161 default(none) \
162 shared(input, normals, response) \
163 num_threads(threads_)
164#else
165#pragma omp parallel for \
166 default(none) \
167 shared(h, input, normals, response, w) \
168 num_threads(threads_)
169#endif
170 for(int j = half_window_size_; j < h; ++j)
171 {
172 for(int i = half_window_size_; i < w; ++i)
173 {
174 if (!isFinite (input (i,j))) continue;
175 const NormalT &center = normals (i,j);
176 if (!isFinite (center)) continue;
177
178 int count = 0;
179 const NormalT &up = getNormalOrNull (i, j-half_window_size_, count);
180 const NormalT &down = getNormalOrNull (i, j+half_window_size_, count);
181 const NormalT &left = getNormalOrNull (i-half_window_size_, j, count);
182 const NormalT &right = getNormalOrNull (i+half_window_size_, j, count);
183 const NormalT &upleft = getNormalOrNull (i-half_window_size_, j-half_window_size_, count);
184 const NormalT &upright = getNormalOrNull (i+half_window_size_, j-half_window_size_, count);
185 const NormalT &downleft = getNormalOrNull (i-half_window_size_, j+half_window_size_, count);
186 const NormalT &downright = getNormalOrNull (i+half_window_size_, j+half_window_size_, count);
187 // Get rid of isolated points
188 if (!count) continue;
189
190 std::vector<float> r (4,0);
191
192 r[0] = squaredNormalsDiff (up, center);
193 r[0]+= squaredNormalsDiff (down, center);
194
195 r[1] = squaredNormalsDiff (upright, center);
196 r[1]+= squaredNormalsDiff (downleft, center);
197
198 r[2] = squaredNormalsDiff (right, center);
199 r[2]+= squaredNormalsDiff (left, center);
200
201 r[3] = squaredNormalsDiff (downright, center);
202 r[3]+= squaredNormalsDiff (upleft, center);
203
204 float d = *(std::min_element (r.begin (), r.end ()));
205
206 if (d < first_threshold_) continue;
207
208 std::vector<float> B (4,0);
209 std::vector<float> A (4,0);
210 std::vector<float> sumAB (4,0);
211 B[0] = normalsDiff (upright, up) * normalsDiff (up, center);
212 B[0]+= normalsDiff (downleft, down) * normalsDiff (down, center);
213 B[1] = normalsDiff (right, upright) * normalsDiff (upright, center);
214 B[1]+= normalsDiff (left, downleft) * normalsDiff (downleft, center);
215 B[2] = normalsDiff (downright, right) * normalsDiff (downright, center);
216 B[2]+= normalsDiff (upleft, left) * normalsDiff (upleft, center);
217 B[3] = normalsDiff (down, downright) * normalsDiff (downright, center);
218 B[3]+= normalsDiff (up, upleft) * normalsDiff (upleft, center);
219 A[0] = r[1] - r[0] - B[0] - B[0];
220 A[1] = r[2] - r[1] - B[1] - B[1];
221 A[2] = r[3] - r[2] - B[2] - B[2];
222 A[3] = r[0] - r[3] - B[3] - B[3];
223 sumAB[0] = A[0] + B[0];
224 sumAB[1] = A[1] + B[1];
225 sumAB[2] = A[2] + B[2];
226 sumAB[3] = A[3] + B[3];
227 if ((*std::max_element (B.begin (), B.end ()) < 0) &&
228 (*std::min_element (sumAB.begin (), sumAB.end ()) > 0))
229 {
230 std::vector<float> D (4,0);
231 D[0] = B[0] * B[0] / A[0];
232 D[1] = B[1] * B[1] / A[1];
233 D[2] = B[2] * B[2] / A[2];
234 D[3] = B[3] * B[3] / A[3];
235 response (i,j) = *(std::min (D.begin (), D.end ()));
236 }
237 else
238 response (i,j) = d;
239 }
240 }
241 }
242 // Non maximas suppression
243 pcl::Indices indices = *indices_;
244 std::sort (indices.begin (), indices.end (), [this] (int p1, int p2) { return greaterCornernessAtIndices (p1, p2); });
245
246 output.clear ();
247 output.reserve (input_->size ());
248
249 std::vector<bool> occupency_map (indices.size (), false);
250 const int width (input_->width);
251 const int height (input_->height);
252
253#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
254#pragma omp parallel for \
255 default(none) \
256 shared(indices, occupency_map, output) \
257 num_threads(threads_)
258#else
259#pragma omp parallel for \
260 default(none) \
261 shared(height, indices, occupency_map, output, width) \
262 num_threads(threads_)
263#endif
264 for (int i = 0; i < static_cast<int>(indices.size ()); ++i)
265 {
266 int idx = indices[static_cast<std::size_t>(i)];
267 if (((*response_)[idx] < second_threshold_) || occupency_map[idx])
268 continue;
269
270 PointOutT p;
271 p.getVector3fMap () = (*input_)[idx].getVector3fMap ();
272 p.intensity = response_->points [idx];
273
274#pragma omp critical
275 {
276 output.push_back (p);
277 keypoints_indices_->indices.push_back (idx);
278 }
279
280 const int x = idx % width;
281 const int y = idx / width;
282 const int u_end = std::min (width, x + half_window_size_);
283 const int v_end = std::min (height, y + half_window_size_);
284 for(int v = std::max (0, y - half_window_size_); v < v_end; ++v)
285 for(int u = std::max (0, x - half_window_size_); u < u_end; ++u)
286 occupency_map[v*width + u] = true;
287 }
288
289 output.height = 1;
290 output.width = output.size();
291 // we don not change the denseness
292 output.is_dense = true;
293}
294
295} // namespace pcl
296
297#define PCL_INSTANTIATE_TrajkovicKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::TrajkovicKeypoint3D<T,U,N>;
298
299#endif
300
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition feature.hpp:194
Surface normal estimation on organized data using integral images.
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
PCL base class.
Definition pcl_base.h:70
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
typename Keypoint< PointInT, PointOutT >::PointCloudIn PointCloudIn
bool initCompute() override
void detectKeypoints(PointCloudOut &output) override
typename Normals::Ptr NormalsPtr
@ B
Definition norms.h:54
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
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
A point structure representing normal coordinates and the surface curvature estimate.