40 #ifndef PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_
41 #define PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_
43 #include <pcl/search/organized.h>
44 #include <pcl/common/eigen.h>
46 #include <Eigen/Eigenvalues>
49 template<
typename Po
intT>
int
52 std::vector<int> &k_indices,
53 std::vector<float> &k_sqr_distances,
54 unsigned int max_nn)
const
57 assert (
isFinite (query) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
60 unsigned left, right, top, bottom;
62 float squared_distance;
63 double squared_radius;
66 k_sqr_distances.clear ();
68 squared_radius = radius * radius;
70 this->getProjectedRadiusSearchBox (query,
static_cast<float> (squared_radius), left, right, top, bottom);
73 if (max_nn == 0 || max_nn >=
static_cast<unsigned int> (input_->points.size ()))
74 max_nn =
static_cast<unsigned int> (input_->points.size ());
76 k_indices.reserve (max_nn);
77 k_sqr_distances.reserve (max_nn);
79 unsigned yEnd = (bottom + 1) * input_->width + right + 1;
80 unsigned idx = top * input_->width + left;
81 unsigned skip = input_->width - right + left - 1;
82 unsigned xEnd = idx - left + right + 1;
84 for (; xEnd != yEnd; idx += skip, xEnd += input_->width)
86 for (; idx < xEnd; ++idx)
88 if (!mask_[idx] || !
isFinite (input_->points[idx]))
91 float dist_x = input_->points[idx].x - query.x;
92 float dist_y = input_->points[idx].y - query.y;
93 float dist_z = input_->points[idx].z - query.z;
94 squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z;
96 if (squared_distance <= squared_radius)
98 k_indices.push_back (idx);
99 k_sqr_distances.push_back (squared_distance);
101 if (k_indices.size () == max_nn)
104 this->sortResults (k_indices, k_sqr_distances);
111 this->sortResults (k_indices, k_sqr_distances);
112 return (
static_cast<int> (k_indices.size ()));
116 template<
typename Po
intT>
int
119 std::vector<int> &k_indices,
120 std::vector<float> &k_sqr_distances)
const
122 assert (
isFinite (query) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
126 k_sqr_distances.clear ();
130 Eigen::Vector3f queryvec (query.x, query.y, query.z);
133 Eigen::Vector3f q (KR_ * queryvec + projection_matrix_.block <3, 1> (0, 3));
134 int xBegin = int(q [0] / q [2] + 0.5f);
135 int yBegin = int(q [1] / q [2] + 0.5f);
136 int xEnd = xBegin + 1;
137 int yEnd = yBegin + 1;
141 unsigned right = input_->width - 1;
143 unsigned bottom = input_->height - 1;
145 std::priority_queue <Entry> results;
150 xBegin <
static_cast<int> (input_->width) &&
152 yBegin <
static_cast<int> (input_->height))
153 testPoint (query, k, results, yBegin * input_->width + xBegin);
157 int dist = std::numeric_limits<int>::max ();
161 else if (xBegin >=
static_cast<int> (input_->width))
162 dist = xBegin -
static_cast<int> (input_->width);
165 dist = std::min (dist, -yBegin);
166 else if (yBegin >=
static_cast<int> (input_->height))
167 dist = std::min (dist, yBegin -
static_cast<int> (input_->height));
190 clipRange (xFrom, xTo, 0, input_->width);
196 if (yBegin >= 0 && yBegin <
static_cast<int> (input_->height))
198 int idx = yBegin * input_->width + xFrom;
199 int idxTo = idx + xTo - xFrom;
200 for (; idx < idxTo; ++idx)
201 stop = testPoint (query, k, results, idx) || stop;
207 if (yEnd > 0 && yEnd <=
static_cast<int> (input_->height))
209 int idx = (yEnd - 1) * input_->width + xFrom;
210 int idxTo = idx + xTo - xFrom;
212 for (; idx < idxTo; ++idx)
213 stop = testPoint (query, k, results, idx) || stop;
217 int yFrom = yBegin + 1;
219 clipRange (yFrom, yTo, 0, input_->height);
224 if (xBegin >= 0 && xBegin <
static_cast<int> (input_->width))
226 int idx = yFrom * input_->width + xBegin;
227 int idxTo = yTo * input_->width + xBegin;
229 for (; idx < idxTo; idx += input_->width)
230 stop = testPoint (query, k, results, idx) || stop;
233 if (xEnd > 0 && xEnd <=
static_cast<int> (input_->width))
235 int idx = yFrom * input_->width + xEnd - 1;
236 int idxTo = yTo * input_->width + xEnd - 1;
238 for (; idx < idxTo; idx += input_->width)
239 stop = testPoint (query, k, results, idx) || stop;
245 getProjectedRadiusSearchBox (query, results.top ().distance, left, right, top, bottom);
249 stop = (
static_cast<int> (left) >= xBegin &&
static_cast<int> (left) < xEnd &&
250 static_cast<int> (right) >= xBegin &&
static_cast<int> (right) < xEnd &&
251 static_cast<int> (top) >= yBegin &&
static_cast<int> (top) < yEnd &&
252 static_cast<int> (bottom) >= yBegin &&
static_cast<int> (bottom) < yEnd);
257 k_indices.resize (results.size ());
258 k_sqr_distances.resize (results.size ());
259 size_t idx = results.size () - 1;
260 while (!results.empty ())
262 k_indices [idx] = results.top ().index;
263 k_sqr_distances [idx] = results.top ().distance;
268 return (
static_cast<int> (k_indices.size ()));
272 template<
typename Po
intT>
void
274 float squared_radius,
278 unsigned &maxY)
const
280 Eigen::Vector3f queryvec (point.x, point.y, point.z);
282 Eigen::Vector3f q (KR_ * queryvec + projection_matrix_.block <3, 1> (0, 3));
284 float a = squared_radius * KR_KRT_.coeff (8) - q [2] * q [2];
285 float b = squared_radius * KR_KRT_.coeff (7) - q [1] * q [2];
286 float c = squared_radius * KR_KRT_.coeff (4) - q [1] * q [1];
289 float det = b * b - a * c;
293 maxY = input_->height - 1;
297 float y1 =
static_cast<float> ((b - sqrt (det)) / a);
298 float y2 =
static_cast<float> ((b + sqrt (det)) / a);
300 min = std::min (
static_cast<int> (floor (y1)),
static_cast<int> (floor (y2)));
301 max = std::max (
static_cast<int> (ceil (y1)),
static_cast<int> (ceil (y2)));
302 minY =
static_cast<unsigned> (std::min (
static_cast<int> (input_->height) - 1, std::max (0, min)));
303 maxY =
static_cast<unsigned> (std::max (std::min (
static_cast<int> (input_->height) - 1, max), 0));
306 b = squared_radius * KR_KRT_.coeff (6) - q [0] * q [2];
307 c = squared_radius * KR_KRT_.coeff (0) - q [0] * q [0];
313 maxX = input_->width - 1;
317 float x1 =
static_cast<float> ((b - sqrt (det)) / a);
318 float x2 =
static_cast<float> ((b + sqrt (det)) / a);
320 min = std::min (
static_cast<int> (floor (x1)),
static_cast<int> (floor (x2)));
321 max = std::max (
static_cast<int> (ceil (x1)),
static_cast<int> (ceil (x2)));
322 minX =
static_cast<unsigned> (std::min (
static_cast<int> (input_->width)- 1, std::max (0, min)));
323 maxX =
static_cast<unsigned> (std::max (std::min (
static_cast<int> (input_->width) - 1, max), 0));
329 template<
typename Po
intT>
void
336 template<
typename Po
intT>
void
340 projection_matrix_.setZero ();
341 if (input_->height == 1 || input_->width == 1)
343 PCL_ERROR (
"[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", this->getName ().c_str ());
347 const unsigned ySkip = (std::max) (input_->height >> pyramid_level_, unsigned (1));
348 const unsigned xSkip = (std::max) (input_->width >> pyramid_level_, unsigned (1));
350 std::vector<int> indices;
351 indices.reserve (input_->size () >> (pyramid_level_ << 1));
353 for (
unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += ySkip, idx += input_->width * ySkip)
355 for (
unsigned xIdx = 0, idx2 = idx; xIdx < input_->width; xIdx += xSkip, idx2 += xSkip)
360 indices.push_back (idx2);
364 double residual_sqr = pcl::estimateProjectionMatrix<PointT> (input_, projection_matrix_, indices);
366 if (fabs (residual_sqr) > eps_ *
float (indices.size ()))
368 PCL_ERROR (
"[pcl::%s::radiusSearch] Input dataset is not from a projective device!\nResidual (MSE) %f, using %d valid points\n", this->getName ().c_str (), residual_sqr /
double (indices.size()), indices.size ());
374 KR_ = projection_matrix_.topLeftCorner <3, 3> ();
377 KR_KRT_ = KR_ * KR_.transpose ();
381 template<
typename Po
intT>
bool
384 Eigen::Vector3f projected = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
385 q.
x = projected [0] / projected [2];
386 q.
y = projected [1] / projected [2];
387 return (projected[2] != 0);
389 #define PCL_INSTANTIATE_OrganizedNeighbor(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor<T>;