Point Cloud Library (PCL)  1.9.0-dev
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 template <typename PointInT, typename PointOutT, typename NormalT> bool
45 {
47  return (false);
48 
49  keypoints_indices_.reset (new pcl::PointIndices);
50  keypoints_indices_->indices.reserve (input_->size ());
51 
52  if (indices_->size () != input_->size ())
53  {
54  PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ());
55  return (false);
56  }
57 
58  if ((window_size_%2) == 0)
59  {
60  PCL_ERROR ("[pcl::%s::initCompute] Window size must be odd!\n", name_.c_str ());
61  return (false);
62  }
63 
64  if (window_size_ < 3)
65  {
66  PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ());
67  return (false);
68  }
69 
70  half_window_size_ = window_size_ / 2;
71 
72  if (!normals_)
73  {
74  NormalsPtr normals (new Normals ());
77  normal_estimation.setInputCloud (input_);
78  normal_estimation.setNormalSmoothingSize (5.0);
79  normal_estimation.compute (*normals);
80  normals_ = normals;
81  }
82 
83  if (normals_->size () != input_->size ())
84  {
85  PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ());
86  return (false);
87  }
88 
89  return (true);
90 }
91 
92 /////////////////////////////////////////////////////////////////////////////////////////////
93 template <typename PointInT, typename PointOutT, typename NormalT> void
95 {
96  response_.reset (new pcl::PointCloud<float> (input_->width, input_->height));
97  const Normals &normals = *normals_;
98  const PointCloudIn &input = *input_;
99  pcl::PointCloud<float>& response = *response_;
100  const int w = static_cast<int> (input_->width) - half_window_size_;
101  const int h = static_cast<int> (input_->height) - half_window_size_;
102 
103  if (method_ == FOUR_CORNERS)
104  {
105 #ifdef _OPENMP
106 #pragma omp parallel for num_threads (threads_)
107 #endif
108  for(int j = half_window_size_; j < h; ++j)
109  {
110  for(int i = half_window_size_; i < w; ++i)
111  {
112  if (!isFinite (input (i,j))) continue;
113  const NormalT &center = normals (i,j);
114  if (!isFinite (center)) continue;
115 
116  int count = 0;
117  const NormalT &up = getNormalOrNull (i, j-half_window_size_, count);
118  const NormalT &down = getNormalOrNull (i, j+half_window_size_, count);
119  const NormalT &left = getNormalOrNull (i-half_window_size_, j, count);
120  const NormalT &right = getNormalOrNull (i+half_window_size_, j, count);
121  // Get rid of isolated points
122  if (!count) continue;
123 
124  float sn1 = squaredNormalsDiff (up, center);
125  float sn2 = squaredNormalsDiff (down, center);
126  float r1 = sn1 + sn2;
127  float r2 = squaredNormalsDiff (right, center) + squaredNormalsDiff (left, center);
128 
129  float d = std::min (r1, r2);
130  if (d < first_threshold_) continue;
131 
132  sn1 = sqrt (sn1);
133  sn2 = sqrt (sn2);
134  float b1 = normalsDiff (right, up) * sn1;
135  b1+= normalsDiff (left, down) * sn2;
136  float b2 = normalsDiff (right, down) * sn2;
137  b2+= normalsDiff (left, up) * sn1;
138  float B = std::min (b1, b2);
139  float A = r2 - r1 - 2*B;
140 
141  response (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d;
142  }
143  }
144  }
145  else
146  {
147 #ifdef _OPENMP
148 #pragma omp parallel for num_threads (threads_)
149 #endif
150  for(int j = half_window_size_; j < h; ++j)
151  {
152  for(int i = half_window_size_; i < w; ++i)
153  {
154  if (!isFinite (input (i,j))) continue;
155  const NormalT &center = normals (i,j);
156  if (!isFinite (center)) continue;
157 
158  int count = 0;
159  const NormalT &up = getNormalOrNull (i, j-half_window_size_, count);
160  const NormalT &down = getNormalOrNull (i, j+half_window_size_, count);
161  const NormalT &left = getNormalOrNull (i-half_window_size_, j, count);
162  const NormalT &right = getNormalOrNull (i+half_window_size_, j, count);
163  const NormalT &upleft = getNormalOrNull (i-half_window_size_, j-half_window_size_, count);
164  const NormalT &upright = getNormalOrNull (i+half_window_size_, j-half_window_size_, count);
165  const NormalT &downleft = getNormalOrNull (i-half_window_size_, j+half_window_size_, count);
166  const NormalT &downright = getNormalOrNull (i+half_window_size_, j+half_window_size_, count);
167  // Get rid of isolated points
168  if (!count) continue;
169 
170  std::vector<float> r (4,0);
171 
172  r[0] = squaredNormalsDiff (up, center);
173  r[0]+= squaredNormalsDiff (down, center);
174 
175  r[1] = squaredNormalsDiff (upright, center);
176  r[1]+= squaredNormalsDiff (downleft, center);
177 
178  r[2] = squaredNormalsDiff (right, center);
179  r[2]+= squaredNormalsDiff (left, center);
180 
181  r[3] = squaredNormalsDiff (downright, center);
182  r[3]+= squaredNormalsDiff (upleft, center);
183 
184  float d = *(std::min_element (r.begin (), r.end ()));
185 
186  if (d < first_threshold_) continue;
187 
188  std::vector<float> B (4,0);
189  std::vector<float> A (4,0);
190  std::vector<float> sumAB (4,0);
191  B[0] = normalsDiff (upright, up) * normalsDiff (up, center);
192  B[0]+= normalsDiff (downleft, down) * normalsDiff (down, center);
193  B[1] = normalsDiff (right, upright) * normalsDiff (upright, center);
194  B[1]+= normalsDiff (left, downleft) * normalsDiff (downleft, center);
195  B[2] = normalsDiff (downright, right) * normalsDiff (downright, center);
196  B[2]+= normalsDiff (upleft, left) * normalsDiff (upleft, center);
197  B[3] = normalsDiff (down, downright) * normalsDiff (downright, center);
198  B[3]+= normalsDiff (up, upleft) * normalsDiff (upleft, center);
199  A[0] = r[1] - r[0] - B[0] - B[0];
200  A[1] = r[2] - r[1] - B[1] - B[1];
201  A[2] = r[3] - r[2] - B[2] - B[2];
202  A[3] = r[0] - r[3] - B[3] - B[3];
203  sumAB[0] = A[0] + B[0];
204  sumAB[1] = A[1] + B[1];
205  sumAB[2] = A[2] + B[2];
206  sumAB[3] = A[3] + B[3];
207  if ((*std::max_element (B.begin (), B.end ()) < 0) &&
208  (*std::min_element (sumAB.begin (), sumAB.end ()) > 0))
209  {
210  std::vector<float> D (4,0);
211  D[0] = B[0] * B[0] / A[0];
212  D[1] = B[1] * B[1] / A[1];
213  D[2] = B[2] * B[2] / A[2];
214  D[3] = B[3] * B[3] / A[3];
215  response (i,j) = *(std::min (D.begin (), D.end ()));
216  }
217  else
218  response (i,j) = d;
219  }
220  }
221  }
222  // Non maximas suppression
223  std::vector<int> indices = *indices_;
224  std::sort (indices.begin (), indices.end (),
225  boost::bind (&TrajkovicKeypoint3D::greaterCornernessAtIndices, this, _1, _2));
226 
227  output.clear ();
228  output.reserve (input_->size ());
229 
230  std::vector<bool> occupency_map (indices.size (), false);
231  const int width (input_->width);
232  const int height (input_->height);
233 
234 #ifdef _OPENMP
235 #pragma omp parallel for shared (output) num_threads (threads_)
236 #endif
237  for (int i = 0; i < static_cast<int>(indices.size ()); ++i)
238  {
239  int idx = indices[static_cast<size_t>(i)];
240  if ((response_->points[idx] < second_threshold_) || occupency_map[idx])
241  continue;
242 
243  PointOutT p;
244  p.getVector3fMap () = input_->points[idx].getVector3fMap ();
245  p.intensity = response_->points [idx];
246 
247 #ifdef _OPENMP
248 #pragma omp critical
249 #endif
250  {
251  output.push_back (p);
252  keypoints_indices_->indices.push_back (idx);
253  }
254 
255  const int x = idx % width;
256  const int y = idx / width;
257  const int u_end = std::min (width, x + half_window_size_);
258  const int v_end = std::min (height, y + half_window_size_);
259  for(int v = std::max (0, y - half_window_size_); v < v_end; ++v)
260  for(int u = std::max (0, x - half_window_size_); u < u_end; ++u)
261  occupency_map[v*width + u] = true;
262  }
263 
264  output.height = 1;
265  output.width = static_cast<uint32_t> (output.size());
266  // we don not change the denseness
267  output.is_dense = true;
268 }
269 
270 #define PCL_INSTANTIATE_TrajkovicKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::TrajkovicKeypoint3D<T,U,N>;
271 #endif
A point structure representing normal coordinates and the surface curvature estimate.
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
size_t size() const
Definition: point_cloud.h:448
void reserve(size_t n)
Definition: point_cloud.h:449
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:480
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
Surface normal estimation on organized data using integral images.
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
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
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
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:189
Definition: norms.h:55
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.