From 25a5a02a4b800439d967aa5abac8b4d64cd95662 Mon Sep 17 00:00:00 2001 From: mmeijerdfki Date: Mon, 30 Dec 2024 18:53:39 +0100 Subject: [PATCH] only perform precise check when needed; remove unneeded functionality --- .../include/seerep_core/core_dataset.h | 10 +- seerep_srv/seerep_core/src/core_dataset.cpp | 111 ++++++------------ 2 files changed, 40 insertions(+), 81 deletions(-) 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 6d4c2176..1c865197 100644 --- a/seerep_srv/seerep_core/include/seerep_core/core_dataset.h +++ b/seerep_srv/seerep_core/include/seerep_core/core_dataset.h @@ -338,22 +338,20 @@ class CoreDataset bool& partialEncapsulation); /** - * @brief checks whether the enclosedMesh is really enclosed by the + * @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 fullEncapsulation flag which is set when enclosedMesh is fully - * encapsulated by the enclosingPolygon * @param partialEncapsulation flag which is set when enclosedMesh is - * partially encapsulated by the enclosingPolygon + * partially encapsulated by the enclosingPolygon */ - void checkIntersectionWithZExtrudedPolygon( + void checkPartialIntersectionWithZExtrudedPolygon( CGSurfaceMesh enclosedMesh, const seerep_core_msgs::Polygon2D& enclosingPolygon, - bool& fullEncapsulation, bool& partialEncapsulation); + bool& partialEncapsulation); /** * @brief Creates a 2DPolygon by removing the 3rd dimension from the points of the Mesh diff --git a/seerep_srv/seerep_core/src/core_dataset.cpp b/seerep_srv/seerep_core/src/core_dataset.cpp index c6dbd6e3..095adf89 100644 --- a/seerep_srv/seerep_core/src/core_dataset.cpp +++ b/seerep_srv/seerep_core/src/core_dataset.cpp @@ -309,31 +309,43 @@ std::optional> CoreDataset::queryRtree intersectionDegree(it->first, polygon, fullyEncapsulated, partiallyEncapsulated); - auto ts_frame_points = coreDatatype.getPolygonConstraintMesh(it->second); - if (ts_frame_points.has_value()) + // 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 only true for partialEncapsulation must + // be verified, as this could be indeed false with the real object + if (partiallyEncapsulated == true && fullyEncapsulated == false) { - CGSurfaceMesh& mesh = ts_frame_points->mesh; - std::vector> ref_points; - - for (auto& idx : mesh.vertices()) + auto ts_frame_points = coreDatatype.getPolygonConstraintMesh(it->second); + if (ts_frame_points.has_value()) { - ref_points.push_back(mesh.point(idx)); - } + CGSurfaceMesh& mesh = ts_frame_points->mesh; + std::vector> ref_points; - // 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()) + { + ref_points.push_back(mesh.point(idx)); + } - for (auto& idx : mesh.vertices()) - { - mesh.point(idx) = points_vec[idx.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); - // check if these point are enclosed by the query polygon - checkIntersectionWithZExtrudedPolygon(mesh, polygon, fullyEncapsulated, - partiallyEncapsulated); + 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 @@ -1103,10 +1115,10 @@ CoreDataset::toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon) return surface_mesh; } -void CoreDataset::checkIntersectionWithZExtrudedPolygon( +void CoreDataset::checkPartialIntersectionWithZExtrudedPolygon( CGSurfaceMesh enclosedMesh, const seerep_core_msgs::Polygon2D& enclosingPolygon, - bool& fullEncapsulation, bool& partialEncapsulation) + bool& partialEncapsulation) { using CGAL::Polygon_mesh_processing::triangulate_faces; @@ -1114,61 +1126,10 @@ void CoreDataset::checkIntersectionWithZExtrudedPolygon( { BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "enclosedMesh passed to checkIntersectionWithZExtrudedPolygon " - "is either not valid or empty"; + "is either not valid or empty. Skipping precise check..."; return; } - fullEncapsulation = false; - partialEncapsulation = false; - - // check if the enclosing polygon encloses every point - // get highest and lowest point of innerPoints - auto highestPoint = std::max_element( - enclosedMesh.vertices_begin(), enclosedMesh.vertices_end(), - [&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 CGSurfaceMesh::Vertex_index& p1, - const CGSurfaceMesh::Vertex_index& p2) { - return enclosedMesh.point(p1).z() > enclosedMesh.point(p2).z(); - }); - - const double& z_low = enclosingPolygon.z; - double z_high = z_low + enclosingPolygon.height; - - double p_low = - enclosedMesh.point(*lowestPoint).z().exact().convert_to(); - double p_high = - enclosedMesh.point(*highestPoint).z().exact().convert_to(); - - // fully enclosed if all points of the enclosedMesh are contained in the enclosing Polygon - if ((z_low <= p_low && z_low <= p_high && z_high >= p_low && z_high >= p_high)) - { - auto enclosedPolygon2CG = reduceZDimension(enclosedMesh); - auto enclosingPolygon2CG = toCGALPolygon(enclosingPolygon); - - bool xy_bounded = true; - for (CGPolygon_2::Vertex_iterator vi = enclosedPolygon2CG.vertices_begin(); - vi != enclosedPolygon2CG.vertices_end(); ++vi) - { - if (enclosingPolygon2CG.bounded_side(*vi) == CGAL::ON_UNBOUNDED_SIDE) - { - xy_bounded = false; - break; - } - } - if (xy_bounded) - { - fullEncapsulation = true; - partialEncapsulation = true; - return; - } - } - auto enclosingMesh = toSurfaceMesh(enclosingPolygon); if (!enclosingMesh.is_valid() || enclosingMesh.is_empty()) @@ -1176,7 +1137,7 @@ void CoreDataset::checkIntersectionWithZExtrudedPolygon( BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "enclosingMesh derived from enclosingPolygon" "passed to checkIntersectionWithZExtrudedPolygon " - "is either not valid or empty"; + "is either not valid or empty. Skipping precise check..."; return; }