From 8481600c045ee96c4a3b7d2f08cd6c32a516b742 Mon Sep 17 00:00:00 2001 From: Seb Mascha Date: Mon, 25 Mar 2024 14:38:33 +0100 Subject: [PATCH 1/5] :sparkles: Create the function ComputeClosestPoint to speedup the query for a single 3D Point --- cpp/open3d/t/geometry/RaycastingScene.cpp | 31 +++++++++++++++++++++++ cpp/open3d/t/geometry/RaycastingScene.h | 17 +++++++++++++ 2 files changed, 48 insertions(+) diff --git a/cpp/open3d/t/geometry/RaycastingScene.cpp b/cpp/open3d/t/geometry/RaycastingScene.cpp index 14f9962c26c..dee3cd17fed 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.cpp +++ b/cpp/open3d/t/geometry/RaycastingScene.cpp @@ -688,6 +688,33 @@ struct RaycastingScene::Impl { LoopFn); } } + + Eigen::Vector3d ComputeClosestPoint(const Eigen::Vector3d& query_point){ + if (!scene_committed_) { + rtcCommitScene(scene_); + scene_committed_ = true; + } + + RTCPointQuery query; + query.x = query_point.x(); + query.y = query_point.y(); + query.z = query_point.z(); + query.radius = std::numeric_limits::infinity(); + query.time = 0.f; + + ClosestPointResult result; + result.geometry_ptrs_ptr = &geometry_ptrs_; + + RTCPointQueryContext instStack; + rtcInitPointQueryContext(&instStack); + rtcPointQuery(scene_, &query, &instStack, &ClosestPointFunc, + (void*)&result); + + // TODO (Sebastien-Mascha): return the normal of the face. + // primitive_ids[i] = result.primID; + + return Eigen::Vector3d(result.p.x, result.p.y, result.p.z); + } }; RaycastingScene::RaycastingScene(int64_t nthreads) @@ -934,6 +961,10 @@ RaycastingScene::ComputeClosestPoints(const core::Tensor& query_points, return result; } +Eigen::Vector3d RaycastingScene::ComputeClosestPoint(const Eigen::Vector3d& query_point) { + return impl_->ComputeClosestPoint(query_point); +} + core::Tensor RaycastingScene::ComputeDistance(const core::Tensor& query_points, const int nthreads) { AssertTensorDtypeLastDimDeviceMinNDim(query_points, "query_points", diff --git a/cpp/open3d/t/geometry/RaycastingScene.h b/cpp/open3d/t/geometry/RaycastingScene.h index f25d994b0b5..fb5b948a05f 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.h +++ b/cpp/open3d/t/geometry/RaycastingScene.h @@ -152,6 +152,23 @@ class RaycastingScene { std::unordered_map ComputeClosestPoints( const core::Tensor &query_points, const int nthreads = 0); + /// \brief Computes the closest point on the surface of the scene to a single query point. + /// + /// This function calculates the closest point on any surface within the scene to the provided query point. + /// It's optimized for single-point queries, making it suitable for applications that require precise, + /// real-time calculations of nearest surface points, such as collision detection or interactive simulations. + /// + /// \param query_point An Eigen::Vector3d object representing the coordinates of the query point in the format [x, y, z]. + /// + /// \return An Eigen::Vector3d object representing the closest point on the surface to the query point. + /// The return format is also [x, y, z], indicating the coordinates of the closest surface point. + /// + /// \note The scene must be committed (prepared for querying) before calling this function. If the scene has not been + /// previously committed, the function will commit it automatically. However, for efficiency, it's recommended + /// to manually commit the scene once all geometries have been added, before performing any queries. + Eigen::Vector3d ComputeClosestPoint( + const Eigen::Vector3d& query_point); + /// \brief Computes the distance to the surface of the scene. /// \param query_points A tensor with >=2 dims, shape {.., 3} and Dtype /// Float32 describing the query points. {..} can be any number of From 99e99266c7562de4789b215a7034152a271df7b0 Mon Sep 17 00:00:00 2001 From: Seb Mascha Date: Mon, 25 Mar 2024 17:30:38 +0100 Subject: [PATCH 2/5] :sparkles: Add changelog and fix bug in output for eigen --- CHANGELOG.md | 1 + cpp/open3d/t/geometry/RaycastingScene.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3509dda95e6..62322e80f61 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -32,6 +32,7 @@ - Fix regression in printing cuda tensor from PR #6444 🐛 - Add Python pathlib support for file IO (PR #6619) - Fix log error message for `probability` argument validation in `PointCloud::SegmentPlane` (PR #6622) +- Add a method for the RaycastingScene class to compute the closest point on the surface of the scene to a single query point. ## 0.13 diff --git a/cpp/open3d/t/geometry/RaycastingScene.cpp b/cpp/open3d/t/geometry/RaycastingScene.cpp index dee3cd17fed..7fea6cd50ed 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.cpp +++ b/cpp/open3d/t/geometry/RaycastingScene.cpp @@ -713,7 +713,7 @@ struct RaycastingScene::Impl { // TODO (Sebastien-Mascha): return the normal of the face. // primitive_ids[i] = result.primID; - return Eigen::Vector3d(result.p.x, result.p.y, result.p.z); + return result.p.cast(); } }; From 46c6b16a29bb3110e246928571a47360871e1c58 Mon Sep 17 00:00:00 2001 From: Seb Mascha Date: Mon, 25 Mar 2024 19:47:09 +0100 Subject: [PATCH 3/5] :art: Format the code --- cpp/open3d/t/geometry/RaycastingScene.cpp | 7 +++-- cpp/open3d/t/geometry/RaycastingScene.h | 34 ++++++++++++++--------- 2 files changed, 25 insertions(+), 16 deletions(-) diff --git a/cpp/open3d/t/geometry/RaycastingScene.cpp b/cpp/open3d/t/geometry/RaycastingScene.cpp index 7fea6cd50ed..13bbe01641b 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.cpp +++ b/cpp/open3d/t/geometry/RaycastingScene.cpp @@ -689,7 +689,7 @@ struct RaycastingScene::Impl { } } - Eigen::Vector3d ComputeClosestPoint(const Eigen::Vector3d& query_point){ + Eigen::Vector3d ComputeClosestPoint(const Eigen::Vector3d& query_point) { if (!scene_committed_) { rtcCommitScene(scene_); scene_committed_ = true; @@ -712,7 +712,7 @@ struct RaycastingScene::Impl { // TODO (Sebastien-Mascha): return the normal of the face. // primitive_ids[i] = result.primID; - + return result.p.cast(); } }; @@ -961,7 +961,8 @@ RaycastingScene::ComputeClosestPoints(const core::Tensor& query_points, return result; } -Eigen::Vector3d RaycastingScene::ComputeClosestPoint(const Eigen::Vector3d& query_point) { +Eigen::Vector3d RaycastingScene::ComputeClosestPoint( + const Eigen::Vector3d& query_point) { return impl_->ComputeClosestPoint(query_point); } diff --git a/cpp/open3d/t/geometry/RaycastingScene.h b/cpp/open3d/t/geometry/RaycastingScene.h index fb5b948a05f..9fc4431ec3b 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.h +++ b/cpp/open3d/t/geometry/RaycastingScene.h @@ -152,22 +152,30 @@ class RaycastingScene { std::unordered_map ComputeClosestPoints( const core::Tensor &query_points, const int nthreads = 0); - /// \brief Computes the closest point on the surface of the scene to a single query point. - /// - /// This function calculates the closest point on any surface within the scene to the provided query point. - /// It's optimized for single-point queries, making it suitable for applications that require precise, - /// real-time calculations of nearest surface points, such as collision detection or interactive simulations. + /// \brief Computes the closest point on the surface of the scene to a + /// single query point. /// - /// \param query_point An Eigen::Vector3d object representing the coordinates of the query point in the format [x, y, z]. + /// This function calculates the closest point on any surface within the + /// scene to the provided query point. It's optimized for single-point + /// queries, making it suitable for applications that require precise, + /// real-time calculations of nearest surface points, such as collision + /// detection or interactive simulations. /// - /// \return An Eigen::Vector3d object representing the closest point on the surface to the query point. - /// The return format is also [x, y, z], indicating the coordinates of the closest surface point. + /// \param query_point An Eigen::Vector3d object representing the + /// coordinates of the query point in the format [x, y, z]. /// - /// \note The scene must be committed (prepared for querying) before calling this function. If the scene has not been - /// previously committed, the function will commit it automatically. However, for efficiency, it's recommended - /// to manually commit the scene once all geometries have been added, before performing any queries. - Eigen::Vector3d ComputeClosestPoint( - const Eigen::Vector3d& query_point); + /// \return An Eigen::Vector3d object representing the closest point on the + /// surface to the query point. + /// The return format is also [x, y, z], indicating the coordinates + /// of the closest surface point. + /// + /// \note The scene must be committed (prepared for querying) before calling + /// this function. If the scene has not been + /// previously committed, the function will commit it automatically. + /// However, for efficiency, it's recommended to manually commit the + /// scene once all geometries have been added, before performing any + /// queries. + Eigen::Vector3d ComputeClosestPoint(const Eigen::Vector3d &query_point); /// \brief Computes the distance to the surface of the scene. /// \param query_points A tensor with >=2 dims, shape {.., 3} and Dtype From 44b10d923a5612207d8fb97b350ae7ce1044cbed Mon Sep 17 00:00:00 2001 From: Seb Mascha Date: Mon, 25 Mar 2024 19:49:37 +0100 Subject: [PATCH 4/5] :art: Format the code python --- examples/python/reconstruction_system/make_fragments.py | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/python/reconstruction_system/make_fragments.py b/examples/python/reconstruction_system/make_fragments.py index 17d875aa86c..5af9d960f4b 100644 --- a/examples/python/reconstruction_system/make_fragments.py +++ b/examples/python/reconstruction_system/make_fragments.py @@ -145,6 +145,7 @@ def make_pointcloud_for_fragment(path_dataset, color_files, depth_files, write_ascii=False, compressed=True) + def process_single_fragment(fragment_id, color_files, depth_files, n_files, n_fragments, config): if config["path_intrinsic"]: From 849ffe5a5d063403f582c33259960baf7fb1cc9d Mon Sep 17 00:00:00 2001 From: Seb Mascha Date: Tue, 26 Mar 2024 09:20:50 +0100 Subject: [PATCH 5/5] :sparkles: Add binding for Python compute_closest_point --- cpp/pybind/t/geometry/raycasting_scene.cpp | 24 ++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/cpp/pybind/t/geometry/raycasting_scene.cpp b/cpp/pybind/t/geometry/raycasting_scene.cpp index e7f6dcecbd5..5fb00b23a7f 100644 --- a/cpp/pybind/t/geometry/raycasting_scene.cpp +++ b/cpp/pybind/t/geometry/raycasting_scene.cpp @@ -310,6 +310,30 @@ Computes the closest points on the surfaces of the scene. A tensor with the normals of the closest triangle . The shape is {.., 3}. +)doc"); + + raycasting_scene.def("compute_closest_point", + &RaycastingScene::ComputeClosestPoint, "query_point"_a, + R"doc( +Computes the closest point on the surface of the scene to a single query point. + +Args: + query_point (open3d.core.Tensor): A tensor with 1 dim, shape {3}, + and Dtype Float32 describing the query point. + The dimension must be 3 and has the format [x, y, z]. + +Returns: + An open3d.core.Tensor representing the closest point on the surface to the + query point. The shape is {3}, and the format is [x, y, z], indicating + the coordinates of the closest surface point. + +Note: + The scene must be committed (prepared for querying) before calling this + function. If the scene has not been previously committed, the function + will commit it automatically. However, for efficiency, it's recommended + to manually commit the scene once all geometries have been added, before + performing any queries. + )doc"); raycasting_scene.def("compute_distance", &RaycastingScene::ComputeDistance,