Point Cloud Library (PCL)
1.10.0
|
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>
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));
139 for (std::vector<int>::const_iterator iIt =
indices_->begin (); iIt !=
indices_->end (); ++iIt)
161 std::vector<int> &k_indices,
162 std::vector<float> &k_sqr_distances,
163 unsigned int max_nn = 0)
const override;
181 std::vector<int> &k_indices,
182 std::vector<float> &k_sqr_distances)
const override;
215 testPoint (
const PointT& query,
unsigned k, std::priority_queue<Entry>& queue,
unsigned index)
const
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>
Defines all the PCL and non-PCL macros used.
This file defines compatibility wrappers for low level I/O functions.
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...
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 override
Search for all neighbors of query point that are within a given radius.
bool operator<(const Entry &other) const
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.
PointCloud represents the base class in PCL for storing collections of 3D points.
~OrganizedNeighbor()
Empty deconstructor.
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool projectPoint(const PointT &p, pcl::PointXY &q) const
projects a point into the image
typename PointCloud::ConstPtr PointCloudConstPtr
Eigen::Matrix< float, 3, 4, Eigen::RowMajor > projection_matrix_
the projection matrix.
std::vector< unsigned char > mask_
mask, indicating whether the point was in the indices list or not.
const unsigned pyramid_level_
using only a subsample of points to calculate the projection matrix.
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...
const float eps_
epsilon value for the MSE of the projection matrix estimation
PointCloudConstPtr input_
OrganizedNeighbor(bool sorted_results=false, float eps=1e-4f, unsigned pyramid_level=5)
Constructor.
void estimateProjectionMatrix()
estimated the projection matrix from the input cloud.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
A 2D point structure representing Euclidean xy coordinates.
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
shared_ptr< pcl::search::OrganizedNeighbor< PointT > > Ptr
shared_ptr< PointCloud< PointT > > Ptr
bool isValid() const
Test whether this search-object is valid (input is organized AND from projective device) User should ...
shared_ptr< const std::vector< int > > IndicesConstPtr
void clipRange(int &begin, int &end, int min, int max) const
typename PointCloud::Ptr PointCloudPtr
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
int nearestKSearch(const PointT &p_q, int k, std::vector< int > &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.
shared_ptr< const pcl::search::OrganizedNeighbor< PointT > > ConstPtr
Entry(int idx, float dist)
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.
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.