42 #include <pcl/search/organized.h>
43 #include <pcl/common/point_tests.h>
44 #include <pcl/common/projection_matrix.h>
45 #include <Eigen/Eigenvalues>
48 template<
typename Po
intT>
int
52 std::vector<float> &k_sqr_distances,
53 unsigned int max_nn)
const
56 assert (
isFinite (query) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
59 unsigned left, right, top, bottom;
61 float squared_distance;
62 const float squared_radius = radius * radius;
65 k_sqr_distances.clear ();
67 this->getProjectedRadiusSearchBox (query, squared_radius, left, right, top, bottom);
70 if (max_nn == 0 || max_nn >=
static_cast<unsigned int> (input_->size ()))
71 max_nn =
static_cast<unsigned int> (input_->size ());
73 k_indices.reserve (max_nn);
74 k_sqr_distances.reserve (max_nn);
76 unsigned yEnd = (bottom + 1) * input_->width + right + 1;
77 unsigned idx = top * input_->width + left;
78 unsigned skip = input_->width - right + left - 1;
79 unsigned xEnd = idx - left + right + 1;
81 for (; xEnd != yEnd; idx += skip, xEnd += input_->width)
83 for (; idx < xEnd; ++idx)
85 if (!mask_[idx] || !
isFinite ((*input_)[idx]))
88 float dist_x = (*input_)[idx].x - query.x;
89 float dist_y = (*input_)[idx].y - query.y;
90 float dist_z = (*input_)[idx].z - query.z;
91 squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z;
93 if (squared_distance <= squared_radius)
95 k_indices.push_back (idx);
96 k_sqr_distances.push_back (squared_distance);
98 if (k_indices.size () == max_nn)
101 this->sortResults (k_indices, k_sqr_distances);
108 this->sortResults (k_indices, k_sqr_distances);
109 return (
static_cast<int> (k_indices.size ()));
113 template<
typename Po
intT>
int
117 std::vector<float> &k_sqr_distances)
const
119 assert (
isFinite (query) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
123 k_sqr_distances.clear ();
127 Eigen::Vector3f queryvec (query.x, query.y, query.z);
130 Eigen::Vector3f q (KR_ * queryvec + projection_matrix_.block <3, 1> (0, 3));
131 int xBegin = int(q [0] / q [2] + 0.5f);
132 int yBegin = int(q [1] / q [2] + 0.5f);
133 int xEnd = xBegin + 1;
134 int yEnd = yBegin + 1;
138 unsigned right = input_->width - 1;
140 unsigned bottom = input_->height - 1;
142 std::vector <Entry> results;
146 xBegin <
static_cast<int> (input_->width) &&
148 yBegin <
static_cast<int> (input_->height))
149 testPoint (query, k, results, yBegin * input_->width + xBegin);
153 int dist = std::numeric_limits<int>::max ();
157 else if (xBegin >=
static_cast<int> (input_->width))
158 dist = xBegin -
static_cast<int> (input_->width);
161 dist = std::min (dist, -yBegin);
162 else if (yBegin >=
static_cast<int> (input_->height))
163 dist = std::min (dist, yBegin -
static_cast<int> (input_->height));
186 clipRange (xFrom, xTo, 0, input_->width);
192 if (yBegin >= 0 && yBegin <
static_cast<int> (input_->height))
194 index_t idx = yBegin * input_->width + xFrom;
195 index_t idxTo = idx + xTo - xFrom;
196 for (; idx < idxTo; ++idx)
197 stop = testPoint (query, k, results, idx) || stop;
203 if (yEnd > 0 && yEnd <=
static_cast<int> (input_->height))
205 index_t idx = (yEnd - 1) * input_->width + xFrom;
206 index_t idxTo = idx + xTo - xFrom;
208 for (; idx < idxTo; ++idx)
209 stop = testPoint (query, k, results, idx) || stop;
213 int yFrom = yBegin + 1;
215 clipRange (yFrom, yTo, 0, input_->height);
220 if (xBegin >= 0 && xBegin <
static_cast<int> (input_->width))
222 index_t idx = yFrom * input_->width + xBegin;
223 index_t idxTo = yTo * input_->width + xBegin;
225 for (; idx < idxTo; idx += input_->width)
226 stop = testPoint (query, k, results, idx) || stop;
229 if (xEnd > 0 && xEnd <=
static_cast<int> (input_->width))
231 index_t idx = yFrom * input_->width + xEnd - 1;
232 index_t idxTo = yTo * input_->width + xEnd - 1;
234 for (; idx < idxTo; idx += input_->width)
235 stop = testPoint (query, k, results, idx) || stop;
241 getProjectedRadiusSearchBox (query, results.back ().distance, left, right, top, bottom);
245 stop = (
static_cast<int> (left) >= xBegin &&
static_cast<int> (left) < xEnd &&
246 static_cast<int> (right) >= xBegin &&
static_cast<int> (right) < xEnd &&
247 static_cast<int> (top) >= yBegin &&
static_cast<int> (top) < yEnd &&
248 static_cast<int> (bottom) >= yBegin &&
static_cast<int> (bottom) < yEnd);
253 const auto results_size = results.size ();
254 k_indices.resize (results_size);
255 k_sqr_distances.resize (results_size);
257 for(
const auto& result : results)
259 k_indices [idx] = result.index;
260 k_sqr_distances [idx] = result.distance;
264 return (
static_cast<int> (results_size));
268 template<
typename Po
intT>
void
270 float squared_radius,
274 unsigned &maxY)
const
276 Eigen::Vector3f queryvec (point.x, point.y, point.z);
278 Eigen::Vector3f q (KR_ * queryvec + projection_matrix_.block <3, 1> (0, 3));
280 float a = squared_radius * KR_KRT_.coeff (8) - q [2] * q [2];
281 float b = squared_radius * KR_KRT_.coeff (7) - q [1] * q [2];
282 float c = squared_radius * KR_KRT_.coeff (4) - q [1] * q [1];
285 float det = b * b - a * c;
289 maxY = input_->height - 1;
293 float y1 =
static_cast<float> ((b - std::sqrt (det)) / a);
294 float y2 =
static_cast<float> ((b + std::sqrt (det)) / a);
296 min = std::min (
static_cast<int> (std::floor (y1)),
static_cast<int> (std::floor (y2)));
297 max = std::max (
static_cast<int> (std::ceil (y1)),
static_cast<int> (std::ceil (y2)));
298 minY =
static_cast<unsigned> (std::min (
static_cast<int> (input_->height) - 1, std::max (0, min)));
299 maxY =
static_cast<unsigned> (std::max (std::min (
static_cast<int> (input_->height) - 1, max), 0));
302 b = squared_radius * KR_KRT_.coeff (6) - q [0] * q [2];
303 c = squared_radius * KR_KRT_.coeff (0) - q [0] * q [0];
309 maxX = input_->width - 1;
313 float x1 =
static_cast<float> ((b - std::sqrt (det)) / a);
314 float x2 =
static_cast<float> ((b + std::sqrt (det)) / a);
316 min = std::min (
static_cast<int> (std::floor (x1)),
static_cast<int> (std::floor (x2)));
317 max = std::max (
static_cast<int> (std::ceil (x1)),
static_cast<int> (std::ceil (x2)));
318 minX =
static_cast<unsigned> (std::min (
static_cast<int> (input_->width)- 1, std::max (0, min)));
319 maxX =
static_cast<unsigned> (std::max (std::min (
static_cast<int> (input_->width) - 1, max), 0));
325 template<
typename Po
intT>
void
332 template<
typename Po
intT>
void
336 projection_matrix_.setZero ();
337 if (input_->height == 1 || input_->width == 1)
339 PCL_ERROR (
"[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", this->getName ().c_str ());
343 const unsigned ySkip = (std::max) (input_->height >> pyramid_level_, unsigned (1));
344 const unsigned xSkip = (std::max) (input_->width >> pyramid_level_, unsigned (1));
347 indices.reserve (input_->size () >> (pyramid_level_ << 1));
349 for (
unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += ySkip, idx += input_->width * ySkip)
351 for (
unsigned xIdx = 0, idx2 = idx; xIdx < input_->width; xIdx += xSkip, idx2 += xSkip)
356 indices.push_back (idx2);
360 double residual_sqr = pcl::estimateProjectionMatrix<PointT> (input_, projection_matrix_, indices);
362 if (std::abs (residual_sqr) > eps_ *
float (indices.size ()))
364 PCL_ERROR (
"[pcl::%s::estimateProjectionMatrix] 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 ());
370 KR_ = projection_matrix_.topLeftCorner <3, 3> ();
373 KR_KRT_ = KR_ * KR_.transpose ();
377 template<
typename Po
intT>
bool
380 Eigen::Vector3f projected = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
381 q.
x = projected [0] / projected [2];
382 q.
y = projected [1] / projected [2];
383 return (projected[2] != 0);
385 #define PCL_INSTANTIATE_OrganizedNeighbor(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor<T>;