diff --git a/examples/python/gRPC/images/gRPC_fb_queryImages.py b/examples/python/gRPC/images/gRPC_fb_queryImages.py index ae601924d..79f0aab4f 100755 --- a/examples/python/gRPC/images/gRPC_fb_queryImages.py +++ b/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, diff --git a/examples/python/gRPC/images/gRPC_fb_sendImages.py b/examples/python/gRPC/images/gRPC_fb_sendImages.py index c7dbc7a39..df3a3d3a5 100644 --- a/examples/python/gRPC/images/gRPC_fb_sendImages.py +++ b/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( diff --git a/seerep_msgs/core/polygon2d.h b/seerep_msgs/core/polygon2d.h index c51802a5a..58dc739e3 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; - int z; - int height; + double z; + double height; }; } // namespace seerep_core_msgs diff --git a/seerep_msgs/core/query.h b/seerep_msgs/core/query.h index 010d501f5..b6713db54 100644 --- a/seerep_msgs/core/query.h +++ b/seerep_msgs/core/query.h @@ -18,6 +18,9 @@ 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_msgs/fbs/query.fbs b/seerep_msgs/fbs/query.fbs index 9777fc673..9b12d9d11 100644 --- a/seerep_msgs/fbs/query.fbs +++ b/seerep_msgs/fbs/query.fbs @@ -7,6 +7,7 @@ namespace seerep.fb; table Query { polygon:Polygon2D; + polygonSensorPosition:Polygon2D; fullyEncapsulated:bool; inMapFrame:bool; timeinterval:TimeInterval; 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 b8da3430d..5e3c66483 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h @@ -55,8 +55,11 @@ class CoreDataset std::vector> dataWithMissingTF = std::vector>(); - /** @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> querySpatial(std::shared_ptr 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> + querySpatialSensorPos(std::shared_ptr datatypeSpecifics, + const seerep_core_msgs::Query& query); + + /** + * @brief queries the + * + * @param rt + * @param polygon + * @param queryFullyEncapsulated + * @return std::optional> + */ + std::optional> + 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 intersectQueryResults( std::optional>& rt_result, + std::optional>& + rt_resultSensorPos, std::optional>& timetree_result, std::optional>& semanticResult, diff --git a/seerep_srv/seerep_core/src/core_dataset.cpp b/seerep_srv/seerep_core/src/core_dataset.cpp index 0c5f26a20..cd3ede0fa 100644 --- a/seerep_srv/seerep_core/src/core_dataset.cpp +++ b/seerep_srv/seerep_core/src/core_dataset.cpp @@ -22,6 +22,7 @@ void CoreDataset::addDatatype( .dataWithMissingTF = std::vector>(), .rt = seerep_core_msgs::rtree(), + .rtSensorPos = seerep_core_msgs::rtree(), .timetree = seerep_core_msgs::timetree(), .categoryLabelDatasetsMap = std::unordered_map< std::string, @@ -70,6 +71,7 @@ void CoreDataset::recreateSpatialRt( std::vector datasets = hdf5Io->getDatasetUuids(); auto datatypeSpecifics = m_datatypeDatatypeSpecificsMap.at(datatype); std::vector insertableData; + std::vector insertableDataSensorPos; for (auto uuid : datasets) { @@ -85,6 +87,9 @@ void CoreDataset::recreateSpatialRt( insertableData.push_back( { transformIndexableAABB(dataset.value()), boost::lexical_cast(uuid) }); + insertableDataSensorPos.push_back( + { getSensorPositionAsAABB(dataset.value()), + boost::lexical_cast(uuid) }); } else { @@ -103,6 +108,9 @@ void CoreDataset::recreateSpatialRt( // recreate spatial rtree datatypeSpecifics->rt = seerep_core_msgs::rtree{ insertableData.begin(), insertableData.end() }; + datatypeSpecifics->rtSensorPos = + seerep_core_msgs::rtree{ insertableDataSensorPos.begin(), + insertableDataSensorPos.end() }; } std::vector @@ -117,6 +125,7 @@ CoreDataset::getData(const seerep_core_msgs::Query& query) m_datatypeDatatypeSpecificsMap.at(query.header.datatype); // space auto resultRt = querySpatial(datatypeSpecifics, query); + auto resultRtSensorPos = querySpatialSensorPos(datatypeSpecifics, query); // time auto resultTime = queryTemporal(datatypeSpecifics, query); // semantic @@ -140,8 +149,8 @@ CoreDataset::getData(const seerep_core_msgs::Query& query) return getAllDatasetUuids(datatypeSpecifics, query.sortByTime); } - return intersectQueryResults(resultRt, resultTime, resultSemantic, - instanceResult, query.dataUuids, + return intersectQueryResults(resultRt, resultRtSensorPos, resultTime, + resultSemantic, instanceResult, query.dataUuids, query.sortByTime); } @@ -246,55 +255,80 @@ CoreDataset::querySpatial(std::shared_ptr datatypeSpecifics, { if (query.polygon) { - // generate rtree result container - std::optional> rt_result = - std::vector(); + return queryRtree(datatypeSpecifics->rt, query.polygon.value(), + query.fullyEncapsulated); + } + else + { + return std::nullopt; + } +} - seerep_core_msgs::AABB aabb = polygonToAABB(query.polygon.value()); +std::optional> +CoreDataset::querySpatialSensorPos( + std::shared_ptr datatypeSpecifics, + const seerep_core_msgs::Query& query) +{ + if (query.polygonSensorPos) + { + return queryRtree(datatypeSpecifics->rtSensorPos, + query.polygonSensorPos.value(), query.fullyEncapsulated); + } + else + { + return std::nullopt; + } +} - // perform the query on the r tree using the aabb - datatypeSpecifics->rt.query(boost::geometry::index::intersects(aabb), - std::back_inserter(rt_result.value())); +std::optional> +CoreDataset::queryRtree(const seerep_core_msgs::rtree& rt, + const seerep_core_msgs::Polygon2D& polygon, + const bool queryFullyEncapsulated) +{ + // generate rtree result container + std::optional> rt_result = + std::vector(); - bool fullyEncapsulated = false; - bool partiallyEncapsulated = false; - std::vector::iterator it; + seerep_core_msgs::AABB aabb = polygonToAABB(polygon); - // traverse query results and confirm if they are contained inside the polygon - for (it = rt_result.value().begin(); it != rt_result.value().end();) - { - // check if query result is not fully distant to the oriented bb box - // provided in the query - intersectionDegree(it->first, query.polygon.value(), fullyEncapsulated, - partiallyEncapsulated); - - // if there is no intersection between the result and the user's - // request, remove it from the iterator - if (!partiallyEncapsulated && !fullyEncapsulated) - { - it = rt_result.value().erase(it); - } + // perform the query on the r tree using the aabb + rt.query(boost::geometry::index::intersects(aabb), + std::back_inserter(rt_result.value())); - // does the user want only results fully encapsulated by the requested - // bb and is this query result not fully encapsulated - else if (query.fullyEncapsulated && !fullyEncapsulated) - { - // if yes, then delete partially encapsulated query results from the result set - it = rt_result.value().erase(it); - } - else - { - ++it; - } - } + bool fullyEncapsulated = false; + bool partiallyEncapsulated = false; + std::vector::iterator it; - // return the result after deletion of undesired elements has been performed - return rt_result; - } - else + // traverse query results and confirm if they are contained inside the polygon + for (it = rt_result.value().begin(); it != rt_result.value().end();) { - return std::nullopt; + // check if query result is not fully distant to the oriented bb box + // provided in the query + intersectionDegree(it->first, polygon, fullyEncapsulated, + partiallyEncapsulated); + + // if there is no intersection between the result and the user's + // request, remove it from the iterator + if (!partiallyEncapsulated && !fullyEncapsulated) + { + it = rt_result.value().erase(it); + } + + // does the user want only results fully encapsulated by the requested + // bb and is this query result not fully encapsulated + else if (queryFullyEncapsulated && !fullyEncapsulated) + { + // if yes, then delete partially encapsulated query results from the result set + it = rt_result.value().erase(it); + } + else + { + ++it; + } } + + // return the result after deletion of undesired elements has been performed + return rt_result; } std::optional> @@ -438,6 +472,7 @@ CoreDataset::querySemanticWithAllTheLabels( std::vector CoreDataset::intersectQueryResults( std::optional>& rt_result, + std::optional>& rt_resultSensorPos, std::optional>& timetree_result, std::optional>& semanticResult, std::optional>& instanceResult, @@ -458,6 +493,18 @@ std::vector CoreDataset::intersectQueryResults( idsPerSingleModality.push_back(std::move(idsSpatial)); } + if (rt_resultSensorPos) + { + std::set idsSpatialSensorPos; + for (auto it = std::make_move_iterator(rt_resultSensorPos.value().begin()), + end = std::make_move_iterator(rt_resultSensorPos.value().end()); + it != end; ++it) + { + idsSpatialSensorPos.insert(std::move(it->second)); + } + idsPerSingleModality.push_back(std::move(idsSpatialSensorPos)); + } + if (timetree_result) { std::set idsTemporal; @@ -642,6 +689,8 @@ void CoreDataset::tryAddingDataWithMissingTF( { datatypeSpecifics->rt.insert( std::make_pair(transformIndexableAABB(**it), (*it)->header.uuidData)); + datatypeSpecifics->rtSensorPos.insert(std::make_pair( + getSensorPositionAsAABB(**it), (*it)->header.uuidData)); it = datatypeSpecifics->dataWithMissingTF.erase(it); } else @@ -661,6 +710,8 @@ void CoreDataset::addDatasetToIndices( { datatypeSpecifics->rt.insert(std::make_pair(transformIndexableAABB(dataset), dataset.header.uuidData)); + datatypeSpecifics->rtSensorPos.insert(std::make_pair( + getSensorPositionAsAABB(dataset), dataset.header.uuidData)); } else { @@ -920,40 +971,55 @@ CoreDataset::getSpatialBounds(std::vector datatypes) seerep_core_msgs::AABB rtree_bounds = m_datatypeDatatypeSpecificsMap.at(dt)->rt.bounds(); + seerep_core_msgs::AABB rtreeSensorPos_bounds = + m_datatypeDatatypeSpecificsMap.at(dt)->rtSensorPos.bounds(); + // update the min if needed for dimension 0 - if (rtree_bounds.min_corner().get<0>() < overallbb.min_corner().get<0>()) + auto min0 = std::min(rtree_bounds.min_corner().get<0>(), + rtreeSensorPos_bounds.min_corner().get<0>()); + if (min0 < overallbb.min_corner().get<0>()) { - overallbb.min_corner().set<0>(rtree_bounds.min_corner().get<0>()); + overallbb.min_corner().set<0>(min0); } // update the min if needed for dimension 1 - if (rtree_bounds.min_corner().get<1>() < overallbb.min_corner().get<1>()) + auto min1 = std::min(rtree_bounds.min_corner().get<1>(), + rtreeSensorPos_bounds.min_corner().get<1>()); + if (min1 < overallbb.min_corner().get<1>()) { - overallbb.min_corner().set<1>(rtree_bounds.min_corner().get<1>()); + overallbb.min_corner().set<1>(min1); } // update the min if needed for dimension 2 - if (rtree_bounds.min_corner().get<2>() < overallbb.min_corner().get<2>()) + auto min2 = std::min(rtree_bounds.min_corner().get<2>(), + rtreeSensorPos_bounds.min_corner().get<2>()); + if (min2 < overallbb.min_corner().get<2>()) { - overallbb.min_corner().set<2>(rtree_bounds.min_corner().get<2>()); + overallbb.min_corner().set<2>(min2); } // update the max if needed for dimension 0 - if (rtree_bounds.max_corner().get<0>() > overallbb.max_corner().get<0>()) + auto max0 = std::max(rtree_bounds.max_corner().get<0>(), + rtreeSensorPos_bounds.max_corner().get<0>()); + if (max0 > overallbb.max_corner().get<0>()) { - overallbb.max_corner().set<0>(rtree_bounds.max_corner().get<0>()); + overallbb.max_corner().set<0>(max0); } // update the max if needed for dimension 1 - if (rtree_bounds.max_corner().get<1>() > overallbb.max_corner().get<1>()) + auto max1 = std::max(rtree_bounds.max_corner().get<1>(), + rtreeSensorPos_bounds.max_corner().get<1>()); + if (max1 > overallbb.max_corner().get<1>()) { - overallbb.max_corner().set<1>(rtree_bounds.max_corner().get<1>()); + overallbb.max_corner().set<1>(max1); } // update the max if needed for dimension 2 - if (rtree_bounds.max_corner().get<2>() > overallbb.max_corner().get<2>()) + auto max2 = std::max(rtree_bounds.max_corner().get<2>(), + rtreeSensorPos_bounds.max_corner().get<2>()); + if (max2 > overallbb.max_corner().get<2>()) { - overallbb.max_corner().set<2>(rtree_bounds.max_corner().get<2>()); + overallbb.max_corner().set<2>(max2); } } return overallbb; @@ -1021,4 +1087,24 @@ seerep_core_msgs::AABB CoreDataset::transformIndexableAABB( indexable.header.timestamp.seconds, indexable.header.timestamp.nanos); } + +seerep_core_msgs::AABB CoreDataset::getSensorPositionAsAABB( + const seerep_core_msgs::DatasetIndexable& indexable) +{ + auto tf = m_tfOverview->getData(indexable.header.timestamp.seconds, + indexable.header.timestamp.nanos, m_frameId, + indexable.header.frameId); + + seerep_core_msgs::AABB sensorPosAABB; + bg::set(sensorPosAABB, tf.value().transform.translation.x); + bg::set(sensorPosAABB, tf.value().transform.translation.y); + bg::set(sensorPosAABB, tf.value().transform.translation.z); + + bg::set(sensorPosAABB, tf.value().transform.translation.x); + bg::set(sensorPosAABB, tf.value().transform.translation.y); + bg::set(sensorPosAABB, tf.value().transform.translation.z); + + return sensorPosAABB; +} + } /* namespace seerep_core */ diff --git a/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_conversion.h b/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_conversion.h index fd59c8d20..fa1da5ae3 100644 --- a/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_conversion.h +++ b/seerep_srv/seerep_core_fb/include/seerep_core_fb/core_fb_conversion.h @@ -329,6 +329,20 @@ class CoreFbConversion static std::optional fromFbQueryPolygon(const seerep::fb::Query* query); + /** + * @brief extracts and converts a polygon for the sensor position from a fb query + * + * @param query fb query message + * @return seerep_core_msgs::Polygon2D polygon2d as a core msg + */ + static std::optional + fromFbQueryPolygonSensorPosition(const seerep::fb::Query* query); + + static seerep_core_msgs::Polygon2D extractPolygon( + const flatbuffers::Vector>* + vertices, + double height, double z); + /** * @brief converts the header of the flatbuffer data message to seerep core * specific message diff --git a/seerep_srv/seerep_core_fb/src/core_fb_conversion.cpp b/seerep_srv/seerep_core_fb/src/core_fb_conversion.cpp index 9d0b9b981..f46f88bbd 100644 --- a/seerep_srv/seerep_core_fb/src/core_fb_conversion.cpp +++ b/seerep_srv/seerep_core_fb/src/core_fb_conversion.cpp @@ -48,6 +48,7 @@ CoreFbConversion::fromFb(const seerep::fb::Query* query, queryCore.withoutData = fromFbQueryWithoutData(query); queryCore.maxNumData = fromFbQueryMaxNumData(query); queryCore.polygon = fromFbQueryPolygon(query); + queryCore.polygonSensorPos = fromFbQueryPolygonSensorPosition(query); queryCore.fullyEncapsulated = fromFbQueryFullyEncapsulated(query); queryCore.inMapFrame = fromFbQueryInMapFrame(query); queryCore.sortByTime = query->sortByTime(); @@ -719,17 +720,24 @@ CoreFbConversion::fromFbQueryPolygon(const seerep::fb::Query* query) { if (flatbuffers::IsFieldPresent(query, seerep::fb::Query::VT_POLYGON)) { - seerep_core_msgs::Polygon2D polygon; - for (auto point : *query->polygon()->vertices()) - { - seerep_core_msgs::Point2D temp(point->x(), point->y()); - polygon.vertices.push_back(temp); - } - - polygon.height = query->polygon()->height(); - polygon.z = query->polygon()->z(); + return extractPolygon(query->polygon()->vertices(), + query->polygon()->height(), query->polygon()->z()); + } + else + { + return std::nullopt; + } +} - return polygon; +std::optional +CoreFbConversion::fromFbQueryPolygonSensorPosition(const seerep::fb::Query* query) +{ + if (flatbuffers::IsFieldPresent(query, + seerep::fb::Query::VT_POLYGONSENSORPOSITION)) + { + return extractPolygon(query->polygonSensorPosition()->vertices(), + query->polygonSensorPosition()->height(), + query->polygonSensorPosition()->z()); } else { @@ -737,6 +745,23 @@ CoreFbConversion::fromFbQueryPolygon(const seerep::fb::Query* query) } } +seerep_core_msgs::Polygon2D CoreFbConversion::extractPolygon( + const flatbuffers::Vector>* vertices, + double height, double z) +{ + seerep_core_msgs::Polygon2D polygon; + for (auto point : *vertices) + { + seerep_core_msgs::Point2D temp(point->x(), point->y()); + polygon.vertices.push_back(temp); + } + + polygon.height = height; + polygon.z = z; + + return polygon; +} + std::vector CoreFbConversion::fromFbDatatypeVector(const seerep::fb::Datatype& dt) {