diff --git a/seerep_hdf5/seerep_hdf5_core/CMakeLists.txt b/seerep_hdf5/seerep_hdf5_core/CMakeLists.txt
index c3fec951..7a63023d 100644
--- a/seerep_hdf5/seerep_hdf5_core/CMakeLists.txt
+++ b/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
diff --git a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_image.h b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_image.h
index 85567076..be76710d 100644
--- a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_image.h
+++ b/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
diff --git a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp
index 055147ff..0c9667ef 100644
--- a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp
+++ b/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)
 {
diff --git a/seerep_msgs/CMakeLists.txt b/seerep_msgs/CMakeLists.txt
index cdab2578..23e872a9 100644
--- a/seerep_msgs/CMakeLists.txt
+++ b/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
diff --git a/seerep_msgs/core/timestamp_frame_points.h b/seerep_msgs/core/timestamp_frame_points.h
index e28f0072..0db7b0bd 100644
--- a/seerep_msgs/core/timestamp_frame_points.h
+++ b/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
diff --git a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h
index da6a534b..cde1d698 100644
--- a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h
+++ b/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,
diff --git a/seerep_srv/seerep_core/include/seerep_core/core_tf.h b/seerep_srv/seerep_core/include/seerep_core/core_tf.h
index 855b7d9b..f7232ed4 100644
--- a/seerep_srv/seerep_core/include/seerep_core/core_tf.h
+++ b/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
diff --git a/seerep_srv/seerep_core/src/core_dataset.cpp b/seerep_srv/seerep_core/src/core_dataset.cpp
index 4541c14f..f9d73adc 100644
--- a/seerep_srv/seerep_core/src/core_dataset.cpp
+++ b/seerep_srv/seerep_core/src/core_dataset.cpp
@@ -312,15 +312,28 @@ std::optional<std::vector<seerep_core_msgs::AabbIdPair>> CoreDataset::queryRtree
     auto ts_frame_points = coreDatatype.getPolygonConstraintPoints(it->second);
     if (ts_frame_points.has_value())
     {
-      auto points_vec = std::vector<seerep_core_msgs::Point>(
-          ts_frame_points->points.begin(), ts_frame_points->points.end());
-      // transform frame from the camera coordinate system to the map frame
-      points_vec =
+      SurfaceMesh& mesh = ts_frame_points->mesh;
+      std::vector<std::reference_wrapper<CGPoint_3>> ref_points;
+
+      for (auto& idx : mesh.vertices())
+      {
+        ref_points.push_back(mesh.point(idx));
+      }
+
+      // transform points from another coordinate system to the map frame of the project
+      auto points_vec =
           m_tfOverview->transform(ts_frame_points->frame_id, m_frameId,
                                   ts_frame_points->timestamp.seconds,
-                                  ts_frame_points->timestamp.nanos, points_vec);
+                                  ts_frame_points->timestamp.nanos, ref_points);
+
+      for (auto& idx : mesh.vertices())
+      {
+        mesh.point(idx) = ref_points[idx.idx()];
+      }
 
       // check if these point are enclosed by the query polygon
+      checkIntersectionWithZExtrudedPolygon(mesh, polygon, fullyEncapsulated,
+                                            partiallyEncapsulated);
     }
 
     // if there is no intersection between the result and the user's
@@ -836,7 +849,8 @@ void CoreDataset::addLabels(
   datatypeSpecifics->datasetInstancesMap.emplace(msgUuid, instanceUuids);
 }
 
