Add default parameters in slam::Model cpp. (#5304)

.dockerignore

@@ -6,3 +6,4 @@ docs/_static/build
 docs/_out
 3rdparty_downloads
 .python-version
+cmake-build-*

CHANGELOG.md

@@ -6,6 +6,7 @@
 * Corrected documentation for KDTree (typo in Notebook) (PR #4744)
 * Remove `setuptools` and `wheel` from requirements for end users (PR #5020)
 * Fix various typos (PR #5070)
+* Exposed more functionality in SLAM pipeline
 * Fix for depth estimation for VoxelBlockGrid
 * Reserve fragment buffer for VoxelBlockGrid operations
 * Fix raycasting scene: Allow setting of number of threads that are used for building a raycasting scene

cpp/open3d/t/geometry/VoxelBlockGrid.cpp

@@ -545,6 +545,17 @@ void VoxelBlockGrid::Save(const std::string &file_name) const {
     }
 }
 
+VoxelBlockGrid VoxelBlockGrid::To(const core::Device &device, bool copy) const {
+    if (!copy && block_hashmap_->GetDevice() == device) {
+        return *this;
+    }
+
+    auto device_hashmap =
+            std::make_shared<core::HashMap>(this->block_hashmap_->To(device));
+    return VoxelBlockGrid(voxel_size_, block_resolution_, device_hashmap,
+                          name_attr_map_);
+}
+
 VoxelBlockGrid VoxelBlockGrid::Load(const std::string &file_name) {
     std::unordered_map<std::string, core::Tensor> tensor_map =
             t::io::ReadNpz(file_name);

cpp/open3d/t/geometry/VoxelBlockGrid.h

@@ -237,9 +237,21 @@ class VoxelBlockGrid {
     /// Load a voxel block grid from a .npz file.
     static VoxelBlockGrid Load(const std::string &file_name);
 
+    /// Convert the hash map to another device.
+    VoxelBlockGrid To(const core::Device &device, bool copy = false) const;
+
 private:
     void AssertInitialized() const;
 
+    VoxelBlockGrid(float voxelSize,
+                   int64_t blockResolution,
+                   const std::shared_ptr<core::HashMap> &blockHashmap,
+                   const std::unordered_map<std::string, int> &nameAttrMap)
+        : voxel_size_(voxelSize),
+          block_resolution_(blockResolution),
+          block_hashmap_(blockHashmap),
+          name_attr_map_(nameAttrMap) {}
+
     float voxel_size_ = -1;
     int64_t block_resolution_ = -1;
 

cpp/open3d/t/pipelines/slam/Model.cpp

@@ -59,14 +59,20 @@ void Model::SynthesizeModelFrame(Frame& raycast_frame,
                                  float depth_min,
                                  float depth_max,
                                  float trunc_voxel_multiplier,
-                                 bool enable_color) {
+                                 bool enable_color,
+                                 float weight_threshold) {
+    if (weight_threshold < 0) {
+        weight_threshold = std::min(frame_id_ * 1.0f, 3.0f);
+    }
+
     auto result = voxel_grid_.RayCast(
             frustum_block_coords_, raycast_frame.GetIntrinsics(),
             t::geometry::InverseTransformation(GetCurrentFramePose()),
             raycast_frame.GetWidth(), raycast_frame.GetHeight(),
             {"depth", "color"}, depth_scale, depth_min, depth_max,
-            std::min(frame_id_ * 1.0f, 3.0f), trunc_voxel_multiplier);
+            weight_threshold, trunc_voxel_multiplier);
     raycast_frame.SetData("depth", result["depth"]);
+
     if (enable_color) {
         raycast_frame.SetData("color", result["color"]);
     } else if (raycast_frame.GetData("color").NumElements() == 0) {
@@ -78,23 +84,26 @@ void Model::SynthesizeModelFrame(Frame& raycast_frame,
     }
 }
 
-odometry::OdometryResult Model::TrackFrameToModel(const Frame& input_frame,
-                                                  const Frame& raycast_frame,
-                                                  float depth_scale,
-                                                  float depth_max,
-                                                  float depth_diff) {
-    const static core::Tensor identity =
+odometry::OdometryResult Model::TrackFrameToModel(
+        const Frame& input_frame,
+        const Frame& raycast_frame,
+        float depth_scale,
+        float depth_max,
+        float depth_diff,
+        const odometry::Method method,
+        const std::vector<odometry::OdometryConvergenceCriteria>& criteria) {
+    // TODO: Expose init_source_to_target as param, and make the input sequence
+    // consistent with RGBDOdometryMultiScale.
+    const static core::Tensor init_source_to_target =
             core::Tensor::Eye(4, core::Float64, core::Device("CPU:0"));
 
-    // TODO: more customized / optimized
     return odometry::RGBDOdometryMultiScale(
             t::geometry::RGBDImage(input_frame.GetDataAsImage("color"),
                                    input_frame.GetDataAsImage("depth")),
             t::geometry::RGBDImage(raycast_frame.GetDataAsImage("color"),
                                    raycast_frame.GetDataAsImage("depth")),
-            raycast_frame.GetIntrinsics(), identity, depth_scale, depth_max,
-            std::vector<odometry::OdometryConvergenceCriteria>{6, 3, 1},
-            odometry::Method::PointToPlane,
+            raycast_frame.GetIntrinsics(), init_source_to_target, depth_scale,
+            depth_max, criteria, method,
             odometry::OdometryLossParams(depth_diff));
 }
 

cpp/open3d/t/pipelines/slam/Model.h

@@ -45,14 +45,14 @@ class Model {
 public:
     Model() {}
     Model(float voxel_size,
-          int block_resolution,
-          int block_count,
+          int block_resolution = 16,
+          int block_count = 1000,
           const core::Tensor& T_init = core::Tensor::Eye(4,
                                                          core::Float64,
                                                          core::Device("CPU:0")),
           const core::Device& device = core::Device("CUDA:0"));
 
-    core::Tensor GetCurrentFramePose() { return T_frame_to_world_; }
+    core::Tensor GetCurrentFramePose() const { return T_frame_to_world_; }
     void UpdateFramePose(int frame_id, const core::Tensor& T_frame_to_world) {
         if (frame_id != frame_id_ + 1) {
             utility::LogWarning("Skipped {} frames in update T!",
@@ -68,34 +68,51 @@ class Model {
     /// \param depth_scale Scale factor to convert raw data into meter metric.
     /// \param depth_min Depth where ray casting starts from.
     /// \param depth_max Depth where ray casting stops at.
+    /// \param trunc_voxel_multiplier Truncation distance multiplier in voxel
+    /// size for signed distance. For instance, --trunc_voxel_multiplier=8 with
+    /// --voxel_size=0.006(m) creates a truncation distance of 0.048(m).
+    /// \param enable_color Enable color in the raycasting results.
+    /// \param weight_threshold Weight threshold of the TSDF voxels to prune
+    /// noise. Use -1 to apply default logic: min(frame_id_ * 1.0f, 3.0f).
     void SynthesizeModelFrame(Frame& raycast_frame,
-                              float depth_scale,
-                              float depth_min,
-                              float depth_max,
+                              float depth_scale = 1000.0,
+                              float depth_min = 0.1,
+                              float depth_max = 3.0,
                               float trunc_voxel_multiplier = 8.0,
-                              bool enable_color = true);
+                              bool enable_color = true,
+                              float weight_threshold = -1.0);
 
-    /// Track using PointToPlane depth odometry.
-    /// TODO(wei): support hybrid or intensity methods.
+    /// Track using depth odometry.
     /// \param input_frame Input RGBD frame.
     /// \param raycast_frame RGBD frame generated by raycasting.
     /// \param depth_scale Scale factor to convert raw data into meter metric.
     /// \param depth_max Depth truncation to discard points far away from the
-    /// camera.
-    odometry::OdometryResult TrackFrameToModel(const Frame& input_frame,
-                                               const Frame& raycast_frame,
-                                               float depth_scale,
-                                               float depth_max,
-                                               float depth_diff);
+    /// camera
+    /// \param method Method used to apply RGBD odometry.
+    /// \param criteria Criteria used to define and terminate iterations.
+    /// In multiscale odometry the order is from coarse to fine. Inputting a
+    /// vector of iterations by default triggers the implicit conversion.
+    odometry::OdometryResult TrackFrameToModel(
+            const Frame& input_frame,
+            const Frame& raycast_frame,
+            float depth_scale = 1000.0,
+            float depth_max = 3.0,
+            float depth_diff = 0.07,
+            odometry::Method method = odometry::Method::PointToPlane,
+            const std::vector<odometry::OdometryConvergenceCriteria>& criteria =
+                    {6, 3, 1});
 
     /// Integrate RGBD frame into the volumetric voxel grid.
     /// \param input_frame Input RGBD frame.
     /// \param depth_scale Scale factor to convert raw data into meter metric.
     /// \param depth_max Depth truncation to discard points far away from the
     /// camera.
+    /// \param trunc_voxel_multiplier Truncation distance multiplier in voxel
+    /// size for signed distance. For instance, --trunc_voxel_multiplier=8 with
+    /// --voxel_size=0.006(m) creates a truncation distance of 0.048(m).
     void Integrate(const Frame& input_frame,
-                   float depth_scale,
-                   float depth_max,
+                   float depth_scale = 1000.0,
+                   float depth_max = 3.0,
                    float trunc_voxel_multiplier = 8.0f);
 
     /// Extract surface point cloud for visualization / model saving.
@@ -122,6 +139,8 @@ class Model {
 public:
     /// Maintained volumetric map.
     t::geometry::VoxelBlockGrid voxel_grid_;
+
+    /// Active block coordinates from prior integration
     core::Tensor frustum_block_coords_;
 
     /// T_frame_to_model, maintained tracking state in a (4, 4), Float64 Tensor
@@ -130,6 +149,7 @@ class Model {
 
     int frame_id_ = -1;
 };
+
 }  // namespace slam
 }  // namespace pipelines
 }  // namespace t

cpp/pybind/t/geometry/voxel_block_grid.cpp

@@ -203,11 +203,34 @@ void pybind_voxel_block_grid(py::module& m) {
             "Extract triangle mesh at isosurface points.",
             "weight_threshold"_a = 3.0f, "estimated_vertex_number"_a = -1);
 
+    // Device transfers.
+    vbg.def("to", &VoxelBlockGrid::To,
+            "Transfer the voxel block grid to a specified device.", "device"_a,
+            "copy"_a = false);
+
+    vbg.def(
+            "cpu",
+            [](const VoxelBlockGrid& voxelBlockGrid) {
+                return voxelBlockGrid.To(core::Device("CPU:0"));
+            },
+            "Transfer the voxel block grid to CPU. If the voxel block grid is "
+            "already on CPU, no copy will be performed.");
+    vbg.def(
+            "cuda",
+            [](const VoxelBlockGrid& voxelBlockGrid, int device_id) {
+                return voxelBlockGrid.To(core::Device("CUDA", device_id));
+            },
+            "Transfer the voxel block grid to a CUDA device. If the voxel "
+            "block grid is already on the specified CUDA device, no copy "
+            "will be performed.",
+            "device_id"_a = 0);
+
     vbg.def("save", &VoxelBlockGrid::Save,
             "Save the voxel block grid to a npz file.", "file_name"_a);
     vbg.def_static("load", &VoxelBlockGrid::Load,
                    "Load a voxel block grid from a npz file.", "file_name"_a);
 }
+
 }  // namespace geometry
 }  // namespace t
 }  // namespace open3d

cpp/pybind/t/pipelines/slam/slam.cpp

@@ -38,8 +38,7 @@ static const std::unordered_map<std::string, std::string>
                 {"voxel_size", "The voxel size of the volume in meters."},
                 {"block_resolution",
                  "Resolution of local dense voxel blocks. By default 16 "
-                 "is "
-                 "used to create 16^3 voxel blocks."},
+                 "is used to create 16^3 voxel blocks."},
                 {"block_count",
                  "Number of estimate blocks per scene with the block "
                  "resolution set to 16 and the 6mm voxel resolution. "
@@ -67,7 +66,12 @@ static const std::unordered_map<std::string, std::string>
                  "Weight threshold to filter outlier voxel blocks."},
                 {"height", "Height of an image frame."},
                 {"width", "Width of an image frame."},
-                {"intrinsics", "Intrinsic matrix stored in a 3x3 Tensor."}};
+                {"intrinsics", "Intrinsic matrix stored in a 3x3 Tensor."},
+                {"trunc_voxel_multiplier",
+                 "Truncation distance multiplier in voxel size for signed "
+                 "distance. For instance, "
+                 "--trunc_voxel_multiplier=8 with --voxel_size=0.006(m) "
+                 "creates a truncation distance of 0.048(m)."}};
 
 void pybind_slam_model(py::module &m) {
     py::class_<Model> model(m, "Model", "Volumetric model for Dense SLAM.");
@@ -91,15 +95,19 @@ void pybind_slam_model(py::module &m) {
               "Synthesize frame from the volumetric model using ray casting.",
               "model_frame"_a, "depth_scale"_a = 1000.0, "depth_min"_a = 0.1,
               "depth_max"_a = 3.0, "trunc_voxel_multiplier"_a = 8.0,
-              "enable_color"_a = false);
+              "enable_color"_a = false, "weight_threshold"_a = -1.0);
     docstring::ClassMethodDocInject(m, "Model", "synthesize_model_frame",
                                     map_shared_argument_docstrings);
 
-    model.def("track_frame_to_model", &Model::TrackFrameToModel,
-              py::call_guard<py::gil_scoped_release>(),
-              "Track input frame against raycasted frame from model.",
-              "input_frame"_a, "model_frame"_a, "depth_scale"_a = 1000.0,
-              "depth_max"_a = 3.0, "depth_diff"_a = 0.07);
+    model.def(
+            "track_frame_to_model", &Model::TrackFrameToModel,
+            py::call_guard<py::gil_scoped_release>(),
+            "Track input frame against raycasted frame from model.",
+            "input_frame"_a, "model_frame"_a, "depth_scale"_a = 1000.0,
+            "depth_max"_a = 3.0, "depth_diff"_a = 0.07,
+            "method"_a = odometry::Method::PointToPlane,
+            "criteria"_a = (std::vector<odometry::OdometryConvergenceCriteria>){
+                    6, 3, 1});
     docstring::ClassMethodDocInject(m, "Model", "track_frame_to_model",
                                     map_shared_argument_docstrings);
 
@@ -130,6 +138,8 @@ void pybind_slam_model(py::module &m) {
             "Get the underlying hash map from 3D coordinates to voxel blocks.");
     model.def_readwrite("voxel_grid", &Model::voxel_grid_,
                         "Get the maintained VoxelBlockGrid.");
+    model.def_readwrite("frustum_block_coords", &Model::frustum_block_coords_,
+                        "Active block coordinates from prior integration");
     model.def_readwrite("transformation_frame_to_world",
                         &Model::T_frame_to_world_,
                         "Get the 4x4 transformation matrix from the current "