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_
Feature< PointInT, PointRFT >::Ptr LRFEstimationPtr
Check if frames_ has been correctly initialized and compute it if needed.
pcl::PCLHeader header
The point cloud header.
uint32_t height
The point cloud height (if organized as an image-structure).
struct pcl::_PointXYZHSV EIGEN_ALIGN16
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
virtual bool initCompute()
This method should get called before starting the actual computation.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
virtual bool deinitCompute()
This method should get called after ending 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 initCompute()
This method should get called before starting the actual computation.
Feature represents the base feature class.
virtual bool initLocalReferenceFrames(const size_t &indices_size, const LRFEstimationPtr &lrf_estimation=LRFEstimationPtr())
PointCloudLRF::Ptr PointCloudLRFPtr
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).
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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...
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...