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