45#include <pcl/features/feature.h>
60 template <
typename Po
intT>
inline bool
62 Eigen::Vector4f &plane_parameters,
float &curvature)
65 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
67 Eigen::Vector4f xyz_centroid;
69 if (cloud.
size () < 3 ||
72 plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
73 curvature = std::numeric_limits<float>::quiet_NaN ();
93 template <
typename Po
intT>
inline bool
95 Eigen::Vector4f &plane_parameters,
float &curvature)
98 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
100 Eigen::Vector4f xyz_centroid;
101 if (indices.size () < 3 ||
104 plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
105 curvature = std::numeric_limits<float>::quiet_NaN ();
121 template <
typename Po
intT,
typename Scalar>
inline void
123 Eigen::Matrix<Scalar, 4, 1>& normal)
125 Eigen::Matrix <Scalar, 4, 1> vp (vp_x - point.x, vp_y - point.y, vp_z - point.z, 0);
128 float cos_theta = vp.dot (normal);
136 normal[3] = -1 * normal.dot (point.getVector4fMap ());
148 template <
typename Po
intT,
typename Scalar>
inline void
150 Eigen::Matrix<Scalar, 3, 1>& normal)
152 Eigen::Matrix <Scalar, 3, 1> vp (vp_x - point.x, vp_y - point.y, vp_z - point.z);
155 if (vp.dot (normal) < 0)
169 template <
typename Po
intT>
inline void
171 float &nx,
float &ny,
float &nz)
179 float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz);
203 template<
typename Po
intNT>
inline bool
206 Eigen::Vector3f &normal)
208 Eigen::Vector3f normal_mean = Eigen::Vector3f::Zero ();
210 for (
const auto &normal_index : normal_indices)
212 const PointNT& cur_pt = normal_cloud[normal_index];
216 normal_mean += cur_pt.getNormalVector3fMap ();
220 if (normal_mean.isZero ())
223 normal_mean.normalize ();
225 if (normal.dot (normal_mean) < 0)
242 template <
typename Po
intInT,
typename Po
intOutT>
246 using Ptr = shared_ptr<NormalEstimation<PointInT, PointOutT> >;
247 using ConstPtr = shared_ptr<const NormalEstimation<PointInT, PointOutT> >;
285 Eigen::Vector4f &plane_parameters,
float &curvature)
287 if (indices.size () < 3 ||
290 plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
291 curvature = std::numeric_limits<float>::quiet_NaN ();
314 float &nx,
float &ny,
float &nz,
float &curvature)
316 if (indices.size () < 3 ||
319 nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
422#ifdef PCL_NO_PRECOMPILE
423#include <pcl/features/impl/normal_3d.hpp>
Define methods for centroid estimation and covariance matrix calculus.
Feature represents the base feature class.
double search_parameter_
The actual search parameter (from either search_radius_ or k_).
const std::string & getClassName() const
Get a string representation of the name of this class.
double search_radius_
The nearest neighbors search radius for each point.
int k_
The number of K nearest neighbors to use for each point.
std::string feature_name_
The feature name.
PointCloudInConstPtr surface_
An input point cloud describing the surface that is to be used for nearest neighbors estimation.
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
NormalEstimation()
Empty constructor.
shared_ptr< const NormalEstimation< PointInT, PointOutT > > ConstPtr
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix_
Placeholder for the 3x3 covariance matrix at each surface patch.
void setViewPoint(float vpx, float vpy, float vpz)
Set the viewpoint.
bool use_sensor_origin_
whether the sensor origin of the input cloud or a user given viewpoint should be used.
void computeFeature(PointCloudOut &output) override
Estimate normals for all points given in <setInputCloud (), setIndices ()> using the surface in setSe...
bool computePointNormal(const pcl::PointCloud< PointInT > &cloud, const pcl::Indices &indices, Eigen::Vector4f &plane_parameters, float &curvature)
Compute the Least-Squares plane fit for a given set of points, using their indices,...
float vpx_
Values describing the viewpoint ("pinhole" camera model assumed).
~NormalEstimation()
Empty destructor.
bool computePointNormal(const pcl::PointCloud< PointInT > &cloud, const pcl::Indices &indices, float &nx, float &ny, float &nz, float &curvature)
Compute the Least-Squares plane fit for a given set of points, using their indices,...
void useSensorOriginAsViewPoint()
sets whether the sensor origin or a user given viewpoint should be used.
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Eigen::Vector4f xyz_centroid_
16-bytes aligned placeholder for the XYZ centroid of a surface patch.
void getViewPoint(float &vpx, float &vpy, float &vpz)
Get the viewpoint.
typename Feature< PointInT, PointOutT >::PointCloudConstPtr PointCloudConstPtr
shared_ptr< NormalEstimation< PointInT, PointOutT > > Ptr
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
PointCloudConstPtr input_
The input point cloud dataset.
typename PointCloud::ConstPtr PointCloudConstPtr
IndicesPtr indices_
A pointer to the vector of point indices to use.
PointCloud represents the base class in PCL for storing collections of 3D points.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
void flipNormalTowardsViewpoint(const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Matrix< Scalar, 4, 1 > &normal)
Flip (in place) the estimated normal of a point towards a given viewpoint.
bool flipNormalTowardsNormalsMean(pcl::PointCloud< PointNT > const &normal_cloud, pcl::Indices const &normal_indices, Eigen::Vector3f &normal)
Flip (in place) normal to get the same sign of the mean of the normals specified by normal_indices.
bool computePointNormal(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &plane_parameters, float &curvature)
Compute the Least-Squares plane fit for a given set of points, and return the estimated plane paramet...
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...
Defines functions, macros and traits for allocating and using memory.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
IndicesAllocator<> Indices
Type used for indices in PCL.
Defines all the PCL and non-PCL macros used.
A point structure representing Euclidean xyz coordinates, and the RGB color.