diff --git a/examples/python/gRPC/images/gRPC_fb_sendImages.py b/examples/python/gRPC/images/gRPC_fb_sendImages.py old mode 100644 new mode 100755 index df3a3d3a5..6ae956169 --- a/examples/python/gRPC/images/gRPC_fb_sendImages.py +++ b/examples/python/gRPC/images/gRPC_fb_sendImages.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 import random import time import uuid diff --git a/pyproject.toml b/pyproject.toml index a21ef2b87..989756554 100644 --- a/pyproject.toml +++ b/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" diff --git a/seerep_hdf5/seerep_hdf5_core/CMakeLists.txt b/seerep_hdf5/seerep_hdf5_core/CMakeLists.txt index c3fec951f..7a63023d9 100644 --- a/seerep_hdf5/seerep_hdf5_core/CMakeLists.txt +++ b/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 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..8fe08a31e 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 @@ -3,6 +3,7 @@ // seerep-msgs #include +#include // std #include @@ -10,6 +11,7 @@ namespace seerep_hdf5_core { + class Hdf5CoreDatatypeInterface { public: @@ -19,6 +21,20 @@ 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 + * + * @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 + getPolygonConstraintMesh(const boost::uuids::uuid& uuid_entry) = 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..79034cca2 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 @@ -4,6 +4,10 @@ // highfive #include +// CGAL +#include +#include + // seerep_hdf5_core #include "hdf5_core_cameraintrinsics.h" #include "hdf5_core_datatype_interface.h" @@ -12,6 +16,7 @@ // seerep_msgs #include #include +#include // std #include @@ -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; + /** * @brief Helper struct to summarize the general attributes of an image * @@ -90,6 +100,19 @@ class Hdf5CoreImage : public virtual Hdf5CoreGeneral, */ std::vector 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 + 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 + 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& points); + /** * @brief Computes the frustum for given camera intrinsic parameters and stores * the result in an axis aligned bounding box 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..780dd82b5 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 @@ -10,6 +10,7 @@ // seerep_msgs #include +#include // std #include @@ -37,6 +38,14 @@ class Hdf5CorePoint : public Hdf5CoreGeneral, public Hdf5CoreDatatypeInterface std::vector getDatasetUuids(); + /** + * @brief THIS IS CURRENTLY A NOOP + * + * @return nullopt + */ + std::optional + getPolygonConstraintMesh(const boost::uuids::uuid& uuid_entry); + 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..d5a0de69f 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 @@ -4,12 +4,17 @@ // highfive #include +// CGAL +#include +#include + // seerep_hdf5_core #include "hdf5_core_datatype_interface.h" #include "hdf5_core_general.h" // seerep-msgs #include +#include // std #include @@ -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; +using CGVertexIndex = CGSurfaceMesh::Vertex_index; + class Hdf5CorePointCloud : public Hdf5CoreGeneral, public Hdf5CoreDatatypeInterface { @@ -38,6 +49,33 @@ class Hdf5CorePointCloud : public Hdf5CoreGeneral, std::vector 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 + 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& bb_coords); + 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..c2c99fbf0 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,49 @@ std::vector Hdf5CoreImage::getDatasetUuids() return getGroupDatasets(HDF5_GROUP_IMAGE); } +std::optional +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( + *dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::CAMERA_INTRINSICS_UUID, + hdf5DataGroupPath); + + auto frame_id = readAttributeFromHdf5( + *dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::HEADER_FRAME_ID, + hdf5DataGroupPath); + + // retrieve timestamp from image + auto stamp_seconds = readAttributeFromHdf5( + *dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::HEADER_STAMP_SECONDS, + hdf5DataGroupPath); + + auto stamp_nanos = readAttributeFromHdf5( + *dataGroupPtr, seerep_hdf5_core::Hdf5CoreImage::HEADER_STAMP_NANOS, + hdf5DataGroupPath); + + seerep_core_msgs::Timestamp ts{ stamp_seconds, + static_cast(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& labelCategory) @@ -123,6 +166,81 @@ const std::string Hdf5CoreImage::getHdf5DataSetPath(const std::string& id) const return getHdf5GroupPath(id) + "/" + RAWDATA; } +std::array +Hdf5CoreImage::computeFrustumPoints(const std::string& camintrinsics_uuid) +{ + seerep_core_msgs::camera_intrinsics ci = m_ioCI->readCameraIntrinsics( + boost::lexical_cast(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(far_fov_x / 2), + static_cast(far_fov_y / 2), + static_cast(far_plane_dist) }; + + seerep_core_msgs::Point far_topright{ static_cast(-far_fov_x / 2), + static_cast(far_fov_y / 2), + static_cast(far_plane_dist) }; + + seerep_core_msgs::Point far_bottomleft{ static_cast(far_fov_x / 2), + static_cast(-far_fov_y / 2), + static_cast(far_plane_dist) }; + + seerep_core_msgs::Point far_bottomright{ static_cast(-far_fov_x / 2), + static_cast(-far_fov_y / 2), + static_cast(far_plane_dist) }; + + return std::array{ near_p, far_topleft, + far_topright, far_bottomleft, + far_bottomright }; +} + +CGSurfaceMesh +Hdf5CoreImage::computeFrustumMesh(std::array& 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 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) { 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..8cf9a9a8e 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,11 @@ std::vector Hdf5CorePoint::getDatasetUuids() return getGroupDatasets(HDF5_GROUP_POINT); } +std::optional +Hdf5CorePoint::getPolygonConstraintMesh(const boost::uuids::uuid& uuid_entry) +{ + (void)uuid_entry; + 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..607b668e5 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 @@ -28,7 +28,7 @@ Hdf5CorePointCloud::readDataset(const std::string& uuid) return std::nullopt; } - std::shared_ptr group_ptr = + const std::shared_ptr group_ptr = std::make_shared(m_file->getGroup(hdf5DatasetPath)); seerep_core_msgs::DatasetIndexable data; @@ -61,4 +61,91 @@ std::vector Hdf5CorePointCloud::getDatasetUuids() return getGroupDatasets(HDF5_GROUP_POINTCLOUD); } +std::optional +Hdf5CorePointCloud::getPolygonConstraintMesh(const boost::uuids::uuid& uuid_entry) +{ + const std::scoped_lock lock(*m_write_mtx); + + std::string uuid_str = boost::lexical_cast(uuid_entry); + + std::string hdf5DatasetPath = HDF5_GROUP_POINTCLOUD + "/" + uuid_str; + + if (!m_file->exist(hdf5DatasetPath)) + { + 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::info); + return std::nullopt; + } + + const std::shared_ptr group_ptr = + std::make_shared(m_file->getGroup(hdf5DatasetPath)); + + std::vector bb; + group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX) + .read(bb); + + seerep_core_msgs::Header head; + + readHeader(uuid_str, *group_ptr, head); + + return seerep_core_msgs::TimestampFrameMesh{ head.timestamp, head.frameId, + this->createMeshFromAABB(bb) }; +} + +CGSurfaceMesh +Hdf5CorePointCloud::createMeshFromAABB(const std::vector& bb_coords) +{ + CGSurfaceMesh mesh; + + // define base indices + int x_base = 0; + int y_base = 1; + int z_base = 2; + + std::vector verts; + + // create vertices with 3bit cube coords + for (uint idx = 0; idx < 0b1000; idx++) + { + int x_i = x_base + 3 * (idx & 0b001); + int y_i = y_base + 3 * ((idx & 0b010) >> 1); + int z_i = z_base + 3 * ((idx & 0b100) >> 2); + + CGPoint_3 p{ bb_coords.at(x_i), bb_coords.at(y_i), bb_coords.at(z_i) }; + + verts.push_back(mesh.add_vertex(p)); + } + + std::vector descriptors; + + // bottom plane + descriptors.push_back(mesh.add_face(verts[0], verts[1], verts[3], verts[2])); + // front plane + descriptors.push_back(mesh.add_face(verts[2], verts[6], verts[4], verts[0])); + // left plane + descriptors.push_back(mesh.add_face(verts[3], verts[7], verts[6], verts[2])); + // back plane + descriptors.push_back(mesh.add_face(verts[1], verts[5], verts[7], verts[3])); + // right plane + descriptors.push_back(mesh.add_face(verts[4], verts[5], verts[1], verts[0])); + // top plane + descriptors.push_back(mesh.add_face(verts[6], verts[7], verts[5], verts[4])); + + std::vector::iterator idx; + // check if any of the faces was not constructed properly + if ((idx = 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 pointcloud AABB! " + "First null_face index: " + + std::to_string(std::distance(descriptors.begin(), idx))); + } + + return mesh; +} + } // namespace seerep_hdf5_core diff --git a/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_pointcloud.cpp b/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_pointcloud.cpp index 05ab43317..13899faf3 100644 --- a/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_pointcloud.cpp +++ b/seerep_hdf5/seerep_hdf5_pb/src/hdf5_pb_pointcloud.cpp @@ -138,7 +138,7 @@ void Hdf5PbPointCloud::writePoints(HighFive::Group& object, std::array min, max; min[0] = min[1] = min[2] = std::numeric_limits::max(); - max[0] = max[1] = max[2] = std::numeric_limits::min(); + max[0] = max[1] = max[2] = std::numeric_limits::lowest(); for (unsigned int i = 0; i < cloud.height(); i++) { diff --git a/seerep_hdf5/seerep_hdf5_py/include/seerep_hdf5_py/impl/hdf5_py_pointcloud.hpp b/seerep_hdf5/seerep_hdf5_py/include/seerep_hdf5_py/impl/hdf5_py_pointcloud.hpp index f213ba077..9b6d3513f 100644 --- a/seerep_hdf5/seerep_hdf5_py/include/seerep_hdf5_py/impl/hdf5_py_pointcloud.hpp +++ b/seerep_hdf5/seerep_hdf5_py/include/seerep_hdf5_py/impl/hdf5_py_pointcloud.hpp @@ -80,6 +80,7 @@ void Hdf5PyPointCloud::getMinMax( { for (std::size_t i = 0; i < Nfields; i++) { + // TODO check if min needs to be replaced by lowest for, due to floating point types min[i] = std::numeric_limits::max(); max[i] = std::numeric_limits::min(); diff --git a/seerep_msgs/CMakeLists.txt b/seerep_msgs/CMakeLists.txt index 07edf9088..bc79d2eb4 100644 --- a/seerep_msgs/CMakeLists.txt +++ b/seerep_msgs/CMakeLists.txt @@ -14,6 +14,9 @@ set(Boost_USE_MULTITHREADED ON) set(Boost_USE_STATIC_RUNTIME OFF) find_package(Boost REQUIRED) +find_package(CGAL REQUIRED) +set(CGAL_DO_NOT_WARN_ABOUT_CMAKE_BUILD_TYPE TRUE) + set(MY_PROTO_FILES protos/boundingbox.proto protos/camera_intrinsics.proto @@ -113,6 +116,7 @@ set(MY_CORE_MSGS ${CMAKE_CURRENT_SOURCE_DIR}/core/sparql_query.h ${CMAKE_CURRENT_SOURCE_DIR}/core/timeinterval.h ${CMAKE_CURRENT_SOURCE_DIR}/core/timestamp.h + ${CMAKE_CURRENT_SOURCE_DIR}/core/timestamp_frame_mesh.h ) add_library(SeerepCoreMsgsTarget INTERFACE) diff --git a/seerep_msgs/core/timestamp_frame_mesh.h b/seerep_msgs/core/timestamp_frame_mesh.h new file mode 100644 index 000000000..ba36787f2 --- /dev/null +++ b/seerep_msgs/core/timestamp_frame_mesh.h @@ -0,0 +1,24 @@ +#ifndef SEEREP_CORE_MSGS_TIMESTAMP_FRAME_MESH_H_ +#define SEEREP_CORE_MSGS_TIMESTAMP_FRAME_MESH_H_ + +#include +#include + +#include "timestamp.h" + +namespace seerep_core_msgs +{ + +using ExactKernel = CGAL::Exact_predicates_exact_constructions_kernel; +using CGPoint_3 = CGAL::Point_3; +using SurfaceMesh = CGAL::Surface_mesh; + +struct TimestampFrameMesh +{ + seerep_core_msgs::Timestamp timestamp; + std::string frame_id; + SurfaceMesh mesh; +}; +} // namespace seerep_core_msgs + +#endif 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..1c865197b 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h @@ -3,10 +3,10 @@ #include #include +#include +#include +#include -#include -#include -#include #include #include @@ -29,7 +29,12 @@ #include // generators #include // streaming operators etc. -typedef CGAL::Exact_predicates_exact_constructions_kernel Kernel; +typedef CGAL::Exact_predicates_exact_constructions_kernel ExactKernel; +typedef CGAL::Point_3 CGPoint_3; +typedef CGAL::Surface_mesh CGSurfaceMesh; +typedef CGAL::Polygon_2 CGPolygon_2; +typedef CGAL::Point_2 CGPoint_2; +typedef CGSurfaceMesh::Face_index CGFaceIdx; namespace seerep_core { @@ -250,7 +255,7 @@ class CoreDataset * @return true The polygon abides by CGAL requirements * @return false The polygon does not abide by CGAL requirements */ - bool verifyPolygonIntegrity(CGAL::Polygon_2& polygon_cgal); + bool verifyPolygonIntegrity(CGAL::Polygon_2& polygon_cgal); /** * @brief transforms the bounding box to the datasets frameId (mostly the map @@ -268,7 +273,7 @@ class CoreDataset * @param polygon core msg polygon * @return CGAL::Polygon_2 cgal polygon */ - CGAL::Polygon_2 + CGAL::Polygon_2 toCGALPolygon(const seerep_core_msgs::Polygon2D& polygon); /** @@ -277,7 +282,7 @@ class CoreDataset * @param polygon core msg aabb * @return CGAL::Polygon_2 cgal aabb */ - CGAL::Polygon_2 toCGALPolygon(const seerep_core_msgs::AABB& aabb); + CGAL::Polygon_2 toCGALPolygon(const seerep_core_msgs::AABB& aabb); /** * @brief determine if the axis aligned bounding box is fully or paritally @@ -294,18 +299,77 @@ class CoreDataset const seerep_core_msgs::Polygon2D& polygon, bool& fullEncapsulation, bool& partialEncapsulation); - void intersectionDegreeCgalPolygons(CGAL::Polygon_2 cgal1, - CGAL::Polygon_2 cgal2, + /** + * @brief check whether cgal1 is encapsulated by cgal2 + * + * @param cgal1 the (possibly) encapsulated polygon + * @param cgal2 the (possibly) encapsulating polygon + * @param z_partially a flag to indicate partial height encapsulation by cgal2 + * @param checkIfFullyEncapsulated whether to check fully encapsulation + * @param fullEncapsulation is set to false when fullEncapsulation is not + * valid. This flag is only set, when checkIfFullyEncapsulated is true + * @param partialEncapsulation is set to true, when cgal1 is partially + * encapsulated by cgal2 + */ + void intersectionDegreeCgalPolygons(CGAL::Polygon_2 cgal1, + CGAL::Polygon_2 cgal2, bool z_partially, bool checkIfFullyEncapsulated, bool& fullEncapsulation, bool& partialEncapsulation); - void intersectionDegreeAABBinPolygon( - const seerep_core_msgs::AABB& aabb, - const seerep_core_msgs::Polygon2D& polygon, - CGAL::Polygon_2 aabb_cgal, CGAL::Polygon_2 polygon_cgal, - bool& fullEncapsulation, bool& partialEncapsulation); + /** + * @brief check if and to what degree AABB is encapsulated by polygon + * + * @param aabb the aabb to perform the check on + * @param polygon the query polygon + * @param aabb_cgal the core msgs aabb converted to a CGAL polygon + * @param polygon_cgal the core msgs polygon converted to a CGAL polygon + * @param fullEncapsulation whether aabb is fully encapsulated by the polygon + * @param partialEncapsulation whether the aabb and polygon intersect on a + * subset of them in some way + */ + void + intersectionDegreeAABBinPolygon(const seerep_core_msgs::AABB& aabb, + const seerep_core_msgs::Polygon2D& polygon, + CGAL::Polygon_2 aabb_cgal, + CGAL::Polygon_2 polygon_cgal, + bool& fullEncapsulation, + bool& partialEncapsulation); + + /** + * @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 partialEncapsulation flag which is set when enclosedMesh is + * partially encapsulated by the enclosingPolygon + */ + void checkPartialIntersectionWithZExtrudedPolygon( + CGSurfaceMesh enclosedMesh, + const seerep_core_msgs::Polygon2D& enclosingPolygon, + bool& partialEncapsulation); + + /** + * @brief Creates a 2DPolygon by removing the 3rd dimension from the points of the Mesh + * + * @param mesh the input mesh + * + * @return the reduced mesh as a CGAL 2D polygon + */ + CGPolygon_2 reduceZDimension(const CGSurfaceMesh& mesh); + + /** + * @brief creates a SurfaceMesh from a seerep polygon 2D + * + * @param seerep_polygon the polygon to create the mesh from + * + * @return the resulting SurfaceMesh + */ + CGSurfaceMesh toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon); void getUuidsFromMap( std::unordered_map, @@ -345,15 +409,17 @@ class CoreDataset const seerep_core_msgs::Query& query); /** - * @brief queries the + * @brief queries the rtree * * @param rt * @param polygon + * @param coreDatatype * @param queryFullyEncapsulated * @return std::optional> */ std::optional> queryRtree(const seerep_core_msgs::rtree& rt, + seerep_hdf5_core::Hdf5CoreDatatypeInterface& coreDatatype, const seerep_core_msgs::Polygon2D& polygon, const bool queryFullyEncapsulated); diff --git a/seerep_srv/seerep_core/include/seerep_core/core_tf.h b/seerep_srv/seerep_core/include/seerep_core/core_tf.h index 48871c595..f7232ed48 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_tf.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_tf.h @@ -22,12 +22,19 @@ #include #include +// CGAL +#include + // logging #include #include namespace seerep_core { + +using ExactKernel = CGAL::Exact_predicates_exact_constructions_kernel; +using CGPoint_3 = CGAL::Point_3; + /** * @brief This is the class handling the TF buffer * @@ -91,6 +98,20 @@ class CoreTf const std::string& targetFrame, const int64_t& timeSecs, const int64_t& timeNanos); + /** + * @brief transform points into another frame + * + * @param sourceFrame the frame the points are in + * @param targetFrame the frame to transform the points to + * @param points the points to transform + * + * @return the transformed points + */ + std::vector + transform(const std::string& sourceFrame, const std::string& targetFrame, + const int64_t& timeSecs, const int64_t& timeNanos, + const std::vector>& points); + /** * @brief Returns a vector of all frames stored in the TF tree by the TF buffer * @return vector of frame names diff --git a/seerep_srv/seerep_core/src/core_dataset.cpp b/seerep_srv/seerep_core/src/core_dataset.cpp index 02f933688..ee5040a6c 100644 --- a/seerep_srv/seerep_core/src/core_dataset.cpp +++ b/seerep_srv/seerep_core/src/core_dataset.cpp @@ -208,10 +208,10 @@ CoreDataset::polygonToAABB(const seerep_core_msgs::Polygon2D& polygon) seerep_core_msgs::Point min, max; bg::set<0>(min, std::numeric_limits::max()); - bg::set<0>(max, std::numeric_limits::min()); + bg::set<0>(max, std::numeric_limits::lowest()); bg::set<1>(min, std::numeric_limits::max()); - bg::set<1>(max, std::numeric_limits::min()); + bg::set<1>(max, std::numeric_limits::lowest()); bg::set<2>(min, polygon.z); bg::set<2>(max, polygon.height + polygon.z); @@ -255,8 +255,8 @@ CoreDataset::querySpatial(std::shared_ptr datatypeSpecifics, { if (query.polygon) { - return queryRtree(datatypeSpecifics->rt, query.polygon.value(), - query.fullyEncapsulated); + return queryRtree(datatypeSpecifics->rt, *datatypeSpecifics->hdf5io, + query.polygon.value(), query.fullyEncapsulated); } else { @@ -272,6 +272,7 @@ CoreDataset::querySpatialSensorPos( if (query.polygonSensorPos) { return queryRtree(datatypeSpecifics->rtSensorPos, + *datatypeSpecifics->hdf5io, query.polygonSensorPos.value(), query.fullyEncapsulated); } else @@ -280,10 +281,11 @@ CoreDataset::querySpatialSensorPos( } } -std::optional> -CoreDataset::queryRtree(const seerep_core_msgs::rtree& rt, - const seerep_core_msgs::Polygon2D& polygon, - const bool queryFullyEncapsulated) +std::optional> CoreDataset::queryRtree( + const seerep_core_msgs::rtree& rt, + seerep_hdf5_core::Hdf5CoreDatatypeInterface& coreDatatype, + const seerep_core_msgs::Polygon2D& polygon, + const bool queryFullyEncapsulated) { // generate rtree result container std::optional> rt_result = @@ -307,6 +309,47 @@ CoreDataset::queryRtree(const seerep_core_msgs::rtree& rt, intersectionDegree(it->first, polygon, fullyEncapsulated, partiallyEncapsulated); + // 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 + // True for partialEncapsulation and false for fullyEncapsulated must + // be verified, as this could be indeed false with the real object + if (partiallyEncapsulated == true && fullyEncapsulated == false) + { + auto ts_frame_points = coreDatatype.getPolygonConstraintMesh(it->second); + if (ts_frame_points.has_value()) + { + CGSurfaceMesh& mesh = ts_frame_points->mesh; + std::vector> ref_points; + + for (auto& idx : mesh.vertices()) + { + ref_points.push_back(mesh.point(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); + + 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 // request, remove it from the iterator if (!partiallyEncapsulated && !fullyEncapsulated) @@ -820,7 +863,8 @@ void CoreDataset::addLabels( datatypeSpecifics->datasetInstancesMap.emplace(msgUuid, instanceUuids); } -bool CoreDataset::verifyPolygonIntegrity(CGAL::Polygon_2& polygon_cgal) +bool CoreDataset::verifyPolygonIntegrity( + CGAL::Polygon_2& polygon_cgal) { if (!polygon_cgal.is_simple()) { @@ -844,44 +888,43 @@ bool CoreDataset::verifyPolygonIntegrity(CGAL::Polygon_2& polygon_cgal) return true; } -CGAL::Polygon_2 +CGAL::Polygon_2 CoreDataset::toCGALPolygon(const seerep_core_msgs::Polygon2D& polygon) { - CGAL::Polygon_2 polygon_cgal; + CGAL::Polygon_2 polygon_cgal; for (auto point : polygon.vertices) { polygon_cgal.push_back( - Kernel::Point_2(bg::get<0>(point), bg::get<1>(point))); + ExactKernel::Point_2(bg::get<0>(point), bg::get<1>(point))); } - return polygon_cgal; } -CGAL::Polygon_2 +CGAL::Polygon_2 CoreDataset::toCGALPolygon(const seerep_core_msgs::AABB& aabb) { /* Check if the bounding box has no spatial extent -> Only add one point to the polygon */ if (bg::get(aabb) == bg::get(aabb) && bg::get(aabb) == bg::get(aabb)) { - Kernel::Point_2 points_aabb[] = { Kernel::Point_2( + ExactKernel::Point_2 points_aabb[] = { ExactKernel::Point_2( bg::get(aabb), bg::get(aabb)) }; - CGAL::Polygon_2 aabb_cgal(points_aabb, points_aabb + 1); + CGAL::Polygon_2 aabb_cgal(points_aabb, points_aabb + 1); return aabb_cgal; } else { - Kernel::Point_2 points_aabb[] = { - Kernel::Point_2(bg::get(aabb), - bg::get(aabb)), - Kernel::Point_2(bg::get(aabb), - bg::get(aabb)), - Kernel::Point_2(bg::get(aabb), - bg::get(aabb)), - Kernel::Point_2(bg::get(aabb), - bg::get(aabb)), + ExactKernel::Point_2 points_aabb[] = { + ExactKernel::Point_2(bg::get(aabb), + bg::get(aabb)), + ExactKernel::Point_2(bg::get(aabb), + bg::get(aabb)), + ExactKernel::Point_2(bg::get(aabb), + bg::get(aabb)), + ExactKernel::Point_2(bg::get(aabb), + bg::get(aabb)), }; - CGAL::Polygon_2 aabb_cgal(points_aabb, points_aabb + 4); + CGAL::Polygon_2 aabb_cgal(points_aabb, points_aabb + 4); return aabb_cgal; } @@ -893,10 +936,10 @@ void CoreDataset::intersectionDegree(const seerep_core_msgs::AABB& aabb, bool& partialEncapsulation) { // convert seerep core aabb to cgal polygon - CGAL::Polygon_2 aabb_cgal = toCGALPolygon(aabb); + CGAL::Polygon_2 aabb_cgal = toCGALPolygon(aabb); // convert seerep core polygon to cgal polygon - CGAL::Polygon_2 polygon_cgal = toCGALPolygon(polygon); + CGAL::Polygon_2 polygon_cgal = toCGALPolygon(polygon); // a cgal polyon needs to be simple, convex and the points should be added // in a counter clockwise order @@ -910,14 +953,12 @@ void CoreDataset::intersectionDegree(const seerep_core_msgs::AABB& aabb, fullEncapsulation, partialEncapsulation); } -void CoreDataset::intersectionDegreeCgalPolygons(CGAL::Polygon_2 cgal1, - CGAL::Polygon_2 cgal2, - bool z_partially, - bool checkIfFullyEncapsulated, - bool& fullEncapsulation, - bool& partialEncapsulation) +void CoreDataset::intersectionDegreeCgalPolygons( + CGAL::Polygon_2 cgal1, CGAL::Polygon_2 cgal2, + bool z_partially, bool checkIfFullyEncapsulated, bool& fullEncapsulation, + bool& partialEncapsulation) { - for (CGAL::Polygon_2::Vertex_iterator vi = cgal1.vertices_begin(); + for (CGAL::Polygon_2::Vertex_iterator vi = cgal1.vertices_begin(); vi != cgal1.vertices_end(); ++vi) { // is the vertex of the aabb inside or on the boundary (not outside) the @@ -939,8 +980,9 @@ void CoreDataset::intersectionDegreeCgalPolygons(CGAL::Polygon_2 cgal1, void CoreDataset::intersectionDegreeAABBinPolygon( const seerep_core_msgs::AABB& aabb, const seerep_core_msgs::Polygon2D& polygon, - CGAL::Polygon_2 aabb_cgal, CGAL::Polygon_2 polygon_cgal, - bool& fullEncapsulation, bool& partialEncapsulation) + CGAL::Polygon_2 aabb_cgal, + CGAL::Polygon_2 polygon_cgal, bool& fullEncapsulation, + bool& partialEncapsulation) { fullEncapsulation = true; partialEncapsulation = false; @@ -994,6 +1036,132 @@ void CoreDataset::intersectionDegreeAABBinPolygon( fullEncapsulation, partialEncapsulation); } +CGSurfaceMesh +CoreDataset::toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon) +{ + using CGVertexIndex = CGSurfaceMesh::Vertex_index; + + if (seerep_polygon.vertices.size() <= 2) + { + throw std::invalid_argument( + "seerep_core_msgs::Polygon2D has not enough vertices to be expanded" + " to a full 3D SurfaceMesh"); + } + + CGSurfaceMesh surface_mesh; + + std::vector lower_surface, upper_surface; + CGVertexIndex topInitial, bottomInitial; + CGVertexIndex topFirst, bottomFirst, topSecond, bottomSecond; + + CGFaceIdx face_desc; + + auto z_top = seerep_polygon.z + seerep_polygon.height; + auto& z_bottom = seerep_polygon.z; + auto& verts = seerep_polygon.vertices; + + auto x = verts[0].get<0>(); + auto y = verts[0].get<1>(); + topInitial = topFirst = surface_mesh.add_vertex(CGPoint_3{ x, y, z_top }); + bottomInitial = bottomFirst = + surface_mesh.add_vertex(CGPoint_3{ x, y, z_bottom }); + + // the vertices are connected in order; The assumption is that the last point + // of the polygon connects to the first + for (size_t j = 1; j < verts.size(); ++j) + { + x = verts[j].get<0>(); + y = verts[j].get<1>(); + topSecond = surface_mesh.add_vertex(CGPoint_3{ x, y, z_top }); + bottomSecond = surface_mesh.add_vertex(CGPoint_3{ x, y, z_bottom }); + + upper_surface.push_back(topFirst); + lower_surface.push_back(bottomFirst); + face_desc = + surface_mesh.add_face(topFirst, topSecond, bottomSecond, bottomFirst); + + if (face_desc == CGSurfaceMesh::null_face()) + { + throw std::invalid_argument( + "Could not add side face from Polygon2d to SurfaceMesh correctly!"); + } + + topFirst = topSecond; + bottomFirst = bottomSecond; + } + upper_surface.push_back(topFirst); + lower_surface.push_back(bottomFirst); + + std::vector descriptors; + + descriptors.push_back( + surface_mesh.add_face(topFirst, topInitial, bottomInitial, bottomFirst)); + descriptors.push_back(surface_mesh.add_face(lower_surface)); + + // for whatever reason the vertices need to be reversed for the latest face to + // construct the mesh correctly. + // If the construction of the latest face does not work ALWAYS try + // reversing the vertex order + std::reverse(upper_surface.begin(), upper_surface.end()); + descriptors.push_back(surface_mesh.add_face(upper_surface)); + + // 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 upper/lower or latest side face from" + " Polygon2d to SurfaceMesh correctly!"); + } + + return surface_mesh; +} + +void CoreDataset::checkPartialIntersectionWithZExtrudedPolygon( + CGSurfaceMesh enclosedMesh, + const seerep_core_msgs::Polygon2D& enclosingPolygon, + 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. Skipping precise check..."; + 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. Skipping precise check..."; + return; + } + + triangulate_faces(enclosedMesh); + triangulate_faces(enclosingMesh); + + partialEncapsulation = + CGAL::Polygon_mesh_processing::do_intersect(enclosedMesh, enclosingMesh); +} + +CGPolygon_2 CoreDataset::reduceZDimension(const CGSurfaceMesh& mesh) +{ + CGPolygon_2 poly2; + + for (CGSurfaceMesh::Vertex_index idx : mesh.vertices()) + { + CGPoint_2 p{ mesh.point(idx).x(), mesh.point(idx).y() }; + poly2.push_back(p); + } + return poly2; +} + seerep_core_msgs::AabbTime CoreDataset::getTimeBounds(std::vector datatypes) { @@ -1029,9 +1197,9 @@ CoreDataset::getSpatialBounds(std::vector datatypes) seerep_core_msgs::AABB overallbb; // set the minimum to minimum possible for the datatype - overallbb.max_corner().set<0>(std::numeric_limits::min()); - overallbb.max_corner().set<1>(std::numeric_limits::min()); - overallbb.max_corner().set<2>(std::numeric_limits::min()); + overallbb.max_corner().set<0>(std::numeric_limits::lowest()); + overallbb.max_corner().set<1>(std::numeric_limits::lowest()); + overallbb.max_corner().set<2>(std::numeric_limits::lowest()); // set the maximum for the maximum possible for the datatype overallbb.min_corner().set<0>(std::numeric_limits::max()); diff --git a/seerep_srv/seerep_core/src/core_tf.cpp b/seerep_srv/seerep_core/src/core_tf.cpp index 2461c6284..e223e6563 100644 --- a/seerep_srv/seerep_core/src/core_tf.cpp +++ b/seerep_srv/seerep_core/src/core_tf.cpp @@ -130,6 +130,54 @@ bool CoreTf::canTransform(const std::string& sourceFrame, ros::Time(timeSecs, timeNanos)); } +std::vector +CoreTf::transform(const std::string& sourceFrame, + const std::string& targetFrame, const int64_t& timeSecs, + const int64_t& timeNanos, + const std::vector>& points) +{ + if (sourceFrame == targetFrame) + { + return std::vector{ points.begin(), points.end() }; + } + + std::vector transformed_points; + if (this->canTransform(sourceFrame, targetFrame, timeSecs, timeNanos)) + { + auto tf = + m_tfBuffer.lookupTransform(targetFrame, sourceFrame, + ros::Time(static_cast(timeSecs), + static_cast(timeNanos))); + auto rot = tf.transform.rotation; + auto trans = tf.transform.translation; + tf2::Quaternion q{ rot.x, rot.y, rot.z, rot.w }; + tf2::Vector3 o{ trans.x, trans.y, trans.z }; + tf2::Transform tf2_tf{ q, o }; + + for (auto&& p : points) + { + tf2::Vector3 vec{ p.get().x().exact().convert_to(), + p.get().y().exact().convert_to(), + p.get().z().exact().convert_to() }; + + tf2::Vector3 vec_target = tf2_tf * vec; + + CGPoint_3 p_target{ vec_target.getX(), vec_target.getY(), + vec_target.getZ() }; + + transformed_points.push_back(p_target); + } + } + else + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) + << "Could not find transform" << sourceFrame << " -> " << targetFrame + << " in tf buffer at time " << (timeSecs * 10 ^ 9) << timeNanos + << "ns."; + } + return transformed_points; +} + std::vector CoreTf::getFrames() { BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) diff --git a/tests/python/gRPC/images/test_gRPC_fb_queryImagePrecise.py b/tests/python/gRPC/images/test_gRPC_fb_queryImagePrecise.py new file mode 100644 index 000000000..c8c6fa50a --- /dev/null +++ b/tests/python/gRPC/images/test_gRPC_fb_queryImagePrecise.py @@ -0,0 +1,228 @@ +# test file for +# gRPC_fb_sendImages.py +# gRPC_fb_queryImages.py + +from typing import Tuple +from uuid import uuid4 + +import flatbuffers +from grpc import Channel +from gRPC.images import gRPC_fb_queryImages, gRPC_fb_sendImages +from quaternion import quaternion as create_quat +from seerep.fb import ( + camera_intrinsics_service_grpc_fb as camera_intrinsic_service, +) +from seerep.fb import tf_service_grpc_fb as tf_service +from seerep.util.fb_helper import ( + createCameraIntrinsics, + createHeader, + createPoint2d, + createPolygon2D, + createQuaternion, + createRegionOfInterest, + createTimeStamp, + createTransform, + createTransformStamped, + createVector3, +) +from seerep.util.fb_to_dict import SchemaFileNames, fb_flatc_dict + + +def send_simple_camintrinsics( + grpc_channel: Channel, project_uuid: str, frame_id: str = "camera" +) -> str: + """ + Create a simple Camera intrinsics with the Frustum in the form of + a square pyramid with a side length and a height of 1. + + Returns: + str: The uuid of the created camera intrinsics. + """ + fbb = flatbuffers.Builder() + camera_intrinsic_service_stub = ( + camera_intrinsic_service.CameraIntrinsicsServiceStub(grpc_channel) + ) + timestamp = createTimeStamp(fbb, 0, 0) + ci_uuid = str(uuid4()) + header = createHeader(fbb, timestamp, frame_id, project_uuid, ci_uuid) + roi = createRegionOfInterest(fbb, 0, 0, 0, 0, False) + + distortion_matrix: list[float] = [4, 5, 6, 7, 8, 9, 10, 11, 12] + rect_matrix: list[float] = [4, 5, 6, 7, 8, 9, 10, 11, 12] + # important for frustum calculations are the indices 0 and 4 + # the frustum calculation can be found in + # seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp + intrins_matrix: list[float] = [1, 5, 6, 7, 1, 9, 10, 11, 12] + proj_matrix: list[float] = [4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15] + + ci_height = 1 + ci_width = 1 + + max_view_dist = 1 + + cameraintrinsics = createCameraIntrinsics( + fbb, + header, + ci_height, + ci_width, + "plumb_bob", + distortion_matrix, + intrins_matrix, + rect_matrix, + proj_matrix, + 4, + 5, + roi, + max_view_dist, + ) + fbb.Finish(cameraintrinsics) + camera_intrinsic_service_stub.TransferCameraIntrinsics(bytes(fbb.Output())) + return ci_uuid + + +def send_tf( + grpc_channel: Channel, + project_uuid: str, + timestamp: Tuple[int, int], + translation: Tuple[float, float, float], + quaternion: Tuple[float, float, float, float], + frame_id: str = "map", + child_frame_id="camera", +) -> bytes: + """ + send a frame_id -> child_frame_id transform. + + Args: + grpc_channel (Channel): The grpc_channel to a SEEREP server. + project_uuid (str): The project target for the transformations. + timestamp (Tuple[int, int]): The timestamp for the sent tf. + translation (Tuple[float, float, float]): the translation vector + in the form (x, y, z) + quaternion (int): the quaternion representing the orientation + of the tf in (w, x, y, z) + frame_id (str): The parent frame for the transformations. + child_frame_id (str): The child frame for the transformations. + + Return: + the serialized tf object + """ + builder = flatbuffers.Builder(1024) + + secs, nanos = timestamp + ts = createTimeStamp(builder, secs, nanos) + + header = createHeader(builder, ts, frame_id, project_uuid, str(uuid4())) + + trans = createVector3(builder, translation) + quat = createQuaternion(builder, create_quat(*quaternion)) + + tf = createTransform(builder, trans, quat) + tfs = createTransformStamped(builder, child_frame_id, header, tf) + + builder.Finish(tfs) + serialized_tf = bytes(builder.Output()) + tf_service.TfServiceStub(grpc_channel).TransferTransformStamped( + iter([serialized_tf]) + ) + return serialized_tf + + +def test_queryImagePreciseIntersect(grpc_channel, project_setup): + _, proj_uuid = project_setup + + timestamp_secs = 1661336507 + timestamp_nanos = 1245 + + ts = (timestamp_secs, timestamp_nanos) + + camera_intrinsics_uuid = send_simple_camintrinsics(grpc_channel, proj_uuid) + + sent_images = gRPC_fb_sendImages.send_images( + grpc_channel, + proj_uuid, + camera_intrinsics_uuid, + gRPC_fb_sendImages.generate_image_ressources(1), + 1 * [ts], + ) + + # make sure the frustum is above the query cube + trans = (-0.5, -0.5, 1.5) + + # this is equivalent to a Euler XYZ rotation with: + # X = 225° + # Y = 0° + # Z = -45° + rot = (-0.354, 0.854, -0.354, 0.146) + send_tf(grpc_channel, proj_uuid, ts, trans, rot) + + builder = flatbuffers.Builder(1024) + + verts = [ + createPoint2d(builder, *p) for p in [(0, 0), (1, 0), (1, 1), (0, 1)] + ] + + polygon = createPolygon2D(builder, height=1, z=0, vertices=verts) + + queried_images = gRPC_fb_queryImages.query_images_raw( + builder, grpc_channel, proj_uuid, polygon2d=polygon + ) + + sent_images = [ + fb_flatc_dict(img, SchemaFileNames.IMAGE) for img in sent_images + ] + + queried_images = [ + fb_flatc_dict(img, SchemaFileNames.IMAGE) for img in queried_images + ] + + assert sorted( + sent_images, key=lambda img: img["header"]["uuid_msgs"] + ) == sorted(queried_images, key=lambda img: img["header"]["uuid_msgs"]) + + +# this test should detect no intersection with the precise method +# but would fail with the AABB approximation +def test_queryImagePreciseNoIntersect(grpc_channel, project_setup): + _, proj_uuid = project_setup + + timestamp_secs = 1661336507 + timestamp_nanos = 1245 + + ts = (timestamp_secs, timestamp_nanos) + + camera_intrinsics_uuid = send_simple_camintrinsics(grpc_channel, proj_uuid) + + sent_images = gRPC_fb_sendImages.send_images( + grpc_channel, + proj_uuid, + camera_intrinsics_uuid, + gRPC_fb_sendImages.generate_image_ressources(1), + 1 * [ts], + ) + + trans = (-2.3, 2, 1) + rot = (1, 0, 0, 0) + send_tf(grpc_channel, proj_uuid, ts, trans, rot) + + builder = flatbuffers.Builder(1024) + + verts = [ + createPoint2d(builder, *p) for p in [(-1, 1), (-1, 3), (-2, 3), (-2, 1)] + ] + + polygon = createPolygon2D(builder, height=2, z=-0.5, vertices=verts) + + queried_images = gRPC_fb_queryImages.query_images_raw( + builder, grpc_channel, proj_uuid, polygon2d=polygon + ) + + sent_images = [ + fb_flatc_dict(img, SchemaFileNames.IMAGE) for img in sent_images + ] + + queried_images = [ + fb_flatc_dict(img, SchemaFileNames.IMAGE) for img in queried_images + ] + + assert len(sent_images) > 0 + assert len(queried_images) == 0 diff --git a/tests/python/gRPC/pointcloud/test_gRPC_fb_queryPCPrecise.py b/tests/python/gRPC/pointcloud/test_gRPC_fb_queryPCPrecise.py new file mode 100644 index 000000000..85417b2f0 --- /dev/null +++ b/tests/python/gRPC/pointcloud/test_gRPC_fb_queryPCPrecise.py @@ -0,0 +1,245 @@ +# test file for +# gRPC_fb_queryPointCloud.py +# gRPC_fb_sendPointCloud.py +from typing import List, Sequence, Tuple, Union +from uuid import uuid4 + +import flatbuffers +import numpy as np +from grpc import Channel +from quaternion import quaternion as create_quat +from seerep.fb import ( + PointCloud2, +) +from seerep.fb import point_cloud_service_grpc_fb as pointCloudService +from seerep.fb import tf_service_grpc_fb as tf_service +from seerep.util.fb_helper import ( + addToPointFieldVector, + create_label, + create_label_category, + createHeader, + createPoint2d, + createPointFields, + createPolygon2D, + createQuaternion, + createQuery, + createTimeStamp, + createTransform, + createTransformStamped, + createVector3, +) +from seerep.util.fb_to_dict import SchemaFileNames, fb_flatc_dict + + +class Point3: + def __init__(self, x: float, y: float, z: float): + self.x = x + self.y = y + self.z = z + + +def create_point_cloud_aabb( + builder, + header, + aabb_point1: Point3, + aabb_point2: Point3, + num_labels: int = 1, +): + """Creates a flatbuffers point cloud message in form of a AABB + using min_point and max_point""" + pointFields = createPointFields(builder, ["x", "y", "z", "rgba"], 7, 4, 1) + pointFieldsVector = addToPointFieldVector(builder, pointFields) + + # create labels + labelsStrings = [f"Label{i}" for i in range(num_labels)] + instance_uuids = [str(uuid4()) for _ in range(num_labels)] + + labels = [] + for i in range(len(labelsStrings)): + labels.append( + create_label( + builder=builder, + label=labelsStrings[i], + label_id=1, + instance_uuid=instance_uuids[i], + instance_id=5, + ) + ) + labelsCategory = [] + labelsCategory.append( + create_label_category( + builder=builder, + labels=labels, + datumaro_json="a very valid datumaro json", + category="category Z", + ) + ) + + PointCloud2.StartLabelsVector(builder, len(labelsCategory)) + for label in labelsCategory: + builder.PrependUOffsetTRelative(label) + labelsCatOffset = builder.EndVector() + + # Note: rgb field is float, for simplification + pointsBox = [[]] + x_min = min(aabb_point1.x, aabb_point2.x) + y_min = min(aabb_point1.y, aabb_point2.y) + z_min = min(aabb_point1.z, aabb_point2.z) + x_max = max(aabb_point1.x, aabb_point2.x) + y_max = max(aabb_point1.y, aabb_point2.y) + z_max = max(aabb_point1.y, aabb_point2.y) + # for x in range(int(x_min), int(x_max) + 1, 1): + # for y in range(int(y_min), int(y_max) + 1, 1): + # for z in range(int(z_min), int(z_max) + 1, 1): + # pointsBox[0].append([x, y, z, 0]) + # also include the minimal and maximal bounding point + pointsBox[0].append([x_min, y_min, z_min, 0]) + pointsBox[0].append([x_max, y_max, z_max, 0]) + points = np.array(pointsBox).astype(np.float32) + + pointsVector = builder.CreateByteVector(points.tobytes()) + + # add all data into the flatbuffers point cloud message + PointCloud2.Start(builder) + PointCloud2.AddHeader(builder, header) + PointCloud2.AddHeight(builder, points.shape[0]) + PointCloud2.AddWidth(builder, points.shape[1]) + PointCloud2.AddIsBigendian(builder, True) + PointCloud2.AddPointStep(builder, 16) + PointCloud2.AddRowStep(builder, points.shape[1] * 16) + PointCloud2.AddFields(builder, pointFieldsVector) + PointCloud2.AddData(builder, pointsVector) + PointCloud2.AddLabels(builder, labelsCatOffset) + return PointCloud2.End(builder) + + +def send_pcs( + grpc_channel: Channel, + pcs: Union[int, Sequence[int], bytearray, Sequence[bytearray]], +) -> List[bytearray]: + """sends a serialized pointcloud flatbuffers message""" + if ( + type(pcs) is not int + and type(pcs) is not bytes + and (type(pcs) is not bytearray) + ): + if len(pcs) > 0 and type(pcs[0]) is int: + pcs = [bytes(pc) for pc in pcs] + elif type(pcs) is int or type(pcs) is bytearray: + pcs = [bytes(pcs)] + else: + pcs = [pcs] + + stub = pointCloudService.PointCloudServiceStub(grpc_channel) + stub.TransferPointCloud2(iter(pcs)) + return pcs + + +def send_tf( + grpc_channel: Channel, + project_uuid: str, + timestamp: Tuple[int, int], + translation: Tuple[float, float, float], + quaternion: Tuple[float, float, float, float], + frame_id: str = "map", + child_frame_id="camera", +) -> bytes: + """ + send a frame_id -> child_frame_id transform. + + Args: + grpc_channel (Channel): The grpc_channel to a SEEREP server. + project_uuid (str): The project target for the transformations. + timestamp (Tuple[int, int]): The timestamp for the sent tf. + translation (Tuple[float, float, float]): the translation vector + in the form (x, y, z) + quaternion (int): the quaternion representing the orientation + of the tf in (w, x, y, z) + frame_id (str): The parent frame for the transformations. + child_frame_id (str): The child frame for the transformations. + + Return: + the serialized tf object + """ + builder = flatbuffers.Builder(1024) + + secs, nanos = timestamp + ts = createTimeStamp(builder, secs, nanos) + + header = createHeader(builder, ts, frame_id, project_uuid, str(uuid4())) + + trans = createVector3(builder, translation) + quat = createQuaternion(builder, create_quat(*quaternion)) + + tf = createTransform(builder, trans, quat) + tfs = createTransformStamped(builder, child_frame_id, header, tf) + + builder.Finish(tfs) + serialized_tf = bytes(builder.Output()) + tf_service.TfServiceStub(grpc_channel).TransferTransformStamped( + iter([serialized_tf]) + ) + return serialized_tf + + +def test_queryPCPreciseNoIntersect(grpc_channel, project_setup): + _, proj_uuid = project_setup + + builder = flatbuffers.Builder() + pcl_stub = pointCloudService.PointCloudServiceStub(grpc_channel) + + timestamp_secs = 1661336507 + timestamp_nanos = 1245 + + ts_obj = createTimeStamp(builder, timestamp_secs, timestamp_nanos) + + header = createHeader(builder, ts_obj, "pc_test", proj_uuid, str(uuid4())) + + pc_msg = create_point_cloud_aabb( + builder, header, Point3(0, 0, 0), Point3(3, 2, 2) + ) + builder.Finish(pc_msg) + sent_pcs = send_pcs(grpc_channel, builder.Output()) + + translation = (-4, -4, -2) + # This is equivalent to XYZ Euler: + # X = 45° + # Y = 0° + # Z = 0° + quaternion = (0.924, 0.383, 0, 0) + send_tf( + grpc_channel, + proj_uuid, + (timestamp_secs, timestamp_nanos), + translation, + quaternion, + child_frame_id="pc_test", + ) + + builder = flatbuffers.Builder() + # build the query + verts = [ + createPoint2d(builder, *p) + for p in [(-4, -3), (-2, -3), (-2, -1), (-4, -1)] + ] + polygon = createPolygon2D(builder, height=2, z=0.5, vertices=verts) + query = createQuery(builder, polygon2d=polygon) + builder.Finish(query) + queried_pcs = pcl_stub.GetPointCloud2(bytes(builder.Output())) + + # convert the sent/queried pcs + sent_pcs = sorted( + [fb_flatc_dict(pcl, SchemaFileNames.POINT_CLOUD_2) for pcl in sent_pcs], + key=lambda pc: pc["header"]["uuid_msgs"], + ) + + queried_pcs = sorted( + [ + fb_flatc_dict(pcl, SchemaFileNames.POINT_CLOUD_2) + for pcl in queried_pcs + ], + key=lambda pc: pc["header"]["uuid_msgs"], + ) + + assert len(queried_pcs) == 0 + assert len(sent_pcs) == 1