fix construction of meshes and intersect tests

seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_image.h

@@ -30,7 +30,7 @@ namespace seerep_hdf5_core
 
 using ExactKernel = CGAL::Exact_predicates_exact_constructions_kernel;
 using CGPoint_3 = ExactKernel::Point_3;
-using SurfaceMesh = CGAL::Surface_mesh<CGPoint_3>;
+using CGSurfaceMesh = CGAL::Surface_mesh<CGPoint_3>;
 
 /**
  * @brief Helper struct to summarize the general attributes of an image
@@ -166,7 +166,8 @@ class Hdf5CoreImage : public virtual Hdf5CoreGeneral,
   std::array<seerep_core_msgs::Point, 5>
   computeFrustumPoints(const std::string& camintrinsics_uuid);
 
-  SurfaceMesh computeFrustumMesh(std::array<seerep_core_msgs::Point, 5>& points);
+  CGSurfaceMesh
+  computeFrustumMesh(std::array<seerep_core_msgs::Point, 5>& points);
 
   /**
    * @brief Computes the frustum for given camera intrinsic parameters and stores

seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp

@@ -105,7 +105,7 @@ Hdf5CoreImage::getPolygonConstraintPoints(
   auto points = this->computeFrustumPoints(camintrinsics_uuid);
   auto mesh = this->computeFrustumMesh(points);
 
-  return seerep_core_msgs::TimestampFramePoints{ ts, SurfaceMesh{}, frame_id };
+  return seerep_core_msgs::TimestampFramePoints{ ts, frame_id, mesh };
 }
 
 void Hdf5CoreImage::writeLabels(
@@ -197,10 +197,10 @@ Hdf5CoreImage::computeFrustumPoints(const std::string& camintrinsics_uuid)
                                                  far_bottomright };
 }
 
-SurfaceMesh
+CGSurfaceMesh
 Hdf5CoreImage::computeFrustumMesh(std::array<seerep_core_msgs::Point, 5>& points)
 {
-  SurfaceMesh mesh;
+  CGSurfaceMesh mesh;
   auto o = mesh.add_vertex(
       CGPoint_3{ points[0].get<0>(), points[0].get<1>(), points[0].get<2>() });
 

seerep_msgs/core/timestamp_frame_points.h

@@ -16,8 +16,8 @@ using SurfaceMesh = CGAL::Surface_mesh<ExactKernel::Point_3>;
 struct TimestampFramePoints
 {
   seerep_core_msgs::Timestamp timestamp;
-  SurfaceMesh mesh;
   std::string frame_id;
+  SurfaceMesh mesh;
 };
 }  // namespace seerep_core_msgs
 

seerep_srv/seerep_core/include/seerep_core/core_dataset.h

@@ -3,8 +3,9 @@
 
 #include <CGAL/Boolean_set_operations_2.h>
 #include <CGAL/Exact_predicates_exact_constructions_kernel.h>
+#include <CGAL/Polygon_mesh_processing/intersection.h>
+#include <CGAL/Polygon_mesh_processing/triangulate_faces.h>
 #include <CGAL/Surface_mesh.h>
-#include <CGAL/polygon_mesh_processing.h>
 
 #include <algorithm>
 #include <cstdint>
@@ -33,9 +34,10 @@
 
 typedef CGAL::Exact_predicates_exact_constructions_kernel ExactKernel;
 typedef CGAL::Point_3<ExactKernel> CGPoint_3;
-typedef CGAL::Surface_mesh<CGPoint_3> SurfaceMesh;
+typedef CGAL::Surface_mesh<CGPoint_3> CGSurfaceMesh;
 typedef CGAL::Polygon_2<ExactKernel> CGPolygon_2;
 typedef CGAL::Point_2<ExactKernel> CGPoint_2;
+typedef CGSurfaceMesh::Face_index CGFaceIdx;
 
 namespace seerep_core
 {
@@ -315,12 +317,18 @@ class CoreDataset
                                   bool& fullEncapsulation,
                                   bool& partialEncapsulation);
 
+  /**
+   *
+   *
+   */
   void checkIntersectionWithZExtrudedPolygon(
-      const SurfaceMesh& enclosedMesh,
+      CGSurfaceMesh enclosedMesh,
       const seerep_core_msgs::Polygon2D& enclosingPolygon,
       bool& fullEncapsulation, bool& partialEncapsulation);
 
