Add more precise spatial check for the query

examples/python/gRPC/images/gRPC_fb_sendImages.py

@@ -1,3 +1,4 @@
+#!/usr/bin/env python3
 import random
 import time
 import uuid

pyproject.toml

@@ -19,14 +19,17 @@ dependencies = [
 ]
 dynamic = ["version"]
 
-[tool.setuptools_scm]
-version_scheme = "no-guess-dev"
-local_scheme = "no-local-version"
-
 [project.urls]
 Repository = "https://github.com/agri-gaia/seerep"
 Documentation = "https://agri-gaia.github.io/seerep/mkdocs/home/index.html"
 
+[tool.basedpyright]
+typeCheckingMode="standard"
+
+[tool.setuptools_scm]
+version_scheme = "no-guess-dev"
+local_scheme = "no-local-version"
+
 [tool.pytest.ini_options]
 minversion = "6.0"
 addopts = "-ra -q"

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_datatype_interface.h

@@ -3,13 +3,15 @@
 
 // seerep-msgs
 #include <seerep_msgs/dataset_indexable.h>
+#include <seerep_msgs/timestamp_frame_mesh.h>
 
 // std
 #include <boost/uuid/uuid.hpp>
 #include <optional>
 
 namespace seerep_hdf5_core
 {
+
 class Hdf5CoreDatatypeInterface
 {
 public:
@@ -19,6 +21,20 @@ class Hdf5CoreDatatypeInterface
   readDataset(const std::string& uuid) = 0;
 
   virtual std::vector<std::string> getDatasetUuids() = 0;
+
+  /**
+   * @brief a option to pass points down to check for spatially, the first
+   * return value are the points which should get checked, the second in which
+   * frame they should be checked
+   *
+   * @param uuid_entry optionally specify the uuid of a specific data entry to
+   * perform additional operations
+   *
+   * @return the points which should get checked (if empty this should be a
+   * noop) and the frame_id in which they need to get transformed from
+   */
+  virtual std::optional<seerep_core_msgs::TimestampFrameMesh>
+  getPolygonConstraintMesh(const boost::uuids::uuid& uuid_entry) = 0;
 };
 
 }  // namespace seerep_hdf5_core

seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_image.h

@@ -4,6 +4,10 @@
 // highfive
 #include <highfive/H5File.hpp>
 
+// CGAL
+#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
+#include <CGAL/Surface_mesh.h>
+
 // seerep_hdf5_core
 #include "hdf5_core_cameraintrinsics.h"
 #include "hdf5_core_datatype_interface.h"
@@ -12,6 +16,7 @@
 // seerep_msgs
 #include <seerep_msgs/dataset_indexable.h>
 #include <seerep_msgs/label_category.h>
+#include <seerep_msgs/timestamp_frame_mesh.h>
 
 // std
 #include <boost/geometry.hpp>
