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 leftX, rightX, leftY, rightY;
168 int radiusSearchPointCount;
170 int maxSearchDistance;
171 double squaredMaxSearchRadius;
175 if (input_->height == 1)
177 PCL_ERROR (
"[pcl::%s::nearestKSearch] Input dataset is not organized!", getName ().c_str ());
181 squaredMaxSearchRadius = max_distance_*max_distance_;
184 std::vector<nearestNeighborCandidate> nearestNeighbors;
187 typename std::vector<radiusSearchLoopkupEntry>::const_iterator radiusSearchLookup_Iterator;
188 radiusSearchLookup_Iterator = radiusSearchLookup_.begin ();
190 nearestNeighbors.reserve (k_arg * 2);
193 pointPlaneProjection (p_q_arg, x_pos, y_pos);
194 x_pos += (int)input_->width/2;
195 y_pos += (
int)input_->height/2;
198 k_indices_arg.clear ();
199 k_sqr_distances_arg.clear ();
202 radiusSearchPointCount = 0;
204 while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ()) && ((
int)nearestNeighbors.size () < k_arg))
207 x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
208 y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
209 radiusSearchLookup_Iterator++;
210 radiusSearchPointCount++;
212 if ((x >= 0) && (y >= 0) && (x < (
int)input_->width) && (y < (
int)input_->height))
214 idx = y * (int)input_->width + x;
215 const PointT& point = input_->points[idx];
217 if ((point.x == point.x) &&
218 (point.y == point.y) &&
219 (point.z == point.z))
221 double squared_distance;
223 const double point_dist_x = point.x - p_q_arg.x;
224 const double point_dist_y = point.y - p_q_arg.y;
225 const double point_dist_z = point.z - p_q_arg.z;
229 = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z * point_dist_z);
231 if (squared_distance <= squaredMaxSearchRadius)
235 newCandidate.
index_ = idx;
238 nearestNeighbors.push_back (newCandidate);
246 std::sort (nearestNeighbors.begin (), nearestNeighbors.end ());
249 if ((
int)nearestNeighbors.size () == k_arg)
251 double squared_radius;
252 unsigned int pointCountRadiusSearch;
253 unsigned int pointCountCircleSearch;
255 squared_radius = std::min<double>(nearestNeighbors.back ().squared_distance_, squaredMaxSearchRadius);
257 this->getProjectedRadiusSearchBox(p_q_arg, squared_radius, leftX, rightX, leftY, rightY);
264 pointCountRadiusSearch = (rightX-leftX)*(rightY-leftY);
267 maxSearchDistance = 0;
268 maxSearchDistance = std::max<int> (maxSearchDistance, leftX + leftY);
269 maxSearchDistance = std::max<int> (maxSearchDistance, leftX + rightY);
270 maxSearchDistance = std::max<int> (maxSearchDistance, rightX + leftY);
271 maxSearchDistance = std::max<int> (maxSearchDistance, rightX + rightY);
273 maxSearchDistance +=1;
274 maxSearchDistance *=maxSearchDistance;
276 pointCountCircleSearch= (int)(PI*(
double)(maxSearchDistance*maxSearchDistance));
281 while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ())
282 && ((*radiusSearchLookup_Iterator).squared_distance_ <= maxSearchDistance))
285 x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
286 y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
287 radiusSearchLookup_Iterator++;
289 if ((x >= 0) && (y >= 0) && (x < (
int)input_->width) && (y < (
int)input_->height))
291 idx = y * (int)input_->width + x;
292 const PointT& point = input_->points[idx];
294 if ((point.x == point.x) &&
295 (point.y == point.y) && (point.z == point.z))
297 double squared_distance;
299 const double point_dist_x = point.x - p_q_arg.x;
300 const double point_dist_y = point.y - p_q_arg.y;
301 const double point_dist_z = point.z - p_q_arg.z;
304 squared_distance = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z
307 if ( squared_distance<=squared_radius )
311 newCandidate.
index_ = idx;
314 nearestNeighbors.push_back (newCandidate);
320 std::vector<int> k_radius_indices;
321 std::vector<float> k_radius_distances;
323 nearestNeighbors.clear();
325 k_radius_indices.reserve (k_arg*2);
326 k_radius_distances.reserve (k_arg*2);
328 radiusSearch (p_q_arg, sqrt(squared_radius),k_radius_indices , k_radius_distances);
330 std::cout << k_radius_indices.size () <<std::endl;
332 for (std::size_t i = 0; i < k_radius_indices.size (); i++)
335 newCandidate.
index_ = k_radius_indices[i];
338 nearestNeighbors.push_back (newCandidate);
342 std::sort (nearestNeighbors.begin (), nearestNeighbors.end ());
345 if (nearestNeighbors.size () > (std::size_t)k_arg)
347 nearestNeighbors.resize (k_arg);
353 k_indices_arg.resize (nearestNeighbors.size ());
354 k_sqr_distances_arg.resize (nearestNeighbors.size ());
356 for (std::size_t i = 0; i < nearestNeighbors.size (); i++)
358 k_indices_arg[i] = nearestNeighbors[i].index_;
359 k_sqr_distances_arg[i] = nearestNeighbors[i].squared_distance_;
362 return k_indices_arg.size ();
367 template<
typename Po
intT>
373 std::size_t count = 0;
374 for (
int y = 0; y < (int)input_->height; y++)
375 for (
int x = 0; x < (int)input_->width; x++)
377 std::size_t i = y * input_->width + x;
378 if ((input_->points[i].x == input_->points[i].x) &&
379 (input_->points[i].y == input_->points[i].y) && (input_->points[i].z == input_->points[i].z))
381 const PointT& point = input_->points[i];
382 if ((
double)(x - input_->width / 2) * (
double)(y - input_->height / 2) * point.z != 0)
385 focalLength_ += point.x / ((double)(x - (
int)input_->width / 2) * point.z);
386 focalLength_ += point.y / ((double)(y - (
int)input_->height / 2) * point.z);
392 focalLength_ /= (double)count;
396 template<
typename Po
intT>
400 if ( (this->radiusLookupTableWidth_!=(
int)width) || (this->radiusLookupTableHeight_!=(
int)height) )
403 this->radiusLookupTableWidth_ = (int)width;
404 this->radiusLookupTableHeight_= (int)height;
406 radiusSearchLookup_.clear ();
407 radiusSearchLookup_.resize ((2*width+1) * (2*height+1));
410 for (
int x = -(
int)width; x < (int)width+1; x++)
411 for (
int y = -(
int)height; y <(int)height+1; y++)
413 radiusSearchLookup_[c++].defineShiftedSearchPoint(x, y);
416 std::sort (radiusSearchLookup_.begin (), radiusSearchLookup_.end ());
422 template<
typename Po
intT>
427 assert (index_arg < (
unsigned int)input_->points.size ());
428 return this->input_->points[index_arg];
435 #define PCL_INSTANTIATE_OrganizedNeighborSearch(T) template class pcl::OrganizedNeighborSearch<T>;