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 k_indices_arg.clear ();
54 k_sqr_distances_arg.clear ();
56 double squared_radius = radius_arg*radius_arg;
58 this->getProjectedRadiusSearchBox(p_q_arg, squared_radius, leftX, rightX, leftY, rightY);
62 for (
int x = leftX; (x <= rightX) && (nnn < max_nn_arg); x++)
63 for (
int y = leftY; (y <= rightY) && (nnn < max_nn_arg); y++)
65 int idx = y * input_->width + x;
66 const PointT& point = input_->points[idx];
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;
73 double squared_distance = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z * point_dist_z);
76 if (squared_distance <= squared_radius)
78 k_indices_arg.push_back (idx);
79 k_sqr_distances_arg.push_back (squared_distance);
89 template<
typename Po
intT>
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;
101 r_sqr = squared_radius_arg;
102 r_quadr = r_sqr * r_sqr;
103 z_sqr = point_arg.z * point_arg.z;
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);
109 x_times_z = point_arg.x * point_arg.z;
110 y_times_z = point_arg.y * point_arg.z;
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;
118 minX_arg = (int)std::floor((
double)input_->width / 2 + (x1 / focalLength_));
119 maxX_arg = (int)std::ceil((
double)input_->width / 2 + (x2 / focalLength_));
121 minY_arg = (int)std::floor((
double)input_->height / 2 + (y1 / focalLength_));
122 maxY_arg = (int)std::ceil((
double)input_->height / 2 + (y2 / focalLength_));
125 minX_arg = std::max<int> (0, minX_arg);
126 maxX_arg = std::min<int> ((int)input_->width - 1, maxX_arg);
128 minY_arg = std::max<int> (0, minY_arg);
129 maxY_arg = std::min<int> ((int)input_->height - 1, maxY_arg);
135 template<
typename Po
intT>
138 std::vector<float> &k_sqr_distances_arg)
141 const PointT searchPoint = getPointByIndex (index_arg);
143 return nearestKSearch (searchPoint, k_arg, k_indices_arg, k_sqr_distances_arg);
147 template<
typename Po
intT>
150 std::vector<int> &k_indices_arg,
151 std::vector<float> &k_sqr_distances_arg)
153 this->setInputCloud (cloud_arg);
155 return nearestKSearch (index_arg, k_arg, k_indices_arg, k_sqr_distances_arg);
159 template<
typename Po
intT>
162 std::vector<float> &k_sqr_distances_arg)
164 int x_pos, y_pos, x, y, idx;
166 int radiusSearchPointCount;
168 double squaredMaxSearchRadius;
172 if (input_->height == 1)
174 PCL_ERROR (
"[pcl::%s::nearestKSearch] Input dataset is not organized!", getName ().c_str ());
178 squaredMaxSearchRadius = max_distance_*max_distance_;
181 std::vector<nearestNeighborCandidate> nearestNeighbors;
184 typename std::vector<radiusSearchLoopkupEntry>::const_iterator radiusSearchLookup_Iterator;
185 radiusSearchLookup_Iterator = radiusSearchLookup_.begin ();
187 nearestNeighbors.reserve (k_arg * 2);
190 pointPlaneProjection (p_q_arg, x_pos, y_pos);
191 x_pos += (int)input_->width/2;
192 y_pos += (
int)input_->height/2;
195 k_indices_arg.clear ();
196 k_sqr_distances_arg.clear ();
199 radiusSearchPointCount = 0;
201 while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ()) && ((
int)nearestNeighbors.size () < k_arg))
204 x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
205 y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
206 ++radiusSearchLookup_Iterator;
207 radiusSearchPointCount++;
209 if ((x >= 0) && (y >= 0) && (x < (
int)input_->width) && (y < (
int)input_->height))
211 idx = y * (int)input_->width + x;
212 const PointT& point = input_->points[idx];
214 if ((point.x == point.x) &&
215 (point.y == point.y) &&
216 (point.z == point.z))
218 double squared_distance;
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;
226 = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z * point_dist_z);
228 if (squared_distance <= squaredMaxSearchRadius)
232 newCandidate.
index_ = idx;
235 nearestNeighbors.push_back (newCandidate);
243 std::sort (nearestNeighbors.begin (), nearestNeighbors.end ());
246 if ((
int)nearestNeighbors.size () == k_arg)
248 double squared_radius;
250 squared_radius = std::min<double>(nearestNeighbors.back ().squared_distance_, squaredMaxSearchRadius);
252 int leftX, rightX, leftY, rightY;
253 this->getProjectedRadiusSearchBox(p_q_arg, squared_radius, leftX, rightX, leftY, rightY);
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);
267 maxSearchDistance +=1;
268 maxSearchDistance *=maxSearchDistance;
271 while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ())
272 && ((*radiusSearchLookup_Iterator).squared_distance_ <= maxSearchDistance))
275 x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
276 y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
277 ++radiusSearchLookup_Iterator;
279 if ((x >= 0) && (y >= 0) && (x < (
int)input_->width) && (y < (
int)input_->height))
281 idx = y * (int)input_->width + x;
282 const PointT& point = input_->points[idx];
284 if ((point.x == point.x) &&
285 (point.y == point.y) && (point.z == point.z))
287 double squared_distance;
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;
294 squared_distance = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z
297 if ( squared_distance<=squared_radius )
301 newCandidate.
index_ = idx;
304 nearestNeighbors.push_back (newCandidate);
310 std::sort (nearestNeighbors.begin (), nearestNeighbors.end ());
313 if (nearestNeighbors.size () > (std::size_t)k_arg)
315 nearestNeighbors.resize (k_arg);
321 k_indices_arg.resize (nearestNeighbors.size ());
322 k_sqr_distances_arg.resize (nearestNeighbors.size ());
324 for (std::size_t i = 0; i < nearestNeighbors.size (); i++)
326 k_indices_arg[i] = nearestNeighbors[i].index_;
327 k_sqr_distances_arg[i] = nearestNeighbors[i].squared_distance_;
330 return k_indices_arg.size ();
335 template<
typename Po
intT>
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++)
345 std::size_t i = y * input_->width + x;
346 if ((input_->points[i].x == input_->points[i].x) &&
347 (input_->points[i].y == input_->points[i].y) && (input_->points[i].z == input_->points[i].z))
349 const PointT& point = input_->points[i];
350 if ((
double)(x - input_->width / 2) * (
double)(y - input_->height / 2) * point.z != 0)
353 focalLength_ += point.x / ((double)(x - (
int)input_->width / 2) * point.z);
354 focalLength_ += point.y / ((double)(y - (
int)input_->height / 2) * point.z);
360 focalLength_ /= (double)count;
364 template<
typename Po
intT>
368 if ( (this->radiusLookupTableWidth_!=(
int)width) || (this->radiusLookupTableHeight_!=(
int)height) )
371 this->radiusLookupTableWidth_ = (int)width;
372 this->radiusLookupTableHeight_= (int)height;
374 radiusSearchLookup_.clear ();
375 radiusSearchLookup_.resize ((2*width+1) * (2*height+1));
378 for (
int x = -(
int)width; x < (int)width+1; x++)
379 for (
int y = -(
int)height; y <(int)height+1; y++)
381 radiusSearchLookup_[c++].defineShiftedSearchPoint(x, y);
384 std::sort (radiusSearchLookup_.begin (), radiusSearchLookup_.end ());
390 template<
typename Po
intT>
395 assert (index_arg < (
unsigned int)input_->points.size ());
396 return this->input_->points[index_arg];
403 #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
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