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 205 std::vector<int>
const &normal_indices,
206 Eigen::Vector3f &normal)
208 Eigen::Vector3f normal_mean = Eigen::Vector3f::Zero ();
210 for (
const int &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> 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...
Defines functions, macros and traits for allocating and using memory.
bool computePointNormal(const pcl::PointCloud< PointInT > &cloud, const std::vector< int > &indices, Eigen::Vector4f &plane_parameters, float &curvature)
Compute the Least-Squares plane fit for a given set of points, using their indices, and return the estimated plane parameters together with the surface curvature.
bool computePointNormal(const pcl::PointCloud< PointInT > &cloud, const std::vector< int > &indices, float &nx, float &ny, float &nz, float &curvature)
Compute the Least-Squares plane fit for a given set of points, using their indices, and return the estimated plane parameters together with the surface curvature.
void setViewPoint(float vpx, float vpy, float vpz)
Set the viewpoint.
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 computeFeature(PointCloudOut &output) override
Estimate normals for all points given in <setInputCloud (), setIndices ()> using the surface in setSe...
std::string feature_name_
The feature name.
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...
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
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...
shared_ptr< const Feature< PointInT, PointNT > > ConstPtr
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.
~NormalEstimation()
Empty destructor.
Eigen::Vector4f xyz_centroid_
16-bytes aligned placeholder for the XYZ centroid of a surface patch.
NormalEstimation()
Empty constructor.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix_
Placeholder for the 3x3 covariance matrix at each surface patch.
bool use_sensor_origin_
whether the sensor origin of the input cloud or a user given viewpoint should be used.
PointCloudConstPtr input_
The input point cloud dataset.
Feature represents the base feature class.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
void getViewPoint(float &vpx, float &vpy, float &vpz)
Get the viewpoint.
bool flipNormalTowardsNormalsMean(pcl::PointCloud< PointNT > const &normal_cloud, std::vector< int > 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...
Defines all the PCL and non-PCL macros used.
shared_ptr< Feature< PointInT, PointNT > > Ptr
typename PointCloud::ConstPtr PointCloudConstPtr
Define methods for centroid estimation and covariance matrix calculus.
float vpx_
Values describing the viewpoint ("pinhole" camera model assumed).
void useSensorOriginAsViewPoint()
sets whether the sensor origin or a user given viewpoint should be used.