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)
213 output.
width =
static_cast<std::uint32_t
> (indices_->size ());
218 output.
width = input_->width;
219 output.
height = input_->height;
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;
334 #endif //#ifndef PCL_FEATURES_IMPL_FEATURE_H_
typename Feature< PointInT, PointRFT >::Ptr LRFEstimationPtr
Check if frames_ has been correctly initialized and compute it if needed.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.
std::uint32_t width
The point cloud width (if organized as an image-structure).
typename PointCloudLRF::Ptr PointCloudLRFPtr
std::uint32_t height
The point cloud height (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
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 using th...
virtual bool initLocalReferenceFrames(const std::size_t &indices_size, const LRFEstimationPtr &lrf_estimation=LRFEstimationPtr())