✨ Create the function ComputeClosestPoint to speedup the que…

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
 

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<float>::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<double>();
+    }
 };
 
 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<float>(query_points, "query_points",

cpp/open3d/t/geometry/RaycastingScene.h

@@ -152,6 +152,31 @@ class RaycastingScene {
     std::unordered_map<std::string, core::Tensor> 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

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,

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"]: