1 #ifndef PCL_FEATURES_FROM_MESHES_H_
2 #define PCL_FEATURES_FROM_MESHES_H_
4 #include "pcl/features/normal_3d.h"
5 #include "pcl/Vertices.h"
17 template <
typename Po
intT,
typename Po
intNT>
inline void
20 int nr_points =
static_cast<int>(cloud.
points.size());
21 int nr_polygons =
static_cast<int>(polygons.size());
26 normals.
points.resize(nr_points);
28 for (
int i = 0; i < nr_points; ++i )
29 normals.
points[i].getNormalVector3fMap() = Eigen::Vector3f::Zero();
34 for (
int i = 0; i < nr_polygons; ++i )
36 const int nr_points_polygon = (int)polygons[i].vertices.size();
37 if (nr_points_polygon < 3)
continue;
40 Eigen::Vector3f vec_a_b = cloud.
points[polygons[i].vertices[0]].getVector3fMap() - cloud.
points[polygons[i].vertices[1]].getVector3fMap();
41 Eigen::Vector3f vec_a_c = cloud.
points[polygons[i].vertices[0]].getVector3fMap() - cloud.
points[polygons[i].vertices[2]].getVector3fMap();
42 Eigen::Vector3f normal = vec_a_b.cross(vec_a_c);
46 for (
int j = 0; j < nr_points_polygon; ++j )
47 normals.
points[polygons[i].vertices[j]].getNormalVector3fMap() += normal;
50 for (
int i = 0; i < nr_points; ++i )
52 normals.
points[i].getNormalVector3fMap().normalize();
65 template <
typename Po
intT,
typename Po
intNT>
inline void
68 std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> >& covariances,
69 double epsilon = 0.001)
73 int nr_points =
static_cast<int>(cloud.
points.size());
74 covariances.resize(nr_points);
75 for (
int i = 0; i < nr_points; ++i)
77 Eigen::Vector3d normal(normals.
points[i].normal_x,
78 normals.
points[i].normal_y,
79 normals.
points[i].normal_z);
86 y = y - normal(1) * normal;
89 rot.row(0) = normal.cross(rot.row(1));
96 covariances[i] = rot.transpose()*cov*rot;
104 #endif // PCL_FEATURES_FROM_MESHES_H_
uint32_t width
The point cloud width (if organized as an image-structure).
This file defines compatibility wrappers for low level I/O functions.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.
uint32_t height
The point cloud height (if organized as an image-structure).
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void computeApproximateCovariances(const pcl::PointCloud< PointT > &cloud, const pcl::PointCloud< PointNT > &normals, std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > &covariances, double epsilon=0.001)
Compute GICP-style covariance matrices given a point cloud and the corresponding surface normals...
void computeApproximateNormals(const pcl::PointCloud< PointT > &cloud, const std::vector< pcl::Vertices > &polygons, pcl::PointCloud< PointNT > &normals)
Compute approximate surface normals on a mesh.
pcl::PCLHeader header
The point cloud header.