41 #ifndef PCL_FEATURES_IMPL_FEATURE_H_
42 #define PCL_FEATURES_IMPL_FEATURE_H_
44 #include <pcl/search/pcl_search.h>
52 const Eigen::Vector4f &point,
53 Eigen::Vector4f &plane_parameters,
float &curvature)
55 solvePlaneParameters (covariance_matrix, plane_parameters [0], plane_parameters [1], plane_parameters [2], curvature);
57 plane_parameters[3] = 0;
59 plane_parameters[3] = -1 * plane_parameters.dot (point);
65 float &nx,
float &ny,
float &nz,
float &curvature)
76 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
77 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
78 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
80 nx = eigen_vector [0];
81 ny = eigen_vector [1];
82 nz = eigen_vector [2];
85 float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8);
87 curvature = std::abs (eigen_value / eig_sum);
93 template <
typename Po
intInT,
typename Po
intOutT>
bool
98 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
103 if (input_->points.empty ())
105 PCL_ERROR (
"[pcl::%s::compute] input_ is empty!\n", getClassName ().c_str ());
114 fake_surface_ =
true;
121 if (surface_->isOrganized () && input_->isOrganized ())
127 if (tree_->getInputCloud () != surface_)
128 tree_->setInputCloud (surface_);
132 if (search_radius_ != 0.0)
136 PCL_ERROR (
"[pcl::%s::compute] ", getClassName ().c_str ());
137 PCL_ERROR (
"Both radius (%f) and K (%d) defined! ", search_radius_, k_);
138 PCL_ERROR (
"Set one of them to zero first and then re-run compute ().\n");
145 search_parameter_ = search_radius_;
147 search_method_surface_ = [
this] (
const PointCloudIn &cloud,
int index,
double radius,
148 std::vector<int> &k_indices, std::vector<float> &k_distances)
150 return tree_->radiusSearch (cloud, index, radius, k_indices, k_distances, 0);
158 search_parameter_ = k_;
160 search_method_surface_ = [
this] (
const PointCloudIn &cloud,
int index,
int k, std::vector<int> &k_indices,
161 std::vector<float> &k_distances)
163 return tree_->nearestKSearch (cloud, index, k, k_indices, k_distances);
168 PCL_ERROR (
"[pcl::%s::compute] Neither radius nor K defined! ", getClassName ().c_str ());
169 PCL_ERROR (
"Set one of them to a positive number first and then re-run compute ().\n");
179 template <
typename Po
intInT,
typename Po
intOutT>
bool
186 fake_surface_ =
false;
192 template <
typename Po
intInT,
typename Po
intOutT>
void
203 output.
header = input_->header;
206 if (output.
points.size () != indices_->size ())
207 output.
points.resize (indices_->size ());
211 if (indices_->size () != input_->points.size () || input_->width * input_->height == 0)
218 output.
width = input_->width;
224 computeFeature (output);
230 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
235 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
242 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
248 if (normals_->points.size () != surface_->points.size ())
250 PCL_ERROR (
"[pcl::%s::initCompute] ", getClassName ().c_str ());
251 PCL_ERROR (
"The number of points in the input dataset (%u) differs from ", surface_->points.size ());
252 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 ());
290 template <
typename Po
intInT,
typename Po
intRFT>
bool
294 if (frames_never_defined_)
302 PCL_ERROR (
"[initLocalReferenceFrames] No input dataset containing reference frames was given!\n");
308 lrf_estimation->compute (*default_frames);
309 frames_ = default_frames;
314 if (frames_->points.size () != indices_size)
318 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");
324 lrf_estimation->compute (*default_frames);
325 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).
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).
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
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...