41 #ifndef PCL_FEATURES_IMPL_FEATURE_H_
42 #define PCL_FEATURES_IMPL_FEATURE_H_
44 #include <pcl/common/eigen.h>
45 #include <pcl/search/auto.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 tree_.reset (pcl::search::autoSelectMethod<PointInT>(surface_,
false));
125 if (tree_->getInputCloud () != surface_) {
126 if(!tree_->setInputCloud (surface_)) {
127 PCL_ERROR (
"[pcl::%s::compute] The given search method cannot work with the given input cloud/search surface.\n", getClassName ().c_str ());
134 if (search_radius_ != 0.0)
138 PCL_ERROR (
"[pcl::%s::compute] ", getClassName ().c_str ());
139 PCL_ERROR (
"Both radius (%f) and K (%d) defined! ", search_radius_, k_);
140 PCL_ERROR (
"Set one of them to zero first and then re-run compute ().\n");
147 search_parameter_ = search_radius_;
149 search_method_surface_ = [
this] (
const PointCloudIn &cloud,
int index,
double radius,
150 pcl::Indices &k_indices, std::vector<float> &k_distances)
152 return tree_->radiusSearch (cloud, index, radius, k_indices, k_distances, 0);
160 search_parameter_ = k_;
163 std::vector<float> &k_distances)
165 return tree_->nearestKSearch (cloud, index, k, k_indices, k_distances);
170 PCL_ERROR (
"[pcl::%s::compute] Neither radius nor K defined! ", getClassName ().c_str ());
171 PCL_ERROR (
"Set one of them to a positive number first and then re-run compute ().\n");
181 template <
typename Po
intInT,
typename Po
intOutT>
bool
188 fake_surface_ =
false;
194 template <
typename Po
intInT,
typename Po
intOutT>
void
205 output.
header = input_->header;
208 if (output.
size () != indices_->size ())
209 output.
resize (indices_->size ());
213 if (indices_->size () != input_->points.size () || input_->width * input_->height == 0)
215 output.
width = indices_->size ();
220 output.
width = input_->width;
221 output.
height = input_->height;
226 computeFeature (output);
232 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
237 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
244 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
250 if (normals_->points.size () != surface_->points.size ())
252 PCL_ERROR (
"[pcl::%s::initCompute] ", getClassName ().c_str ());
253 PCL_ERROR(
"The number of points in the surface dataset (%zu) differs from ",
254 static_cast<std::size_t
>(surface_->points.size()));
255 PCL_ERROR(
"the number of points in the dataset containing the normals (%zu)!\n",
256 static_cast<std::size_t
>(normals_->points.size()));
265 template <
typename Po
intInT,
typename Po
intLT,
typename Po
intOutT>
bool
270 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
277 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing labels was given!\n", getClassName ().c_str ());
283 if (labels_->points.size () != surface_->points.size ())
285 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 ());
294 template <
typename Po
intInT,
typename Po
intRFT>
bool
298 if (frames_never_defined_)
306 PCL_ERROR (
"[initLocalReferenceFrames] No input dataset containing reference frames was given!\n");
312 lrf_estimation->compute (*default_frames);
313 frames_ = default_frames;
318 if (frames_->points.size () != indices_size)
322 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");
328 lrf_estimation->compute (*default_frames);
329 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.
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.