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