41 #ifndef PCL_FEATURES_IMPL_FEATURE_H_
42 #define PCL_FEATURES_IMPL_FEATURE_H_
44 #include <pcl/search/kdtree.h>
45 #include <pcl/search/organized.h>
53 const Eigen::Vector4f &point,
54 Eigen::Vector4f &plane_parameters,
float &curvature)
56 solvePlaneParameters (covariance_matrix, plane_parameters [0], plane_parameters [1], plane_parameters [2], curvature);
58 plane_parameters[3] = 0;
60 plane_parameters[3] = -1 * plane_parameters.dot (point);
66 float &nx,
float &ny,
float &nz,
float &curvature)
77 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
78 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
79 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
81 nx = eigen_vector [0];
82 ny = eigen_vector [1];
83 nz = eigen_vector [2];
86 float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8);
88 curvature = std::abs (eigen_value / eig_sum);
94 template <
typename Po
intInT,
typename Po
intOutT>
bool
99 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
104 if (input_->points.empty ())
106 PCL_ERROR (
"[pcl::%s::compute] input_ is empty!\n", getClassName ().c_str ());
115 fake_surface_ =
true;
122 if (surface_->isOrganized () && input_->isOrganized ()) {
124 if(!tree_->setInputCloud (surface_)) {
132 if (tree_->getInputCloud () != surface_) {
133 if(!tree_->setInputCloud (surface_)) {
134 PCL_ERROR (
"[pcl::%s::compute] The given search method cannot work with the given input cloud/search surface.\n", getClassName ().c_str ());
141 if (search_radius_ != 0.0)
145 PCL_ERROR (
"[pcl::%s::compute] ", getClassName ().c_str ());
146 PCL_ERROR (
"Both radius (%f) and K (%d) defined! ", search_radius_, k_);
147 PCL_ERROR (
"Set one of them to zero first and then re-run compute ().\n");
154 search_parameter_ = search_radius_;
156 search_method_surface_ = [
this] (
const PointCloudIn &cloud,
int index,
double radius,
157 pcl::Indices &k_indices, std::vector<float> &k_distances)
159 return tree_->radiusSearch (cloud, index, radius, k_indices, k_distances, 0);
167 search_parameter_ = k_;
170 std::vector<float> &k_distances)
172 return tree_->nearestKSearch (cloud, index, k, k_indices, k_distances);
177 PCL_ERROR (
"[pcl::%s::compute] Neither radius nor K defined! ", getClassName ().c_str ());
178 PCL_ERROR (
"Set one of them to a positive number first and then re-run compute ().\n");
188 template <
typename Po
intInT,
typename Po
intOutT>
bool
195 fake_surface_ =
false;
201 template <
typename Po
intInT,
typename Po
intOutT>
void
212 output.
header = input_->header;
215 if (output.
size () != indices_->size ())
220 if (indices_->size () != input_->points.size () || input_->width * input_->height == 0)
222 output.
width = indices_->size ();
227 output.
width = input_->width;
228 output.
height = input_->height;
233 computeFeature (output);
239 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
244 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
251 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
257 if (normals_->points.size () != surface_->points.size ())
259 PCL_ERROR (
"[pcl::%s::initCompute] ", getClassName ().c_str ());
260 PCL_ERROR(
"The number of points in the surface dataset (%zu) differs from ",
261 static_cast<std::size_t
>(surface_->points.size()));
262 PCL_ERROR(
"the number of points in the dataset containing the normals (%zu)!\n",
263 static_cast<std::size_t
>(normals_->points.size()));
272 template <
typename Po
intInT,
typename Po
intLT,
typename Po
intOutT>
bool
277 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
284 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing labels was given!\n", getClassName ().c_str ());
290 if (labels_->points.size () != surface_->points.size ())
292 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 ());
301 template <
typename Po
intInT,
typename Po
intRFT>
bool
305 if (frames_never_defined_)
313 PCL_ERROR (
"[initLocalReferenceFrames] No input dataset containing reference frames was given!\n");
319 lrf_estimation->compute (*default_frames);
320 frames_ = default_frames;
325 if (frames_->points.size () != indices_size)
329 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");
335 lrf_estimation->compute (*default_frames);
336 frames_ = default_frames;
virtual bool initCompute()
This method should get called before starting the actual computation.
virtual bool initCompute()
This method should get called before starting the actual computation.
Feature represents the base feature class.
virtual bool initCompute()
This method should get called before starting the actual computation.
virtual bool deinitCompute()
This method should get called after ending the actual computation.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
typename PointCloudLRF::Ptr PointCloudLRFPtr
typename Feature< PointInT, PointRFT >::Ptr LRFEstimationPtr
Check if frames_ has been correctly initialized and compute it if needed.
virtual bool initLocalReferenceFrames(const std::size_t &indices_size, const LRFEstimationPtr &lrf_estimation=LRFEstimationPtr())
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
void clear()
Removes all points in a cloud and sets the width and height to 0.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
OrganizedNeighbor is a class for optimized nearest neighbor search in organized projectable point clo...
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...
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...
IndicesAllocator<> Indices
Type used for indices in PCL.