add mesh in polygon check for image frustum

seerep_hdf5/seerep_hdf5_core/CMakeLists.txt

@@ -21,6 +21,9 @@ find_package(
   REQUIRED
 )
 
+find_package(CGAL REQUIRED)
+set(CGAL_DO_NOT_WARN_ABOUT_CMAKE_BUILD_TYPE TRUE)
+
 enable_testing()
 find_package(GTest 1.12.0 REQUIRED)
 
@@ -59,6 +62,7 @@ target_link_libraries(
   ${Boost_LOG_LIBRARY}
   ${Boost_LOG_SETUP_LIBRARY}
   ${catkin_LIBRARIES}
+  CGAL::CGAL
 )
 
 # create executable with tests

seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_image.h

@@ -27,6 +27,11 @@
 
 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>;
+
 /**
  * @brief Helper struct to summarize the general attributes of an image
  *
@@ -156,11 +161,13 @@ class Hdf5CoreImage : public virtual Hdf5CoreGeneral,
    *  2. far plane top left
    *  3. far plane top right
    *  4. far plane bottom left
-   *  5. far plane bottom rigth
+   *  5. far plane bottom right
    */
   std::array<seerep_core_msgs::Point, 5>
   computeFrustumPoints(const std::string& camintrinsics_uuid);
 