-  CGPolygon_2 reduceZDimension(const SurfaceMesh& mesh);
+  CGPolygon_2 reduceZDimension(const CGSurfaceMesh& mesh);
+
+  CGSurfaceMesh toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon);
 
   void getUuidsFromMap(
       std::unordered_map<boost::uuids::uuid, std::vector<boost::uuids::uuid>,

seerep_srv/seerep_core/src/core_dataset.cpp

@@ -312,7 +312,7 @@ std::optional<std::vector<seerep_core_msgs::AabbIdPair>> CoreDataset::queryRtree
     auto ts_frame_points = coreDatatype.getPolygonConstraintPoints(it->second);
     if (ts_frame_points.has_value())
     {
-      SurfaceMesh& mesh = ts_frame_points->mesh;
+      CGSurfaceMesh& mesh = ts_frame_points->mesh;
       std::vector<std::reference_wrapper<CGPoint_3>> ref_points;
 
       for (auto& idx : mesh.vertices())
@@ -1022,21 +1022,26 @@ void CoreDataset::intersectionDegreeAABBinPolygon(
                                  fullEncapsulation, partialEncapsulation);
 }
 
-SurfaceMesh toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon)
+CGSurfaceMesh
+CoreDataset::toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon)
 {
-  using VertexIndex = SurfaceMesh::Vertex_index;
+  using CGVertexIndex = CGSurfaceMesh::Vertex_index;
 
   if (seerep_polygon.vertices.size() <= 2)
   {
     // TODO: throw error
+    BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug)
+        << "seerep_core_msgs::Polygon2D has not enough vertices to be expanded "
+           "to a full 3D SurfaceMesh";
   }
 
-  SurfaceMesh surface_mesh;
+  CGSurfaceMesh surface_mesh;
 
-  std::vector<VertexIndex> lower_surface, upper_surface;
+  std::vector<CGVertexIndex> lower_surface, upper_surface;
+  CGVertexIndex topInitial, bottomInitial;
+  CGVertexIndex topFirst, bottomFirst, topSecond, bottomSecond;
 
-  VertexIndex topInitial, bottomInitial;
-  VertexIndex topFirst, bottomFirst, topSecond, bottomSecond;
+  CGFaceIdx face_desc;
 
   auto z_top = seerep_polygon.z + seerep_polygon.height;
   auto& z_bottom = seerep_polygon.z;
@@ -1048,8 +1053,8 @@ SurfaceMesh toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon)
   bottomInitial = bottomFirst =
       surface_mesh.add_vertex(CGPoint_3{ x, y, z_bottom });
 
-  // the vertices are connected in order; It is assumed that the last point of
-  // the polygon connects to the first
+  // the vertices are connected in order; The assumption is that the last point
+  // of the polygon connects to the first
   for (size_t j = 1; j < verts.size(); ++j)
   {
     x = verts[j].get<0>();
@@ -1058,43 +1063,78 @@ SurfaceMesh toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon)
     bottomSecond = surface_mesh.add_vertex(CGPoint_3{ x, y, z_bottom });
 
     upper_surface.push_back(topFirst);
-    upper_surface.push_back(topSecond);
     lower_surface.push_back(bottomFirst);
-    lower_surface.push_back(bottomSecond);
-    surface_mesh.add_face(topFirst, topSecond, bottomFirst, bottomSecond);
+    face_desc =
+        surface_mesh.add_face(topFirst, topSecond, bottomSecond, bottomFirst);
+
+    if (face_desc == CGSurfaceMesh::null_face())
+    {
+      throw std::invalid_argument(
+          "Could not add side face from Polygon2d to SurfaceMesh correctly!");
+    }
 
     topFirst = topSecond;
     bottomFirst = bottomSecond;
   }
