Point Cloud Library (PCL)  1.10.0
organized_neighbor_search.hpp
1 #ifndef POINTCLOUD_DEPTH_NEIGHBOR_SEARCH_HPP
2 #define POINTCLOUD_DEPTH_NEIGHBOR_SEARCH_HPP
3 
4 #ifndef PI
5  #define PI 3.14159
6 #endif
7 
8 namespace pcl
9 {
10 
11  //////////////////////////////////////////////////////////////////////////////////////////////
12  template<typename PointT>
13  int
15  double radius_arg, std::vector<int> &k_indices_arg,
16  std::vector<float> &k_sqr_distances_arg, int max_nn_arg)
17  {
18  this->setInputCloud (cloud_arg);
19 
20  return radiusSearch (index_arg, radius_arg, k_indices_arg, k_sqr_distances_arg, max_nn_arg);
21  }
22 
23  //////////////////////////////////////////////////////////////////////////////////////////////
24  template<typename PointT>
25  int
26  OrganizedNeighborSearch<PointT>::radiusSearch (int index_arg, const double radius_arg,
27  std::vector<int> &k_indices_arg,
28  std::vector<float> &k_sqr_distances_arg, int max_nn_arg) const
29  {
30 
31  const PointT searchPoint = getPointByIndex (index_arg);
32 
33  return radiusSearch (searchPoint, radius_arg, k_indices_arg, k_sqr_distances_arg, max_nn_arg);
34 
35  }
36 
37  //////////////////////////////////////////////////////////////////////////////////////////////
38  template<typename PointT>
39  int
40  OrganizedNeighborSearch<PointT>::radiusSearch (const PointT &p_q_arg, const double radius_arg,
41  std::vector<int> &k_indices_arg,
42  std::vector<float> &k_sqr_distances_arg, int max_nn_arg) const
43  {
44  if (input_->height == 1)
45  {
46  PCL_ERROR ("[pcl::%s::radiusSearch] Input dataset is not organized!", getName ().c_str ());
47  return 0;
48  }
49 
50  // search window
51  int leftX, rightX, leftY, rightY;
52 
53  k_indices_arg.clear ();
54  k_sqr_distances_arg.clear ();
55 
56  double squared_radius = radius_arg*radius_arg;
57 
58  this->getProjectedRadiusSearchBox(p_q_arg, squared_radius, leftX, rightX, leftY, rightY);
59 
60  // iterate over all children
61  int nnn = 0;
62  for (int x = leftX; (x <= rightX) && (nnn < max_nn_arg); x++)
63  for (int y = leftY; (y <= rightY) && (nnn < max_nn_arg); y++)
64  {
65  int idx = y * input_->width + x;
66  const PointT& point = input_->points[idx];
67 
68  const double point_dist_x = point.x - p_q_arg.x;
69  const double point_dist_y = point.y - p_q_arg.y;
70  const double point_dist_z = point.z - p_q_arg.z;
71 
72  // calculate squared distance
73  double squared_distance = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z * point_dist_z);
74 
75  // check distance and add to results
76  if (squared_distance <= squared_radius)
77  {
78  k_indices_arg.push_back (idx);
79  k_sqr_distances_arg.push_back (squared_distance);
80  nnn++;
81  }
82  }
83 
84  return nnn;
85 
86  }
87 
88  //////////////////////////////////////////////////////////////////////////////////////////////
89  template<typename PointT>
90  void
91  OrganizedNeighborSearch<PointT>::getProjectedRadiusSearchBox (const PointT& point_arg, double squared_radius_arg, int& minX_arg, int& maxX_arg, int& minY_arg, int& maxY_arg ) const
92  {
93  double r_sqr, r_quadr, z_sqr;
94  double sqrt_term_y, sqrt_term_x, norm;
95  double x_times_z, y_times_z;
96  double x1, x2, y1, y2;
97 
98  // see http://www.wolframalpha.com/input/?i=solve+%5By%2Fsqrt%28f^2%2By^2%29*c-f%2Fsqrt%28f^2%2By^2%29*b%2Br%3D%3D0%2C+f%3D1%2C+y%5D
99  // where b = p_q_arg.y, c = p_q_arg.z, r = radius_arg, f = focalLength_
100 
101  r_sqr = squared_radius_arg;
102  r_quadr = r_sqr * r_sqr;
103  z_sqr = point_arg.z * point_arg.z;
104 
105  sqrt_term_y = sqrt (point_arg.y * point_arg.y * r_sqr + z_sqr * r_sqr - r_quadr);
106  sqrt_term_x = sqrt (point_arg.x * point_arg.x * r_sqr + z_sqr * r_sqr - r_quadr);
107  norm = 1.0 / (z_sqr - r_sqr);
108 
109  x_times_z = point_arg.x * point_arg.z;
110  y_times_z = point_arg.y * point_arg.z;
111 
112  y1 = (y_times_z - sqrt_term_y) * norm;
113  y2 = (y_times_z + sqrt_term_y) * norm;
114  x1 = (x_times_z - sqrt_term_x) * norm;
115  x2 = (x_times_z + sqrt_term_x) * norm;
116 
117  // determine 2-D search window
118  minX_arg = (int)std::floor((double)input_->width / 2 + (x1 / focalLength_));
119  maxX_arg = (int)std::ceil((double)input_->width / 2 + (x2 / focalLength_));
120 
121  minY_arg = (int)std::floor((double)input_->height / 2 + (y1 / focalLength_));
122  maxY_arg = (int)std::ceil((double)input_->height / 2 + (y2 / focalLength_));
123 
124  // make sure the coordinates fit to point cloud resolution
125  minX_arg = std::max<int> (0, minX_arg);
126  maxX_arg = std::min<int> ((int)input_->width - 1, maxX_arg);
127 
128  minY_arg = std::max<int> (0, minY_arg);
129  maxY_arg = std::min<int> ((int)input_->height - 1, maxY_arg);
130  }
131 
132 
133 
134  //////////////////////////////////////////////////////////////////////////////////////////////
135  template<typename PointT>
136  int
137  OrganizedNeighborSearch<PointT>::nearestKSearch (int index_arg, int k_arg, std::vector<int> &k_indices_arg,
138  std::vector<float> &k_sqr_distances_arg)
139  {
140 
141  const PointT searchPoint = getPointByIndex (index_arg);
142 
143  return nearestKSearch (searchPoint, k_arg, k_indices_arg, k_sqr_distances_arg);
144  }
145 
146  //////////////////////////////////////////////////////////////////////////////////////////////
147  template<typename PointT>
148  int
149  OrganizedNeighborSearch<PointT>::nearestKSearch (const PointCloudConstPtr &cloud_arg, int index_arg, int k_arg,
150  std::vector<int> &k_indices_arg,
151  std::vector<float> &k_sqr_distances_arg)
152  {
153  this->setInputCloud (cloud_arg);
154 
155  return nearestKSearch (index_arg, k_arg, k_indices_arg, k_sqr_distances_arg);
156  }
157 
158  //////////////////////////////////////////////////////////////////////////////////////////////
159  template<typename PointT>
160  int
161  OrganizedNeighborSearch<PointT>::nearestKSearch (const PointT &p_q_arg, int k_arg, std::vector<int> &k_indices_arg,
162  std::vector<float> &k_sqr_distances_arg)
163  {
164  int x_pos, y_pos, x, y, idx;
165 
166  int leftX, rightX, leftY, rightY;
167 
168  int radiusSearchPointCount;
169 
170  int maxSearchDistance;
171  double squaredMaxSearchRadius;
172 
173  assert (k_arg>0);
174 
175  if (input_->height == 1)
176  {
177  PCL_ERROR ("[pcl::%s::nearestKSearch] Input dataset is not organized!", getName ().c_str ());
178  return 0;
179  }
180 
181  squaredMaxSearchRadius = max_distance_*max_distance_;
182 
183  // vector for nearest neighbor candidates
184  std::vector<nearestNeighborCandidate> nearestNeighbors;
185 
186  // iterator for radius search lookup table
187  typename std::vector<radiusSearchLoopkupEntry>::const_iterator radiusSearchLookup_Iterator;
188  radiusSearchLookup_Iterator = radiusSearchLookup_.begin ();
189 
190  nearestNeighbors.reserve (k_arg * 2);
191 
192  // project search point to plane
193  pointPlaneProjection (p_q_arg, x_pos, y_pos);
194  x_pos += (int)input_->width/2;
195  y_pos += (int)input_->height/2;
196 
197  // initialize result vectors
198  k_indices_arg.clear ();
199  k_sqr_distances_arg.clear ();
200 
201 
202  radiusSearchPointCount = 0;
203  // search for k_arg nearest neighbor candidates using the radius lookup table
204  while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ()) && ((int)nearestNeighbors.size () < k_arg))
205  {
206  // select point from organized pointcloud
207  x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
208  y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
209  radiusSearchLookup_Iterator++;
210  radiusSearchPointCount++;
211 
212  if ((x >= 0) && (y >= 0) && (x < (int)input_->width) && (y < (int)input_->height))
213  {
214  idx = y * (int)input_->width + x;
215  const PointT& point = input_->points[idx];
216 
217  if ((point.x == point.x) && // check for NaNs
218  (point.y == point.y) &&
219  (point.z == point.z))
220  {
221  double squared_distance;
222 
223  const double point_dist_x = point.x - p_q_arg.x;
224  const double point_dist_y = point.y - p_q_arg.y;
225  const double point_dist_z = point.z - p_q_arg.z;
226 
227  // calculate squared distance
228  squared_distance
229  = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z * point_dist_z);
230 
231  if (squared_distance <= squaredMaxSearchRadius)
232  {
233  // we have a new candidate -> add it
234  nearestNeighborCandidate newCandidate;
235  newCandidate.index_ = idx;
236  newCandidate.squared_distance_ = squared_distance;
237 
238  nearestNeighbors.push_back (newCandidate);
239  }
240 
241  }
242  }
243  }
244 
245  // sort candidate list
246  std::sort (nearestNeighbors.begin (), nearestNeighbors.end ());
247 
248  // we found k_arg candidates -> do radius search
249  if ((int)nearestNeighbors.size () == k_arg)
250  {
251  double squared_radius;
252  unsigned int pointCountRadiusSearch;
253  unsigned int pointCountCircleSearch;
254 
255  squared_radius = std::min<double>(nearestNeighbors.back ().squared_distance_, squaredMaxSearchRadius);
256 
257  this->getProjectedRadiusSearchBox(p_q_arg, squared_radius, leftX, rightX, leftY, rightY);
258 
259  leftX *=leftX;
260  rightX *= rightX;
261  leftY *=leftY;
262  rightY *= rightY;
263 
264  pointCountRadiusSearch = (rightX-leftX)*(rightY-leftY);
265 
266  // search for maximum distance between search point to window borders in 2-D search window
267  maxSearchDistance = 0;
268  maxSearchDistance = std::max<int> (maxSearchDistance, leftX + leftY);
269  maxSearchDistance = std::max<int> (maxSearchDistance, leftX + rightY);
270  maxSearchDistance = std::max<int> (maxSearchDistance, rightX + leftY);
271  maxSearchDistance = std::max<int> (maxSearchDistance, rightX + rightY);
272 
273  maxSearchDistance +=1;
274  maxSearchDistance *=maxSearchDistance;
275 
276  pointCountCircleSearch= (int)(PI*(double)(maxSearchDistance*maxSearchDistance));
277 
278  if (1){//(pointCountCircleSearch<pointCountRadiusSearch) {
279 
280  // check for nearest neighbors within window
281  while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ())
282  && ((*radiusSearchLookup_Iterator).squared_distance_ <= maxSearchDistance))
283  {
284  // select point from organized point cloud
285  x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
286  y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
287  radiusSearchLookup_Iterator++;
288 
289  if ((x >= 0) && (y >= 0) && (x < (int)input_->width) && (y < (int)input_->height))
290  {
291  idx = y * (int)input_->width + x;
292  const PointT& point = input_->points[idx];
293 
294  if ((point.x == point.x) && // check for NaNs
295  (point.y == point.y) && (point.z == point.z))
296  {
297  double squared_distance;
298 
299  const double point_dist_x = point.x - p_q_arg.x;
300  const double point_dist_y = point.y - p_q_arg.y;
301  const double point_dist_z = point.z - p_q_arg.z;
302 
303  // calculate squared distance
304  squared_distance = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z
305  * point_dist_z);
306 
307  if ( squared_distance<=squared_radius )
308  {
309  // add candidate
310  nearestNeighborCandidate newCandidate;
311  newCandidate.index_ = idx;
312  newCandidate.squared_distance_ = squared_distance;
313 
314  nearestNeighbors.push_back (newCandidate);
315  }
316  }
317  }
318  }
319  } else {
320  std::vector<int> k_radius_indices;
321  std::vector<float> k_radius_distances;
322 
323  nearestNeighbors.clear();
324 
325  k_radius_indices.reserve (k_arg*2);
326  k_radius_distances.reserve (k_arg*2);
327 
328  radiusSearch (p_q_arg, sqrt(squared_radius),k_radius_indices , k_radius_distances);
329 
330  std::cout << k_radius_indices.size () <<std::endl;
331 
332  for (std::size_t i = 0; i < k_radius_indices.size (); i++)
333  {
334  nearestNeighborCandidate newCandidate;
335  newCandidate.index_ = k_radius_indices[i];
336  newCandidate.squared_distance_ = k_radius_distances[i];
337 
338  nearestNeighbors.push_back (newCandidate);
339  }
340  }
341 
342  std::sort (nearestNeighbors.begin (), nearestNeighbors.end ());
343 
344  // truncate sorted nearest neighbor vector if we found more than k_arg candidates
345  if (nearestNeighbors.size () > (std::size_t)k_arg)
346  {
347  nearestNeighbors.resize (k_arg);
348  }
349 
350  }
351 
352  // copy results from nearestNeighbors vector to separate indices and distance vector
353  k_indices_arg.resize (nearestNeighbors.size ());
354  k_sqr_distances_arg.resize (nearestNeighbors.size ());
355 
356  for (std::size_t i = 0; i < nearestNeighbors.size (); i++)
357  {
358  k_indices_arg[i] = nearestNeighbors[i].index_;
359  k_sqr_distances_arg[i] = nearestNeighbors[i].squared_distance_;
360  }
361 
362  return k_indices_arg.size ();
363 
364  }
365 
366  //////////////////////////////////////////////////////////////////////////////////////////////
367  template<typename PointT>
368  void
370  {
371  focalLength_ = 0;
372 
373  std::size_t count = 0;
374  for (int y = 0; y < (int)input_->height; y++)
375  for (int x = 0; x < (int)input_->width; x++)
376  {
377  std::size_t i = y * input_->width + x;
378  if ((input_->points[i].x == input_->points[i].x) && // check for NaNs
379  (input_->points[i].y == input_->points[i].y) && (input_->points[i].z == input_->points[i].z))
380  {
381  const PointT& point = input_->points[i];
382  if ((double)(x - input_->width / 2) * (double)(y - input_->height / 2) * point.z != 0)
383  {
384  // estimate the focal length for point.x and point.y
385  focalLength_ += point.x / ((double)(x - (int)input_->width / 2) * point.z);
386  focalLength_ += point.y / ((double)(y - (int)input_->height / 2) * point.z);
387  count += 2;
388  }
389  }
390  }
391  // calculate an average of the focalLength
392  focalLength_ /= (double)count;
393  }
394 
395  //////////////////////////////////////////////////////////////////////////////////////////////
396  template<typename PointT>
397  void
398  OrganizedNeighborSearch<PointT>::generateRadiusLookupTable (unsigned int width, unsigned int height)
399  {
400  if ( (this->radiusLookupTableWidth_!=(int)width) || (this->radiusLookupTableHeight_!=(int)height) )
401  {
402 
403  this->radiusLookupTableWidth_ = (int)width;
404  this->radiusLookupTableHeight_= (int)height;
405 
406  radiusSearchLookup_.clear ();
407  radiusSearchLookup_.resize ((2*width+1) * (2*height+1));
408 
409  int c = 0;
410  for (int x = -(int)width; x < (int)width+1; x++)
411  for (int y = -(int)height; y <(int)height+1; y++)
412  {
413  radiusSearchLookup_[c++].defineShiftedSearchPoint(x, y);
414  }
415 
416  std::sort (radiusSearchLookup_.begin (), radiusSearchLookup_.end ());
417  }
418 
419  }
420 
421  //////////////////////////////////////////////////////////////////////////////////////////////
422  template<typename PointT>
423  const PointT&
424  OrganizedNeighborSearch<PointT>::getPointByIndex (const unsigned int index_arg) const
425  {
426  // retrieve point from input cloud
427  assert (index_arg < (unsigned int)input_->points.size ());
428  return this->input_->points[index_arg];
429 
430  }
431 
432 }
433 
434 
435 #define PCL_INSTANTIATE_OrganizedNeighborSearch(T) template class pcl::OrganizedNeighborSearch<T>;
436 
437 #endif
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::OrganizedNeighborSearch::radiusSearch
int radiusSearch(const PointCloudConstPtr &cloud_arg, int index_arg, double radius_arg, std::vector< int > &k_indices_arg, std::vector< float > &k_sqr_distances_arg, int max_nn_arg=INT_MAX)
Search for all neighbors of query point that are within a given radius.
Definition: organized_neighbor_search.hpp:14
pcl::OrganizedNeighborSearch::getProjectedRadiusSearchBox
void getProjectedRadiusSearchBox(const PointT &point_arg, double squared_radius_arg, int &minX_arg, int &minY_arg, int &maxX_arg, int &maxY_arg) const
Definition: organized_neighbor_search.hpp:91
pcl::OrganizedNeighborSearch::nearestNeighborCandidate
nearestNeighborCandidate entry for the nearest neighbor candidate queue
Definition: organized_neighbor_search.h:252
pcl::OrganizedNeighborSearch::estimateFocalLengthFromInputCloud
void estimateFocalLengthFromInputCloud()
Estimate focal length parameter that was used during point cloud generation.
Definition: organized_neighbor_search.hpp:369
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:623
pcl::OrganizedNeighborSearch::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: organized_neighbor_search.h:83
pcl::OrganizedNeighborSearch::generateRadiusLookupTable
void generateRadiusLookupTable(unsigned int width, unsigned int height)
Generate radius lookup table.
Definition: organized_neighbor_search.hpp:398
pcl::OrganizedNeighborSearch::getPointByIndex
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
Definition: organized_neighbor_search.hpp:424
pcl::OrganizedNeighborSearch::nearestKSearch
int nearestKSearch(const PointCloudConstPtr &cloud_arg, int index_arg, int k_arg, std::vector< int > &k_indices_arg, std::vector< float > &k_sqr_distances_arg)
Search for k-nearest neighbors at the query point.
Definition: organized_neighbor_search.hpp:149
pcl::OrganizedNeighborSearch::nearestNeighborCandidate::index_
int index_
Definition: organized_neighbor_search.h:274
pcl::OrganizedNeighborSearch::nearestNeighborCandidate::squared_distance_
double squared_distance_
Definition: organized_neighbor_search.h:275