1 #ifndef POINTCLOUD_DEPTH_NEIGHBOR_SEARCH_HPP
2 #define POINTCLOUD_DEPTH_NEIGHBOR_SEARCH_HPP
12 template<
typename Po
intT>
15 double radius_arg, std::vector<int> &k_indices_arg,
16 std::vector<float> &k_sqr_distances_arg,
int max_nn_arg)
18 this->setInputCloud (cloud_arg);
20 return radiusSearch (index_arg, radius_arg, k_indices_arg, k_sqr_distances_arg, max_nn_arg);
24 template<
typename Po
intT>
27 std::vector<int> &k_indices_arg,
28 std::vector<float> &k_sqr_distances_arg,
int max_nn_arg)
const
31 const PointT searchPoint = getPointByIndex (index_arg);
33 return radiusSearch (searchPoint, radius_arg, k_indices_arg, k_sqr_distances_arg, max_nn_arg);
38 template<
typename Po
intT>
41 std::vector<int> &k_indices_arg,
42 std::vector<float> &k_sqr_distances_arg,
int max_nn_arg)
const
44 if (input_->height == 1)
46 PCL_ERROR (
"[pcl::%s::radiusSearch] Input dataset is not organized!", getName ().c_str ());
51 int leftX, rightX, leftY, rightY;
53 double squared_distance, squared_radius;
56 k_indices_arg.clear ();
57 k_sqr_distances_arg.clear ();
59 squared_radius = radius_arg*radius_arg;
61 this->getProjectedRadiusSearchBox(p_q_arg, squared_radius, leftX, rightX, leftY, rightY);
68 for (x = leftX; (x <= rightX) && (nnn < max_nn_arg); x++)
69 for (y = leftY; (y <= rightY) && (nnn < max_nn_arg); y++)
71 idx = y * input_->width + x;
72 const PointT& point = input_->points[idx];
74 const double point_dist_x = point.x - p_q_arg.x;
75 const double point_dist_y = point.y - p_q_arg.y;
76 const double point_dist_z = point.z - p_q_arg.z;
79 squared_distance = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z * point_dist_z);
82 if (squared_distance <= squared_radius)
84 k_indices_arg.push_back (idx);
85 k_sqr_distances_arg.push_back (squared_distance);
95 template<
typename Po
intT>
99 double r_sqr, r_quadr, z_sqr;
100 double sqrt_term_y, sqrt_term_x, norm;
101 double x_times_z, y_times_z;
102 double x1, x2, y1, y2;
107 r_sqr = squared_radius_arg;
108 r_quadr = r_sqr * r_sqr;
109 z_sqr = point_arg.z * point_arg.z;
111 sqrt_term_y = sqrt (point_arg.y * point_arg.y * r_sqr + z_sqr * r_sqr - r_quadr);
112 sqrt_term_x = sqrt (point_arg.x * point_arg.x * r_sqr + z_sqr * r_sqr - r_quadr);
113 norm = 1.0 / (z_sqr - r_sqr);
115 x_times_z = point_arg.x * point_arg.z;
116 y_times_z = point_arg.y * point_arg.z;
118 y1 = (y_times_z - sqrt_term_y) * norm;
119 y2 = (y_times_z + sqrt_term_y) * norm;
120 x1 = (x_times_z - sqrt_term_x) * norm;
121 x2 = (x_times_z + sqrt_term_x) * norm;
124 minX_arg = (int)floor((
double)input_->width / 2 + (x1 / focalLength_));
125 maxX_arg = (int)ceil((
double)input_->width / 2 + (x2 / focalLength_));
127 minY_arg = (int)floor((
double)input_->height / 2 + (y1 / focalLength_));
128 maxY_arg = (int)ceil((
double)input_->height / 2 + (y2 / focalLength_));
131 minX_arg = std::max<int> (0, minX_arg);
132 maxX_arg = std::min<int> ((int)input_->width - 1, maxX_arg);
134 minY_arg = std::max<int> (0, minY_arg);
135 maxY_arg = std::min<int> ((int)input_->height - 1, maxY_arg);
141 template<
typename Po
intT>
144 std::vector<float> &k_sqr_distances_arg)
147 const PointT searchPoint = getPointByIndex (index_arg);
149 return nearestKSearch (searchPoint, k_arg, k_indices_arg, k_sqr_distances_arg);
153 template<
typename Po
intT>
156 std::vector<int> &k_indices_arg,
157 std::vector<float> &k_sqr_distances_arg)
159 this->setInputCloud (cloud_arg);
161 return nearestKSearch (index_arg, k_arg, k_indices_arg, k_sqr_distances_arg);
165 template<
typename Po
intT>
168 std::vector<float> &k_sqr_distances_arg)
170 int x_pos, y_pos, x, y, idx;
173 int leftX, rightX, leftY, rightY;
175 int radiusSearchPointCount;
177 int maxSearchDistance;
178 double squaredMaxSearchRadius;
182 if (input_->height == 1)
184 PCL_ERROR (
"[pcl::%s::nearestKSearch] Input dataset is not organized!", getName ().c_str ());
188 squaredMaxSearchRadius = max_distance_*max_distance_;
191 std::vector<nearestNeighborCandidate> nearestNeighbors;
194 typename std::vector<radiusSearchLoopkupEntry>::const_iterator radiusSearchLookup_Iterator;
195 radiusSearchLookup_Iterator = radiusSearchLookup_.begin ();
197 nearestNeighbors.reserve (k_arg * 2);
200 pointPlaneProjection (p_q_arg, x_pos, y_pos);
201 x_pos += (int)input_->width/2;
202 y_pos += (
int)input_->height/2;
205 k_indices_arg.clear ();
206 k_sqr_distances_arg.clear ();
209 radiusSearchPointCount = 0;
211 while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ()) && ((
int)nearestNeighbors.size () < k_arg))
214 x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
215 y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
216 radiusSearchLookup_Iterator++;
217 radiusSearchPointCount++;
219 if ((x >= 0) && (y >= 0) && (x < (
int)input_->width) && (y < (
int)input_->height))
221 idx = y * (int)input_->width + x;
222 const PointT& point = input_->points[idx];
224 if ((point.x == point.x) &&
225 (point.y == point.y) &&
226 (point.z == point.z))
228 double squared_distance;
230 const double point_dist_x = point.x - p_q_arg.x;
231 const double point_dist_y = point.y - p_q_arg.y;
232 const double point_dist_z = point.z - p_q_arg.z;
236 = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z * point_dist_z);
238 if (squared_distance <= squaredMaxSearchRadius)
242 newCandidate.
index_ = idx;
245 nearestNeighbors.push_back (newCandidate);
253 std::sort (nearestNeighbors.begin (), nearestNeighbors.end ());
256 if ((
int)nearestNeighbors.size () == k_arg)
258 double squared_radius;
259 unsigned int pointCountRadiusSearch;
260 unsigned int pointCountCircleSearch;
262 squared_radius = std::min<double>(nearestNeighbors.back ().squared_distance_, squaredMaxSearchRadius);
264 this->getProjectedRadiusSearchBox(p_q_arg, squared_radius, leftX, rightX, leftY, rightY);
271 pointCountRadiusSearch = (rightX-leftX)*(rightY-leftY);
274 maxSearchDistance = 0;
275 maxSearchDistance = std::max<int> (maxSearchDistance, leftX + leftY);
276 maxSearchDistance = std::max<int> (maxSearchDistance, leftX + rightY);
277 maxSearchDistance = std::max<int> (maxSearchDistance, rightX + leftY);
278 maxSearchDistance = std::max<int> (maxSearchDistance, rightX + rightY);
280 maxSearchDistance +=1;
281 maxSearchDistance *=maxSearchDistance;
283 pointCountCircleSearch= (int)(PI*(
double)(maxSearchDistance*maxSearchDistance));
288 while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ())
289 && ((*radiusSearchLookup_Iterator).squared_distance_ <= maxSearchDistance))
292 x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
293 y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
294 radiusSearchLookup_Iterator++;
296 if ((x >= 0) && (y >= 0) && (x < (
int)input_->width) && (y < (
int)input_->height))
298 idx = y * (int)input_->width + x;
299 const PointT& point = input_->points[idx];
301 if ((point.x == point.x) &&
302 (point.y == point.y) && (point.z == point.z))
304 double squared_distance;
306 const double point_dist_x = point.x - p_q_arg.x;
307 const double point_dist_y = point.y - p_q_arg.y;
308 const double point_dist_z = point.z - p_q_arg.z;
311 squared_distance = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z
314 if ( squared_distance<=squared_radius )
318 newCandidate.
index_ = idx;
321 nearestNeighbors.push_back (newCandidate);
327 std::vector<int> k_radius_indices;
328 std::vector<float> k_radius_distances;
330 nearestNeighbors.clear();
332 k_radius_indices.reserve (k_arg*2);
333 k_radius_distances.reserve (k_arg*2);
335 radiusSearch (p_q_arg, sqrt(squared_radius),k_radius_indices , k_radius_distances);
337 std::cout << k_radius_indices.size () <<std::endl;
339 for (i = 0; i < k_radius_indices.size (); i++)
342 newCandidate.
index_ = k_radius_indices[i];
345 nearestNeighbors.push_back (newCandidate);
351 std::sort (nearestNeighbors.begin (), nearestNeighbors.end ());
354 if (nearestNeighbors.size () > (size_t)k_arg)
356 nearestNeighbors.resize (k_arg);
362 k_indices_arg.resize (nearestNeighbors.size ());
363 k_sqr_distances_arg.resize (nearestNeighbors.size ());
365 for (i = 0; i < nearestNeighbors.size (); i++)
367 k_indices_arg[i] = nearestNeighbors[i].index_;
368 k_sqr_distances_arg[i] = nearestNeighbors[i].squared_distance_;
371 return k_indices_arg.size ();
376 template<
typename Po
intT>
386 for (y = 0; y < (int)input_->height; y++)
387 for (x = 0; x < (int)input_->width; x++)
389 i = y * input_->width + x;
390 if ((input_->points[i].x == input_->points[i].x) &&
391 (input_->points[i].y == input_->points[i].y) && (input_->points[i].z == input_->points[i].z))
393 const PointT& point = input_->points[i];
394 if ((
double)(x - input_->width / 2) * (
double)(y - input_->height / 2) * point.z != 0)
397 focalLength_ += point.x / ((double)(x - (
int)input_->width / 2) * point.z);
398 focalLength_ += point.y / ((double)(y - (
int)input_->height / 2) * point.z);
404 focalLength_ /= (double)count;
409 template<
typename Po
intT>
415 if ( (this->radiusLookupTableWidth_!=(
int)width) || (this->radiusLookupTableHeight_!=(
int)height) )
418 this->radiusLookupTableWidth_ = (int)width;
419 this->radiusLookupTableHeight_= (int)height;
421 radiusSearchLookup_.clear ();
422 radiusSearchLookup_.resize ((2*width+1) * (2*height+1));
425 for (x = -(
int)width; x < (int)width+1; x++)
426 for (y = -(
int)height; y <(int)height+1; y++)
428 radiusSearchLookup_[c++].defineShiftedSearchPoint(x, y);
431 std::sort (radiusSearchLookup_.begin (), radiusSearchLookup_.end ());
437 template<
typename Po
intT>
442 assert (index_arg < (
unsigned int)input_->points.size ());
443 return this->input_->points[index_arg];
450 #define PCL_INSTANTIATE_OrganizedNeighborSearch(T) template class pcl::OrganizedNeighborSearch<T>;
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
This file defines compatibility wrappers for low level I/O functions.
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.
boost::shared_ptr< const PointCloud > PointCloudConstPtr
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.