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 ());
207 if (indices_->size () != input_->points.size () || input_->width * input_->height == 0)
209 output.
width =
static_cast<uint32_t
> (indices_->size ());
214 output.
width = input_->width;
215 output.
height = input_->height;
220 computeFeature (output);
228 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
233 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
240 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
246 if (normals_->points.size () != surface_->points.size ())
248 PCL_ERROR (
"[pcl::%s::initCompute] ", getClassName ().c_str ());
249 PCL_ERROR (
"The number of points in the input dataset (%u) differs from ", surface_->points.size ());
250 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 ());
292 template <
typename Po
intInT,
typename Po
intRFT>
bool
296 if (frames_never_defined_)
304 PCL_ERROR (
"[initLocalReferenceFrames] No input dataset containing reference frames was given!\n");
310 lrf_estimation->compute (*default_frames);
311 frames_ = default_frames;
316 if (frames_->points.size () != indices_size)
320 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");
326 lrf_estimation->compute (*default_frames);
327 frames_ = default_frames;
334 #endif //#ifndef PCL_FEATURES_IMPL_FEATURE_H_