Point Cloud Library (PCL)  1.8.1-dev
from_meshes.h
1 #ifndef PCL_FEATURES_FROM_MESHES_H_
2 #define PCL_FEATURES_FROM_MESHES_H_
3 
4 #include "pcl/features/normal_3d.h"
5 #include "pcl/Vertices.h"
6 
7 namespace pcl
8 {
9  namespace features
10  {
11 
12  /** \brief Compute approximate surface normals on a mesh.
13  * \param[in] cloud Point cloud containing the XYZ coordinates.
14  * \param[in] polygons Polygons from the mesh.
15  * \param[out] normals Point cloud with computed surface normals
16  */
17  template <typename PointT, typename PointNT> inline void
18  computeApproximateNormals(const pcl::PointCloud<PointT>& cloud, const std::vector<pcl::Vertices>& polygons, pcl::PointCloud<PointNT>& normals)
19  {
20  int nr_points = static_cast<int>(cloud.points.size());
21  int nr_polygons = static_cast<int>(polygons.size());
22 
23  normals.header = cloud.header;
24  normals.width = cloud.width;
25  normals.height = cloud.height;
26  normals.points.resize(nr_points);
27 
28  for ( int i = 0; i < nr_points; ++i )
29  normals.points[i].getNormalVector3fMap() = Eigen::Vector3f::Zero();
30 
31  // NOTE: for efficiency the weight is computed implicitly by using the
32  // cross product, this causes inaccurate normals for meshes containing
33  // non-triangle polygons (quads or other types)
34  for ( int i = 0; i < nr_polygons; ++i )
35  {
36  const int nr_points_polygon = (int)polygons[i].vertices.size();
37  if (nr_points_polygon < 3) continue;
38 
39  // compute normal for triangle
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);
43  pcl::flipNormalTowardsViewpoint(cloud.points[polygons[i].vertices[0]], 0.0f, 0.0f, 0.0f, normal(0), normal(1), normal(2));
44 
45  // add normal to all points in polygon
46  for ( int j = 0; j < nr_points_polygon; ++j )
47  normals.points[polygons[i].vertices[j]].getNormalVector3fMap() += normal;
48  }
49 
50  for ( int i = 0; i < nr_points; ++i )
51  {
52  normals.points[i].getNormalVector3fMap().normalize();
53  pcl::flipNormalTowardsViewpoint(cloud.points[i], 0.0f, 0.0f, 0.0f, normals.points[i].normal_x, normals.points[i].normal_y, normals.points[i].normal_z);
54  }
55  }
56 
57 
58  /** \brief Compute GICP-style covariance matrices given a point cloud and
59  * the corresponding surface normals.
60  * \param[in] cloud Point cloud containing the XYZ coordinates,
61  * \param[in] normals Point cloud containing the corresponding surface normals.
62  * \param[out] covariances Vector of computed covariances.
63  * \param[in] Optional: Epsilon for the expected noise along the surface normal (default: 0.001)
64  */
65  template <typename PointT, typename PointNT> inline void
67  const pcl::PointCloud<PointNT>& normals,
68  std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> >& covariances,
69  double epsilon = 0.001)
70  {
71  assert(cloud.points.size() == normals.points.size());
72 
73  int nr_points = static_cast<int>(cloud.points.size());
74  covariances.resize(nr_points);
75  for (int i = 0; i < nr_points; ++i)
76  {
77  Eigen::Vector3d normal(normals.points[i].normal_x,
78  normals.points[i].normal_y,
79  normals.points[i].normal_z);
80 
81  // compute rotation matrix
82  Eigen::Matrix3d rot;
83  Eigen::Vector3d y;
84  y << 0, 1, 0;
85  rot.row(2) = normal;
86  y = y - normal(1) * normal;
87  y.normalize();
88  rot.row(1) = y;
89  rot.row(0) = normal.cross(rot.row(1));
90 
91  // comnpute approximate covariance
92  Eigen::Matrix3d cov;
93  cov << 1, 0, 0,
94  0, 1, 0,
95  0, 0, epsilon;
96  covariances[i] = rot.transpose()*cov*rot;
97  }
98  }
99 
100  }
101 }
102 
103 
104 #endif // PCL_FEATURES_FROM_MESHES_H_
105 
106 
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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.
Definition: normal_3d.h:121
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...
Definition: from_meshes.h:66
void computeApproximateNormals(const pcl::PointCloud< PointT > &cloud, const std::vector< pcl::Vertices > &polygons, pcl::PointCloud< PointNT > &normals)
Compute approximate surface normals on a mesh.
Definition: from_meshes.h:18
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415