42#include <pcl/search/organized.h>
43#include <pcl/common/point_tests.h>
44#include <pcl/common/projection_matrix.h>
45#include <Eigen/Eigenvalues>
48template<
typename Po
intT>
int
56 assert (
isFinite (
query) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
59 unsigned 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 ());
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;
83 for (; idx <
xEnd; ++idx)
85 if (!mask_[idx] || !
isFinite ((*input_)[idx]))
113template<
typename Po
intT>
int
119 assert (
isFinite (
query) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
130 Eigen::Vector3f
q (KR_ *
queryvec + projection_matrix_.block <3, 1> (0, 3));
138 unsigned right = input_->width - 1;
140 unsigned bottom = input_->height - 1;
146 xBegin <
static_cast<int> (input_->width) &&
148 yBegin <
static_cast<int> (input_->height))
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);
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))
196 for (; idx <
idxTo; ++idx)
203 if (
yEnd > 0 &&
yEnd <=
static_cast<int> (input_->height))
208 for (; idx <
idxTo; ++idx)
215 clipRange (
yFrom,
yTo, 0, input_->height);
220 if (
xBegin >= 0 &&
xBegin <
static_cast<int> (input_->width))
225 for (; idx <
idxTo; idx += input_->width)
229 if (
xEnd > 0 &&
xEnd <=
static_cast<int> (input_->width))
234 for (; idx <
idxTo; idx += input_->width)
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);
268template<
typename Po
intT>
void
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));
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));
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));
325template<
typename Po
intT>
void
332template<
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));
356 indices.push_back (
idx2);
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 ();
377template<
typename Po
intT>
bool
380 Eigen::Vector3f
projected = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
385#define PCL_INSTANTIATE_OrganizedNeighbor(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor<T>;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
int radiusSearch(const PointT &p_q, double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
Search for all neighbors of query point that are within a given radius.
int nearestKSearch(const PointT &p_q, int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for a given query point.
void computeCameraMatrix(Eigen::Matrix3f &camera_matrix) const
Compute the camera matrix.
void estimateProjectionMatrix()
estimated the projection matrix from the input cloud.
bool projectPoint(const PointT &p, pcl::PointXY &q) const
projects a point into the image
void getProjectedRadiusSearchBox(const PointT &point, float squared_radius, unsigned &minX, unsigned &minY, unsigned &maxX, unsigned &maxY) const
Obtain a search box in 2D from a sphere with a radius in 3D.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
IndicesAllocator<> Indices
Type used for indices in PCL.
PCL_EXPORTS void getCameraMatrixFromProjectionMatrix(const Eigen::Matrix< float, 3, 4, Eigen::RowMajor > &projection_matrix, Eigen::Matrix3f &camera_matrix)
Determines the camera matrix from the given projection matrix.
A 2D point structure representing Euclidean xy coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.