Voxel hashing GUI

cpp/benchmarks/t/pipelines/odometry/RGBDOdometry.cpp

@@ -51,8 +51,8 @@ static core::Tensor CreateIntrisicTensor() {
              {0, 0, 1}});
 }
 
-static void ComputePosePointToPlane(benchmark::State& state,
-                                    const core::Device& device) {
+static void ComputeOdometryResultPointToPlane(benchmark::State& state,
+                                              const core::Device& device) {
     if (!t::geometry::Image::HAVE_IPPICV &&
         device.GetType() == core::Device::DeviceType::CPU) {
         return;
@@ -85,26 +85,25 @@ static void ComputePosePointToPlane(benchmark::State& state,
             core::Tensor::Eye(4, core::Dtype::Float64, core::Device("CPU:0"));
 
     for (int i = 0; i < 20; ++i) {
-        core::Tensor delta_src_to_dst =
-                t::pipelines::odometry::ComputePosePointToPlane(
-                        src_vertex_map.AsTensor(), dst_vertex_map.AsTensor(),
-                        src_normal_map.AsTensor(), intrinsic_t, trans,
-                        depth_diff);
-        trans = delta_src_to_dst.Matmul(trans).Contiguous();
+        auto result = t::pipelines::odometry::ComputeOdometryResultPointToPlane(
+                src_vertex_map.AsTensor(), dst_vertex_map.AsTensor(),
+                src_normal_map.AsTensor(), intrinsic_t, trans, depth_diff,
+                depth_diff * 0.5);
+        trans = result.transformation_.Matmul(trans).Contiguous();
     }
 
     for (auto _ : state) {
         core::Tensor trans = core::Tensor::Eye(4, core::Dtype::Float64,
                                                core::Device("CPU:0"));
 
         for (int i = 0; i < 20; ++i) {
-            core::Tensor delta_src_to_dst =
-                    t::pipelines::odometry::ComputePosePointToPlane(
+            auto result =
+                    t::pipelines::odometry::ComputeOdometryResultPointToPlane(
                             src_vertex_map.AsTensor(),
                             dst_vertex_map.AsTensor(),
                             src_normal_map.AsTensor(), intrinsic_t, trans,
-                            depth_diff);
-            trans = delta_src_to_dst.Matmul(trans).Contiguous();
+                            depth_diff, depth_diff * 0.5);
+            trans = result.transformation_.Matmul(trans).Contiguous();
         }
     }
 }
@@ -140,25 +139,36 @@ static void RGBDOdometryMultiScale(
 
     core::Tensor intrinsic_t = CreateIntrisicTensor();
 
+    // Very strict criteria to ensure running most the iterations
+    t::pipelines::odometry::OdometryLossParams loss(depth_diff);
+    std::vector<t::pipelines::odometry::OdometryConvergenceCriteria> criteria{
+            t::pipelines::odometry::OdometryConvergenceCriteria(10, 1e-12,
+                                                                1e-12),
+            t::pipelines::odometry::OdometryConvergenceCriteria(5, 1e-12,
+                                                                1e-12),
+            t::pipelines::odometry::OdometryConvergenceCriteria(3, 1e-12,
+                                                                1e-12)};
+
     // Warp up
     RGBDOdometryMultiScale(
             source, target, intrinsic_t,
             core::Tensor::Eye(4, core::Dtype::Float64, core::Device("CPU:0")),
-            depth_scale, depth_max, depth_diff, {10, 5, 3}, method);
+            depth_scale, depth_max, criteria, method, loss);
 
     for (auto _ : state) {
         RGBDOdometryMultiScale(source, target, intrinsic_t,
                                core::Tensor::Eye(4, core::Dtype::Float64,
                                                  core::Device("CPU:0")),
-                               depth_scale, depth_max, depth_diff, {10, 5, 3},
-                               method);
+                               depth_scale, depth_max, criteria, method, loss);
     }
 }
 
-BENCHMARK_CAPTURE(ComputePosePointToPlane, CPU, core::Device("CPU:0"))
+BENCHMARK_CAPTURE(ComputeOdometryResultPointToPlane, CPU, core::Device("CPU:0"))
         ->Unit(benchmark::kMillisecond);
 #ifdef BUILD_CUDA_MODULE
-BENCHMARK_CAPTURE(ComputePosePointToPlane, CUDA, core::Device("CUDA:0"))
+BENCHMARK_CAPTURE(ComputeOdometryResultPointToPlane,
+                  CUDA,
+                  core::Device("CUDA:0"))
         ->Unit(benchmark::kMillisecond);
 #endif
 

cpp/open3d/core/hashmap/CUDA/StdGPUHashmap.h

@@ -378,6 +378,7 @@ void StdGPUHashmap<Key, Hash>::Allocate(int64_t capacity) {
 
     impl_ = stdgpu::unordered_map<Key, addr_t, Hash>::createDeviceObject(
             this->capacity_);
+    OPEN3D_CUDA_CHECK(cudaDeviceSynchronize());
 }
 
 template <typename Key, typename Hash>

cpp/open3d/t/geometry/Image.cpp

@@ -484,13 +484,13 @@ Image Image::ClipTransform(float scale,
     }
     if (!(std::isnan(clip_fill) || std::isinf(clip_fill) || clip_fill == 0)) {
         utility::LogWarning(
-                "The clip_fill value {} is not recommended Inf, NaN or, 0",
+                "The clip_fill value {} is not recommended. Please use Inf, "
+                "NaN or, 0",
                 clip_fill);
     }
 
-    Image dst_im;
-    dst_im.data_ = core::Tensor::Empty({GetRows(), GetCols(), 1},
-                                       core::Dtype::Float32, data_.GetDevice());
+    Image dst_im(GetRows(), GetCols(), 1, core::Dtype::Float32,
+                 data_.GetDevice());
     kernel::image::ClipTransform(data_, dst_im.data_, scale, min_value,
                                  max_value, clip_fill);
     return dst_im;
@@ -509,9 +509,7 @@ Image Image::CreateVertexMap(const core::Tensor &intrinsics,
                           GetDtype().ToString());
     }
 
