41 #ifndef PCL_FEATURES_IMPL_FEATURE_H_
42 #define PCL_FEATURES_IMPL_FEATURE_H_
44 #include <pcl/search/kdtree.h>
45 #include <pcl/search/organized.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 if (surface_->isOrganized () && input_->isOrganized ())
128 if (tree_->getInputCloud () != surface_)
129 tree_->setInputCloud (surface_);
133 if (search_radius_ != 0.0)
137 PCL_ERROR (
"[pcl::%s::compute] ", getClassName ().c_str ());
138 PCL_ERROR (
"Both radius (%f) and K (%d) defined! ", search_radius_, k_);
139 PCL_ERROR (
"Set one of them to zero first and then re-run compute ().\n");
146 search_parameter_ = search_radius_;
148 search_method_surface_ = [
this] (
const PointCloudIn &cloud,
int index,
double radius,
149 pcl::Indices &k_indices, std::vector<float> &k_distances)
151 return tree_->radiusSearch (cloud, index, radius, k_indices, k_distances, 0);
159 search_parameter_ = k_;
162 std::vector<float> &k_distances)
164 return tree_->nearestKSearch (cloud, index, k, k_indices, k_distances);
169 PCL_ERROR (
"[pcl::%s::compute] Neither radius nor K defined! ", getClassName ().c_str ());
170 PCL_ERROR (
"Set one of them to a positive number first and then re-run compute ().\n");
180 template <
typename Po
intInT,
typename Po
intOutT>
bool
187 fake_surface_ =
false;
193 template <
typename Po
intInT,
typename Po
intOutT>
void
204 output.
header = input_->header;
207 if (output.
size () != indices_->size ())
208 output.
resize (indices_->size ());
212 if (indices_->size () != input_->points.size () || input_->width * input_->height == 0)
214 output.
width = indices_->size ();
219 output.
width = input_->width;
220 output.
height = input_->height;
225 computeFeature (output);
231 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
236 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
243 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
249 if (normals_->points.size () != surface_->points.size ())
251 PCL_ERROR (
"[pcl::%s::initCompute] ", getClassName ().c_str ());
252 PCL_ERROR(
"The number of points in the input dataset (%zu) differs from ",
253 static_cast<std::size_t
>(surface_->points.size()));
254 PCL_ERROR(
"the number of points in the dataset containing the normals (%zu)!\n",
255 static_cast<std::size_t
>(normals_->points.size()));
264 template <
typename Po
intInT,
typename Po
intLT,
typename Po
intOutT>
bool
269 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
276 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing labels was given!\n", getClassName ().c_str ());
282 if (labels_->points.size () != surface_->points.size ())
284 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 ());
293 template <
typename Po
intInT,
typename Po
intRFT>
bool
297 if (frames_never_defined_)
305 PCL_ERROR (
"[initLocalReferenceFrames] No input dataset containing reference frames was given!\n");
311 lrf_estimation->compute (*default_frames);
312 frames_ = default_frames;
317 if (frames_->points.size () != indices_size)
321 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");
327 lrf_estimation->compute (*default_frames);
328 frames_ = default_frames;
337 #endif //#ifndef PCL_FEATURES_IMPL_FEATURE_H_