19 const auto nr_points = cloud.
size();
21 normals.header = cloud.header;
22 normals.width = cloud.width;
23 normals.height = cloud.height;
24 normals.resize(nr_points);
26 for (
auto& point: normals.points)
27 point.getNormalVector3fMap() = Eigen::Vector3f::Zero();
32 for (
const auto&
polygon: polygons)
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();
47 for (std::size_t i = 0; i < nr_points; ++i)
49 normals[i].getNormalVector3fMap().normalize();