Point Cloud Library (PCL)  1.11.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 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 
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_->points[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_->points[i].x == input_->points[i].x) && // check for NaNs
347  (input_->points[i].y == input_->points[i].y) && (input_->points[i].z == input_->points[i].z))
348  {
349  const PointT& point = input_->points[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
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.
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.
typename PointCloud::ConstPtr PointCloudConstPtr