+  SurfaceMesh computeFrustumMesh(std::array<seerep_core_msgs::Point, 5>& points);
+
   /**
    * @brief Computes the frustum for given camera intrinsic parameters and stores
    * the result in an axis aligned bounding box

seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp

@@ -71,38 +71,41 @@ std::optional<seerep_core_msgs::TimestampFramePoints>
 Hdf5CoreImage::getPolygonConstraintPoints(
     std::optional<boost::uuids::uuid> uuid_entry)
 {
-  if (uuid_entry.has_value())
+  if (!uuid_entry.has_value())
   {
-    std::string hdf5DataGroupPath =
-        getHdf5GroupPath(boost::uuids::to_string(uuid_entry.value()));
+    // TODO throw exception
+    return std::nullopt;
+  }
+  std::string hdf5DataGroupPath =
+      getHdf5GroupPath(boost::uuids::to_string(uuid_entry.value()));
 
-    auto dataGroupPtr = getHdf5Group(hdf5DataGroupPath);
+  auto dataGroupPtr = getHdf5Group(hdf5DataGroupPath);
 
-    // fetch the camera_intrinsics directly
-    auto camintrinsics_uuid = readAttributeFromHdf5<std::string>(
-        *dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::CAMERA_INTRINSICS_UUID,
-        hdf5DataGroupPath);
+  // fetch the camera_intrinsics directly
+  auto camintrinsics_uuid = readAttributeFromHdf5<std::string>(
+      *dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::CAMERA_INTRINSICS_UUID,
+      hdf5DataGroupPath);
 
-    auto frame_id = readAttributeFromHdf5<std::string>(
-        *dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::HEADER_FRAME_ID,
-        hdf5DataGroupPath);
+  auto frame_id = readAttributeFromHdf5<std::string>(
+      *dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::HEADER_FRAME_ID,
+      hdf5DataGroupPath);
 
-    // retrieve timestamp from image
-    auto stamp_seconds = readAttributeFromHdf5<int>(
-        *dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::HEADER_STAMP_SECONDS,
-        hdf5DataGroupPath);
+  // retrieve timestamp from image
+  auto stamp_seconds = readAttributeFromHdf5<int>(
+      *dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::HEADER_STAMP_SECONDS,
+      hdf5DataGroupPath);
 
-    auto stamp_nanos = readAttributeFromHdf5<int>(
-        *dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::HEADER_STAMP_NANOS,
-        hdf5DataGroupPath);
+  auto stamp_nanos = readAttributeFromHdf5<int>(
+      *dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::HEADER_STAMP_NANOS,
+      hdf5DataGroupPath);
 
-    seerep_core_msgs::Timestamp ts{ stamp_seconds,
-                                    static_cast<uint32_t>(stamp_nanos) };
+  seerep_core_msgs::Timestamp ts{ stamp_seconds,
+                                  static_cast<uint32_t>(stamp_nanos) };
 
-    auto points = this->computeFrustumPoints(camintrinsics_uuid);
+  auto points = this->computeFrustumPoints(camintrinsics_uuid);
+  auto mesh = this->computeFrustumMesh(points);
 
-    return seerep_core_msgs::TimestampFramePoints{ ts, points, frame_id };
-  }
+  return seerep_core_msgs::TimestampFramePoints{ ts, SurfaceMesh{}, frame_id };
 }
 
 void Hdf5CoreImage::writeLabels(
@@ -173,20 +176,54 @@ Hdf5CoreImage::computeFrustumPoints(const std::string& camintrinsics_uuid)
   double far_fov_y = (far_plane_dist * ci.width) / ci.intrinsic_matrix[4];
 
   seerep_core_msgs::Point near_p{ 0, 0, 0 };
-  seerep_core_msgs::Point far_topleft{ far_fov_x / 2, far_fov_y / 2,
-                                       far_plane_dist };
-  seerep_core_msgs::Point far_topright{ -far_fov_x / 2, far_fov_y / 2,
-                                        far_plane_dist };
-  seerep_core_msgs::Point far_bottomleft{ far_fov_x / 2, -far_fov_y / 2,
-                                          far_plane_dist };
-  seerep_core_msgs::Point far_bottomright{ -far_fov_x / 2, -far_fov_y / 2,
-                                           far_plane_dist };
+  seerep_core_msgs::Point far_topleft{ static_cast<float>(far_fov_x / 2),
+                                       static_cast<float>(far_fov_y / 2),
+                                       static_cast<float>(far_plane_dist) };
+
+  seerep_core_msgs::Point far_topright{ static_cast<float>(-far_fov_x / 2),
+                                        static_cast<float>(far_fov_y / 2),
+                                        static_cast<float>(far_plane_dist) };
+
+  seerep_core_msgs::Point far_bottomleft{ static_cast<float>(far_fov_x / 2),
+                                          static_cast<float>(-far_fov_y / 2),
+                                          static_cast<float>(far_plane_dist) };
+
+  seerep_core_msgs::Point far_bottomright{ static_cast<float>(-far_fov_x / 2),
+                                           static_cast<float>(-far_fov_y / 2),
+                                           static_cast<float>(far_plane_dist) };
 
   return std::array<seerep_core_msgs::Point, 5>{ near_p, far_topleft,
                                                  far_topright, far_bottomleft,
                                                  far_bottomright };
 }
 
+SurfaceMesh
+Hdf5CoreImage::computeFrustumMesh(std::array<seerep_core_msgs::Point, 5>& points)
+{
+  SurfaceMesh mesh;
+  auto o = mesh.add_vertex(
+      CGPoint_3{ points[0].get<0>(), points[0].get<1>(), points[0].get<2>() });
+
+  auto tl = mesh.add_vertex(
+      CGPoint_3{ points[1].get<0>(), points[1].get<1>(), points[1].get<2>() });
+
+  auto tr = mesh.add_vertex(
+      CGPoint_3{ points[2].get<0>(), points[2].get<1>(), points[2].get<2>() });
+
+  auto bl = mesh.add_vertex(
+      CGPoint_3{ points[3].get<0>(), points[3].get<1>(), points[3].get<2>() });
+
+  auto br = mesh.add_vertex(
+      CGPoint_3{ points[4].get<0>(), points[4].get<1>(), points[4].get<2>() });
+
+  mesh.add_face(o, tl, tr);
+  mesh.add_face(o, tr, br);
+  mesh.add_face(o, br, bl);
+  mesh.add_face(o, bl, tl);
+  mesh.add_face(tl, tr, br, bl);
+  return mesh;
+}
+
 void Hdf5CoreImage::computeFrustumBB(const std::string& camintrinsics_uuid,
                                      seerep_core_msgs::AABB& bb)
 {

seerep_msgs/CMakeLists.txt

@@ -14,6 +14,9 @@ set(Boost_USE_MULTITHREADED ON)
 set(Boost_USE_STATIC_RUNTIME OFF)
 find_package(Boost REQUIRED)
 
+find_package(CGAL REQUIRED)
+set(CGAL_DO_NOT_WARN_ABOUT_CMAKE_BUILD_TYPE TRUE)
+
 set(MY_PROTO_FILES
     protos/boundingbox.proto
     protos/camera_intrinsics.proto

seerep_msgs/core/timestamp_frame_points.h

@@ -1,15 +1,22 @@
 #ifndef SEEREP_CORE_MSGS_TIMESTAMP_FRAME_POINTS_H_
 #define SEEREP_CORE_MSGS_TIMESTAMP_FRAME_POINTS_H_
 
-#include "aabb.h"
+#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
+#include <CGAL/Surface_mesh.h>
+
 #include "timestamp.h"
 
 namespace seerep_core_msgs
 {
+
+using ExactKernel = CGAL::Exact_predicates_exact_constructions_kernel;
+using CGPoint_3 = CGAL::Point_3<ExactKernel>;
+using SurfaceMesh = CGAL::Surface_mesh<ExactKernel::Point_3>;
+
 struct TimestampFramePoints
 {
   seerep_core_msgs::Timestamp timestamp;
-  std::array<seerep_core_msgs::Point, 5> points;
+  SurfaceMesh mesh;
   std::string frame_id;
 };
 }  // namespace seerep_core_msgs

seerep_srv/seerep_core/include/seerep_core/core_dataset.h

@@ -3,6 +3,8 @@
 
 #include <CGAL/Boolean_set_operations_2.h>
 #include <CGAL/Exact_predicates_exact_constructions_kernel.h>
+#include <CGAL/Surface_mesh.h>
+#include <CGAL/polygon_mesh_processing.h>
 
 #include <algorithm>
 #include <cstdint>
@@ -29,7 +31,11 @@
 #include <boost/uuid/uuid_generators.hpp>  // generators
 #include <boost/uuid/uuid_io.hpp>          // streaming operators etc.
 
-typedef CGAL::Exact_predicates_exact_constructions_kernel Kernel;
+typedef CGAL::Exact_predicates_exact_constructions_kernel ExactKernel;
+typedef CGAL::Point_3<ExactKernel> CGPoint_3;
+typedef CGAL::Surface_mesh<CGPoint_3> SurfaceMesh;
+typedef CGAL::Polygon_2<ExactKernel> CGPolygon_2;
+typedef CGAL::Point_2<ExactKernel> CGPoint_2;
 
 namespace seerep_core
 {
@@ -250,7 +256,7 @@ class CoreDataset
    * @return true The polygon abides by CGAL requirements
    * @return false The polygon does not abide by CGAL requirements
    */
