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 ());
205 if (indices_->size () != input_->points.size ())
207 output.
width =
static_cast<int> (indices_->size ());
212 output.
width = input_->width;
213 output.
height = input_->height;
218 computeFeature (output);
226 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
231 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
238 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
244 if (normals_->points.size () != surface_->points.size ())
246 PCL_ERROR (
"[pcl::%s::initCompute] ", getClassName ().c_str ());
247 PCL_ERROR (
"The number of points in the input dataset (%u) differs from ", surface_->points.size ());
248 PCL_ERROR (
"the number of points in the dataset containing the normals (%u)!\n", normals_->points.size ());
259 template <
typename Po
intInT,
typename Po
intLT,
typename Po
intOutT>
bool
264 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
271 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing labels was given!\n", getClassName ().c_str ());
277 if (labels_->points.size () != surface_->points.size ())
279 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;
332 #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).