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 14f9962c26c..13bbe01641b 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 result.p.cast(); + } }; RaycastingScene::RaycastingScene(int64_t nthreads) @@ -934,6 +961,11 @@ 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..9fc4431ec3b 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.h +++ b/cpp/open3d/t/geometry/RaycastingScene.h @@ -152,6 +152,31 @@ 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 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, 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"]: