only perform precise check when needed; remove unneeded functionality

seerep_srv/seerep_core/include/seerep_core/core_dataset.h

@@ -338,22 +338,20 @@ class CoreDataset
                                   bool& partialEncapsulation);
 
   /**
-   * @brief checks whether the enclosedMesh is really enclosed by the
+   * @brief checks whether the enclosedMesh is really partially enclosed by the
    * enclosingPolygon
    *
    * Both the enclosedMesh and enclosingPolygon have to be convex objects
    *
    * @param enclosedMesh mesh to check whether it is enclosed by the polygon
    * @param enclosingPolygon polygon which is enclosing object
-   * @param fullEncapsulation flag which is set when enclosedMesh is fully
-   * encapsulated by the enclosingPolygon
    * @param partialEncapsulation flag which is set when enclosedMesh is
-   * partially encapsulated by the enclosingPolygon
+   *   partially encapsulated by the enclosingPolygon
    */
-  void checkIntersectionWithZExtrudedPolygon(
+  void checkPartialIntersectionWithZExtrudedPolygon(
       CGSurfaceMesh enclosedMesh,
       const seerep_core_msgs::Polygon2D& enclosingPolygon,
-      bool& fullEncapsulation, bool& partialEncapsulation);
+      bool& partialEncapsulation);
 
   /**
    * @brief Creates a 2DPolygon by removing the 3rd dimension from the points of the Mesh

seerep_srv/seerep_core/src/core_dataset.cpp

@@ -309,31 +309,43 @@ std::optional<std::vector<seerep_core_msgs::AabbIdPair>> CoreDataset::queryRtree
     intersectionDegree(it->first, polygon, fullyEncapsulated,
                        partiallyEncapsulated);
 
-    auto ts_frame_points = coreDatatype.getPolygonConstraintMesh(it->second);
-    if (ts_frame_points.has_value())
+    // precise check
+    // this is only needed when fullyEncapsulated = false and
+    // partiallyEncapsulated = true as the aabb approximation through
+    // intersectionDegree is a upper bound for the real 3D object if
+    // fullyEncapsulated = true => the real object is inside the approx and thus
+    // inside the query if both are false using the approx for the real object
+    // the values must be false aswell only true for partialEncapsulation must
+    // be verified, as this could be indeed false with the real object
+    if (partiallyEncapsulated == true && fullyEncapsulated == false)
     {
-      CGSurfaceMesh& mesh = ts_frame_points->mesh;
-      std::vector<std::reference_wrapper<CGPoint_3>> ref_points;
-
-      for (auto& idx : mesh.vertices())
+      auto ts_frame_points = coreDatatype.getPolygonConstraintMesh(it->second);
+      if (ts_frame_points.has_value())
       {
-        ref_points.push_back(mesh.point(idx));
-      }
+        CGSurfaceMesh& mesh = ts_frame_points->mesh;
+        std::vector<std::reference_wrapper<CGPoint_3>> ref_points;
 
-      // 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, ref_points);
+        for (auto& idx : mesh.vertices())
+        {
+          ref_points.push_back(mesh.point(idx));
+        }
 
-      for (auto& idx : mesh.vertices())
-      {
-        mesh.point(idx) = points_vec[idx.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,
+                                    ref_points);
 
-      // check if these point are enclosed by the query polygon
-      checkIntersectionWithZExtrudedPolygon(mesh, polygon, fullyEncapsulated,
-                                            partiallyEncapsulated);
+        for (auto& idx : mesh.vertices())
+        {
+          mesh.point(idx) = points_vec[idx.idx()];
+        }
+
+        // check if these point are enclosed by the query polygon
+        checkPartialIntersectionWithZExtrudedPolygon(mesh, polygon,
+                                                     partiallyEncapsulated);
+      }
     }
 
     // if there is no intersection between the result and the user's
@@ -1103,80 +1115,29 @@ CoreDataset::toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon)
   return surface_mesh;
 }
 
-void CoreDataset::checkIntersectionWithZExtrudedPolygon(
+void CoreDataset::checkPartialIntersectionWithZExtrudedPolygon(
     CGSurfaceMesh enclosedMesh,
     const seerep_core_msgs::Polygon2D& enclosingPolygon,
-    bool& fullEncapsulation, bool& partialEncapsulation)
+    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";
+           "is either not valid or empty. Skipping precise check...";
     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 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 CGSurfaceMesh::Vertex_index& p1,
-                      const CGSurfaceMesh::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);
 
   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";
+           "is either not valid or empty. Skipping precise check...";
     return;
   }