-bool CoreDataset::verifyPolygonIntegrity(CGAL::Polygon_2<Kernel>& polygon_cgal)
+bool CoreDataset::verifyPolygonIntegrity(
+    CGAL::Polygon_2<ExactKernel>& polygon_cgal)
 {
   if (!polygon_cgal.is_simple())
   {
@@ -860,44 +874,43 @@ bool CoreDataset::verifyPolygonIntegrity(CGAL::Polygon_2<Kernel>& polygon_cgal)
   return true;
 }
 
-CGAL::Polygon_2<Kernel>
+CGAL::Polygon_2<ExactKernel>
 CoreDataset::toCGALPolygon(const seerep_core_msgs::Polygon2D& polygon)
 {
-  CGAL::Polygon_2<Kernel> polygon_cgal;
+  CGAL::Polygon_2<ExactKernel> polygon_cgal;
   for (auto point : polygon.vertices)
   {
     polygon_cgal.push_back(
-        Kernel::Point_2(bg::get<0>(point), bg::get<1>(point)));
+        ExactKernel::Point_2(bg::get<0>(point), bg::get<1>(point)));
   }
-
   return polygon_cgal;
 }
 
-CGAL::Polygon_2<Kernel>
+CGAL::Polygon_2<ExactKernel>
 CoreDataset::toCGALPolygon(const seerep_core_msgs::AABB& aabb)
 {
   /* Check if the bounding box has no spatial extent -> Only add one point to the polygon */
   if (bg::get<bg::min_corner, 0>(aabb) == bg::get<bg::max_corner, 0>(aabb) &&
       bg::get<bg::min_corner, 1>(aabb) == bg::get<bg::max_corner, 1>(aabb))
   {
-    Kernel::Point_2 points_aabb[] = { Kernel::Point_2(
+    ExactKernel::Point_2 points_aabb[] = { ExactKernel::Point_2(
         bg::get<bg::min_corner, 0>(aabb), bg::get<bg::min_corner, 1>(aabb)) };
-    CGAL::Polygon_2<Kernel> aabb_cgal(points_aabb, points_aabb + 1);
+    CGAL::Polygon_2<ExactKernel> aabb_cgal(points_aabb, points_aabb + 1);
     return aabb_cgal;
   }
   else
   {
-    Kernel::Point_2 points_aabb[] = {
-      Kernel::Point_2(bg::get<bg::min_corner, 0>(aabb),
-                      bg::get<bg::min_corner, 1>(aabb)),
-      Kernel::Point_2(bg::get<bg::max_corner, 0>(aabb),
-                      bg::get<bg::min_corner, 1>(aabb)),
-      Kernel::Point_2(bg::get<bg::max_corner, 0>(aabb),
-                      bg::get<bg::max_corner, 1>(aabb)),
-      Kernel::Point_2(bg::get<bg::min_corner, 0>(aabb),
-                      bg::get<bg::max_corner, 1>(aabb)),
+    ExactKernel::Point_2 points_aabb[] = {
+      ExactKernel::Point_2(bg::get<bg::min_corner, 0>(aabb),
+                           bg::get<bg::min_corner, 1>(aabb)),
+      ExactKernel::Point_2(bg::get<bg::max_corner, 0>(aabb),
+                           bg::get<bg::min_corner, 1>(aabb)),
+      ExactKernel::Point_2(bg::get<bg::max_corner, 0>(aabb),
+                           bg::get<bg::max_corner, 1>(aabb)),
+      ExactKernel::Point_2(bg::get<bg::min_corner, 0>(aabb),
+                           bg::get<bg::max_corner, 1>(aabb)),
     };
-    CGAL::Polygon_2<Kernel> aabb_cgal(points_aabb, points_aabb + 4);
+    CGAL::Polygon_2<ExactKernel> aabb_cgal(points_aabb, points_aabb + 4);
 
     return aabb_cgal;
   }
@@ -909,10 +922,10 @@ void CoreDataset::intersectionDegree(const seerep_core_msgs::AABB& aabb,
                                      bool& partialEncapsulation)
 {
   // convert seerep core aabb to cgal polygon
-  CGAL::Polygon_2<Kernel> aabb_cgal = toCGALPolygon(aabb);
+  CGAL::Polygon_2<ExactKernel> aabb_cgal = toCGALPolygon(aabb);
 
   // convert seerep core polygon to cgal polygon
-  CGAL::Polygon_2<Kernel> polygon_cgal = toCGALPolygon(polygon);
+  CGAL::Polygon_2<ExactKernel> polygon_cgal = toCGALPolygon(polygon);
 
   // a cgal polyon needs to be simple, convex and the points should be added
   // in a counter clockwise order
@@ -926,14 +939,12 @@ void CoreDataset::intersectionDegree(const seerep_core_msgs::AABB& aabb,
                                   fullEncapsulation, partialEncapsulation);
 }
 
-void CoreDataset::intersectionDegreeCgalPolygons(CGAL::Polygon_2<Kernel> cgal1,
-                                                 CGAL::Polygon_2<Kernel> cgal2,
-                                                 bool z_partially,
-                                                 bool checkIfFullyEncapsulated,
-                                                 bool& fullEncapsulation,
-                                                 bool& partialEncapsulation)
+void CoreDataset::intersectionDegreeCgalPolygons(
+    CGAL::Polygon_2<ExactKernel> cgal1, CGAL::Polygon_2<ExactKernel> cgal2,
+    bool z_partially, bool checkIfFullyEncapsulated, bool& fullEncapsulation,
+    bool& partialEncapsulation)
 {
-  for (CGAL::Polygon_2<Kernel>::Vertex_iterator vi = cgal1.vertices_begin();
+  for (CGAL::Polygon_2<ExactKernel>::Vertex_iterator vi = cgal1.vertices_begin();
        vi != cgal1.vertices_end(); ++vi)
   {
     // is the vertex of the aabb inside or on the boundary (not outside) the
@@ -955,8 +966,9 @@ void CoreDataset::intersectionDegreeCgalPolygons(CGAL::Polygon_2<Kernel> cgal1,
 void CoreDataset::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,
-    bool& fullEncapsulation, bool& partialEncapsulation)
+    CGAL::Polygon_2<ExactKernel> aabb_cgal,
+    CGAL::Polygon_2<ExactKernel> polygon_cgal, bool& fullEncapsulation,
+    bool& partialEncapsulation)
 {
   fullEncapsulation = true;
   partialEncapsulation = false;
@@ -1010,6 +1022,132 @@ void CoreDataset::intersectionDegreeAABBinPolygon(
                                  fullEncapsulation, partialEncapsulation);
 }
 
+SurfaceMesh toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon)
+{
+  using VertexIndex = SurfaceMesh::Vertex_index;
+
+  if (seerep_polygon.vertices.size() <= 2)
+  {
+    // TODO: throw error
+  }
+
+  SurfaceMesh surface_mesh;
+
+  std::vector<VertexIndex> lower_surface, upper_surface;
+
+  VertexIndex topInitial, bottomInitial;
+  VertexIndex topFirst, bottomFirst, topSecond, bottomSecond;
+
+  auto z_top = seerep_polygon.z + seerep_polygon.height;
+  auto& z_bottom = seerep_polygon.z;
+  auto& verts = seerep_polygon.vertices;
+
+  auto x = verts[0].get<0>();
+  auto y = verts[0].get<1>();
+  topInitial = topFirst = surface_mesh.add_vertex(CGPoint_3{ x, y, z_top });
+  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
+  for (size_t j = 1; j < verts.size(); ++j)
+  {
+    x = verts[j].get<0>();
+    y = verts[j].get<1>();
+    topSecond = surface_mesh.add_vertex(CGPoint_3{ x, y, z_top });
+    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);
+
+    topFirst = topSecond;
+    bottomFirst = bottomSecond;
+  }
+  surface_mesh.add_face(topFirst, topInitial, bottomFirst, bottomInitial);
+  surface_mesh.add_face(lower_surface);
+  surface_mesh.add_face(upper_surface);
+  return surface_mesh;
+}
+
+void CoreDataset::checkIntersectionWithZExtrudedPolygon(
+    const SurfaceMesh& enclosedMesh,
+    const seerep_core_msgs::Polygon2D& enclosingPolygon,
+    bool& fullEncapsulation, bool& partialEncapsulation)
+{
+  using CGAL::Polygon_mesh_processing::triangulate_faces;
+
+  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) {
+        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) {
+        return enclosedMesh.point(p1).z() > enclosedMesh.point(p2).z();
+      });
+
+  const double& z_low = enclosingPolygon.z;
+  double z_high = z_low + enclosingPolygon.height;
+
+  double p_low =
+      enclosedMesh.point(*lowestPoint).z().exact().convert_to<double>();
+  double p_high =
+      enclosedMesh.point(*highestPoint).z().exact().convert_to<double>();
+
+  // fully enclosed if all points of the enclosedMesh are contained in the enclosing Polygon
+  if ((z_low <= p_low && z_low <= p_high && z_high >= p_low && z_high >= p_high))
+  {
+    auto enclosedPolygon2CG = reduceZDimension(enclosedMesh);
+    auto enclosingPolygon2CG = toCGALPolygon(enclosingPolygon);
+
+    bool xy_bounded = true;
+    for (CGPolygon_2::Vertex_iterator vi = enclosedPolygon2CG.vertices_begin();
+         vi != enclosedPolygon2CG.vertices_end(); ++vi)
+    {
+      if (enclosingPolygon2CG.bounded_side(*vi) == CGAL::ON_UNBOUNDED_SIDE)
+      {
+        xy_bounded = false;
+        break;
+      }
+    }
+    if (xy_bounded)
+    {
+      fullEncapsulation = true;
+      partialEncapsulation = true;
+      return;
+    }
+  }
+
+  auto enclosingMesh = toSurfaceMesh(enclosingPolygon);
+
+  partialEncapsulation =
+      CGAL::Polygon_mesh_processing::do_intersect(enclosedMesh, enclosingMesh);
+}
+
+CGPolygon_2 CoreDataset::reduceZDimension(const SurfaceMesh& mesh)
+{
+  CGPolygon_2 poly2;
+
+  for (SurfaceMesh::Vertex_index idx : mesh.vertices())
+  {
+    CGPoint_2 p{ mesh.point(idx).x(), mesh.point(idx).y() };
+    poly2.push_back(p);
+  }
+  return poly2;
+}
+
 seerep_core_msgs::AabbTime
 CoreDataset::getTimeBounds(std::vector<seerep_core_msgs::Datatype> datatypes)
 {
diff --git a/seerep_srv/seerep_core/src/core_tf.cpp b/seerep_srv/seerep_core/src/core_tf.cpp
index 76b91612..d9f7ca9c 100644
--- a/seerep_srv/seerep_core/src/core_tf.cpp
+++ b/seerep_srv/seerep_core/src/core_tf.cpp
@@ -130,13 +130,13 @@ bool CoreTf::canTransform(const std::string& sourceFrame,
                                  ros::Time(timeSecs, timeNanos));
 }
 
-std::vector<seerep_core_msgs::Point>
+std::vector<CGPoint_3>
 CoreTf::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)
 {
-  std::vector<seerep_core_msgs::Point> transformed_points;
+  std::vector<CGPoint_3> transformed_points;
   if (this->canTransform(sourceFrame, targetFrame, timeSecs, timeNanos))
   {
     auto tf =
@@ -151,12 +151,14 @@ CoreTf::transform(const std::string& sourceFrame,
 
     for (auto&& p : points)
     {
-      tf2::Vector3 vec{ p.get<0>(), p.get<1>(), p.get<2>() };
+      tf2::Vector3 vec{ p.get().x().exact().convert_to<double>(),
+                        p.get().y().exact().convert_to<double>(),
+                        p.get().z().exact().convert_to<double>() };
+
       tf2::Vector3 vec_target = tf2_tf * vec;
 
-      seerep_core_msgs::Point p_target{ static_cast<float>(vec_target.getX()),
-                                        static_cast<float>(vec_target.getY()),
-                                        static_cast<float>(vec_target.getZ()) };
+      CGPoint_3 p_target{ vec_target.getX(), vec_target.getY(),
+                          vec_target.getZ() };
 
       transformed_points.push_back(p_target);
     }