diff --git a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point_cloud.cpp b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point_cloud.cpp index f66f65d7..607b668e 100644 --- a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point_cloud.cpp +++ b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point_cloud.cpp @@ -120,18 +120,17 @@ Hdf5CorePointCloud::createMeshFromAABB(const std::vector& bb_coords) std::vector descriptors; // bottom plane - descriptors.push_back(mesh.add_face(verts[0], verts[4], verts[6], verts[2])); - // front plane descriptors.push_back(mesh.add_face(verts[0], verts[1], verts[3], verts[2])); + // front plane + descriptors.push_back(mesh.add_face(verts[2], verts[6], verts[4], verts[0])); // left plane - descriptors.push_back(mesh.add_face(verts[4], verts[5], verts[1], verts[0])); + descriptors.push_back(mesh.add_face(verts[3], verts[7], verts[6], verts[2])); // back plane - descriptors.push_back(mesh.add_face(verts[4], verts[5], verts[7], verts[6])); + descriptors.push_back(mesh.add_face(verts[1], verts[5], verts[7], verts[3])); // right plane - descriptors.push_back(mesh.add_face(verts[2], verts[3], verts[7], verts[6])); + descriptors.push_back(mesh.add_face(verts[4], verts[5], verts[1], verts[0])); // top plane - // for whatever the reason the vertices have to be reversed... - descriptors.push_back(mesh.add_face(verts[3], verts[7], verts[5], verts[1])); + descriptors.push_back(mesh.add_face(verts[6], verts[7], verts[5], verts[4])); std::vector::iterator idx; // check if any of the faces was not constructed properly @@ -139,10 +138,11 @@ Hdf5CorePointCloud::createMeshFromAABB(const std::vector& bb_coords) return elem == CGSurfaceMesh::null_face(); })) != descriptors.end()) { - throw std::invalid_argument("Could not create the faces for the " - "SurfaceMesh from the pointcloud AABB! " - "First null_face index: " + - boost::lexical_cast(idx->idx())); + throw std::invalid_argument( + "Could not create the faces for the " + "SurfaceMesh from the pointcloud AABB! " + "First null_face index: " + + std::to_string(std::distance(descriptors.begin(), idx))); } return mesh;