-    Image dst_im;
-    dst_im.data_ = core::Tensor::Empty({GetRows(), GetCols(), 3}, GetDtype(),
-                                       GetDevice());
+    Image dst_im(GetRows(), GetCols(), 3, GetDtype(), GetDevice());
     kernel::image::CreateVertexMap(data_, dst_im.data_, intrinsics,
                                    invalid_fill);
     return dst_im;
@@ -529,9 +527,7 @@ Image Image::CreateNormalMap(float invalid_fill) {
                           GetDtype().ToString());
     }
 
-    Image dst_im;
-    dst_im.data_ = core::Tensor::Empty({GetRows(), GetCols(), 3}, GetDtype(),
-                                       GetDevice());
+    Image dst_im(GetRows(), GetCols(), 3, GetDtype(), GetDevice());
     kernel::image::CreateNormalMap(data_, dst_im.data_, invalid_fill);
     return dst_im;
 }
@@ -555,9 +551,7 @@ Image Image::ColorizeDepth(float scale, float min_value, float max_value) {
                 scale, min_value, max_value);
     }
 
-    Image dst_im;
-    dst_im.data_ = core::Tensor::Empty({GetRows(), GetCols(), 3},
-                                       core::Dtype::UInt8, data_.GetDevice());
+    Image dst_im(GetRows(), GetCols(), 3, core::Dtype::UInt8, GetDevice());
     kernel::image::ColorizeDepth(data_, dst_im.data_, scale, min_value,
                                  max_value);
     return dst_im;

cpp/open3d/t/geometry/Image.h

