41 #ifndef PCL_FEATURES_IMPL_FEATURE_H_
42 #define PCL_FEATURES_IMPL_FEATURE_H_
44 #include <pcl/search/pcl_search.h>
49 const Eigen::Vector4f &point,
50 Eigen::Vector4f &plane_parameters,
float &curvature)
52 solvePlaneParameters (covariance_matrix, plane_parameters [0], plane_parameters [1], plane_parameters [2], curvature);
54 plane_parameters[3] = 0;
56 plane_parameters[3] = -1 * plane_parameters.dot (point);
62 float &nx,
float &ny,
float &nz,
float &curvature)
75 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
77 nx = eigen_vector [0];
78 ny = eigen_vector [1];
79 nz = eigen_vector [2];
82 float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8);
84 curvature = fabsf (eigen_value / eig_sum);
92 template <
typename Po
intInT,
typename Po
intOutT>
bool
97 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
102 if (input_->points.empty ())
104 PCL_ERROR (
"[pcl::%s::compute] input_ is empty!\n", getClassName ().c_str ());
113 fake_surface_ =
true;
120 if (surface_->isOrganized () && input_->isOrganized ())
126 if (tree_->getInputCloud () != surface_)
127 tree_->setInputCloud (surface_);
131 if (search_radius_ != 0.0)
135 PCL_ERROR (
"[pcl::%s::compute] ", getClassName ().c_str ());
136 PCL_ERROR (
"Both radius (%f) and K (%d) defined! ", search_radius_, k_);
137 PCL_ERROR (
"Set one of them to zero first and then re-run compute ().\n");
144 search_parameter_ = search_radius_;
146 int (
KdTree::*radiusSearchSurface)(
const PointCloudIn &cloud,
int index,
double radius,
147 std::vector<int> &k_indices, std::vector<float> &k_distances,
149 search_method_surface_ = boost::bind (radiusSearchSurface, boost::ref (tree_), _1, _2, _3, _4, _5, 0);
156 search_parameter_ = k_;
158 int (
KdTree::*nearestKSearchSurface)(
const PointCloudIn &cloud,
int index,
int k, std::vector<int> &k_indices,
159 std::vector<float> &k_distances)
const = &KdTree::nearestKSearch;
160 search_method_surface_ = boost::bind (nearestKSearchSurface, boost::ref (tree_), _1, _2, _3, _4, _5);
164 PCL_ERROR (
"[pcl::%s::compute] Neither radius nor K defined! ", getClassName ().c_str ());
165 PCL_ERROR (
"Set one of them to a positive number first and then re-run compute ().\n");
175 template <
typename Po
intInT,
typename Po
intOutT>
bool
182 fake_surface_ =
false;
188 template <
typename Po
intInT,
typename Po
intOutT>
void
199 output.
header = input_->header;
202 if (output.
points.size () != indices_->size ())
203 output.
points.resize (indices_->size ());
207 if (indices_->size () != input_->points.size () || input_->width * input_->height == 0)
209 output.
width =
static_cast<uint32_t
> (indices_->size ());
214 output.
width = input_->width;
215 output.
height = input_->height;
220 computeFeature (output);
228 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
233 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
240 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
246 if (normals_->points.size () != surface_->points.size ())
248 PCL_ERROR (
"[pcl::%s::initCompute] ", getClassName ().c_str ());
249 PCL_ERROR (
"The number of points in the input dataset (%u) differs from ", surface_->points.size ());
250 PCL_ERROR (
"the number of points in the dataset containing the normals (%u)!\n", normals_->points.size ());
261 template <
typename Po
intInT,
typename Po
intLT,
typename Po
intOutT>
bool
266 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
273 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing labels was given!\n", getClassName ().c_str ());
279 if (labels_->points.size () != surface_->points.size ())
281 PCL_ERROR (
"[pcl::%s::initCompute] The number of points in the input dataset differs from the number of points in the dataset containing the labels!\n", getClassName ().c_str ());
292 template <
typename Po
intInT,
typename Po
intRFT>
bool
296 if (frames_never_defined_)
304 PCL_ERROR (
"[initLocalReferenceFrames] No input dataset containing reference frames was given!\n");
310 lrf_estimation->compute (*default_frames);
311 frames_ = default_frames;
316 if (frames_->points.size () != indices_size)
320 PCL_ERROR (
"[initLocalReferenceFrames] The number of points in the input dataset differs from the number of points in the dataset containing the reference frames!\n");
326 lrf_estimation->compute (*default_frames);
327 frames_ = default_frames;
334 #endif //#ifndef PCL_FEATURES_IMPL_FEATURE_H_
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
struct pcl::_PointXYZHSV EIGEN_ALIGN16
virtual bool initCompute()
This method should get called before starting the actual computation.
void solvePlaneParameters(const Eigen::Matrix3f &covariance_matrix, const Eigen::Vector4f &point, Eigen::Vector4f &plane_parameters, float &curvature)
Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squar...
virtual bool deinitCompute()
This method should get called after ending the actual computation.
virtual bool initCompute()
This method should get called before starting the actual computation.
uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Feature< PointInT, PointRFT >::Ptr LRFEstimationPtr
Check if frames_ has been correctly initialized and compute it if needed.
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
virtual bool initCompute()
This method should get called before starting the actual computation.
virtual bool initLocalReferenceFrames(const size_t &indices_size, const LRFEstimationPtr &lrf_estimation=LRFEstimationPtr())
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
Feature represents the base feature class.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
PointCloudLRF::Ptr PointCloudLRFPtr
uint32_t height
The point cloud height (if organized as an image-structure).