42 #include <pcl/memory.h>
43 #include <pcl/pcl_macros.h>
44 #include <pcl/point_cloud.h>
45 #include <pcl/point_types.h>
46 #include <pcl/search/search.h>
47 #include <pcl/common/eigen.h>
52 #include <pcl/common/projection_matrix.h>
62 template<
typename Po
intT>
73 using Ptr = shared_ptr<pcl::search::OrganizedNeighbor<PointT> >;
74 using ConstPtr = shared_ptr<const pcl::search::OrganizedNeighbor<PointT> >;
88 OrganizedNeighbor (
bool sorted_results =
false,
float eps = 1e-4f,
unsigned pyramid_level = 5)
112 float min_f = 0.043744332f *
static_cast<float>(
input_->width);
114 return (determinant3x3Matrix<Eigen::Matrix3f> (
KR_ / std::sqrt (
KR_KRT_.coeff (8))) >= (min_f * min_f));
162 std::vector<float> &k_sqr_distances,
163 unsigned int max_nn = 0)
const override;
182 std::vector<float> &k_sqr_distances)
const override;
218 if (
mask_ [index] && std::isfinite (point.x))
221 float dist_x = point.x - query.x;
222 float dist_y = point.y - query.y;
223 float dist_z = point.z - query.z;
224 float squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z;
225 if (queue.size () < k)
227 queue.push (
Entry (index, squared_distance));
228 return queue.size () == k;
230 if (queue.top ().distance > squared_distance)
233 queue.push (
Entry (index, squared_distance));
241 clipRange (
int& begin,
int &end,
int min,
int max)
const
243 begin = std::max (std::min (begin, max), min);
244 end = std::min (std::max (end, min), max);
257 unsigned& maxX,
unsigned& maxY)
const;
264 Eigen::Matrix<float, 3, 3, Eigen::RowMajor>
KR_;
267 Eigen::Matrix<float, 3, 3, Eigen::RowMajor>
KR_KRT_;
283 #ifdef PCL_NO_PRECOMPILE
284 #include <pcl/search/impl/organized.hpp>
~OrganizedNeighbor()
Empty deconstructor.
shared_ptr< const pcl::search::OrganizedNeighbor< PointT > > ConstPtr
typename PointCloud::Ptr PointCloudPtr
shared_ptr< PointCloud< PointT > > Ptr
bool projectPoint(const PointT &p, pcl::PointXY &q) const
projects a point into the image
Eigen::Matrix< float, 3, 3, Eigen::RowMajor > KR_KRT_
inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the ro...
std::vector< index_t > Indices
Type used for indices in PCL.
pcl::IndicesConstPtr IndicesConstPtr
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
PointCloudConstPtr input_
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.
void estimateProjectionMatrix()
estimated the projection matrix from the input cloud.
bool testPoint(const PointT &query, unsigned k, std::priority_queue< Entry > &queue, index_t index) const
test if point given by index is among the k NN in results to the query point.
Eigen::Matrix< float, 3, 3, Eigen::RowMajor > KR_
inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the ro...
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
A 2D point structure representing Euclidean xy coordinates.
Entry(index_t idx, float dist)
bool isValid() const
Test whether this search-object is valid (input is organized AND from projective device) User should ...
typename PointCloud::ConstPtr PointCloudConstPtr
const unsigned pyramid_level_
using only a subsample of points to calculate the projection matrix.
std::vector< unsigned char > mask_
mask, indicating whether the point was in the indices list or not.
void computeCameraMatrix(Eigen::Matrix3f &camera_matrix) const
Compute the camera matrix.
OrganizedNeighbor(bool sorted_results=false, float eps=1e-4f, unsigned pyramid_level=5)
Constructor.
Eigen::Matrix< float, 3, 4, Eigen::RowMajor > projection_matrix_
the projection matrix.
const float eps_
epsilon value for the MSE of the projection matrix estimation
void clipRange(int &begin, int &end, int min, int max) const
shared_ptr< pcl::search::OrganizedNeighbor< PointT > > Ptr
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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 setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input data set, if user has focal length he must set it before calling this...
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.
shared_ptr< const PointCloud< PointT > > ConstPtr
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
bool operator<(const Entry &other) const
A point structure representing Euclidean xyz coordinates, and the RGB color.