-  surface_mesh.add_face(topFirst, topInitial, bottomFirst, bottomInitial);
-  surface_mesh.add_face(lower_surface);
-  surface_mesh.add_face(upper_surface);
+  upper_surface.push_back(topFirst);
+  lower_surface.push_back(bottomFirst);
+
+  face_desc =
+      surface_mesh.add_face(topFirst, topInitial, bottomInitial, bottomFirst);
+  if (face_desc == CGSurfaceMesh::null_face())
+  {
+    throw std::invalid_argument("Could not add last side face from Polygon2d "
+                                "to SurfaceMesh correctly!");
+  }
+  face_desc = surface_mesh.add_face(lower_surface);
+  if (face_desc == CGSurfaceMesh::null_face())
+  {
+    throw std::invalid_argument("Could not add lower surface face from "
+                                "Polygon2d to SurfaceMesh correctly!");
+  }
+
+  std::reverse(upper_surface.begin(), upper_surface.end());
+
+  face_desc = surface_mesh.add_face(upper_surface);
+  if (face_desc == CGSurfaceMesh::null_face())
+  {
+    throw std::invalid_argument("Could not add upper surface face from "
+                                "Polygon2d to SurfaceMesh correctly!");
+  }
   return surface_mesh;
 }
 
 void CoreDataset::checkIntersectionWithZExtrudedPolygon(
-    const SurfaceMesh& enclosedMesh,
+    CGSurfaceMesh enclosedMesh,
     const seerep_core_msgs::Polygon2D& enclosingPolygon,
     bool& fullEncapsulation, bool& partialEncapsulation)
 {
   using CGAL::Polygon_mesh_processing::triangulate_faces;
 
+  if (!enclosedMesh.is_valid() || enclosedMesh.is_empty())
+  {
+    BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug)
+        << "enclosedMesh passed to checkIntersectionWithZExtrudedPolygon "
+           "is either not valid or empty";
+    return;
+  }
+
   fullEncapsulation = false;
   partialEncapsulation = false;
 
   // check if the enclosing polygon encloses every point
   // get highest and lowest point of innerPoints
   auto highestPoint = std::max_element(
       enclosedMesh.vertices_begin(), enclosedMesh.vertices_end(),
-      [&enclosedMesh](const SurfaceMesh::Vertex_index& p1,
-                      const SurfaceMesh::Vertex_index& p2) {
+      [&enclosedMesh](const CGSurfaceMesh::Vertex_index& p1,
+                      const CGSurfaceMesh::Vertex_index& p2) {
         return enclosedMesh.point(p1).z() < enclosedMesh.point(p2).z();
       });
 
   auto lowestPoint = std::min_element(
       enclosedMesh.vertices_begin(), enclosedMesh.vertices_end(),
-      [&enclosedMesh](const SurfaceMesh::Vertex_index& p1,
-                      const SurfaceMesh::Vertex_index& p2) {
+      [&enclosedMesh](const CGSurfaceMesh::Vertex_index& p1,
+                      const CGSurfaceMesh::Vertex_index& p2) {
         return enclosedMesh.point(p1).z() > enclosedMesh.point(p2).z();
       });
 
@@ -1132,15 +1172,27 @@ void CoreDataset::checkIntersectionWithZExtrudedPolygon(
 
   auto enclosingMesh = toSurfaceMesh(enclosingPolygon);
 
+  if (!enclosingMesh.is_valid() || enclosingMesh.is_empty())
+  {
+    BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug)
+        << "enclosingMesh derived from enclosingPolygon"
+           "passed to checkIntersectionWithZExtrudedPolygon "
+           "is either not valid or empty";
+    return;
+  }
+
+  triangulate_faces(enclosedMesh);
+  triangulate_faces(enclosingMesh);
+
   partialEncapsulation =
       CGAL::Polygon_mesh_processing::do_intersect(enclosedMesh, enclosingMesh);
 }
 
-CGPolygon_2 CoreDataset::reduceZDimension(const SurfaceMesh& mesh)
+CGPolygon_2 CoreDataset::reduceZDimension(const CGSurfaceMesh& mesh)
 {
   CGPolygon_2 poly2;
 
-  for (SurfaceMesh::Vertex_index idx : mesh.vertices())
+  for (CGSurfaceMesh::Vertex_index idx : mesh.vertices())
   {
     CGPoint_2 p{ mesh.point(idx).x(), mesh.point(idx).y() };
     poly2.push_back(p);