-  bool verifyPolygonIntegrity(CGAL::Polygon_2<Kernel>& polygon_cgal);
+  bool verifyPolygonIntegrity(CGAL::Polygon_2<ExactKernel>& polygon_cgal);
 
   /**
    * @brief transforms the bounding box to the datasets frameId (mostly the map
@@ -268,7 +274,7 @@ class CoreDataset
    * @param polygon core msg polygon
    * @return CGAL::Polygon_2<Kernel> cgal polygon
    */
-  CGAL::Polygon_2<Kernel>
+  CGAL::Polygon_2<ExactKernel>
   toCGALPolygon(const seerep_core_msgs::Polygon2D& polygon);
 
   /**
@@ -277,7 +283,7 @@ class CoreDataset
    * @param polygon core msg aabb
    * @return CGAL::Polygon_2<Kernel> cgal aabb
    */
-  CGAL::Polygon_2<Kernel> toCGALPolygon(const seerep_core_msgs::AABB& aabb);
+  CGAL::Polygon_2<ExactKernel> toCGALPolygon(const seerep_core_msgs::AABB& aabb);
 
   /**
    * @brief determine if the axis aligned bounding box is fully or paritally
@@ -294,19 +300,28 @@ class CoreDataset
                           const seerep_core_msgs::Polygon2D& polygon,
                           bool& fullEncapsulation, bool& partialEncapsulation);
 
-  void intersectionDegreeCgalPolygons(CGAL::Polygon_2<Kernel> cgal1,
-                                      CGAL::Polygon_2<Kernel> cgal2,
+  void intersectionDegreeCgalPolygons(CGAL::Polygon_2<ExactKernel> cgal1,
+                                      CGAL::Polygon_2<ExactKernel> cgal2,
                                       bool z_partially,
                                       bool checkIfFullyEncapsulated,
                                       bool& fullEncapsulation,
                                       bool& partialEncapsulation);
 
-  void intersectionDegreeAABBinPolygon(
-      const seerep_core_msgs::AABB& aabb,
-      const seerep_core_msgs::Polygon2D& polygon,
-      CGAL::Polygon_2<Kernel> aabb_cgal, CGAL::Polygon_2<Kernel> polygon_cgal,
+  void
+  intersectionDegreeAABBinPolygon(const seerep_core_msgs::AABB& aabb,
+                                  const seerep_core_msgs::Polygon2D& polygon,
+                                  CGAL::Polygon_2<ExactKernel> aabb_cgal,
+                                  CGAL::Polygon_2<ExactKernel> polygon_cgal,
+                                  bool& fullEncapsulation,
+                                  bool& partialEncapsulation);
+
+  void checkIntersectionWithZExtrudedPolygon(
+      const SurfaceMesh& enclosedMesh,
+      const seerep_core_msgs::Polygon2D& enclosingPolygon,
       bool& fullEncapsulation, bool& partialEncapsulation);
 
+  CGPolygon_2 reduceZDimension(const SurfaceMesh& mesh);
+
   void getUuidsFromMap(
       std::unordered_map<boost::uuids::uuid, std::vector<boost::uuids::uuid>,
                          boost::hash<boost::uuids::uuid>>& datasetInstancesMap,

seerep_srv/seerep_core/include/seerep_core/core_tf.h

@@ -22,12 +22,19 @@
 #include <tf2/buffer_core.h>
 #include <tf2/transform_datatypes.h>
 
+// CGAL
+#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
+
 // logging
 #include <boost/log/sources/severity_logger.hpp>
 #include <boost/log/trivial.hpp>
 
 namespace seerep_core
 {
+
+using ExactKernel = CGAL::Exact_predicates_exact_constructions_kernel;
+using CGPoint_3 = CGAL::Point_3<ExactKernel>;
+
 /**
  * @brief This is the class handling the TF buffer
  *
@@ -100,10 +107,10 @@ class CoreTf
    *
    * @return the transformed points
    */
-  std::vector<seerep_core_msgs::Point>
+  std::vector<CGPoint_3>
   transform(const std::string& sourceFrame, const std::string& targetFrame,
             const int64_t& timeSecs, const int64_t& timeNanos,
-            const std::vector<seerep_core_msgs::Point>& points);
+            const std::vector<std::reference_wrapper<CGPoint_3>>& points);
 
   /**
    * @brief Returns a vector of all frames stored in the TF tree by the TF buffer