From 59daa92d24e33d96f5f50b9e46ad2aa9a889346f Mon Sep 17 00:00:00 2001 From: mmeijerdfki Date: Sun, 8 Dec 2024 23:36:27 +0100 Subject: [PATCH] fix construction of meshes and intersect tests --- .../seerep_hdf5_core/hdf5_core_image.h | 5 +- .../seerep_hdf5_core/src/hdf5_core_image.cpp | 6 +- seerep_msgs/core/timestamp_frame_points.h | 2 +- .../include/seerep_core/core_dataset.h | 16 +++- seerep_srv/seerep_core/src/core_dataset.cpp | 96 ++++++++++++++----- 5 files changed, 93 insertions(+), 32 deletions(-) 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 be76710d..560f7d93 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 @@ -30,7 +30,7 @@ namespace seerep_hdf5_core using ExactKernel = CGAL::Exact_predicates_exact_constructions_kernel; using CGPoint_3 = ExactKernel::Point_3; -using SurfaceMesh = CGAL::Surface_mesh; +using CGSurfaceMesh = CGAL::Surface_mesh; /** * @brief Helper struct to summarize the general attributes of an image @@ -166,7 +166,8 @@ class Hdf5CoreImage : public virtual Hdf5CoreGeneral, std::array computeFrustumPoints(const std::string& camintrinsics_uuid); - SurfaceMesh computeFrustumMesh(std::array& points); + CGSurfaceMesh + computeFrustumMesh(std::array& points); /** * @brief Computes the frustum for given camera intrinsic parameters and stores 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 0c9667ef..0fc27653 100644 --- a/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp +++ b/seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp @@ -105,7 +105,7 @@ Hdf5CoreImage::getPolygonConstraintPoints( auto points = this->computeFrustumPoints(camintrinsics_uuid); auto mesh = this->computeFrustumMesh(points); - return seerep_core_msgs::TimestampFramePoints{ ts, SurfaceMesh{}, frame_id }; + return seerep_core_msgs::TimestampFramePoints{ ts, frame_id, mesh }; } void Hdf5CoreImage::writeLabels( @@ -197,10 +197,10 @@ Hdf5CoreImage::computeFrustumPoints(const std::string& camintrinsics_uuid) far_bottomright }; } -SurfaceMesh +CGSurfaceMesh Hdf5CoreImage::computeFrustumMesh(std::array& points) { - SurfaceMesh mesh; + CGSurfaceMesh mesh; auto o = mesh.add_vertex( CGPoint_3{ points[0].get<0>(), points[0].get<1>(), points[0].get<2>() }); diff --git a/seerep_msgs/core/timestamp_frame_points.h b/seerep_msgs/core/timestamp_frame_points.h index 0db7b0bd..87e8a0a7 100644 --- a/seerep_msgs/core/timestamp_frame_points.h +++ b/seerep_msgs/core/timestamp_frame_points.h @@ -16,8 +16,8 @@ using SurfaceMesh = CGAL::Surface_mesh; struct TimestampFramePoints { seerep_core_msgs::Timestamp timestamp; - SurfaceMesh mesh; std::string frame_id; + SurfaceMesh mesh; }; } // namespace seerep_core_msgs 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 cde1d698..91d7d730 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h @@ -3,8 +3,9 @@ #include #include +#include +#include #include -#include #include #include @@ -33,9 +34,10 @@ typedef CGAL::Exact_predicates_exact_constructions_kernel ExactKernel; typedef CGAL::Point_3 CGPoint_3; -typedef CGAL::Surface_mesh SurfaceMesh; +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 { @@ -315,12 +317,18 @@ class CoreDataset bool& fullEncapsulation, bool& partialEncapsulation); + /** + * + * + */ void checkIntersectionWithZExtrudedPolygon( - const SurfaceMesh& enclosedMesh, + CGSurfaceMesh enclosedMesh, const seerep_core_msgs::Polygon2D& enclosingPolygon, bool& fullEncapsulation, bool& partialEncapsulation); - CGPolygon_2 reduceZDimension(const SurfaceMesh& mesh); + CGPolygon_2 reduceZDimension(const CGSurfaceMesh& mesh); + + CGSurfaceMesh toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon); void getUuidsFromMap( std::unordered_map, diff --git a/seerep_srv/seerep_core/src/core_dataset.cpp b/seerep_srv/seerep_core/src/core_dataset.cpp index f9d73adc..947a2634 100644 --- a/seerep_srv/seerep_core/src/core_dataset.cpp +++ b/seerep_srv/seerep_core/src/core_dataset.cpp @@ -312,7 +312,7 @@ std::optional> CoreDataset::queryRtree auto ts_frame_points = coreDatatype.getPolygonConstraintPoints(it->second); if (ts_frame_points.has_value()) { - SurfaceMesh& mesh = ts_frame_points->mesh; + CGSurfaceMesh& mesh = ts_frame_points->mesh; std::vector> ref_points; for (auto& idx : mesh.vertices()) @@ -1022,21 +1022,26 @@ void CoreDataset::intersectionDegreeAABBinPolygon( fullEncapsulation, partialEncapsulation); } -SurfaceMesh toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon) +CGSurfaceMesh +CoreDataset::toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon) { - using VertexIndex = SurfaceMesh::Vertex_index; + using CGVertexIndex = CGSurfaceMesh::Vertex_index; if (seerep_polygon.vertices.size() <= 2) { // TODO: throw error + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "seerep_core_msgs::Polygon2D has not enough vertices to be expanded " + "to a full 3D SurfaceMesh"; } - SurfaceMesh surface_mesh; + CGSurfaceMesh surface_mesh; - std::vector lower_surface, upper_surface; + std::vector lower_surface, upper_surface; + CGVertexIndex topInitial, bottomInitial; + CGVertexIndex topFirst, bottomFirst, topSecond, bottomSecond; - VertexIndex topInitial, bottomInitial; - VertexIndex topFirst, bottomFirst, topSecond, bottomSecond; + CGFaceIdx face_desc; auto z_top = seerep_polygon.z + seerep_polygon.height; auto& z_bottom = seerep_polygon.z; @@ -1048,8 +1053,8 @@ SurfaceMesh toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon) bottomInitial = bottomFirst = surface_mesh.add_vertex(CGPoint_3{ x, y, z_bottom }); - // the vertices are connected in order; It is assumed that the last point of - // the polygon connects to the first + // 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>(); @@ -1058,27 +1063,62 @@ SurfaceMesh toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon) bottomSecond = surface_mesh.add_vertex(CGPoint_3{ x, y, z_bottom }); upper_surface.push_back(topFirst); - upper_surface.push_back(topSecond); lower_surface.push_back(bottomFirst); - lower_surface.push_back(bottomSecond); - surface_mesh.add_face(topFirst, topSecond, bottomFirst, bottomSecond); + 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; } - surface_mesh.add_face(topFirst, topInitial, bottomFirst, bottomInitial); - surface_mesh.add_face(lower_surface); - surface_mesh.add_face(upper_surface); + upper_surface.push_back(topFirst); + lower_surface.push_back(bottomFirst); + + face_desc = + surface_mesh.add_face(topFirst, topInitial, bottomInitial, bottomFirst); + if (face_desc == CGSurfaceMesh::null_face()) + { + throw std::invalid_argument("Could not add last side face from Polygon2d " + "to SurfaceMesh correctly!"); + } + face_desc = surface_mesh.add_face(lower_surface); + if (face_desc == CGSurfaceMesh::null_face()) + { + throw std::invalid_argument("Could not add lower surface face from " + "Polygon2d to SurfaceMesh correctly!"); + } + + std::reverse(upper_surface.begin(), upper_surface.end()); + + face_desc = surface_mesh.add_face(upper_surface); + if (face_desc == CGSurfaceMesh::null_face()) + { + throw std::invalid_argument("Could not add upper surface face from " + "Polygon2d to SurfaceMesh correctly!"); + } return surface_mesh; } void CoreDataset::checkIntersectionWithZExtrudedPolygon( - const SurfaceMesh& enclosedMesh, + CGSurfaceMesh enclosedMesh, const seerep_core_msgs::Polygon2D& enclosingPolygon, bool& fullEncapsulation, 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"; + return; + } + fullEncapsulation = false; partialEncapsulation = false; @@ -1086,15 +1126,15 @@ void CoreDataset::checkIntersectionWithZExtrudedPolygon( // get highest and lowest point of innerPoints auto highestPoint = std::max_element( enclosedMesh.vertices_begin(), enclosedMesh.vertices_end(), - [&enclosedMesh](const SurfaceMesh::Vertex_index& p1, - const SurfaceMesh::Vertex_index& p2) { + [&enclosedMesh](const CGSurfaceMesh::Vertex_index& p1, + const CGSurfaceMesh::Vertex_index& p2) { return enclosedMesh.point(p1).z() < enclosedMesh.point(p2).z(); }); auto lowestPoint = std::min_element( enclosedMesh.vertices_begin(), enclosedMesh.vertices_end(), - [&enclosedMesh](const SurfaceMesh::Vertex_index& p1, - const SurfaceMesh::Vertex_index& p2) { + [&enclosedMesh](const CGSurfaceMesh::Vertex_index& p1, + const CGSurfaceMesh::Vertex_index& p2) { return enclosedMesh.point(p1).z() > enclosedMesh.point(p2).z(); }); @@ -1132,15 +1172,27 @@ void CoreDataset::checkIntersectionWithZExtrudedPolygon( 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"; + return; + } + + triangulate_faces(enclosedMesh); + triangulate_faces(enclosingMesh); + partialEncapsulation = CGAL::Polygon_mesh_processing::do_intersect(enclosedMesh, enclosingMesh); } -CGPolygon_2 CoreDataset::reduceZDimension(const SurfaceMesh& mesh) +CGPolygon_2 CoreDataset::reduceZDimension(const CGSurfaceMesh& mesh) { CGPolygon_2 poly2; - for (SurfaceMesh::Vertex_index idx : mesh.vertices()) + for (CGSurfaceMesh::Vertex_index idx : mesh.vertices()) { CGPoint_2 p{ mesh.point(idx).x(), mesh.point(idx).y() }; poly2.push_back(p);