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)
73 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
74 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
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 = std::abs (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 search_method_surface_ = [
this] (
const PointCloudIn &cloud,
int index,
double radius,
147 std::vector<int> &k_indices, std::vector<float> &k_distances)
149 return tree_->radiusSearch (cloud, index, radius, k_indices, k_distances, 0);
157 search_parameter_ = k_;
159 search_method_surface_ = [
this] (
const PointCloudIn &cloud,
int index,
int k, std::vector<int> &k_indices,
160 std::vector<float> &k_distances)
162 return tree_->nearestKSearch (cloud, index, k, k_indices, k_distances);
167 PCL_ERROR (
"[pcl::%s::compute] Neither radius nor K defined! ", getClassName ().c_str ());
168 PCL_ERROR (
"Set one of them to a positive number first and then re-run compute ().\n");
178 template <
typename Po
intInT,
typename Po
intOutT>
bool
185 fake_surface_ =
false;
191 template <
typename Po
intInT,
typename Po
intOutT>
void
202 output.
header = input_->header;
205 if (output.
points.size () != indices_->size ())
206 output.
points.resize (indices_->size ());
210 if (indices_->size () != input_->points.size () || input_->width * input_->height == 0)
212 output.
width =
static_cast<std::uint32_t
> (indices_->size ());
217 output.
width = input_->width;
218 output.
height = input_->height;
223 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 (%u) differs from ", surface_->points.size ());
253 PCL_ERROR (
"the number of points in the dataset containing the normals (%u)!\n", 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 ());
295 template <
typename Po
intInT,
typename Po
intRFT>
bool
299 if (frames_never_defined_)
307 PCL_ERROR (
"[initLocalReferenceFrames] No input dataset containing reference frames was given!\n");
313 lrf_estimation->compute (*default_frames);
314 frames_ = default_frames;
319 if (frames_->points.size () != indices_size)
323 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");
329 lrf_estimation->compute (*default_frames);
330 frames_ = default_frames;
337 #endif //#ifndef PCL_FEATURES_IMPL_FEATURE_H_