From 7a15fb906682b905a62e06351321a6ee4ba4851a Mon Sep 17 00:00:00 2001 From: mmeijerdfki Date: Sun, 3 Nov 2024 15:54:07 +0100 Subject: [PATCH] initial changes --- .../hdf5_core_datatype_interface.h | 13 +++++++++++++ .../include/seerep_hdf5_core/hdf5_core_image.h | 9 +++++++++ .../include/seerep_hdf5_core/hdf5_core_point.h | 7 +++++++ .../seerep_hdf5_core/hdf5_core_point_cloud.h | 7 +++++++ .../seerep_hdf5_core/src/hdf5_core_image.cpp | 5 +++++ .../seerep_hdf5_core/src/hdf5_core_point.cpp | 5 +++++ .../seerep_hdf5_core/src/hdf5_core_point_cloud.cpp | 5 +++++ seerep_msgs/core/dataset_indexable.h | 5 +++-- seerep_msgs/core/polygon2d.h | 4 ++-- seerep_msgs/core/query.h | 3 --- .../seerep_core/include/seerep_core/core_dataset.h | 9 +++++---- seerep_srv/seerep_core/src/core_dataset.cpp | 14 ++++++++++++++ 12 files changed, 75 insertions(+), 11 deletions(-) diff --git a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_datatype_interface.h b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_datatype_interface.h index cdf3a4a3a..fc60bcb01 100644 --- a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_datatype_interface.h +++ b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_datatype_interface.h @@ -10,6 +10,9 @@ namespace seerep_hdf5_core { +typedef std::optional>> + frame_to_points_mapping; + class Hdf5CoreDatatypeInterface { public: @@ -19,6 +22,16 @@ class Hdf5CoreDatatypeInterface readDataset(const std::string& uuid) = 0; virtual std::vector 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 + * + * @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 frame_to_points_mapping getPolygonConstraintPoints() = 0; }; } // namespace seerep_hdf5_core 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 764c0057f..5ac909c59 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 @@ -90,6 +90,15 @@ class Hdf5CoreImage : public virtual Hdf5CoreGeneral, */ std::vector getDatasetUuids(); + /** + * @brief Get points which should get constraint by the query polygon, + * otherwise the data is not fullyEncapsulated by the polygon + * + * @return the points which should get checked and the frame as a string in + * which they should be checked + */ + frame_to_points_mapping getPolygonConstraintPoints(); + /** * @brief Write generals labels based on C++ data structures to HdF5 * diff --git a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_point.h b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_point.h index a21932525..0eccfc62b 100644 --- a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_point.h +++ b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_point.h @@ -37,6 +37,13 @@ class Hdf5CorePoint : public Hdf5CoreGeneral, public Hdf5CoreDatatypeInterface std::vector getDatasetUuids(); + /** + * @brief THIS IS CURRENTLY A NOOP + * + * @return a empty vector and string + */ + frame_to_points_mapping getPolygonConstraintPoints(); + public: inline static const std::string RAWDATA = "rawdata"; diff --git a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_point_cloud.h b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_point_cloud.h index d1033244a..e213c9107 100644 --- a/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_point_cloud.h +++ b/seerep_hdf5/seerep_hdf5_core/include/seerep_hdf5_core/hdf5_core_point_cloud.h @@ -38,6 +38,13 @@ class Hdf5CorePointCloud : public Hdf5CoreGeneral, std::vector getDatasetUuids(); + /** + * @brief THIS IS CURRENTLY A NOOP + * + * @return a empty vector and string + */ + frame_to_points_mapping getPolygonConstraintPoints(); + public: // image / pointcloud attribute keys inline static const std::string HEIGHT = "height"; 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 749c81902..248866c11 100644 --- a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp +++ b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp @@ -67,6 +67,11 @@ std::vector Hdf5CoreImage::getDatasetUuids() return getGroupDatasets(HDF5_GROUP_IMAGE); } +frame_to_points_mapping Hdf5CoreImage::getPolygonConstraintPoints() +{ + return std::nullopt; +} + void Hdf5CoreImage::writeLabels( const std::string& uuid, const std::vector& labelCategory) diff --git a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point.cpp b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point.cpp index 68efd5b99..5173f0ab5 100644 --- a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point.cpp +++ b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point.cpp @@ -64,4 +64,9 @@ std::vector Hdf5CorePoint::getDatasetUuids() return getGroupDatasets(HDF5_GROUP_POINT); } +frame_to_points_mapping Hdf5CorePoint::getPolygonConstraintPoints() +{ + return std::nullopt; +} + } // namespace seerep_hdf5_core diff --git a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point_cloud.cpp b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point_cloud.cpp index aa309543e..5ad13a56f 100644 --- a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point_cloud.cpp +++ b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point_cloud.cpp @@ -61,4 +61,9 @@ std::vector Hdf5CorePointCloud::getDatasetUuids() return getGroupDatasets(HDF5_GROUP_POINTCLOUD); } +frame_to_points_mapping Hdf5CorePointCloud::getPolygonConstraintPoints() +{ + return std::nullopt; +} + } // namespace seerep_hdf5_core diff --git a/seerep_msgs/core/dataset_indexable.h b/seerep_msgs/core/dataset_indexable.h index 186ee6789..812ad9727 100644 --- a/seerep_msgs/core/dataset_indexable.h +++ b/seerep_msgs/core/dataset_indexable.h @@ -5,7 +5,7 @@ #include "aabb.h" #include "header.h" -#include "label_datumaro.h" +#include "label_with_instance.h" namespace seerep_core_msgs { @@ -13,7 +13,8 @@ struct DatasetIndexable { Header header; AABB boundingbox; - std::unordered_map labelsCategory; + std::unordered_map> + labelsWithInstancesWithCategory; }; } /* namespace seerep_core_msgs */ diff --git a/seerep_msgs/core/polygon2d.h b/seerep_msgs/core/polygon2d.h index 58dc739e3..c51802a5a 100644 --- a/seerep_msgs/core/polygon2d.h +++ b/seerep_msgs/core/polygon2d.h @@ -8,8 +8,8 @@ namespace seerep_core_msgs struct Polygon2D { std::vector vertices; - double z; - double height; + int z; + int height; }; } // namespace seerep_core_msgs diff --git a/seerep_msgs/core/query.h b/seerep_msgs/core/query.h index b6713db54..010d501f5 100644 --- a/seerep_msgs/core/query.h +++ b/seerep_msgs/core/query.h @@ -18,9 +18,6 @@ struct Query projects; ///< search all projects if not set std::optional polygon; // query dataset in the region defined by this polygon - std::optional - 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, 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 5a2e977f7..48c010ed7 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h @@ -352,10 +352,11 @@ class CoreDataset * @param queryFullyEncapsulated * @return std::optional> */ - std::optional> - queryRtree(const seerep_core_msgs::rtree& rt, - const seerep_core_msgs::Polygon2D& polygon, - const bool queryFullyEncapsulated); + std::optional> queryRtree( + const seerep_core_msgs::rtree& rt, + const seerep_core_msgs::Polygon2D& polygon, + // const seerep_hdf5_core::frame_to_points_mapping frame_and_points, + const bool queryFullyEncapsulated); /** * @brief queries the temporal index and returns a vector of temporal bounding diff --git a/seerep_srv/seerep_core/src/core_dataset.cpp b/seerep_srv/seerep_core/src/core_dataset.cpp index d291a0d5b..058c35ae9 100644 --- a/seerep_srv/seerep_core/src/core_dataset.cpp +++ b/seerep_srv/seerep_core/src/core_dataset.cpp @@ -283,6 +283,7 @@ CoreDataset::querySpatialSensorPos( std::optional> CoreDataset::queryRtree(const seerep_core_msgs::rtree& rt, const seerep_core_msgs::Polygon2D& polygon, + // const seerep_hdf5_core::frame_to_points_mapping, const bool queryFullyEncapsulated) { // generate rtree result container @@ -307,6 +308,19 @@ CoreDataset::queryRtree(const seerep_core_msgs::rtree& rt, intersectionDegree(it->first, polygon, fullyEncapsulated, partiallyEncapsulated); + // check thoroughly if a image has been passed and check whether the + // corresponding Frustum is in the polygon + // partial encapsulation is pre-condition for the case + // if the aabb of the data, which is a upper bound for the check, is + // already encapsulated we can skip the check + // if (partialEncapsulation && !fullEncapsulation) + // { + // if (isPreciselyFullEncapsulated()) + // { + // fullEncapsulation = true; + // } + // } + // if there is no intersection between the result and the user's // request, remove it from the iterator if (!partiallyEncapsulated && !fullyEncapsulated)