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;
249 unsigned int pointCountRadiusSearch;
250 unsigned int pointCountCircleSearch;
252 squared_radius = std::min<double>(nearestNeighbors.back ().squared_distance_, squaredMaxSearchRadius);
254 int leftX, rightX, leftY, rightY;
255 this->getProjectedRadiusSearchBox(p_q_arg, squared_radius, leftX, rightX, leftY, rightY);
262 pointCountRadiusSearch = (rightX-leftX)*(rightY-leftY);
265 int maxSearchDistance = 0;
266 maxSearchDistance = std::max<int> (maxSearchDistance, leftX + leftY);
267 maxSearchDistance = std::max<int> (maxSearchDistance, leftX + rightY);
268 maxSearchDistance = std::max<int> (maxSearchDistance, rightX + leftY);
269 maxSearchDistance = std::max<int> (maxSearchDistance, rightX + rightY);
271 maxSearchDistance +=1;
272 maxSearchDistance *=maxSearchDistance;
274 pointCountCircleSearch= (int)(PI*(
double)(maxSearchDistance*maxSearchDistance));
279 while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ())
280 && ((*radiusSearchLookup_Iterator).squared_distance_ <= maxSearchDistance))
283 x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
284 y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
285 ++radiusSearchLookup_Iterator;
287 if ((x >= 0) && (y >= 0) && (x < (
int)input_->width) && (y < (
int)input_->height))
289 idx = y * (int)input_->width + x;
290 const PointT& point = input_->points[idx];
292 if ((point.x == point.x) &&
293 (point.y == point.y) && (point.z == point.z))
295 double squared_distance;
297 const double point_dist_x = point.x - p_q_arg.x;
298 const double point_dist_y = point.y - p_q_arg.y;
299 const double point_dist_z = point.z - p_q_arg.z;
302 squared_distance = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z
305 if ( squared_distance<=squared_radius )
309 newCandidate.
index_ = idx;
312 nearestNeighbors.push_back (newCandidate);
318 std::vector<int> k_radius_indices;
319 std::vector<float> k_radius_distances;
321 nearestNeighbors.clear();
323 k_radius_indices.reserve (k_arg*2);
324 k_radius_distances.reserve (k_arg*2);
326 radiusSearch (p_q_arg, sqrt(squared_radius),k_radius_indices , k_radius_distances);
328 std::cout << k_radius_indices.size () <<std::endl;
330 for (std::size_t i = 0; i < k_radius_indices.size (); i++)
333 newCandidate.
index_ = k_radius_indices[i];
336 nearestNeighbors.push_back (newCandidate);
340 std::sort (nearestNeighbors.begin (), nearestNeighbors.end ());
343 if (nearestNeighbors.size () > (std::size_t)k_arg)
345 nearestNeighbors.resize (k_arg);
351 k_indices_arg.resize (nearestNeighbors.size ());
352 k_sqr_distances_arg.resize (nearestNeighbors.size ());
354 for (std::size_t i = 0; i < nearestNeighbors.size (); i++)
356 k_indices_arg[i] = nearestNeighbors[i].index_;
357 k_sqr_distances_arg[i] = nearestNeighbors[i].squared_distance_;
360 return k_indices_arg.size ();
365 template<
typename Po
intT>
371 std::size_t count = 0;
372 for (
int y = 0; y < (int)input_->height; y++)
373 for (
int x = 0; x < (int)input_->width; x++)
375 std::size_t i = y * input_->width + x;
376 if ((input_->points[i].x == input_->points[i].x) &&
377 (input_->points[i].y == input_->points[i].y) && (input_->points[i].z == input_->points[i].z))
379 const PointT& point = input_->points[i];
380 if ((
double)(x - input_->width / 2) * (
double)(y - input_->height / 2) * point.z != 0)
383 focalLength_ += point.x / ((double)(x - (
int)input_->width / 2) * point.z);
384 focalLength_ += point.y / ((double)(y - (
int)input_->height / 2) * point.z);
390 focalLength_ /= (double)count;
394 template<
typename Po
intT>
398 if ( (this->radiusLookupTableWidth_!=(
int)width) || (this->radiusLookupTableHeight_!=(
int)height) )
401 this->radiusLookupTableWidth_ = (int)width;
402 this->radiusLookupTableHeight_= (int)height;
404 radiusSearchLookup_.clear ();
405 radiusSearchLookup_.resize ((2*width+1) * (2*height+1));
408 for (
int x = -(
int)width; x < (int)width+1; x++)
409 for (
int y = -(
int)height; y <(int)height+1; y++)
411 radiusSearchLookup_[c++].defineShiftedSearchPoint(x, y);
414 std::sort (radiusSearchLookup_.begin (), radiusSearchLookup_.end ());
420 template<
typename Po
intT>
425 assert (index_arg < (
unsigned int)input_->points.size ());
426 return this->input_->points[index_arg];
433 #define PCL_INSTANTIATE_OrganizedNeighborSearch(T) template class pcl::OrganizedNeighborSearch<T>;