Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
8namespace 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.