From 4c5ded85b29e724cb046d97dc67770bf298aed2f Mon Sep 17 00:00:00 2001 From: Benjamin Ummenhofer Date: Fri, 23 Dec 2022 12:54:02 +0100 Subject: [PATCH] add voting and ray jitter to compute the inside outside (#5797) * add voting and ray jitter to compute the inside outside * renamed samples argument and improved documentation * change ray generation * fix wrong arg name * change back to use a local rng for ray generation --- cpp/open3d/t/geometry/RaycastingScene.cpp | 129 +++++++++++++----- cpp/open3d/t/geometry/RaycastingScene.h | 16 ++- cpp/pybind/t/geometry/raycasting_scene.cpp | 18 ++- .../test/t/geometry/test_raycasting_scene.py | 45 ++++++ 4 files changed, 166 insertions(+), 42 deletions(-) diff --git a/cpp/open3d/t/geometry/RaycastingScene.cpp b/cpp/open3d/t/geometry/RaycastingScene.cpp index e3edbbb6ef0..56dbd24a9c6 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.cpp +++ b/cpp/open3d/t/geometry/RaycastingScene.cpp @@ -761,60 +761,117 @@ core::Tensor RaycastingScene::ComputeDistance(const core::Tensor& query_points, return distance; } +namespace { +// Helper function to determine the inside and outside with voting. +core::Tensor VoteInsideOutside(RaycastingScene& scene, + const core::Tensor& query_points, + const int nthreads = 0, + const int num_votes = 3, + const int inside_val = 1, + const int outside_val = 0) { + auto shape = query_points.GetShape(); + shape.pop_back(); // Remove last dim, we want to use this shape for the + // results. + size_t num_query_points = shape.NumElements(); + + // Use local RNG here to generate rays with a similar direction in a + // deterministic manner. + std::mt19937 gen(42); + std::uniform_real_distribution dist(-0.001, 0.001); + Eigen::MatrixXf ray_dirs(3, num_votes); + ray_dirs = ray_dirs.unaryExpr([&](float) { return 1 + dist(gen); }); + + auto query_points_ = query_points.Contiguous(); + Eigen::Map query_points_map( + query_points_.GetDataPtr(), 3, num_query_points); + + core::Tensor rays({int64_t(num_votes * num_query_points), 6}, + core::Float32); + Eigen::Map rays_map(rays.GetDataPtr(), 6, + num_votes * num_query_points); + if (num_votes > 1) { + for (size_t i = 0; i < num_query_points; ++i) { + for (int j = 0; j < num_votes; ++j) { + rays_map.col(i * num_votes + j).topRows<3>() = + query_points_map.col(i); + rays_map.col(i * num_votes + j).bottomRows<3>() = + ray_dirs.col(j); + } + } + } else { + for (size_t i = 0; i < num_query_points; ++i) { + rays_map.col(i).topRows<3>() = query_points_map.col(i); + rays_map.col(i).bottomRows<3>() = ray_dirs; + } + } + + auto intersections = scene.CountIntersections(rays, nthreads); + Eigen::Map intersections_map( + intersections.GetDataPtr(), num_votes, num_query_points); + + if (num_votes > 1) { + core::Tensor result({int64_t(num_query_points)}, core::Int32); + Eigen::Map result_map(result.GetDataPtr(), + num_query_points); + result_map = + intersections_map.unaryExpr([&](const int x) { return x % 2; }) + .colwise() + .sum() + .unaryExpr([&](const int x) { + return (x > num_votes / 2) ? inside_val + : outside_val; + }); + return result.Reshape(shape); + } else { + intersections_map = intersections_map.unaryExpr([&](const int x) { + return (x % 2) ? inside_val : outside_val; + }); + return intersections.Reshape(shape); + } +} +} // namespace + core::Tensor RaycastingScene::ComputeSignedDistance( - const core::Tensor& query_points, const int nthreads) { + const core::Tensor& query_points, + const int nthreads, + const int nsamples) { AssertTensorDtypeLastDimDeviceMinNDim(query_points, "query_points", 3, impl_->tensor_device_); + + if (nsamples < 1 || (nsamples % 2) != 1) { + open3d::utility::LogError("nsamples must be odd and >= 1 but is {}", + nsamples); + } auto shape = query_points.GetShape(); shape.pop_back(); // Remove last dim, we want to use this shape for the // results. size_t num_query_points = shape.NumElements(); - auto data = query_points.Contiguous(); auto distance = ComputeDistance(data, nthreads); - core::Tensor rays({int64_t(num_query_points), 6}, core::Float32); - rays.SetItem({core::TensorKey::Slice(0, num_query_points, 1), - core::TensorKey::Slice(0, 3, 1)}, - data.Reshape({int64_t(num_query_points), 3})); - rays.SetItem({core::TensorKey::Slice(0, num_query_points, 1), - core::TensorKey::Slice(3, 6, 1)}, - core::Tensor::Ones({1}, core::Float32, impl_->tensor_device_) - .Expand({int64_t(num_query_points), 3})); - auto intersections = CountIntersections(rays, nthreads); - Eigen::Map distance_map(distance.GetDataPtr(), num_query_points); - Eigen::Map intersections_map( - intersections.GetDataPtr(), num_query_points); - intersections_map = intersections_map.unaryExpr( - [](const int x) { return (x % 2) ? -1 : 1; }); - distance_map.array() *= intersections_map.array().cast(); + + auto inside_outside = + VoteInsideOutside(*this, data, nthreads, nsamples, -1, 1); + Eigen::Map inside_outside_map( + inside_outside.GetDataPtr(), num_query_points); + distance_map.array() *= inside_outside_map.array().cast(); return distance; } core::Tensor RaycastingScene::ComputeOccupancy(const core::Tensor& query_points, - const int nthreads) { + const int nthreads, + const int nsamples) { AssertTensorDtypeLastDimDeviceMinNDim(query_points, "query_points", 3, impl_->tensor_device_); - auto shape = query_points.GetShape(); - shape.pop_back(); // Remove last dim, we want to use this shape for the - // results. - size_t num_query_points = shape.NumElements(); - core::Tensor rays({int64_t(num_query_points), 6}, core::Float32); - rays.SetItem({core::TensorKey::Slice(0, num_query_points, 1), - core::TensorKey::Slice(0, 3, 1)}, - query_points.Reshape({int64_t(num_query_points), 3})); - rays.SetItem({core::TensorKey::Slice(0, num_query_points, 1), - core::TensorKey::Slice(3, 6, 1)}, - core::Tensor::Ones({1}, core::Float32, impl_->tensor_device_) - .Expand({int64_t(num_query_points), 3})); - auto intersections = CountIntersections(rays, nthreads); - Eigen::Map intersections_map( - intersections.GetDataPtr(), num_query_points); - intersections_map = - intersections_map.unaryExpr([](const int x) { return x % 2; }); - return intersections.To(core::Float32).Reshape(shape); + if (nsamples < 1 || (nsamples % 2) != 1) { + open3d::utility::LogError("samples must be odd and >= 1 but is {}", + nsamples); + } + + auto result = VoteInsideOutside(*this, query_points, nthreads, nsamples); + return result.To(core::Float32); } core::Tensor RaycastingScene::CreateRaysPinhole( diff --git a/cpp/open3d/t/geometry/RaycastingScene.h b/cpp/open3d/t/geometry/RaycastingScene.h index 2e21a4d11b3..20ff3c40ec6 100644 --- a/cpp/open3d/t/geometry/RaycastingScene.h +++ b/cpp/open3d/t/geometry/RaycastingScene.h @@ -170,11 +170,17 @@ class RaycastingScene { /// 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. + /// \param nsamples The number of rays used for determining the inside. + /// This must be an odd number. The default is 1. Use a higher value if you + /// notice sign flipping, which can occur when rays hit exactly an edge or + /// vertex in the scene. + /// /// \return A tensor with the signed distances to /// the surface. The shape is /// {..}. Negative distances mean a point is inside a closed surface. core::Tensor ComputeSignedDistance(const core::Tensor &query_points, - const int nthreads = 0); + const int nthreads = 0, + const int nsamples = 1); /// \brief Computes the occupancy at the query point positions. /// @@ -191,10 +197,16 @@ class RaycastingScene { /// {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. + /// \param nsamples The number of rays used for determining the inside. + /// This must be an odd number. The default is 1. Use a higher value if you + /// notice errors in the occupancy values. Errors can occur when rays hit + /// exactly an edge or vertex in the scene. + /// /// \return A tensor with the occupancy values. The shape is {..}. Values /// are either 0 or 1. A point is occupied or inside if the value is 1. core::Tensor ComputeOccupancy(const core::Tensor &query_points, - const int nthreads = 0); + const int nthreads = 0, + const int nsamples = 1); /// \brief Creates rays for the given camera parameters. /// diff --git a/cpp/pybind/t/geometry/raycasting_scene.cpp b/cpp/pybind/t/geometry/raycasting_scene.cpp index 7b1307df799..338f255e755 100644 --- a/cpp/pybind/t/geometry/raycasting_scene.cpp +++ b/cpp/pybind/t/geometry/raycasting_scene.cpp @@ -253,9 +253,9 @@ Computes the distance to the surface of the scene. A tensor with the distances to the surface. The shape is {..}. )doc"); - raycasting_scene.def("compute_signed_distance", - &RaycastingScene::ComputeSignedDistance, - "query_points"_a, "nthreads"_a = 0, R"doc( + raycasting_scene.def( + "compute_signed_distance", &RaycastingScene::ComputeSignedDistance, + "query_points"_a, "nthreads"_a = 0, "nsamples"_a = 1, R"doc( Computes the signed distance to the surface of the scene. This function computes the signed distance to the meshes in the scene. @@ -274,6 +274,11 @@ the intersections of a rays starting at the query points. nthreads (int): The number of threads to use. Set to 0 for automatic. + nsamples (int): The number of rays used for determining the inside. + This must be an odd number. The default is 1. Use a higher value if you + notice sign flipping, which can occur when rays hit exactly an edge or + vertex in the scene. + Returns: A tensor with the signed distances to the surface. The shape is {..}. Negative distances mean a point is inside a closed surface. @@ -281,7 +286,7 @@ the intersections of a rays starting at the query points. raycasting_scene.def("compute_occupancy", &RaycastingScene::ComputeOccupancy, "query_points"_a, - "nthreads"_a = 0, + "nthreads"_a = 0, "nsamples"_a = 1, R"doc( Computes the occupancy at the query point positions. @@ -301,6 +306,11 @@ intersections of a rays starting at the query points. nthreads (int): The number of threads to use. Set to 0 for automatic. + nsamples (int): The number of rays used for determining the inside. + This must be an odd number. The default is 1. Use a higher value if you + notice errors in the occupancy values. Errors can occur when rays hit + exactly an edge or vertex in the scene. + Returns: A tensor with the occupancy values. The shape is {..}. Values are either 0 or 1. A point is occupied or inside if the value is 1. diff --git a/python/test/t/geometry/test_raycasting_scene.py b/python/test/t/geometry/test_raycasting_scene.py index 635eab193ff..4e1c1352de5 100644 --- a/python/test/t/geometry/test_raycasting_scene.py +++ b/python/test/t/geometry/test_raycasting_scene.py @@ -285,3 +285,48 @@ def test_output_shapes(shape): 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 + # without ray jitter produced wrong results for a sphere because some + # rays miss hitting exactly a vertex or an edge. + mesh = o3d.geometry.TriangleMesh.create_sphere(0.8) + mesh = o3d.t.geometry.TriangleMesh.from_legacy(mesh) + + scene = o3d.t.geometry.RaycastingScene() + scene.add_triangles(mesh) + + min_bound = mesh.vertex.positions.min(0).numpy() * 1.1 + max_bound = mesh.vertex.positions.max(0).numpy() * 1.1 + + xyz_range = np.linspace(min_bound, max_bound, num=6) + query_points = np.stack(np.meshgrid(*xyz_range.T), + axis=-1).astype(np.float32) + + occupancy = scene.compute_occupancy(query_points) + expected = np.array( + [[[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], + [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1.0, 1.0, 0.0, 0.0], + [0.0, 1.0, 1.0, 1.0, 1.0, 0.0], [0.0, 1.0, 1.0, 1.0, 1.0, 0.0], + [0.0, 0.0, 1.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], + [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 1.0, 1.0, 1.0, 1.0, 0.0], + [0.0, 1.0, 1.0, 1.0, 1.0, 0.0], [0.0, 1.0, 1.0, 1.0, 1.0, 0.0], + [0.0, 1.0, 1.0, 1.0, 1.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], + [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 1.0, 1.0, 1.0, 1.0, 0.0], + [0.0, 1.0, 1.0, 1.0, 1.0, 0.0], [0.0, 1.0, 1.0, 1.0, 1.0, 0.0], + [0.0, 1.0, 1.0, 1.0, 1.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], + [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1.0, 1.0, 0.0, 0.0], + [0.0, 1.0, 1.0, 1.0, 1.0, 0.0], [0.0, 1.0, 1.0, 1.0, 1.0, 0.0], + [0.0, 0.0, 1.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], + [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]], + dtype=np.float32) + np.testing.assert_equal(occupancy.numpy(), expected) + + # we should get the same result with more samples + occupancy_3samples = scene.compute_occupancy(query_points, nsamples=3) + np.testing.assert_equal(occupancy_3samples.numpy(), expected)