40 #ifndef PCL_SEARCH_ORGANIZED_NEIGHBOR_SEARCH_H_ 41 #define PCL_SEARCH_ORGANIZED_NEIGHBOR_SEARCH_H_ 43 #include <pcl/point_cloud.h> 45 #include <pcl/search/search.h> 46 #include <pcl/common/eigen.h> 51 #include <pcl/common/projection_matrix.h> 61 template<
typename Po
intT>
73 typedef boost::shared_ptr<pcl::search::OrganizedNeighbor<PointT> >
Ptr;
74 typedef boost::shared_ptr<const pcl::search::OrganizedNeighbor<PointT> >
ConstPtr;
88 OrganizedNeighbor (
bool sorted_results =
false,
float eps = 1e-4f,
unsigned pyramid_level = 5)
113 float min_f = 0.043744332f *
static_cast<float>(
input_->width);
115 return (determinant3x3Matrix<Eigen::Matrix3f> (
KR_ / std::sqrt (
KR_KRT_.coeff (8))) >= (min_f * min_f));
129 setInputCloud (
const PointCloudConstPtr& cloud,
const IndicesConstPtr &indices = IndicesConstPtr ())
140 for (std::vector<int>::const_iterator iIt =
indices_->begin (); iIt !=
indices_->end (); ++iIt)
162 std::vector<int> &k_indices,
163 std::vector<float> &k_sqr_distances,
164 unsigned int max_nn = 0)
const;
182 std::vector<int> &k_indices,
183 std::vector<float> &k_sqr_distances)
const;
219 if (
mask_ [index] && pcl_isfinite (point.x))
222 float dist_x = point.x - query.x;
223 float dist_y = point.y - query.y;
224 float dist_z = point.z - query.z;
225 float squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z;
226 if (queue.size () < k)
227 queue.push (
Entry (index, squared_distance));
228 else if (queue.top ().distance > squared_distance)
231 queue.push (
Entry (index, squared_distance));
239 clipRange (
int& begin,
int &end,
int min,
int max)
const 241 begin = std::max (std::min (begin, max), min);
242 end = std::min (std::max (end, min), max);
255 unsigned& maxX,
unsigned& maxY)
const;
262 Eigen::Matrix<float, 3, 3, Eigen::RowMajor>
KR_;
265 Eigen::Matrix<float, 3, 3, Eigen::RowMajor>
KR_KRT_;
276 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
281 #ifdef PCL_NO_PRECOMPILE 282 #include <pcl/search/impl/organized.hpp> bool operator<(const Entry &other) const
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 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...
PointCloudConstPtr input_
virtual ~OrganizedNeighbor()
Empty deconstructor.
void estimateProjectionMatrix()
estimated the projection matrix from the input cloud.
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...
bool testPoint(const PointT &query, unsigned k, std::priority_queue< Entry > &queue, unsigned index) const
test if point given by index is among the k NN in results to the query point.
boost::shared_ptr< pcl::search::OrganizedNeighbor< PointT > > Ptr
boost::shared_ptr< PointCloud > PointCloudPtr
A 2D point structure representing Euclidean xy coordinates.
boost::shared_ptr< const PointCloud > PointCloudConstPtr
int nearestKSearch(const PointT &p_q, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const
Search for the k-nearest neighbors for a given query point.
bool isValid() const
Test whether this search-object is valid (input is organized AND from projective device) User should ...
Defines all the PCL implemented PointT point type structures.
int radiusSearch(const PointT &p_q, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
Search for all neighbors of query point that are within a given radius.
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.
boost::shared_ptr< const pcl::search::OrganizedNeighbor< PointT > > ConstPtr
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
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void computeCameraMatrix(Eigen::Matrix3f &camera_matrix) const
Compute the camera matrix.
void clipRange(int &begin, int &end, int min, int max) const
virtual void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input data set, if user has focal length he must set it before calling this...
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
A point structure representing Euclidean xyz coordinates, and the RGB color.
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
Entry(int idx, float dist)
pcl::PointCloud< PointT > PointCloud