From d7d8d4473b2611cbfaee65210ecbe4451cd659e4 Mon Sep 17 00:00:00 2001 From: dmitrishastin Date: Sun, 22 Oct 2023 14:09:48 +0100 Subject: [PATCH 01/14] add list intersections function --- cpp/open3d/t/geometry/RaycastingScene.cpp | 197 ++++++++++++++++++++- cpp/open3d/t/geometry/RaycastingScene.h | 17 ++ cpp/pybind/t/geometry/raycasting_scene.cpp | 37 +++- 3 files changed, 249 insertions(+), 2 deletions(-) diff --git a/cpp/open3d/t/geometry/RaycastingScene.cpp b/cpp/open3d/t/geometry/RaycastingScene.cpp index 412d6130862..c2099f72307 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.cpp +++ b/cpp/open3d/t/geometry/RaycastingScene.cpp @@ -110,6 +110,72 @@ void CountIntersectionsFunc(const RTCFilterFunctionNArguments* args) { } } +struct ListIntersectionsContext { + RTCIntersectContext context; + std::vector>* + previous_geom_prim_ID_tfar; + unsigned int* ray_ids; + unsigned int* geometry_ids; + unsigned int* primitive_ids; + float* t_hit; + Eigen::VectorXi cumsum; + unsigned int* track_intersections; +}; + +void ListIntersectionsFunc(const RTCFilterFunctionNArguments* args) { + int* valid = args->valid; + const ListIntersectionsContext* context = + reinterpret_cast(args->context); + struct RTCRayN* rayN = args->ray; + struct RTCHitN* hitN = args->hit; + const unsigned int N = args->N; + + // Avoid crashing when debug visualizations are used. + if (context == nullptr) return; + + std::vector>* + previous_geom_prim_ID_tfar = context->previous_geom_prim_ID_tfar; + unsigned int* ray_ids = context->ray_ids; + unsigned int* geometry_ids = context->geometry_ids; + unsigned int* primitive_ids = context->primitive_ids; + float* t_hit = context->t_hit; + Eigen::VectorXi cumsum = context->cumsum; + unsigned int* track_intersections = context->track_intersections; + + // Iterate over all rays in ray packet. + for (unsigned int ui = 0; ui < N; ui += 1) { + // Calculate loop and execution mask + unsigned int vi = ui + 0; + if (vi >= N) continue; + + // Ignore inactive rays. + if (valid[vi] != -1) continue; + + // Read ray/hit from ray structure. + RTCRay ray = rtcGetRayFromRayN(rayN, N, ui); + RTCHit hit = rtcGetHitFromHitN(hitN, N, ui); + + unsigned int ray_id = ray.id; + std::tuple gpID(hit.geomID, hit.primID, + ray.tfar); + auto& prev_gpIDtfar = previous_geom_prim_ID_tfar->operator[](ray_id); + if (std::get<0>(prev_gpIDtfar) != hit.geomID || + (std::get<1>(prev_gpIDtfar) != hit.primID && + std::get<2>(prev_gpIDtfar) != ray.tfar)) { + size_t idx = cumsum[ray_id] + track_intersections[ray_id]; + ray_ids[idx] = ray_id; + geometry_ids[idx] = hit.geomID; + primitive_ids[idx] = hit.primID; + t_hit[idx] = ray.tfar; + previous_geom_prim_ID_tfar->operator[](ray_id) = gpID; + ++(track_intersections[ray_id]); + } + // Always ignore hit + valid[ui] = 0; + } +} + + // Adapted from common/math/closest_point.h inline Vec3fa closestPointTriangle(Vec3fa const& p, Vec3fa const& a, @@ -481,6 +547,82 @@ struct RaycastingScene::Impl { LoopFn); } } + + + void ListIntersections(const float* const rays, + const size_t num_rays, + const size_t num_intersections, + Eigen::VectorXi cumsum, + unsigned int* track_intersections, + unsigned int* ray_ids, + unsigned int* geometry_ids, + unsigned int* primitive_ids, + float* t_hit, + const int nthreads) { + CommitScene(); + + memset(track_intersections, 0, sizeof(uint32_t) * num_rays); + memset(ray_ids, 0, sizeof(uint32_t) * num_intersections); + memset(geometry_ids, 0, sizeof(uint32_t) * num_intersections); + memset(primitive_ids, 0, sizeof(uint32_t) * num_intersections); + memset(t_hit, 0, sizeof(float) * num_intersections); + + std::vector> + previous_geom_prim_ID_tfar( + num_rays, + std::make_tuple(uint32_t(RTC_INVALID_GEOMETRY_ID), + uint32_t(RTC_INVALID_GEOMETRY_ID), + 0.f)); + + ListIntersectionsContext context; + rtcInitIntersectContext(&context.context); + context.context.filter = ListIntersectionsFunc; + context.previous_geom_prim_ID_tfar = &previous_geom_prim_ID_tfar; + context.ray_ids = ray_ids; + context.geometry_ids = geometry_ids; + context.primitive_ids = primitive_ids; + context.t_hit = t_hit; + context.cumsum = cumsum; + context.track_intersections = track_intersections; + + auto LoopFn = [&](const tbb::blocked_range& range) { + std::vector rayhits(range.size()); + + for (size_t i = range.begin(); i < range.end(); ++i) { + RTCRayHit* rh = &rayhits[i - range.begin()]; + const float* r = &rays[i * 6]; + rh->ray.org_x = r[0]; + rh->ray.org_y = r[1]; + rh->ray.org_z = r[2]; + rh->ray.dir_x = r[3]; + rh->ray.dir_y = r[4]; + rh->ray.dir_z = r[5]; + rh->ray.tnear = 0; + rh->ray.tfar = std::numeric_limits::infinity(); + rh->ray.mask = 0; + rh->ray.flags = 0; + rh->ray.id = i; + rh->hit.geomID = RTC_INVALID_GEOMETRY_ID; + rh->hit.instID[0] = RTC_INVALID_GEOMETRY_ID; + } + rtcIntersect1M(scene_, &context.context, &rayhits[0], range.size(), + sizeof(RTCRayHit)); + }; + + if (nthreads > 0) { + tbb::task_arena arena(nthreads); + arena.execute([&]() { + tbb::parallel_for( + tbb::blocked_range(0, num_rays, BATCH_SIZE), + LoopFn); + }); + } else { + tbb::parallel_for( + tbb::blocked_range(0, num_rays, BATCH_SIZE), + LoopFn); + } + } + void ComputeClosestPoints(const float* const query_points, const size_t num_query_points, @@ -691,6 +833,59 @@ core::Tensor RaycastingScene::CountIntersections(const core::Tensor& rays, return intersections; } + +std::unordered_map +RaycastingScene::ListIntersections(const core::Tensor& rays, + const int nthreads) { + AssertTensorDtypeLastDimDeviceMinNDim(rays, "rays", 6, + impl_->tensor_device_); + auto shape = rays.GetShape(); + shape.pop_back(); // Remove last dim, we want to use this shape for the + // results. + size_t num_rays = shape.NumElements(); + + // determine total number of intersections + core::Tensor intersections(shape, core::Dtype::FromType()); + core::Tensor track_intersections(shape, core::Dtype::FromType()); + auto data = rays.Contiguous(); + impl_->CountIntersections(data.GetDataPtr(), num_rays, + intersections.GetDataPtr(), nthreads); + + // prepare shape with that number of elements + // not sure how to do proper conversion + const core::SizeVector dim = {0}; + Eigen::Map intersections_vector( + intersections.GetDataPtr(), num_rays); + Eigen::Map num_intersections( + intersections.Sum(dim).GetDataPtr(), 1); + shape = {num_intersections[0], 1}; + + // prepare ray allocations (cumsum) + Eigen::VectorXi cumsum = Eigen::MatrixXi::Zero(num_rays, 1); + std::partial_sum(intersections_vector.begin(), intersections_vector.end() - 1, + cumsum.begin() + 1, std::plus()); + + // generate results structure + std::unordered_map result; + result["ray_ids"] = core::Tensor(shape, core::UInt32); + result["geometry_ids"] = core::Tensor(shape, core::UInt32); + result["primitive_ids"] = core::Tensor(shape, core::UInt32); + result["t_hit"] = core::Tensor(shape, core::Float32); + + impl_->ListIntersections(data.GetDataPtr(), + num_rays, + num_intersections[0], + cumsum, + track_intersections.GetDataPtr(), + result["ray_ids"].GetDataPtr(), + result["geometry_ids"].GetDataPtr(), + result["primitive_ids"].GetDataPtr(), + result["t_hit"].GetDataPtr(), + nthreads); + return result; +} + + std::unordered_map RaycastingScene::ComputeClosestPoints(const core::Tensor& query_points, const int nthreads) { @@ -961,4 +1156,4 @@ uint32_t RaycastingScene::INVALID_ID() { return RTC_INVALID_GEOMETRY_ID; } } // namespace geometry } // namespace t -} // namespace open3d +} // namespace open3d \ No newline at end of file diff --git a/cpp/open3d/t/geometry/RaycastingScene.h b/cpp/open3d/t/geometry/RaycastingScene.h index b22639148f5..3e102d128f2 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.h +++ b/cpp/open3d/t/geometry/RaycastingScene.h @@ -122,6 +122,23 @@ class RaycastingScene { /// the closest points within the triangles. The shape is {.., 2}. /// - \b primitive_normals A tensor with the normals of the /// closest triangle . The shape is {.., 3}. + std::unordered_map ListIntersections( + const core::Tensor &rays, const int nthreads = 0); + + /// \brief Lists the intersections of the rays with the scene + /// \param query_points A tensor with >=2 dims, shape {.., 3} and Dtype + /// Float32 describing the query points. {..} can be any number of + /// dimensions, e.g., to organize the query_point to create a 3D grid the + /// shape can be {depth, height, width, 3}. The last dimension must be 3 and + /// has the format [x, y, z]. + /// \param nthreads The number of threads to use. Set to 0 for automatic. + /// \return The returned dictionary contains: + /// - \b ray_ids A tensor with ray IDs. The shape is {..}. + /// - \b geometry_ids A tensor with the geometry IDs. The shape is + /// {..}. + /// - \b primitive_ids A tensor with the primitive IDs, which + /// corresponds to the triangle index. The shape is {..}. + /// - \b t_hit A tensor with the distance to the hit. The shape is {..}. std::unordered_map ComputeClosestPoints( const core::Tensor &query_points, const int nthreads = 0); diff --git a/cpp/pybind/t/geometry/raycasting_scene.cpp b/cpp/pybind/t/geometry/raycasting_scene.cpp index bdab66e02e1..f770a5c1238 100644 --- a/cpp/pybind/t/geometry/raycasting_scene.cpp +++ b/cpp/pybind/t/geometry/raycasting_scene.cpp @@ -177,6 +177,41 @@ Computes the number of intersection of the rays with the scene. Returns: A tensor with the number of intersections. The shape is {..}. +)doc"); + + raycasting_scene.def("list_intersections", + &RaycastingScene::ListIntersections, "rays"_a, + "nthreads"_a = 0, R"doc( +Lists the intersections of the rays with the scene. + +Args: + rays (open3d.core.Tensor): A tensor with >=2 dims, shape {.., 6}, and Dtype + Float32 describing the rays. + {..} can be any number of dimensions, e.g., to organize rays for + creating an image the shape can be {height, width, 6}. + The last dimension must be 6 and has the format [ox, oy, oz, dx, dy, dz] + with [ox,oy,oz] as the origin and [dx,dy,dz] as the direction. It is not + necessary to normalize the direction although it should be normalised if + t_hit is to be calculated in coordinate units. + + nthreads (int): The number of threads to use. Set to 0 for automatic. + +Returns: + The returned dictionary contains + + ray_ids + A tensor with ray IDs. The shape is {..}. + + geometry_ids + A tensor with the geometry IDs. The shape is {..}. + + primitive_ids + A tensor with the primitive IDs, which corresponds to the triangle + index. The shape is {..}. + + t_hit + A tensor with the distance to the hit. The shape is {..}. + )doc"); raycasting_scene.def("compute_closest_points", @@ -350,4 +385,4 @@ The value for invalid IDs } } // namespace geometry } // namespace t -} // namespace open3d +} // namespace open3d \ No newline at end of file From b80c5a90fddc33794616be51cd597dff6eca4e8f Mon Sep 17 00:00:00 2001 From: dmitrishastin Date: Sun, 22 Oct 2023 14:55:34 +0100 Subject: [PATCH 02/14] style_checks --- cpp/open3d/t/geometry/RaycastingScene.cpp | 91 +++++++++++------------ cpp/open3d/t/geometry/RaycastingScene.h | 5 +- 2 files changed, 45 insertions(+), 51 deletions(-) diff --git a/cpp/open3d/t/geometry/RaycastingScene.cpp b/cpp/open3d/t/geometry/RaycastingScene.cpp index c2099f72307..2895b7ac2db 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.cpp +++ b/cpp/open3d/t/geometry/RaycastingScene.cpp @@ -128,7 +128,7 @@ void ListIntersectionsFunc(const RTCFilterFunctionNArguments* args) { reinterpret_cast(args->context); struct RTCRayN* rayN = args->ray; struct RTCHitN* hitN = args->hit; - const unsigned int N = args->N; + const unsigned int N = args->N; // Avoid crashing when debug visualizations are used. if (context == nullptr) return; @@ -138,7 +138,7 @@ void ListIntersectionsFunc(const RTCFilterFunctionNArguments* args) { unsigned int* ray_ids = context->ray_ids; unsigned int* geometry_ids = context->geometry_ids; unsigned int* primitive_ids = context->primitive_ids; - float* t_hit = context->t_hit; + float* t_hit = context->t_hit; Eigen::VectorXi cumsum = context->cumsum; unsigned int* track_intersections = context->track_intersections; @@ -162,20 +162,19 @@ void ListIntersectionsFunc(const RTCFilterFunctionNArguments* args) { if (std::get<0>(prev_gpIDtfar) != hit.geomID || (std::get<1>(prev_gpIDtfar) != hit.primID && std::get<2>(prev_gpIDtfar) != ray.tfar)) { - size_t idx = cumsum[ray_id] + track_intersections[ray_id]; - ray_ids[idx] = ray_id; - geometry_ids[idx] = hit.geomID; - primitive_ids[idx] = hit.primID; - t_hit[idx] = ray.tfar; - previous_geom_prim_ID_tfar->operator[](ray_id) = gpID; - ++(track_intersections[ray_id]); + size_t idx = cumsum[ray_id] + track_intersections[ray_id]; + ray_ids[idx] = ray_id; + geometry_ids[idx] = hit.geomID; + primitive_ids[idx] = hit.primID; + t_hit[idx] = ray.tfar; + previous_geom_prim_ID_tfar->operator[](ray_id) = gpID; + ++(track_intersections[ray_id]); } // Always ignore hit valid[ui] = 0; } } - // Adapted from common/math/closest_point.h inline Vec3fa closestPointTriangle(Vec3fa const& p, Vec3fa const& a, @@ -547,18 +546,17 @@ struct RaycastingScene::Impl { LoopFn); } } - - + void ListIntersections(const float* const rays, - const size_t num_rays, - const size_t num_intersections, - Eigen::VectorXi cumsum, - unsigned int* track_intersections, - unsigned int* ray_ids, - unsigned int* geometry_ids, - unsigned int* primitive_ids, - float* t_hit, - const int nthreads) { + const size_t num_rays, + const size_t num_intersections, + Eigen::VectorXi cumsum, + unsigned int* track_intersections, + unsigned int* ray_ids, + unsigned int* geometry_ids, + unsigned int* primitive_ids, + float* t_hit, + const int nthreads) { CommitScene(); memset(track_intersections, 0, sizeof(uint32_t) * num_rays); @@ -621,8 +619,7 @@ struct RaycastingScene::Impl { tbb::blocked_range(0, num_rays, BATCH_SIZE), LoopFn); } - } - + } void ComputeClosestPoints(const float* const query_points, const size_t num_query_points, @@ -833,59 +830,55 @@ core::Tensor RaycastingScene::CountIntersections(const core::Tensor& rays, return intersections; } - std::unordered_map RaycastingScene::ListIntersections(const core::Tensor& rays, - const int nthreads) { + const int nthreads) { AssertTensorDtypeLastDimDeviceMinNDim(rays, "rays", 6, impl_->tensor_device_); auto shape = rays.GetShape(); shape.pop_back(); // Remove last dim, we want to use this shape for the // results. size_t num_rays = shape.NumElements(); - + // determine total number of intersections core::Tensor intersections(shape, core::Dtype::FromType()); core::Tensor track_intersections(shape, core::Dtype::FromType()); auto data = rays.Contiguous(); impl_->CountIntersections(data.GetDataPtr(), num_rays, - intersections.GetDataPtr(), nthreads); - - // prepare shape with that number of elements + intersections.GetDataPtr(), nthreads); + + // prepare shape with that number of elements // not sure how to do proper conversion - const core::SizeVector dim = {0}; + const core::SizeVector dim = {0}; Eigen::Map intersections_vector( - intersections.GetDataPtr(), num_rays); + intersections.GetDataPtr(), num_rays); Eigen::Map num_intersections( - intersections.Sum(dim).GetDataPtr(), 1); + intersections.Sum(dim).GetDataPtr(), 1); shape = {num_intersections[0], 1}; - + // prepare ray allocations (cumsum) Eigen::VectorXi cumsum = Eigen::MatrixXi::Zero(num_rays, 1); - std::partial_sum(intersections_vector.begin(), intersections_vector.end() - 1, - cumsum.begin() + 1, std::plus()); - + std::partial_sum(intersections_vector.begin(), + intersections_vector.end() - 1, cumsum.begin() + 1, + std::plus()); + // generate results structure std::unordered_map result; result["ray_ids"] = core::Tensor(shape, core::UInt32); - result["geometry_ids"] = core::Tensor(shape, core::UInt32); + result["geometry_ids"] = core::Tensor(shape, core::UInt32); result["primitive_ids"] = core::Tensor(shape, core::UInt32); result["t_hit"] = core::Tensor(shape, core::Float32); - - impl_->ListIntersections(data.GetDataPtr(), - num_rays, - num_intersections[0], - cumsum, - track_intersections.GetDataPtr(), - result["ray_ids"].GetDataPtr(), - result["geometry_ids"].GetDataPtr(), - result["primitive_ids"].GetDataPtr(), - result["t_hit"].GetDataPtr(), - nthreads); + + impl_->ListIntersections(data.GetDataPtr(), num_rays, + num_intersections[0], cumsum, + track_intersections.GetDataPtr(), + result["ray_ids"].GetDataPtr(), + result["geometry_ids"].GetDataPtr(), + result["primitive_ids"].GetDataPtr(), + result["t_hit"].GetDataPtr(), nthreads); return result; } - std::unordered_map RaycastingScene::ComputeClosestPoints(const core::Tensor& query_points, const int nthreads) { diff --git a/cpp/open3d/t/geometry/RaycastingScene.h b/cpp/open3d/t/geometry/RaycastingScene.h index 3e102d128f2..527ee90ddb6 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.h +++ b/cpp/open3d/t/geometry/RaycastingScene.h @@ -124,7 +124,7 @@ class RaycastingScene { /// closest triangle . The shape is {.., 3}. std::unordered_map ListIntersections( const core::Tensor &rays, const int nthreads = 0); - + /// \brief Lists the intersections of the rays with the scene /// \param query_points A tensor with >=2 dims, shape {.., 3} and Dtype /// Float32 describing the query points. {..} can be any number of @@ -138,7 +138,8 @@ class RaycastingScene { /// {..}. /// - \b primitive_ids A tensor with the primitive IDs, which /// corresponds to the triangle index. The shape is {..}. - /// - \b t_hit A tensor with the distance to the hit. The shape is {..}. + /// - \b t_hit A tensor with the distance to the hit. The shape is + /// {..}. std::unordered_map ComputeClosestPoints( const core::Tensor &query_points, const int nthreads = 0); From 51b1e14994b4d9b600ae505851408b88383059de Mon Sep 17 00:00:00 2001 From: dmitrishastin Date: Thu, 26 Oct 2023 14:49:28 +0100 Subject: [PATCH 03/14] addressing suggestions + adding primitive_uvs to output --- cpp/open3d/t/geometry/RaycastingScene.cpp | 19 +++++--- cpp/open3d/t/geometry/RaycastingScene.h | 2 + cpp/pybind/t/geometry/raycasting_scene.cpp | 50 +++++++++++++++++++++- 3 files changed, 64 insertions(+), 7 deletions(-) diff --git a/cpp/open3d/t/geometry/RaycastingScene.cpp b/cpp/open3d/t/geometry/RaycastingScene.cpp index 2895b7ac2db..c049833d96e 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.cpp +++ b/cpp/open3d/t/geometry/RaycastingScene.cpp @@ -117,6 +117,7 @@ struct ListIntersectionsContext { unsigned int* ray_ids; unsigned int* geometry_ids; unsigned int* primitive_ids; + float* primitive_uvs; float* t_hit; Eigen::VectorXi cumsum; unsigned int* track_intersections; @@ -138,6 +139,7 @@ void ListIntersectionsFunc(const RTCFilterFunctionNArguments* args) { unsigned int* ray_ids = context->ray_ids; unsigned int* geometry_ids = context->geometry_ids; unsigned int* primitive_ids = context->primitive_ids; + float* primitive_uvs = context->primitive_uvs; float* t_hit = context->t_hit; Eigen::VectorXi cumsum = context->cumsum; unsigned int* track_intersections = context->track_intersections; @@ -166,6 +168,8 @@ void ListIntersectionsFunc(const RTCFilterFunctionNArguments* args) { ray_ids[idx] = ray_id; geometry_ids[idx] = hit.geomID; primitive_ids[idx] = hit.primID; + primitive_uvs[idx * 2 + 0] = hit.u; + primitive_uvs[idx * 2 + 1] = hit.v; t_hit[idx] = ray.tfar; previous_geom_prim_ID_tfar->operator[](ray_id) = gpID; ++(track_intersections[ray_id]); @@ -555,6 +559,7 @@ struct RaycastingScene::Impl { unsigned int* ray_ids, unsigned int* geometry_ids, unsigned int* primitive_ids, + float* primitive_uvs, float* t_hit, const int nthreads) { CommitScene(); @@ -563,6 +568,7 @@ struct RaycastingScene::Impl { memset(ray_ids, 0, sizeof(uint32_t) * num_intersections); memset(geometry_ids, 0, sizeof(uint32_t) * num_intersections); memset(primitive_ids, 0, sizeof(uint32_t) * num_intersections); + memset(primitive_uvs, 0, sizeof(float) * num_intersections * 2); memset(t_hit, 0, sizeof(float) * num_intersections); std::vector> @@ -579,6 +585,7 @@ struct RaycastingScene::Impl { context.ray_ids = ray_ids; context.geometry_ids = geometry_ids; context.primitive_ids = primitive_ids; + context.primitive_uvs = primitive_uvs; context.t_hit = t_hit; context.cumsum = cumsum; context.track_intersections = track_intersections; @@ -848,13 +855,9 @@ RaycastingScene::ListIntersections(const core::Tensor& rays, intersections.GetDataPtr(), nthreads); // prepare shape with that number of elements - // not sure how to do proper conversion - const core::SizeVector dim = {0}; Eigen::Map intersections_vector( intersections.GetDataPtr(), num_rays); - Eigen::Map num_intersections( - intersections.Sum(dim).GetDataPtr(), 1); - shape = {num_intersections[0], 1}; + size_t num_intersections = intersections_vector.sum(); // prepare ray allocations (cumsum) Eigen::VectorXi cumsum = Eigen::MatrixXi::Zero(num_rays, 1); @@ -864,17 +867,21 @@ RaycastingScene::ListIntersections(const core::Tensor& rays, // generate results structure std::unordered_map result; + shape = {intersections_vector.sum(), 1}; result["ray_ids"] = core::Tensor(shape, core::UInt32); result["geometry_ids"] = core::Tensor(shape, core::UInt32); result["primitive_ids"] = core::Tensor(shape, core::UInt32); result["t_hit"] = core::Tensor(shape, core::Float32); + shape.back() = 2; + result["primitive_uvs"] = core::Tensor(shape, core::Float32); impl_->ListIntersections(data.GetDataPtr(), num_rays, - num_intersections[0], cumsum, + num_intersections, cumsum, track_intersections.GetDataPtr(), result["ray_ids"].GetDataPtr(), result["geometry_ids"].GetDataPtr(), result["primitive_ids"].GetDataPtr(), + result["primitive_uvs"].GetDataPtr(), result["t_hit"].GetDataPtr(), nthreads); return result; } diff --git a/cpp/open3d/t/geometry/RaycastingScene.h b/cpp/open3d/t/geometry/RaycastingScene.h index 527ee90ddb6..52c3d96980d 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.h +++ b/cpp/open3d/t/geometry/RaycastingScene.h @@ -138,6 +138,8 @@ class RaycastingScene { /// {..}. /// - \b primitive_ids A tensor with the primitive IDs, which /// corresponds to the triangle index. The shape is {..}. + /// - \b primitive_uvs A tensor with the barycentric coordinates of + /// the closest points within the triangles. The shape is {.., 2}. /// - \b t_hit A tensor with the distance to the hit. The shape is /// {..}. std::unordered_map ComputeClosestPoints( diff --git a/cpp/pybind/t/geometry/raycasting_scene.cpp b/cpp/pybind/t/geometry/raycasting_scene.cpp index f770a5c1238..46252329083 100644 --- a/cpp/pybind/t/geometry/raycasting_scene.cpp +++ b/cpp/pybind/t/geometry/raycasting_scene.cpp @@ -182,7 +182,51 @@ Computes the number of intersection of the rays with the scene. raycasting_scene.def("list_intersections", &RaycastingScene::ListIntersections, "rays"_a, "nthreads"_a = 0, R"doc( -Lists the intersections of the rays with the scene. +Lists the intersections of the rays with the scene:: + + import open3d as o3d + import numpy as np + + # Create scene and add the monkey model. + scene = o3d.t.geometry.RaycastingScene() + d = o3d.data.MonkeyModel() + mesh = o3d.t.io.read_triangle_mesh(d.path) + mesh_id = scene.add_triangles(mesh) + + # Create an offset grid of rays. + p_min = np.min(mesh.vertex['positions'].numpy() - 1, axis=0) + p_max = np.max(mesh.vertex['positions'].numpy() - 1, axis=0) + x = np.arange(p_min[0], p_max[0], .1) + y = np.arange(p_min[1], p_max[1], .1) + xv, yv = np.meshgrid(x, y) + orig = np.vstack([xv.flatten(), yv.flatten(), np.tile(p_min[2], xv.size)]).T + dest = np.copy(orig) + dest[:, 2] = p_max[2] + 1 + rays = np.hstack([orig, dest - orig]).astype('float32') + + # Compute the ray intersections. + lx = scene.list_intersections(rays) + + # Calculate intersection coordinates. + v = mesh.vertex['positions'].numpy() + t = mesh.triangle['indices'].numpy() + tidx = lx['primitive_ids'].numpy() + uv = lx['primitive_uvs'].numpy() + w = 1 - np.sum(uv, axis=1) + c = \ + v[t[tidx, 1].flatten(), :] * uv[:, 0][:, None] + \ + v[t[tidx, 2].flatten(), :] * uv[:, 1][:, None] + \ + v[t[tidx, 0].flatten(), :] * w[:, None] + + # Visualize the intersections. + entry = o3d.geometry.PointCloud(points = o3d.utility.Vector3dVector(orig)) + target = o3d.geometry.PointCloud(points = o3d.utility.Vector3dVector(dest)) + correspondence = [(i, i) for i in range(rays.shape[0])] + traj = o3d.geometry.LineSet.create_from_point_cloud_correspondences(entry , target , correspondence) + traj.colors = o3d.utility.Vector3dVector(np.tile([1, 0, 0], [rays.shape[0], 1])) + x = o3d.geometry.PointCloud(points = o3d.utility.Vector3dVector(c)) + o3d.visualization.draw([mesh, traj, x]) + Args: rays (open3d.core.Tensor): A tensor with >=2 dims, shape {.., 6}, and Dtype @@ -208,6 +252,10 @@ Lists the intersections of the rays with the scene. primitive_ids A tensor with the primitive IDs, which corresponds to the triangle index. The shape is {..}. + + primitive_uvs + A tensor with the barycentric coordinates of the closest points within + the triangles. The shape is {.., 2}. t_hit A tensor with the distance to the hit. The shape is {..}. From 55898c84befa065e62dd7c9582a31d704c147970 Mon Sep 17 00:00:00 2001 From: dmitrishastin Date: Thu, 26 Oct 2023 14:58:38 +0100 Subject: [PATCH 04/14] Update RaycastingScene.cpp --- cpp/open3d/t/geometry/RaycastingScene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cpp/open3d/t/geometry/RaycastingScene.cpp b/cpp/open3d/t/geometry/RaycastingScene.cpp index c049833d96e..c0e4d92de6a 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.cpp +++ b/cpp/open3d/t/geometry/RaycastingScene.cpp @@ -554,7 +554,7 @@ struct RaycastingScene::Impl { void ListIntersections(const float* const rays, const size_t num_rays, const size_t num_intersections, - Eigen::VectorXi cumsum, + const Eigen::VectorXi cumsum, unsigned int* track_intersections, unsigned int* ray_ids, unsigned int* geometry_ids, From 0d62edf6ffaaa4b22fee3a8a091604a16487031c Mon Sep 17 00:00:00 2001 From: dmitrishastin Date: Thu, 26 Oct 2023 15:30:19 +0100 Subject: [PATCH 05/14] slight correction in docs --- cpp/open3d/t/geometry/RaycastingScene.h | 3 ++- cpp/pybind/t/geometry/raycasting_scene.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/cpp/open3d/t/geometry/RaycastingScene.h b/cpp/open3d/t/geometry/RaycastingScene.h index 52c3d96980d..e3a785519d4 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.h +++ b/cpp/open3d/t/geometry/RaycastingScene.h @@ -139,7 +139,8 @@ class RaycastingScene { /// - \b primitive_ids A tensor with the primitive IDs, which /// corresponds to the triangle index. The shape is {..}. /// - \b primitive_uvs A tensor with the barycentric coordinates of - /// the closest points within the triangles. The shape is {.., 2}. + /// the intersection points within the triangles. The shape is + /// {.., 2}. /// - \b t_hit A tensor with the distance to the hit. The shape is /// {..}. std::unordered_map ComputeClosestPoints( diff --git a/cpp/pybind/t/geometry/raycasting_scene.cpp b/cpp/pybind/t/geometry/raycasting_scene.cpp index 46252329083..7da504651a1 100644 --- a/cpp/pybind/t/geometry/raycasting_scene.cpp +++ b/cpp/pybind/t/geometry/raycasting_scene.cpp @@ -254,7 +254,7 @@ Lists the intersections of the rays with the scene:: index. The shape is {..}. primitive_uvs - A tensor with the barycentric coordinates of the closest points within + A tensor with the barycentric coordinates of the intersection points within the triangles. The shape is {.., 2}. t_hit From 5e4c09719376058aee3725a3a74e100cbdb92a28 Mon Sep 17 00:00:00 2001 From: dmitrishastin Date: Sun, 29 Oct 2023 00:18:06 +0100 Subject: [PATCH 06/14] further corrections --- cpp/open3d/t/geometry/RaycastingScene.cpp | 13 +++-- cpp/open3d/t/geometry/RaycastingScene.h | 3 ++ cpp/pybind/t/geometry/raycasting_scene.cpp | 13 +++++ .../test/t/geometry/test_raycasting_scene.py | 52 ++++++++++++++++++- 4 files changed, 77 insertions(+), 4 deletions(-) diff --git a/cpp/open3d/t/geometry/RaycastingScene.cpp b/cpp/open3d/t/geometry/RaycastingScene.cpp index c0e4d92de6a..b06d6211752 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.cpp +++ b/cpp/open3d/t/geometry/RaycastingScene.cpp @@ -554,7 +554,7 @@ struct RaycastingScene::Impl { void ListIntersections(const float* const rays, const size_t num_rays, const size_t num_intersections, - const Eigen::VectorXi cumsum, + const Eigen::VectorXi& cumsum, unsigned int* track_intersections, unsigned int* ray_ids, unsigned int* geometry_ids, @@ -842,6 +842,7 @@ RaycastingScene::ListIntersections(const core::Tensor& rays, const int nthreads) { AssertTensorDtypeLastDimDeviceMinNDim(rays, "rays", 6, impl_->tensor_device_); + auto shape = rays.GetShape(); shape.pop_back(); // Remove last dim, we want to use this shape for the // results. @@ -867,12 +868,18 @@ RaycastingScene::ListIntersections(const core::Tensor& rays, // generate results structure std::unordered_map result; - shape = {intersections_vector.sum(), 1}; + result["ray_splits"] = core::Tensor({cumsum.size() + 1}, core::UInt32); + uint32_t* ptr = result["ray_splits"].GetDataPtr(); + ptr[0] = 0; + for (int i = 1; i < cumsum.size() + 1; ++i) { + ptr[i] = cumsum[i - 1]; + } + shape = {intersections_vector.sum()}; result["ray_ids"] = core::Tensor(shape, core::UInt32); result["geometry_ids"] = core::Tensor(shape, core::UInt32); result["primitive_ids"] = core::Tensor(shape, core::UInt32); result["t_hit"] = core::Tensor(shape, core::Float32); - shape.back() = 2; + shape.push_back(2); result["primitive_uvs"] = core::Tensor(shape, core::Float32); impl_->ListIntersections(data.GetDataPtr(), num_rays, diff --git a/cpp/open3d/t/geometry/RaycastingScene.h b/cpp/open3d/t/geometry/RaycastingScene.h index e3a785519d4..f18e4ae3e62 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.h +++ b/cpp/open3d/t/geometry/RaycastingScene.h @@ -134,6 +134,9 @@ class RaycastingScene { /// \param nthreads The number of threads to use. Set to 0 for automatic. /// \return The returned dictionary contains: /// - \b ray_ids A tensor with ray IDs. The shape is {..}. + /// - \b ray_splits A tensor with ray intersection splits. Can be + /// used to iterate over all intersections for each ray. The shape + /// is {..}. /// - \b geometry_ids A tensor with the geometry IDs. The shape is /// {..}. /// - \b primitive_ids A tensor with the primitive IDs, which diff --git a/cpp/pybind/t/geometry/raycasting_scene.cpp b/cpp/pybind/t/geometry/raycasting_scene.cpp index 7da504651a1..15813d305ff 100644 --- a/cpp/pybind/t/geometry/raycasting_scene.cpp +++ b/cpp/pybind/t/geometry/raycasting_scene.cpp @@ -246,6 +246,9 @@ Lists the intersections of the rays with the scene:: ray_ids A tensor with ray IDs. The shape is {..}. + ray_splits + A tensor with ray intersection splits. Can be used to iterate over all intersections for each ray. The shape is {..}. + geometry_ids A tensor with the geometry IDs. The shape is {..}. @@ -260,6 +263,16 @@ Lists the intersections of the rays with the scene:: t_hit A tensor with the distance to the hit. The shape is {..}. +An example of using ray_splits:: + + ray_splits: [0, 2, 3, 6, 6, 8] # note that the length of this is num_rays+1 + t_hit: [t1, t2, t3, t4, t5, t6, t7, t8] + + for ray_id, (start, end) in enumerate(zip(ray_splits[:-1], ray_splits[1:])): + for i,t in enumerate(t_hit[start:end]): + print(f'ray {ray_id}, intersection {i} at {t}') + + )doc"); raycasting_scene.def("compute_closest_points", diff --git a/python/test/t/geometry/test_raycasting_scene.py b/python/test/t/geometry/test_raycasting_scene.py index f89bb0b6644..d89fdc404b3 100644 --- a/python/test/t/geometry/test_raycasting_scene.py +++ b/python/test/t/geometry/test_raycasting_scene.py @@ -137,6 +137,39 @@ def test_count_lots_of_intersections(): _ = scene.count_intersections(rays) +def test_list_intersections(): + cube = o3d.t.geometry.TriangleMesh.from_legacy( + o3d.geometry.TriangleMesh.create_box()) + + scene = o3d.t.geometry.RaycastingScene() + scene.add_triangles(cube) + + rays = o3d.core.Tensor([[0.5, 0.5, -1, 0, 0, 1], [0.5, 0.5, 0.5, 0, 0, 1], + [10, 10, 10, 1, 0, 0]], + dtype=o3d.core.float32) + ans = scene.list_intersections(rays) + + np.testing.assert_allclose(ans['t_hit'].numpy(), + np.array([[1.0], [2.0], [0.5]]), + rtol=1e-6, + atol=1e-6) + + +# list lots of random ray intersections to test the internal batching +# we expect no errors for this test +def test_list_lots_of_intersections(): + cube = o3d.t.geometry.TriangleMesh.from_legacy( + o3d.geometry.TriangleMesh.create_box()) + + scene = o3d.t.geometry.RaycastingScene() + scene.add_triangles(cube) + + rs = np.random.RandomState(123) + rays = o3d.core.Tensor.from_numpy(rs.rand(123456, 6).astype(np.float32)) + + _ = scene.list_intersections(rays) + + def test_compute_closest_points(): vertices = o3d.core.Tensor([[0, 0, 0], [1, 0, 0], [1, 1, 0]], dtype=o3d.core.float32) @@ -248,7 +281,9 @@ def test_output_shapes(shape): 'primitive_ids': [], 'primitive_uvs': [2], 'primitive_normals': [3], - 'points': [3] + 'points': [3], + 'ray_ids': [], + 'ray_splits': [] } ans = scene.cast_rays(rays) @@ -267,6 +302,21 @@ def test_output_shapes(shape): ) == expected_shape, 'shape mismatch: expected {} but got {} for {}'.format( expected_shape, list(v.shape), k) + ans = scene.list_intersections(rays) + nx = np.sum(scene.count_intersections(rays).numpy()).tolist() + for k, v in ans.items(): + alt_shape = np.copy(shape) + if k == 'ray_splits': + alt_shape[0] = shape[0] + 1 + else: + alt_shape[0] = nx + #use np.append otherwise issues if alt_shape = [0] and last_dim[k] = [] + expected_shape = np.append(alt_shape, last_dim[k]).tolist() + assert list( + v.shape + ) == expected_shape, 'shape mismatch: expected {} but got {} for {}'.format( + expected_shape, list(v.shape), k) + def test_sphere_wrong_occupancy(): # This test checks a specific scenario where the old implementation From bd0511a1631e34df9be7ba95b151118f7d617433 Mon Sep 17 00:00:00 2001 From: dmitrishastin Date: Sun, 29 Oct 2023 03:06:47 +0000 Subject: [PATCH 07/14] sorting out wrong shapes and unit test error --- cpp/open3d/t/geometry/RaycastingScene.cpp | 5 +++-- python/test/t/geometry/test_raycasting_scene.py | 2 +- util/check_style.py | 10 +++++----- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/cpp/open3d/t/geometry/RaycastingScene.cpp b/cpp/open3d/t/geometry/RaycastingScene.cpp index b06d6211752..4774beeafa4 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.cpp +++ b/cpp/open3d/t/geometry/RaycastingScene.cpp @@ -868,13 +868,14 @@ RaycastingScene::ListIntersections(const core::Tensor& rays, // generate results structure std::unordered_map result; - result["ray_splits"] = core::Tensor({cumsum.size() + 1}, core::UInt32); + shape[0] = shape[0] + 1; + result["ray_splits"] = core::Tensor(shape, core::UInt32); uint32_t* ptr = result["ray_splits"].GetDataPtr(); ptr[0] = 0; for (int i = 1; i < cumsum.size() + 1; ++i) { ptr[i] = cumsum[i - 1]; } - shape = {intersections_vector.sum()}; + shape[0] = intersections_vector.sum(); result["ray_ids"] = core::Tensor(shape, core::UInt32); result["geometry_ids"] = core::Tensor(shape, core::UInt32); result["primitive_ids"] = core::Tensor(shape, core::UInt32); diff --git a/python/test/t/geometry/test_raycasting_scene.py b/python/test/t/geometry/test_raycasting_scene.py index d89fdc404b3..9b930a877da 100644 --- a/python/test/t/geometry/test_raycasting_scene.py +++ b/python/test/t/geometry/test_raycasting_scene.py @@ -150,7 +150,7 @@ def test_list_intersections(): ans = scene.list_intersections(rays) np.testing.assert_allclose(ans['t_hit'].numpy(), - np.array([[1.0], [2.0], [0.5]]), + np.array([1.0, 2.0, 0.5]), rtol=1e-6, atol=1e-6) diff --git a/util/check_style.py b/util/check_style.py index 97b7c1ba58c..437446092f5 100644 --- a/util/check_style.py +++ b/util/check_style.py @@ -78,7 +78,7 @@ def _check_style(file_path, clang_format_bin): """ Returns (true, true) if (style, header) is valid. """ - with open(file_path, 'r') as f: + with open(file_path, 'r', encoding='utf-8') as f: is_valid_header = f.read().startswith(CppFormatter.standard_header) cmd = [ @@ -156,7 +156,7 @@ def _check_style(file_path, style_config): Returns (true, true) if (style, header) is valid. """ - with open(file_path, 'r') as f: + with open(file_path, 'r', encoding='utf-8') as f: content = f.read() is_valid_header = (len(content) == 0 or content.startswith( PythonFormatter.standard_header)) @@ -218,7 +218,7 @@ def _check_or_apply_style(file_path, style_config, apply): are merged into one. """ # Ref: https://gist.github.com/oskopek/496c0d96c79fb6a13692657b39d7c709 - with open(file_path, "r") as f: + with open(file_path, "r", encoding='utf-8') as f: notebook = nbformat.read(f, as_version=nbformat.NO_CONVERT) nbformat.validate(notebook) @@ -241,7 +241,7 @@ def _check_or_apply_style(file_path, style_config, apply): changed = True if apply: - with open(file_path, "w") as f: + with open(file_path, "w", encoding='utf-8') as f: nbformat.write(notebook, f, version=nbformat.NO_CONVERT) return not changed @@ -444,4 +444,4 @@ def main(): if __name__ == "__main__": - main() + main() \ No newline at end of file From 9d7dccf6df7b67eaca356528d852373c229a634d Mon Sep 17 00:00:00 2001 From: dmitrishastin Date: Sun, 29 Oct 2023 03:10:42 +0000 Subject: [PATCH 08/14] sorting out wrong shapes and unit test error --- cpp/open3d/t/geometry/RaycastingScene.cpp | 5 +++-- python/test/t/geometry/test_raycasting_scene.py | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/cpp/open3d/t/geometry/RaycastingScene.cpp b/cpp/open3d/t/geometry/RaycastingScene.cpp index b06d6211752..4774beeafa4 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.cpp +++ b/cpp/open3d/t/geometry/RaycastingScene.cpp @@ -868,13 +868,14 @@ RaycastingScene::ListIntersections(const core::Tensor& rays, // generate results structure std::unordered_map result; - result["ray_splits"] = core::Tensor({cumsum.size() + 1}, core::UInt32); + shape[0] = shape[0] + 1; + result["ray_splits"] = core::Tensor(shape, core::UInt32); uint32_t* ptr = result["ray_splits"].GetDataPtr(); ptr[0] = 0; for (int i = 1; i < cumsum.size() + 1; ++i) { ptr[i] = cumsum[i - 1]; } - shape = {intersections_vector.sum()}; + shape[0] = intersections_vector.sum(); result["ray_ids"] = core::Tensor(shape, core::UInt32); result["geometry_ids"] = core::Tensor(shape, core::UInt32); result["primitive_ids"] = core::Tensor(shape, core::UInt32); diff --git a/python/test/t/geometry/test_raycasting_scene.py b/python/test/t/geometry/test_raycasting_scene.py index d89fdc404b3..9b930a877da 100644 --- a/python/test/t/geometry/test_raycasting_scene.py +++ b/python/test/t/geometry/test_raycasting_scene.py @@ -150,7 +150,7 @@ def test_list_intersections(): ans = scene.list_intersections(rays) np.testing.assert_allclose(ans['t_hit'].numpy(), - np.array([[1.0], [2.0], [0.5]]), + np.array([1.0, 2.0, 0.5]), rtol=1e-6, atol=1e-6) From cc4caf422989e3c67f6a36b0a73bbb57c318fb93 Mon Sep 17 00:00:00 2001 From: dmitrishastin Date: Sun, 29 Oct 2023 03:12:17 +0000 Subject: [PATCH 09/14] Revert "Merge branch 'list_intersections' of https://github.com/dmitrishastin/Open3D into list_intersections" This reverts commit 58abef615e141a1007c3a63e9c1b30ab63c06412, reversing changes made to 9d7dccf6df7b67eaca356528d852373c229a634d. --- util/check_style.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/util/check_style.py b/util/check_style.py index 437446092f5..97b7c1ba58c 100644 --- a/util/check_style.py +++ b/util/check_style.py @@ -78,7 +78,7 @@ def _check_style(file_path, clang_format_bin): """ Returns (true, true) if (style, header) is valid. """ - with open(file_path, 'r', encoding='utf-8') as f: + with open(file_path, 'r') as f: is_valid_header = f.read().startswith(CppFormatter.standard_header) cmd = [ @@ -156,7 +156,7 @@ def _check_style(file_path, style_config): Returns (true, true) if (style, header) is valid. """ - with open(file_path, 'r', encoding='utf-8') as f: + with open(file_path, 'r') as f: content = f.read() is_valid_header = (len(content) == 0 or content.startswith( PythonFormatter.standard_header)) @@ -218,7 +218,7 @@ def _check_or_apply_style(file_path, style_config, apply): are merged into one. """ # Ref: https://gist.github.com/oskopek/496c0d96c79fb6a13692657b39d7c709 - with open(file_path, "r", encoding='utf-8') as f: + with open(file_path, "r") as f: notebook = nbformat.read(f, as_version=nbformat.NO_CONVERT) nbformat.validate(notebook) @@ -241,7 +241,7 @@ def _check_or_apply_style(file_path, style_config, apply): changed = True if apply: - with open(file_path, "w", encoding='utf-8') as f: + with open(file_path, "w") as f: nbformat.write(notebook, f, version=nbformat.NO_CONVERT) return not changed @@ -444,4 +444,4 @@ def main(): if __name__ == "__main__": - main() \ No newline at end of file + main() From 20878e26de3ca9b18d9058112fd66ab2e549336f Mon Sep 17 00:00:00 2001 From: Benjamin Ummenhofer Date: Tue, 31 Oct 2023 18:10:23 +0100 Subject: [PATCH 10/14] minor changes to list_intersections example --- cpp/pybind/t/geometry/raycasting_scene.cpp | 57 +++++++++++----------- 1 file changed, 29 insertions(+), 28 deletions(-) diff --git a/cpp/pybind/t/geometry/raycasting_scene.cpp b/cpp/pybind/t/geometry/raycasting_scene.cpp index 15813d305ff..8f7355600ae 100644 --- a/cpp/pybind/t/geometry/raycasting_scene.cpp +++ b/cpp/pybind/t/geometry/raycasting_scene.cpp @@ -190,43 +190,44 @@ Lists the intersections of the rays with the scene:: # Create scene and add the monkey model. scene = o3d.t.geometry.RaycastingScene() d = o3d.data.MonkeyModel() - mesh = o3d.t.io.read_triangle_mesh(d.path) + mesh = o3d.t.io.read_triangle_mesh(d.path) mesh_id = scene.add_triangles(mesh) - - # Create an offset grid of rays. - p_min = np.min(mesh.vertex['positions'].numpy() - 1, axis=0) - p_max = np.max(mesh.vertex['positions'].numpy() - 1, axis=0) - x = np.arange(p_min[0], p_max[0], .1) - y = np.arange(p_min[1], p_max[1], .1) - xv, yv = np.meshgrid(x, y) - orig = np.vstack([xv.flatten(), yv.flatten(), np.tile(p_min[2], xv.size)]).T - dest = np.copy(orig) - dest[:, 2] = p_max[2] + 1 - rays = np.hstack([orig, dest - orig]).astype('float32') + + # Create a grid of rays covering the bounding box + bb_min = mesh.vertex['positions'].min(dim=0).numpy() + bb_max = mesh.vertex['positions'].max(dim=0).numpy() + x,y = np.linspace(bb_min, bb_max, num=10)[:,:2].T + xv, yv = np.meshgrid(x,y) + orig = np.stack([xv, yv, np.full_like(xv, bb_min[2]-1)], axis=-1).reshape(-1,3) + dest = orig + np.full(orig.shape, (0,0,2+bb_max[2]-bb_min[2]),dtype=np.float32) + rays = np.concatenate([orig, dest-orig], axis=-1).astype(np.float32) # Compute the ray intersections. lx = scene.list_intersections(rays) + lx = {k:v.numpy() for k,v in lx.items()} - # Calculate intersection coordinates. + # Calculate intersection coordinates using the primitive uvs and the mesh v = mesh.vertex['positions'].numpy() t = mesh.triangle['indices'].numpy() - tidx = lx['primitive_ids'].numpy() - uv = lx['primitive_uvs'].numpy() + tidx = lx['primitive_ids'] + uv = lx['primitive_uvs'] w = 1 - np.sum(uv, axis=1) c = \ - v[t[tidx, 1].flatten(), :] * uv[:, 0][:, None] + \ - v[t[tidx, 2].flatten(), :] * uv[:, 1][:, None] + \ - v[t[tidx, 0].flatten(), :] * w[:, None] - - # Visualize the intersections. - entry = o3d.geometry.PointCloud(points = o3d.utility.Vector3dVector(orig)) - target = o3d.geometry.PointCloud(points = o3d.utility.Vector3dVector(dest)) - correspondence = [(i, i) for i in range(rays.shape[0])] - traj = o3d.geometry.LineSet.create_from_point_cloud_correspondences(entry , target , correspondence) - traj.colors = o3d.utility.Vector3dVector(np.tile([1, 0, 0], [rays.shape[0], 1])) - x = o3d.geometry.PointCloud(points = o3d.utility.Vector3dVector(c)) - o3d.visualization.draw([mesh, traj, x]) - + v[t[tidx, 1].flatten(), :] * uv[:, 0][:, None] + \ + v[t[tidx, 2].flatten(), :] * uv[:, 1][:, None] + \ + v[t[tidx, 0].flatten(), :] * w[:, None] + + # Calculate intersection coordinates using ray_ids + c = rays[lx['ray_ids']][:,:3] + rays[lx['ray_ids']][:,3:]*lx['t_hit'][...,None] + + # Visualize the rays and intersections. + lines = o3d.t.geometry.LineSet() + lines.point.positions = np.hstack([orig,dest]).reshape(-1,3) + lines.line.indices = np.arange(lines.point.positions.shape[0]).reshape(-1,2) + lines.line.colors = np.full((lines.line.indices.shape[0],3), (1,0,0)) + x = o3d.t.geometry.PointCloud(positions=c) + o3d.visualization.draw([mesh, lines, x], point_size=8) + Args: rays (open3d.core.Tensor): A tensor with >=2 dims, shape {.., 6}, and Dtype From 26b4236fcaa86245a928d9ffefe8eff44f68441d Mon Sep 17 00:00:00 2001 From: dmitrishastin Date: Thu, 2 Nov 2023 01:28:24 +0000 Subject: [PATCH 11/14] correcting shape handling and documentation --- cpp/open3d/t/geometry/RaycastingScene.cpp | 3 +- cpp/open3d/t/geometry/RaycastingScene.h | 41 +++++++++++-------- cpp/pybind/t/geometry/raycasting_scene.cpp | 23 +++++------ .../test/t/geometry/test_raycasting_scene.py | 5 +-- 4 files changed, 38 insertions(+), 34 deletions(-) diff --git a/cpp/open3d/t/geometry/RaycastingScene.cpp b/cpp/open3d/t/geometry/RaycastingScene.cpp index 4774beeafa4..8218b6bf93c 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.cpp +++ b/cpp/open3d/t/geometry/RaycastingScene.cpp @@ -868,7 +868,8 @@ RaycastingScene::ListIntersections(const core::Tensor& rays, // generate results structure std::unordered_map result; - shape[0] = shape[0] + 1; + shape.clear(); + shape.push_back(num_rays + 1); result["ray_splits"] = core::Tensor(shape, core::UInt32); uint32_t* ptr = result["ray_splits"].GetDataPtr(); ptr[0] = 0; diff --git a/cpp/open3d/t/geometry/RaycastingScene.h b/cpp/open3d/t/geometry/RaycastingScene.h index f18e4ae3e62..1d81688d658 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.h +++ b/cpp/open3d/t/geometry/RaycastingScene.h @@ -105,11 +105,13 @@ class RaycastingScene { const int nthreads = 0); /// \brief Computes the closest points on the surfaces 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 - /// dimensions, e.g., to organize the query_point to create a 3D grid the - /// shape can be {depth, height, width, 3}. The last dimension must be 3 and - /// has the format [x, y, z]. + /// \param rays A tensor with >=2 dims, shape {.., 6}, and Dtype Float32 + /// describing the rays. + /// {..} can be any number of dimensions, e.g., to organize rays for + /// creating an image the shape can be {height, width, 6}; + /// The last dimension must be 6 and has the format [ox, oy, oz, dx, dy, dz] + /// with [ox,oy,oz] as the origin and [dx,dy,dz] as the direction. It is not + /// necessary to normalize the direction. /// \param nthreads The number of threads to use. Set to 0 for automatic. /// \return The returned dictionary contains: /// - \b points A tensor with the closest surface points. The shape @@ -126,26 +128,29 @@ class RaycastingScene { const core::Tensor &rays, const int nthreads = 0); /// \brief Lists the intersections of the rays with the scene - /// \param query_points A tensor with >=2 dims, shape {.., 3} and Dtype - /// Float32 describing the query points. {..} can be any number of - /// dimensions, e.g., to organize the query_point to create a 3D grid the - /// shape can be {depth, height, width, 3}. The last dimension must be 3 and - /// has the format [x, y, z]. + /// \param rays A tensor with >=2 dims, shape {.., 6}, and Dtype Float32 + /// describing the rays; {..} can be any number of dimensions. + /// The last dimension must be 6 and has the format [ox, oy, oz, dx, dy, dz] + /// with [ox,oy,oz] as the origin and [dx,dy,dz] as the direction. It is not + /// necessary to normalize the direction although it should be normalised if + /// t_hit is to be calculated in coordinate units. /// \param nthreads The number of threads to use. Set to 0 for automatic. - /// \return The returned dictionary contains: - /// - \b ray_ids A tensor with ray IDs. The shape is {..}. + /// \return The returned dictionary contains: /// /// - \b ray_splits A tensor with ray intersection splits. Can be /// used to iterate over all intersections for each ray. The shape - /// is {..}. + /// is {num_rays + 1}. + /// - \b ray_ids A tensor with ray IDs. The shape is + /// {num_intersections}. + /// - \b t_hit A tensor with the distance to the hit. The shape is + /// {num_intersections}. /// - \b geometry_ids A tensor with the geometry IDs. The shape is - /// {..}. + /// {num_intersections}. /// - \b primitive_ids A tensor with the primitive IDs, which - /// corresponds to the triangle index. The shape is {..}. + /// corresponds to the triangle index. The shape is + /// {num_intersections}. /// - \b primitive_uvs A tensor with the barycentric coordinates of /// the intersection points within the triangles. The shape is - /// {.., 2}. - /// - \b t_hit A tensor with the distance to the hit. The shape is - /// {..}. + /// {num_intersections, 2}. std::unordered_map ComputeClosestPoints( const core::Tensor &query_points, const int nthreads = 0); diff --git a/cpp/pybind/t/geometry/raycasting_scene.cpp b/cpp/pybind/t/geometry/raycasting_scene.cpp index 8f7355600ae..e7f6dcecbd5 100644 --- a/cpp/pybind/t/geometry/raycasting_scene.cpp +++ b/cpp/pybind/t/geometry/raycasting_scene.cpp @@ -231,9 +231,7 @@ Lists the intersections of the rays with the scene:: Args: rays (open3d.core.Tensor): A tensor with >=2 dims, shape {.., 6}, and Dtype - Float32 describing the rays. - {..} can be any number of dimensions, e.g., to organize rays for - creating an image the shape can be {height, width, 6}. + Float32 describing the rays; {..} can be any number of dimensions. The last dimension must be 6 and has the format [ox, oy, oz, dx, dy, dz] with [ox,oy,oz] as the origin and [dx,dy,dz] as the direction. It is not necessary to normalize the direction although it should be normalised if @@ -244,25 +242,26 @@ Lists the intersections of the rays with the scene:: Returns: The returned dictionary contains + ray_splits + A tensor with ray intersection splits. Can be used to iterate over all intersections for each ray. The shape is {num_rays + 1}. + ray_ids - A tensor with ray IDs. The shape is {..}. + A tensor with ray IDs. The shape is {num_intersections}. - ray_splits - A tensor with ray intersection splits. Can be used to iterate over all intersections for each ray. The shape is {..}. + t_hit + A tensor with the distance to the hit. The shape is {num_intersections}. geometry_ids - A tensor with the geometry IDs. The shape is {..}. + A tensor with the geometry IDs. The shape is {num_intersections}. primitive_ids A tensor with the primitive IDs, which corresponds to the triangle - index. The shape is {..}. + index. The shape is {num_intersections}. primitive_uvs A tensor with the barycentric coordinates of the intersection points within - the triangles. The shape is {.., 2}. - - t_hit - A tensor with the distance to the hit. The shape is {..}. + the triangles. The shape is {num_intersections, 2}. + An example of using ray_splits:: diff --git a/python/test/t/geometry/test_raycasting_scene.py b/python/test/t/geometry/test_raycasting_scene.py index 9b930a877da..3ce024a2b29 100644 --- a/python/test/t/geometry/test_raycasting_scene.py +++ b/python/test/t/geometry/test_raycasting_scene.py @@ -305,11 +305,10 @@ def test_output_shapes(shape): ans = scene.list_intersections(rays) nx = np.sum(scene.count_intersections(rays).numpy()).tolist() for k, v in ans.items(): - alt_shape = np.copy(shape) if k == 'ray_splits': - alt_shape[0] = shape[0] + 1 + alt_shape = [np.prod(rays.shape[:-1]) + 1] else: - alt_shape[0] = nx + alt_shape = [nx] #use np.append otherwise issues if alt_shape = [0] and last_dim[k] = [] expected_shape = np.append(alt_shape, last_dim[k]).tolist() assert list( From 767ea352f4875e0ead1f78505d5eac66122444e5 Mon Sep 17 00:00:00 2001 From: dmitrishastin Date: Thu, 2 Nov 2023 16:51:55 +0000 Subject: [PATCH 12/14] correcting ray_splits, reversing --- cpp/open3d/t/geometry/RaycastingScene.cpp | 6 +++--- cpp/open3d/t/geometry/RaycastingScene.h | 12 +++++------- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/cpp/open3d/t/geometry/RaycastingScene.cpp b/cpp/open3d/t/geometry/RaycastingScene.cpp index 8218b6bf93c..af7dd3f1480 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.cpp +++ b/cpp/open3d/t/geometry/RaycastingScene.cpp @@ -872,10 +872,10 @@ RaycastingScene::ListIntersections(const core::Tensor& rays, shape.push_back(num_rays + 1); result["ray_splits"] = core::Tensor(shape, core::UInt32); uint32_t* ptr = result["ray_splits"].GetDataPtr(); - ptr[0] = 0; - for (int i = 1; i < cumsum.size() + 1; ++i) { - ptr[i] = cumsum[i - 1]; + for (int i = 0; i < cumsum.size(); ++i) { + ptr[i] = cumsum[i]; } + ptr[num_rays] = num_intersections; shape[0] = intersections_vector.sum(); result["ray_ids"] = core::Tensor(shape, core::UInt32); result["geometry_ids"] = core::Tensor(shape, core::UInt32); diff --git a/cpp/open3d/t/geometry/RaycastingScene.h b/cpp/open3d/t/geometry/RaycastingScene.h index 1d81688d658..68ecade7033 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.h +++ b/cpp/open3d/t/geometry/RaycastingScene.h @@ -105,13 +105,11 @@ class RaycastingScene { const int nthreads = 0); /// \brief Computes the closest points on the surfaces of the scene. - /// \param rays A tensor with >=2 dims, shape {.., 6}, and Dtype Float32 - /// describing the rays. - /// {..} can be any number of dimensions, e.g., to organize rays for - /// creating an image the shape can be {height, width, 6}; - /// The last dimension must be 6 and has the format [ox, oy, oz, dx, dy, dz] - /// with [ox,oy,oz] as the origin and [dx,dy,dz] as the direction. It is not - /// necessary to normalize the direction. + /// \param query_points A tensor with >=2 dims, shape {.., 3} and Dtype + /// Float32 describing the query points. {..} can be any number of + /// dimensions, e.g., to organize the query_point to create a 3D grid the + /// shape can be {depth, height, width, 3}. The last dimension must be 3 and + /// has the format [x, y, z]. /// \param nthreads The number of threads to use. Set to 0 for automatic. /// \return The returned dictionary contains: /// - \b points A tensor with the closest surface points. The shape From d71ec138d8bf673889d2f2c28f589c1bdebeb760 Mon Sep 17 00:00:00 2001 From: Benjamin Ummenhofer Date: Fri, 3 Nov 2023 10:35:56 +0100 Subject: [PATCH 13/14] Update RaycastingScene.h --- cpp/open3d/t/geometry/RaycastingScene.h | 41 +++++++++++++------------ 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/cpp/open3d/t/geometry/RaycastingScene.h b/cpp/open3d/t/geometry/RaycastingScene.h index 68ecade7033..086622f8837 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.h +++ b/cpp/open3d/t/geometry/RaycastingScene.h @@ -104,26 +104,6 @@ class RaycastingScene { core::Tensor CountIntersections(const core::Tensor &rays, const int nthreads = 0); - /// \brief Computes the closest points on the surfaces 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 - /// dimensions, e.g., to organize the query_point to create a 3D grid the - /// shape can be {depth, height, width, 3}. The last dimension must be 3 and - /// has the format [x, y, z]. - /// \param nthreads The number of threads to use. Set to 0 for automatic. - /// \return The returned dictionary contains: - /// - \b points A tensor with the closest surface points. The shape - /// is {..}. - /// - \b geometry_ids A tensor with the geometry IDs. The shape is - /// {..}. - /// - \b primitive_ids A tensor with the primitive IDs, which - /// corresponds to the triangle index. The shape is {..}. - /// - \b primitive_uvs A tensor with the barycentric coordinates of - /// the closest points within the triangles. The shape is {.., 2}. - /// - \b primitive_normals A tensor with the normals of the - /// closest triangle . The shape is {.., 3}. - std::unordered_map ListIntersections( - const core::Tensor &rays, const int nthreads = 0); /// \brief Lists the intersections of the rays with the scene /// \param rays A tensor with >=2 dims, shape {.., 6}, and Dtype Float32 @@ -149,6 +129,27 @@ class RaycastingScene { /// - \b primitive_uvs A tensor with the barycentric coordinates of /// the intersection points within the triangles. The shape is /// {num_intersections, 2}. + std::unordered_map ListIntersections( + const core::Tensor &rays, const int nthreads = 0); + + /// \brief Computes the closest points on the surfaces 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 + /// dimensions, e.g., to organize the query_point to create a 3D grid the + /// shape can be {depth, height, width, 3}. The last dimension must be 3 and + /// has the format [x, y, z]. + /// \param nthreads The number of threads to use. Set to 0 for automatic. + /// \return The returned dictionary contains: + /// - \b points A tensor with the closest surface points. The shape + /// is {..}. + /// - \b geometry_ids A tensor with the geometry IDs. The shape is + /// {..}. + /// - \b primitive_ids A tensor with the primitive IDs, which + /// corresponds to the triangle index. The shape is {..}. + /// - \b primitive_uvs A tensor with the barycentric coordinates of + /// the closest points within the triangles. The shape is {.., 2}. + /// - \b primitive_normals A tensor with the normals of the + /// closest triangle . The shape is {.., 3}. std::unordered_map ComputeClosestPoints( const core::Tensor &query_points, const int nthreads = 0); From c4141b3b91c70c00cb2678a31fb2745ed6655916 Mon Sep 17 00:00:00 2001 From: Benjamin Ummenhofer Date: Fri, 3 Nov 2023 11:37:09 +0100 Subject: [PATCH 14/14] apply style --- cpp/open3d/t/geometry/RaycastingScene.h | 1 - 1 file changed, 1 deletion(-) diff --git a/cpp/open3d/t/geometry/RaycastingScene.h b/cpp/open3d/t/geometry/RaycastingScene.h index 086622f8837..f25d994b0b5 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.h +++ b/cpp/open3d/t/geometry/RaycastingScene.h @@ -104,7 +104,6 @@ class RaycastingScene { core::Tensor CountIntersections(const core::Tensor &rays, const int nthreads = 0); - /// \brief Lists the intersections of the rays with the scene /// \param rays A tensor with >=2 dims, shape {.., 6}, and Dtype Float32 /// describing the rays; {..} can be any number of dimensions.