Point Cloud Library (PCL)  1.9.1-dev
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
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.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
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.
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.
typename PointCloud::ConstPtr PointCloudConstPtr
void getProjectedRadiusSearchBox(const PointT &point_arg, double squared_radius_arg, int &minX_arg, int &minY_arg, int &maxX_arg, int &maxY_arg) const