@@ -239,36 +239,41 @@ class Image : public Geometry {
     /// resize (ratio = 0.5) operation.
     Image PyrDown() const;
 
-    /// Preprocess a image of (H, W, 1), typically used for a depth image.
-    /// Each pixel will be transformed by
+    /// Preprocess a image of shape (rows, cols, channels=1), typically used for
+    /// a depth image. UInt16 and Float32 Dtypes supported. Each pixel will be
+    /// transformed by
     /// x = x / scale
     /// x = x < min_value ? clip_fill : x
     /// x = x > max_value ? clip_fill : x
+    /// Use INF, NAN or 0.0 (default) for \p clip_fill
     Image ClipTransform(float scale,
                         float min_value,
                         float max_value,
                         float clip_fill = 0.0f);
 
-    /// Create a vertex map (H, W, 3) in Float32 from an image of (H, W, 1) in
-    /// Float32 using unprojection. The input depth is expected to be the output
-    /// of ClipTransform.
+    /// Create a vertex map (rows, cols, channels=3) in Float32 from an image of
+    /// shape (rows, cols, channels=1) in Float32 using unprojection. The input
+    /// depth is expected to be the output of ClipTransform.
     /// \param intrinsics Pinhole camera model of (3, 3) in Float64.
     /// \param invalid_fill Value to fill in for invalid depths. Must be
-    /// consistent with \p clip_fill in ClipTransform
+    /// consistent with \p clip_fill in ClipTransform.
     Image CreateVertexMap(const core::Tensor &intrinsics, float invalid_fill);
 
-    /// Create a normal map (H, W, 3) in Float32 from an image of (H, W, 3)
-    /// in Float32 using cross product of V(u, v+1)-V(u, v) and V(u+1, v)-V(u,
-    /// v). The input vertex map is expected to be the output of
-    /// CreateVertexMap.
-    /// \param invalid_mask Condition to check if a point is invalid in
-    /// vertexmap, and to fill-in if no valid neighbor is found. Must be
-    /// consistent with CreateVertexMap.
+    /// Create a normal map (rows, cols, channels=3) in Float32 from an image of
+    /// (rows, cols, channels=3) in Float32 using cross product of V(r,
+    /// c+1)-V(r, c) and V(r+1, c)-V(r, c). The input vertex map is expected to
+    /// be the output of CreateVertexMap. You may need to start with a filtered
+    /// depth image (e.g. with FilterBilateral) to obtain good results.
+    /// \param invalid_fill Value to fill in for invalid points, and to fill-in
+    /// if no valid neighbor is found. Must be consistent with \p invalid_fill
+    /// in CreateVertexMap.
     Image CreateNormalMap(float invalid_fill);
 
-    /// Colorize an input depth image with the Turbo colormap, rescaled within
-    /// (min_range, max_range)
-    Image ColorizeDepth(float scale, float min_range, float max_range);
+    /// Colorize an input depth image (with Dtype UInt16 or Float32). The image
+    /// values are divided by scale, then clamped within [min_value, max_value]
+    /// and finally converted to a 3 channel UInt8 RGB image using the Turbo
+    /// colormap as a lookup table.
+    Image ColorizeDepth(float scale, float min_value, float max_value);
 
     /// Compute min 2D coordinates for the data (always {0, 0}).
     core::Tensor GetMinBound() const {

cpp/open3d/t/geometry/kernel/GeometryMacros.h

@@ -59,7 +59,7 @@ __device__ inline float atomicMaxf(float *addr, float value) {
 }
 #endif
 
-OPEN3D_HOST_DEVICE inline int sign(int x) {
+OPEN3D_HOST_DEVICE inline int Sign(int x) {
     return (x > 0) ? 1 : ((x < 0) ? -1 : 0);
 }
 

cpp/open3d/t/geometry/kernel/ImageImpl.h

@@ -36,6 +36,11 @@ namespace geometry {
 namespace kernel {
 namespace image {
 
+#ifndef __CUDACC__
+using std::isinf;
+using std::isnan;
+#endif
+
 #ifdef __CUDACC__
 void ClipTransformCUDA
 #else
@@ -173,15 +178,23 @@ void CreateVertexMapCPU
     core::kernel::CUDALauncher launcher;
 #else
     core::kernel::CPULauncher launcher;
+    using std::isinf;
+    using std::isnan;
 #endif
     launcher.LaunchGeneralKernel(n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
+        auto is_invalid = [invalid_fill] OPEN3D_DEVICE(float v) {
+            if (isinf(invalid_fill)) return isinf(v);
+            if (isnan(invalid_fill)) return isnan(v);
+            return v == invalid_fill;
+        };
+
         int64_t y = workload_idx / cols;
         int64_t x = workload_idx % cols;
 
         float d = *src_indexer.GetDataPtrFromCoord<float>(x, y);
 
         float* vertex = dst_indexer.GetDataPtrFromCoord<float>(x, y);
-        if (d != invalid_fill) {
+        if (!is_invalid(d)) {
             ti.Unproject(static_cast<float>(x), static_cast<float>(y), d,
                          vertex + 0, vertex + 1, vertex + 2);
         } else {
@@ -191,7 +204,6 @@ void CreateVertexMapCPU
         }
     });
 }
-
 #ifdef __CUDACC__
 void CreateNormalMapCUDA
 #else

cpp/open3d/t/geometry/kernel/TSDFVoxel.h

@@ -182,9 +182,9 @@ inline OPEN3D_DEVICE voxel_t* DeviceGetVoxelAt(
     int yn = (yo + resolution) % resolution;
     int zn = (zo + resolution) % resolution;
 
-    int64_t dxb = sign(xo - xn);
-    int64_t dyb = sign(yo - yn);
-    int64_t dzb = sign(zo - zn);
+    int64_t dxb = Sign(xo - xn);
+    int64_t dyb = Sign(yo - yn);
+    int64_t dzb = Sign(zo - zn);
 
     int64_t nb_idx = (dxb + 1) + (dyb + 1) * 3 + (dzb + 1) * 9;
 

cpp/open3d/t/geometry/kernel/TSDFVoxelGridImpl.h

@@ -1250,9 +1250,9 @@ void RayCastCPU
                         int y_vn = (y_v + block_resolution) % block_resolution;
                         int z_vn = (z_v + block_resolution) % block_resolution;
 
-                        int dx_b = sign(x_v - x_vn);
-                        int dy_b = sign(y_v - y_vn);
-                        int dz_b = sign(z_v - z_vn);
+                        int dx_b = Sign(x_v - x_vn);
+                        int dy_b = Sign(y_v - y_vn);
+                        int dz_b = Sign(z_v - z_vn);
 
                         if (dx_b == 0 && dy_b == 0 && dz_b == 0) {
                             return voxel_block_buffer_indexer