3 #include "pcl/features/normal_3d.h"
4 #include "pcl/Vertices.h"
16 template <
typename Po
intT,
typename Po
intNT>
inline void
19 const auto nr_points = cloud.
size();
26 for (
auto& point: normals.
points)
27 point.getNormalVector3fMap() = Eigen::Vector3f::Zero();
32 for (
const auto& polygon: polygons)
34 if (polygon.vertices.size() < 3)
continue;
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);
43 for (
const auto& vertex: polygon.vertices)
44 normals[vertex].getNormalVector3fMap() += normal;
47 for (std::size_t i = 0; i < nr_points; ++i)
49 normals[i].getNormalVector3fMap().normalize();
62 template <
typename Po
intT,
typename Po
intNT>
inline void
65 std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> >& covariances,
66 double epsilon = 0.001)
68 assert(cloud.
size() == normals.
size());
70 const auto nr_points = cloud.
size();
72 covariances.reserve (nr_points);
73 for (
const auto& point: normals.
points)
75 Eigen::Vector3d normal (point.normal_x,
84 y -= normal(1) * normal;
87 rot.row(0) = normal.cross(rot.row(1));
94 covariances.emplace_back (rot.transpose()*cov*rot);
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);