Merge pull request #406 from agri-gaia/feat/sensorPosRtree

add another rtree as index for the sensor position

examples/python/gRPC/images/gRPC_fb_queryImages.py

@@ -116,7 +116,8 @@ def query_images(
         fbb,
         grpc_channel,
         project_uuid,
-        polygon2d=polygon_2d,
+        # polygon2d=polygon_2d,
+        polygon2dSensorPos=polygon_2d,
         labels=labelsCategory,
         withoutData=True,
         fullyEncapsulated=False,

examples/python/gRPC/images/gRPC_fb_sendImages.py

@@ -16,6 +16,8 @@
 from seerep.fb import tf_service_grpc_fb as tf_service
 from seerep.util.common import get_gRPC_channel
 from seerep.util.fb_helper import (
+    create_label,
+    create_label_category,
     createCameraIntrinsics,
     createHeader,
     createImage,
@@ -35,7 +37,7 @@ def send_images(
     camera_intrinsic_uuid: str,
     image_payloads: List[Union[np.ndarray, str]],
     timestamps: Union[List[Tuple[int, int]], None] = None,
-    frame_id: str = "map",
+    frame_id: str = "camera",
 ) -> List[bytes]:
     """
     Send images to a SEEREP project
@@ -71,6 +73,26 @@ def send_images(
         fbb, *list(map(int, str(time.time()).split(".")))
     )
 
+    labels = [
+        create_label(
+            builder=fbb,
+            label=label_str,
+            label_id=i,
+            instance_uuid=str(uuid4()),
+            instance_id=i + 100,
+        )
+        for i, label_str in enumerate(["label1", "label2"])
+    ]
+
+    labelsCategory = [
+        create_label_category(
+            builder=fbb,
+            labels=labels,
+            datumaro_json="a very valid datumaro json",
+            category="category A",
+        )
+    ]
+
     fb_msgs = []
     for idx, image in enumerate(image_payloads):
         if timestamps is None:
@@ -93,6 +115,7 @@ def send_images(
             width=image.shape[1] if type(image) is np.ndarray else 640,
             image=image if type(image) is np.ndarray else None,
             uri=None if type(image) is np.ndarray else image,
+            labels=labelsCategory,
         )
         fbb.Finish(fb_image)
         fb_msgs.append(bytes(fbb.Output()))
@@ -242,7 +265,7 @@ def tfs_gen() -> Iterator[bytes]:
     nanos_factor = 1e-9
 
     timestamps = [
-        (t, timestamp_nanos) for t in range(1661336507, 1661336608, 10)
+        (t, timestamp_nanos) for t in range(1661336507, 1661336606, 10)
     ]
 
     img_bufs = send_images(

seerep_msgs/core/polygon2d.h

@@ -8,8 +8,8 @@ namespace seerep_core_msgs
 struct Polygon2D
 {
   std::vector<Point2D> vertices;
-  int z;
-  int height;
+  double z;
+  double height;
 };
 }  // namespace seerep_core_msgs
 

seerep_msgs/core/query.h

@@ -18,6 +18,9 @@ struct Query
       projects;  ///< search all projects if not set
   std::optional<Polygon2D>
       polygon;  // query dataset in the region defined by this polygon
+  std::optional<Polygon2D>
+      polygonSensorPos;  // query dataset with the sensor position in the region
+                         // defined by this polygon
   bool fullyEncapsulated;  // if true, only return results fully inside the
                            // polygon defined above
   bool inMapFrame;  // if false the query polygon is in geodetic coordinates,

seerep_msgs/fbs/query.fbs

@@ -7,6 +7,7 @@ namespace seerep.fb;
 
 table Query {
   polygon:Polygon2D;
+  polygonSensorPosition:Polygon2D;
   fullyEncapsulated:bool;
   inMapFrame:bool;
   timeinterval:TimeInterval;

seerep_srv/seerep_core/include/seerep_core/core_dataset.h

@@ -55,8 +55,11 @@ class CoreDataset
     std::vector<std::shared_ptr<seerep_core_msgs::DatasetIndexable>>
         dataWithMissingTF =
             std::vector<std::shared_ptr<seerep_core_msgs::DatasetIndexable>>();
-    /** @brief the spatial r-tree for the spatial index*/
+    /** @brief the spatial r-tree of the spatial extent of the sensor data for
+     * the spatial index*/
     seerep_core_msgs::rtree rt = seerep_core_msgs::rtree();
+    /** @brief the spatial r-tree of the sensor position for the spatial index*/
+    seerep_core_msgs::rtree rtSensorPos = seerep_core_msgs::rtree();
     /** @brief the temporal r-tree for the temporal index*/
     seerep_core_msgs::timetree timetree = seerep_core_msgs::timetree();
     /** @brief map from the category of labels to the map from label to the
@@ -229,6 +232,15 @@ class CoreDataset
   bool
   isSpatiallyTransformable(const seerep_core_msgs::DatasetIndexable& indexable);
 
+  /**
+   * @brief Get the Sensor Position As A A B B object
+   *
+   * @param indexable
+   * @return seerep_core_msgs::AABB
+   */
+  seerep_core_msgs::AABB
+  getSensorPositionAsAABB(const seerep_core_msgs::DatasetIndexable& indexable);
+
   /**
    * @brief Check if the created CGAL polygon follows the requirements. It
    * should be simple (no more than two vertices on an edge), convex (no
@@ -307,6 +319,31 @@ class CoreDataset
   std::optional<std::vector<seerep_core_msgs::AabbIdPair>>
   querySpatial(std::shared_ptr<DatatypeSpecifics> datatypeSpecifics,
                const seerep_core_msgs::Query& query);
+
+  /**
+   * @brief queries the spatial index of the sensor position and returns a vector
+   * of bounding box / UUID pairs matching the query
+   * @param datatypeSpecifics the datatype specific information to be used in the query
+   * @param query the query parameters
+   * @return vector of bounding box / UUID pairs matching the query
+   */
+  std::optional<std::vector<seerep_core_msgs::AabbIdPair>>
+  querySpatialSensorPos(std::shared_ptr<DatatypeSpecifics> datatypeSpecifics,
+                        const seerep_core_msgs::Query& query);
+
+  /**
+   * @brief queries the
+   *
+   * @param rt
+   * @param polygon
+   * @param queryFullyEncapsulated
+   * @return std::optional<std::vector<seerep_core_msgs::AabbIdPair>>
+   */
+  std::optional<std::vector<seerep_core_msgs::AabbIdPair>>
+  queryRtree(const seerep_core_msgs::rtree& rt,
+             const seerep_core_msgs::Polygon2D& polygon,
+             const bool queryFullyEncapsulated);
+
   /**
    * @brief queries the temporal index and returns a vector of temporal bounding
    * box / UUID pairs matching the query
@@ -353,6 +390,7 @@ class CoreDataset
    * @brief intersects the results of the spatial, temporal and semantic query
    * and returns the UUIDs of the images matching the query in all three modalities
    * @param rt_result the result of the spatial query
+   * @param rt_resultSensorPos the result of the spatial query of the sensor position
    * @param timetree_result the result of the temporal query
    * @param semanticResult the result of the semantic query
    * @param instanceResult the result of the instance based query
@@ -361,6 +399,8 @@ class CoreDataset
    */
   std::vector<boost::uuids::uuid> intersectQueryResults(
       std::optional<std::vector<seerep_core_msgs::AabbIdPair>>& rt_result,
+      std::optional<std::vector<seerep_core_msgs::AabbIdPair>>&
+          rt_resultSensorPos,
       std::optional<std::vector<seerep_core_msgs::AabbTimeIdPair>>&
           timetree_result,
       std::optional<std::set<boost::uuids::uuid>>& semanticResult,