Point Cloud Library (PCL)  1.10.1
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 radiusSearchPointCount;
167 
168  double squaredMaxSearchRadius;
169 
170  assert (k_arg>0);
171 
172  if (input_->height == 1)
173  {
174  PCL_ERROR ("[pcl::%s::nearestKSearch] Input dataset is not organized!", getName ().c_str ());
175  return 0;
176  }
177 
178  squaredMaxSearchRadius = max_distance_*max_distance_;
179 
180  // vector for nearest neighbor candidates
181  std::vector<nearestNeighborCandidate> nearestNeighbors;
182 
183  // iterator for radius search lookup table
184  typename std::vector<radiusSearchLoopkupEntry>::const_iterator radiusSearchLookup_Iterator;
185  radiusSearchLookup_Iterator = radiusSearchLookup_.begin ();
186 
187  nearestNeighbors.reserve (k_arg * 2);
188 
189  // project search point to plane
190  pointPlaneProjection (p_q_arg, x_pos, y_pos);
191  x_pos += (int)input_->width/2;
192  y_pos += (int)input_->height/2;
193 
194  // initialize result vectors
195  k_indices_arg.clear ();
196  k_sqr_distances_arg.clear ();
197 
198 
199  radiusSearchPointCount = 0;
200  // search for k_arg nearest neighbor candidates using the radius lookup table
201  while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ()) && ((int)nearestNeighbors.size () < k_arg))
202  {
203  // select point from organized pointcloud
204  x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
205  y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
206  ++radiusSearchLookup_Iterator;
207  radiusSearchPointCount++;
208 
209  if ((x >= 0) && (y >= 0) && (x < (int)input_->width) && (y < (int)input_->height))
210  {
211  idx = y * (int)input_->width + x;
212  const PointT& point = input_->points[idx];
213 
214  if ((point.x == point.x) && // check for NaNs
215  (point.y == point.y) &&
216  (point.z == point.z))
217  {
218  double squared_distance;
219 
220  const double point_dist_x = point.x - p_q_arg.x;
221  const double point_dist_y = point.y - p_q_arg.y;
222  const double point_dist_z = point.z - p_q_arg.z;
223 
224  // calculate squared distance
225  squared_distance
226  = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z * point_dist_z);
227 
228  if (squared_distance <= squaredMaxSearchRadius)
229  {
230  // we have a new candidate -> add it
231  nearestNeighborCandidate newCandidate;
232  newCandidate.index_ = idx;
233  newCandidate.squared_distance_ = squared_distance;
234 
235  nearestNeighbors.push_back (newCandidate);
236  }
237 
238  }
239  }
240  }
241 
242  // sort candidate list
243  std::sort (nearestNeighbors.begin (), nearestNeighbors.end ());
244 
245  // we found k_arg candidates -> do radius search
246  if ((int)nearestNeighbors.size () == k_arg)
247  {
248  double squared_radius;
249  unsigned int pointCountRadiusSearch;
250  unsigned int pointCountCircleSearch;
251 
252  squared_radius = std::min<double>(nearestNeighbors.back ().squared_distance_, squaredMaxSearchRadius);
253 
254  int leftX, rightX, leftY, rightY;
255  this->getProjectedRadiusSearchBox(p_q_arg, squared_radius, leftX, rightX, leftY, rightY);
256 
257  leftX *=leftX;
258  rightX *= rightX;
259  leftY *=leftY;
260  rightY *= rightY;
261 
262  pointCountRadiusSearch = (rightX-leftX)*(rightY-leftY);
263 
264  // search for maximum distance between search point to window borders in 2-D search window
265  int maxSearchDistance = 0;
266  maxSearchDistance = std::max<int> (maxSearchDistance, leftX + leftY);
267  maxSearchDistance = std::max<int> (maxSearchDistance, leftX + rightY);
268  maxSearchDistance = std::max<int> (maxSearchDistance, rightX + leftY);
269  maxSearchDistance = std::max<int> (maxSearchDistance, rightX + rightY);
270 
271  maxSearchDistance +=1;
272  maxSearchDistance *=maxSearchDistance;
273 
274  pointCountCircleSearch= (int)(PI*(double)(maxSearchDistance*maxSearchDistance));
275 
276  if (1){//(pointCountCircleSearch<pointCountRadiusSearch) {
277 
278  // check for nearest neighbors within window
279  while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ())
280  && ((*radiusSearchLookup_Iterator).squared_distance_ <= maxSearchDistance))
281  {
282  // select point from organized point cloud
283  x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
284  y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
285  ++radiusSearchLookup_Iterator;
286 
287  if ((x >= 0) && (y >= 0) && (x < (int)input_->width) && (y < (int)input_->height))
288  {
289  idx = y * (int)input_->width + x;
290  const PointT& point = input_->points[idx];
291 
292  if ((point.x == point.x) && // check for NaNs
293  (point.y == point.y) && (point.z == point.z))
294  {
295  double squared_distance;
296 
297  const double point_dist_x = point.x - p_q_arg.x;
298  const double point_dist_y = point.y - p_q_arg.y;
299  const double point_dist_z = point.z - p_q_arg.z;
300 
301  // calculate squared distance
302  squared_distance = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z
303  * point_dist_z);
304 
305  if ( squared_distance<=squared_radius )
306  {
307  // add candidate
308  nearestNeighborCandidate newCandidate;
309  newCandidate.index_ = idx;
310  newCandidate.squared_distance_ = squared_distance;
311 
312  nearestNeighbors.push_back (newCandidate);
313  }
314  }
315  }
316  }
317  } else {
318  std::vector<int> k_radius_indices;
319  std::vector<float> k_radius_distances;
320 
321  nearestNeighbors.clear();
322 
323  k_radius_indices.reserve (k_arg*2);
324  k_radius_distances.reserve (k_arg*2);
325 
326  radiusSearch (p_q_arg, sqrt(squared_radius),k_radius_indices , k_radius_distances);
327 
328  std::cout << k_radius_indices.size () <<std::endl;
329 
330  for (std::size_t i = 0; i < k_radius_indices.size (); i++)
331  {
332  nearestNeighborCandidate newCandidate;
333  newCandidate.index_ = k_radius_indices[i];
334  newCandidate.squared_distance_ = k_radius_distances[i];
335 
336  nearestNeighbors.push_back (newCandidate);
337  }
338  }
339 
340  std::sort (nearestNeighbors.begin (), nearestNeighbors.end ());
341 
342  // truncate sorted nearest neighbor vector if we found more than k_arg candidates
343  if (nearestNeighbors.size () > (std::size_t)k_arg)
344  {
345  nearestNeighbors.resize (k_arg);
346  }
347 
348  }
349 
350  // copy results from nearestNeighbors vector to separate indices and distance vector
351  k_indices_arg.resize (nearestNeighbors.size ());
352  k_sqr_distances_arg.resize (nearestNeighbors.size ());
353 
354  for (std::size_t i = 0; i < nearestNeighbors.size (); i++)
355  {
356  k_indices_arg[i] = nearestNeighbors[i].index_;
357  k_sqr_distances_arg[i] = nearestNeighbors[i].squared_distance_;
358  }
359 
360  return k_indices_arg.size ();
361 
362  }
363 
364  //////////////////////////////////////////////////////////////////////////////////////////////
365  template<typename PointT>
366  void
368  {
369  focalLength_ = 0;
370 
371  std::size_t count = 0;
372  for (int y = 0; y < (int)input_->height; y++)
373  for (int x = 0; x < (int)input_->width; x++)
374  {
375  std::size_t i = y * input_->width + x;
376  if ((input_->points[i].x == input_->points[i].x) && // check for NaNs
377  (input_->points[i].y == input_->points[i].y) && (input_->points[i].z == input_->points[i].z))
378  {
379  const PointT& point = input_->points[i];
380  if ((double)(x - input_->width / 2) * (double)(y - input_->height / 2) * point.z != 0)
381  {
382  // estimate the focal length for point.x and point.y
383  focalLength_ += point.x / ((double)(x - (int)input_->width / 2) * point.z);
384  focalLength_ += point.y / ((double)(y - (int)input_->height / 2) * point.z);
385  count += 2;
386  }
387  }
388  }
389  // calculate an average of the focalLength
390  focalLength_ /= (double)count;
391  }
392 
393  //////////////////////////////////////////////////////////////////////////////////////////////
394  template<typename PointT>
395  void
396  OrganizedNeighborSearch<PointT>::generateRadiusLookupTable (unsigned int width, unsigned int height)
397  {
398  if ( (this->radiusLookupTableWidth_!=(int)width) || (this->radiusLookupTableHeight_!=(int)height) )
399  {
400 
401  this->radiusLookupTableWidth_ = (int)width;
402  this->radiusLookupTableHeight_= (int)height;
403 
404  radiusSearchLookup_.clear ();
405  radiusSearchLookup_.resize ((2*width+1) * (2*height+1));
406 
407  int c = 0;
408  for (int x = -(int)width; x < (int)width+1; x++)
409  for (int y = -(int)height; y <(int)height+1; y++)
410  {
411  radiusSearchLookup_[c++].defineShiftedSearchPoint(x, y);
412  }
413 
414  std::sort (radiusSearchLookup_.begin (), radiusSearchLookup_.end ());
415  }
416 
417  }
418 
419  //////////////////////////////////////////////////////////////////////////////////////////////
420  template<typename PointT>
421  const PointT&
422  OrganizedNeighborSearch<PointT>::getPointByIndex (const unsigned int index_arg) const
423  {
424  // retrieve point from input cloud
425  assert (index_arg < (unsigned int)input_->points.size ());
426  return this->input_->points[index_arg];
427 
428  }
429 
430 }
431 
432 
433 #define PCL_INSTANTIATE_OrganizedNeighborSearch(T) template class pcl::OrganizedNeighborSearch<T>;
434 
435 #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:367
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:620
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:396
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:422
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