@@ -26,6 +31,11 @@
 
 namespace seerep_hdf5_core
 {
+
+using ExactKernel = CGAL::Exact_predicates_exact_constructions_kernel;
+using CGPoint_3 = ExactKernel::Point_3;
+using CGSurfaceMesh = CGAL::Surface_mesh<CGPoint_3>;
+
 /**
  * @brief Helper struct to summarize the general attributes of an image
  *
@@ -90,6 +100,19 @@ class Hdf5CoreImage : public virtual Hdf5CoreGeneral,
    */
   std::vector<std::string> getDatasetUuids();
 
+  /**
+   * @brief Get a timestamp-frameid-mesh struct which will be subject to
+   *  encapsulation checks of a query polygon
+   *
+   * The mesh is created from the camera frustum.
+   *
+   * @param uuid_entry of the image datatype entry to fetch the data from
+   *
+   * @return a struct containing the timestamp, frame_id of the data and the mesh itself
+   */
+  std::optional<seerep_core_msgs::TimestampFrameMesh>
+  getPolygonConstraintMesh(const boost::uuids::uuid& uuid_entry);
+
   /**
    * @brief Write generals labels based on C++ data structures to HdF5
    *
@@ -133,6 +156,37 @@ class Hdf5CoreImage : public virtual Hdf5CoreGeneral,
    */
   const std::string getHdf5DataSetPath(const std::string& id) const;
 
+  /**
+   * @brief Compute the frustum corner points from the camera_intrinsics uuid
+   *
+   * @param cameraintrinsics_uuid to fetch the cameraintrinsics,
+   * from which the frustum should be based on
+   *
+   * @return the frustum corner points ordered as follows:
+   *  1. near plane camera origin point (0,0,0)
+   *  2. far plane top left
+   *  3. far plane top right
+   *  4. far plane bottom left
+   *  5. far plane bottom right
+   */
+  std::array<seerep_core_msgs::Point, 5>
+  computeFrustumPoints(const std::string& camintrinsics_uuid);
+
+  /**
+   * @brief create a surface mesh from given frustum points
+   *
+   * @param points frustum corner points ordered as follows:
+   *  1. near plane camera origin point (0,0,0)
+   *  2. far plane top left
+   *  3. far plane top right
+   *  4. far plane bottom left
+   *  5. far plane bottom right
+   *
+   * @return the created SurfaceMesh
+   */
+  CGSurfaceMesh
+  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/include/seerep_hdf5_core/hdf5_core_point.h

@@ -10,6 +10,7 @@
 
 // seerep_msgs
 #include <seerep_msgs/dataset_indexable.h>
+#include <seerep_msgs/timestamp_frame_mesh.h>
 
 // std
 #include <boost/geometry.hpp>
@@ -37,6 +38,14 @@ class Hdf5CorePoint : public Hdf5CoreGeneral, public Hdf5CoreDatatypeInterface
 
   std::vector<std::string> getDatasetUuids();
 
+  /**
+   * @brief THIS IS CURRENTLY A NOOP
+   *
+   * @return nullopt
+   */
+  std::optional<seerep_core_msgs::TimestampFrameMesh>
+  getPolygonConstraintMesh(const boost::uuids::uuid& uuid_entry);
+
 public:
   inline static const std::string RAWDATA = "rawdata";
 

seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_point_cloud.h

@@ -4,12 +4,17 @@
 // highfive
 #include <highfive/H5File.hpp>
 
+// CGAL
+#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
+#include <CGAL/Surface_mesh.h>
+
 // seerep_hdf5_core
 #include "hdf5_core_datatype_interface.h"
 #include "hdf5_core_general.h"
 
 // seerep-msgs
 #include <seerep_msgs/dataset_indexable.h>
+#include <seerep_msgs/timestamp_frame_mesh.h>
 
 // std
 #include <boost/geometry.hpp>
@@ -24,6 +29,12 @@
 
 namespace seerep_hdf5_core
 {
+
+using ExactKernel = CGAL::Exact_predicates_exact_constructions_kernel;
+using CGPoint_3 = ExactKernel::Point_3;
+using CGSurfaceMesh = CGAL::Surface_mesh<CGPoint_3>;
+using CGVertexIndex = CGSurfaceMesh::Vertex_index;
+
 class Hdf5CorePointCloud : public Hdf5CoreGeneral,
                            public Hdf5CoreDatatypeInterface
 {
@@ -38,6 +49,33 @@ class Hdf5CorePointCloud : public Hdf5CoreGeneral,
 
   std::vector<std::string> getDatasetUuids();
 
+  /**
+   * @brief Get a timestamp-frameid-mesh struct which will be subject to
+   *  encapsulation checks of a query polygon
+   *
+   * The mesh is created from the AABB bounding a pointcloud datatype entry
+   *
+   * @param uuid_entry of the pointcloud datatype entry to fetch the data from
+   *
+   * @return a struct containing the timestamp, frame_id of the data and the mesh itself
+   */
+  std::optional<seerep_core_msgs::TimestampFrameMesh>
+  getPolygonConstraintMesh(const boost::uuids::uuid& uuid_entry);
+
+  /**
+   * @brief create mesh from a given AABB
+   *
+   * @param bb_coords the coordinates of the AABB in the following order:
+   *       0. min_corner_x
+   *       1. min_corner_y
+   *       2. min_corner_z
+   *       3. max_corner_x
+   *       4. max_corner_y
+   *       5. max_corner_z
+   *  @return the surface mesh based on these coordinates
+   */
+  CGSurfaceMesh createMeshFromAABB(const std::vector<float>& bb_coords);
+
 public:
   // image / pointcloud attribute keys
   inline static const std::string HEIGHT = "height";

seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp

@@ -67,6 +67,49 @@ std::vector<std::string> Hdf5CoreImage::getDatasetUuids()
   return getGroupDatasets(HDF5_GROUP_IMAGE);
 }
 
+std::optional<seerep_core_msgs::TimestampFrameMesh>
+Hdf5CoreImage::getPolygonConstraintMesh(const boost::uuids::uuid& uuid_entry)
+{
+  std::string uuid_str = boost::uuids::to_string(uuid_entry);
+  std::string hdf5DataGroupPath = getHdf5GroupPath(uuid_str);
+
+  auto dataGroupPtr = getHdf5Group(hdf5DataGroupPath);
+
+  if (dataGroupPtr == nullptr)
+  {
+    throw std::invalid_argument("Hdf5DatasetPath not present for uuid " +
+                                uuid_str + "! Skipping precise check...");
+    BOOST_LOG_SEV(this->m_logger, boost::log::trivial::severity_level::warning);
+    return std::nullopt;
+  }
+
+  // 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);
+
+  // 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);
+
+  seerep_core_msgs::Timestamp ts{ stamp_seconds,
+                                  static_cast<uint32_t>(stamp_nanos) };
+
+  auto points = this->computeFrustumPoints(camintrinsics_uuid);
+  auto mesh = this->computeFrustumMesh(points);
+
+  return seerep_core_msgs::TimestampFrameMesh{ ts, frame_id, mesh };
+}
+
 void Hdf5CoreImage::writeLabels(
     const std::string& uuid,
     const std::vector<seerep_core_msgs::LabelCategory>& labelCategory)
@@ -123,6 +166,81 @@ const std::string Hdf5CoreImage::getHdf5DataSetPath(const std::string& id) const
   return getHdf5GroupPath(id) + "/" + RAWDATA;
 }
 
+std::array<seerep_core_msgs::Point, 5>
+Hdf5CoreImage::computeFrustumPoints(const std::string& camintrinsics_uuid)
+{
+  seerep_core_msgs::camera_intrinsics ci = m_ioCI->readCameraIntrinsics(
+      boost::lexical_cast<boost::uuids::uuid>(camintrinsics_uuid));
+
+  double far_plane_dist = ci.maximum_viewing_distance;
+
+  double far_fov_x = (far_plane_dist * ci.height) / ci.intrinsic_matrix[0];
+  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{ 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 };
+}
+
+CGSurfaceMesh
+Hdf5CoreImage::computeFrustumMesh(std::array<seerep_core_msgs::Point, 5>& points)
+{
+  CGSurfaceMesh 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>() });
+
+  std::vector<CGSurfaceMesh::Face_index> descriptors;
+
+  descriptors.push_back(mesh.add_face(o, tl, tr));
+  descriptors.push_back(mesh.add_face(o, tr, br));
+  descriptors.push_back(mesh.add_face(o, br, bl));
+  descriptors.push_back(mesh.add_face(o, bl, tl));
+  // for whatever reason the face needs to be constructed in this way,
+  // when the add_face returns a null_face() ALWAYS try reversing the vertex
+  // order
+  descriptors.push_back(mesh.add_face(bl, br, tr, tl));
+
+  // check if any of the faces was not constructed properly
+  if (std::find_if(descriptors.begin(), descriptors.end(), [](auto elem) {
+        return elem == CGSurfaceMesh::null_face();
+      }) != descriptors.end())
+  {
+    throw std::invalid_argument("Could not create the faces for the "
+                                "SurfaceMesh from the camera frustum points!");
+  }
+
+  return mesh;
+}
+
 void Hdf5CoreImage::computeFrustumBB(const std::string& camintrinsics_uuid,
                                      seerep_core_msgs::AABB& bb)
 {