Point Cloud Library (PCL)  1.14.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!\n", 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_)[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!\n", 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_)[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 
250  squared_radius = std::min<double>(nearestNeighbors.back ().squared_distance_, squaredMaxSearchRadius);
251 
252  int leftX, rightX, leftY, rightY;
253  this->getProjectedRadiusSearchBox(p_q_arg, squared_radius, leftX, rightX, leftY, rightY);
254 
255  leftX *=leftX;
256  rightX *= rightX;
257  leftY *=leftY;
258  rightY *= rightY;
259 
260  // search for maximum distance between search point to window borders in 2-D search window
261  int maxSearchDistance = 0;
262  maxSearchDistance = std::max<int> (maxSearchDistance, leftX + leftY);
263  maxSearchDistance = std::max<int> (maxSearchDistance, leftX + rightY);
264  maxSearchDistance = std::max<int> (maxSearchDistance, rightX + leftY);
265  maxSearchDistance = std::max<int> (maxSearchDistance, rightX + rightY);
266 
267  maxSearchDistance +=1;
268  maxSearchDistance *=maxSearchDistance;
269 
270  // check for nearest neighbors within window
271  while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ())
272  && ((*radiusSearchLookup_Iterator).squared_distance_ <= maxSearchDistance))
273  {
274  // select point from organized point cloud
275  x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
276  y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
277  ++radiusSearchLookup_Iterator;
278 
279  if ((x >= 0) && (y >= 0) && (x < (int)input_->width) && (y < (int)input_->height))
280  {
281  idx = y * (int)input_->width + x;
282  const PointT& point = (*input_)[idx];
283 
284  if ((point.x == point.x) && // check for NaNs
285  (point.y == point.y) && (point.z == point.z))
286  {
287  double squared_distance;
288 
289  const double point_dist_x = point.x - p_q_arg.x;
290  const double point_dist_y = point.y - p_q_arg.y;
291  const double point_dist_z = point.z - p_q_arg.z;
292 
293  // calculate squared distance
294  squared_distance = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z
295  * point_dist_z);
296 
297  if ( squared_distance<=squared_radius )
298  {
299  // add candidate
300  nearestNeighborCandidate newCandidate;
301  newCandidate.index_ = idx;
302  newCandidate.squared_distance_ = squared_distance;
303 
304  nearestNeighbors.push_back (newCandidate);
305  }
306  }
307  }
308  }
309 
310  std::sort (nearestNeighbors.begin (), nearestNeighbors.end ());
311 
312  // truncate sorted nearest neighbor vector if we found more than k_arg candidates
313  if (nearestNeighbors.size () > (std::size_t)k_arg)
314  {
315  nearestNeighbors.resize (k_arg);
316  }
317 
318  }
319 
320  // copy results from nearestNeighbors vector to separate indices and distance vector
321  k_indices_arg.resize (nearestNeighbors.size ());
322  k_sqr_distances_arg.resize (nearestNeighbors.size ());
323 
324  for (std::size_t i = 0; i < nearestNeighbors.size (); i++)
325  {
326  k_indices_arg[i] = nearestNeighbors[i].index_;
327  k_sqr_distances_arg[i] = nearestNeighbors[i].squared_distance_;
328  }
329 
330  return k_indices_arg.size ();
331 
332  }
333 
334  //////////////////////////////////////////////////////////////////////////////////////////////
335  template<typename PointT>
336  void
338  {
339  focalLength_ = 0;
340 
341  std::size_t count = 0;
342  for (int y = 0; y < (int)input_->height; y++)
343  for (int x = 0; x < (int)input_->width; x++)
344  {
345  std::size_t i = y * input_->width + x;
346  if (((*input_)[i].x == (*input_)[i].x) && // check for NaNs
347  ((*input_)[i].y == (*input_)[i].y) && ((*input_)[i].z == (*input_)[i].z))
348  {
349  const PointT& point = (*input_)[i];
350  if ((double)(x - input_->width / 2) * (double)(y - input_->height / 2) * point.z != 0)
351  {
352  // estimate the focal length for point.x and point.y
353  focalLength_ += point.x / ((double)(x - (int)input_->width / 2) * point.z);
354  focalLength_ += point.y / ((double)(y - (int)input_->height / 2) * point.z);
355  count += 2;
356  }
357  }
358  }
359  // calculate an average of the focalLength
360  focalLength_ /= (double)count;
361  }
362 
363  //////////////////////////////////////////////////////////////////////////////////////////////
364  template<typename PointT>
365  void
366  OrganizedNeighborSearch<PointT>::generateRadiusLookupTable (unsigned int width, unsigned int height)
367  {
368  if ( (this->radiusLookupTableWidth_!=(int)width) || (this->radiusLookupTableHeight_!=(int)height) )
369  {
370 
371  this->radiusLookupTableWidth_ = (int)width;
372  this->radiusLookupTableHeight_= (int)height;
373 
374  radiusSearchLookup_.clear ();
375  radiusSearchLookup_.resize ((2*width+1) * (2*height+1));
376 
377  int c = 0;
378  for (int x = -(int)width; x < (int)width+1; x++)
379  for (int y = -(int)height; y <(int)height+1; y++)
380  {
381  radiusSearchLookup_[c++].defineShiftedSearchPoint(x, y);
382  }
383 
384  std::sort (radiusSearchLookup_.begin (), radiusSearchLookup_.end ());
385  }
386 
387  }
388 
389  //////////////////////////////////////////////////////////////////////////////////////////////
390  template<typename PointT>
391  const PointT&
392  OrganizedNeighborSearch<PointT>::getPointByIndex (const unsigned int index_arg) const
393  {
394  // retrieve point from input cloud
395  assert (index_arg < (unsigned int)input_->points.size ());
396  return this->input_->points[index_arg];
397 
398  }
399 
400 }
401 
402 
403 #define PCL_INSTANTIATE_OrganizedNeighborSearch(T) template class pcl::OrganizedNeighborSearch<T>;
404 
405 #endif
nearestNeighborCandidate entry for the nearest neighbor candidate queue
void getProjectedRadiusSearchBox(const PointT &point_arg, double squared_radius_arg, int &minX_arg, int &minY_arg, int &maxX_arg, int &maxY_arg) const
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 estimateFocalLengthFromInputCloud()
Estimate focal length parameter that was used during point cloud generation.
typename PointCloud::ConstPtr PointCloudConstPtr
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=std::numeric_limits< 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.
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
A point structure representing Euclidean xyz coordinates, and the RGB color.