From 30935c8ee8b54ac4a51fc675e384e904e0d71e95 Mon Sep 17 00:00:00 2001 From: jdavidberger Date: Fri, 26 Aug 2022 10:37:14 -0600 Subject: [PATCH] Add default parameters in slam::Model cpp. (#5304) --- .dockerignore | 1 + CHANGELOG.md | 1 + cpp/open3d/t/geometry/VoxelBlockGrid.cpp | 11 +++++ cpp/open3d/t/geometry/VoxelBlockGrid.h | 12 +++++ cpp/open3d/t/pipelines/slam/Model.cpp | 33 ++++++++----- cpp/open3d/t/pipelines/slam/Model.h | 54 +++++++++++++++------- cpp/pybind/t/geometry/voxel_block_grid.cpp | 23 +++++++++ cpp/pybind/t/pipelines/slam/slam.cpp | 28 +++++++---- 8 files changed, 125 insertions(+), 38 deletions(-) diff --git a/.dockerignore b/.dockerignore index d2f844dd49e..41ed9f89aeb 100644 --- a/.dockerignore +++ b/.dockerignore @@ -6,3 +6,4 @@ docs/_static/build docs/_out 3rdparty_downloads .python-version +cmake-build-* diff --git a/CHANGELOG.md b/CHANGELOG.md index 3613f30d7a4..dbe0ba8e853 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/cpp/open3d/t/geometry/VoxelBlockGrid.cpp b/cpp/open3d/t/geometry/VoxelBlockGrid.cpp index 28d41a1967a..c0a2e0515af 100644 --- a/cpp/open3d/t/geometry/VoxelBlockGrid.cpp +++ b/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(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 tensor_map = t::io::ReadNpz(file_name); diff --git a/cpp/open3d/t/geometry/VoxelBlockGrid.h b/cpp/open3d/t/geometry/VoxelBlockGrid.h index 041abc5520f..f544fb44595 100644 --- a/cpp/open3d/t/geometry/VoxelBlockGrid.h +++ b/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 &blockHashmap, + const std::unordered_map &nameAttrMap) + : voxel_size_(voxelSize), + block_resolution_(blockResolution), + block_hashmap_(blockHashmap), + name_attr_map_(nameAttrMap) {} + float voxel_size_ = -1; int64_t block_resolution_ = -1; diff --git a/cpp/open3d/t/pipelines/slam/Model.cpp b/cpp/open3d/t/pipelines/slam/Model.cpp index c8c890897dd..5e5f11f48d5 100644 --- a/cpp/open3d/t/pipelines/slam/Model.cpp +++ b/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& 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{6, 3, 1}, - odometry::Method::PointToPlane, + raycast_frame.GetIntrinsics(), init_source_to_target, depth_scale, + depth_max, criteria, method, odometry::OdometryLossParams(depth_diff)); } diff --git a/cpp/open3d/t/pipelines/slam/Model.h b/cpp/open3d/t/pipelines/slam/Model.h index a5cf23f8306..f41a010c2f1 100644 --- a/cpp/open3d/t/pipelines/slam/Model.h +++ b/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& 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 diff --git a/cpp/pybind/t/geometry/voxel_block_grid.cpp b/cpp/pybind/t/geometry/voxel_block_grid.cpp index b30059bb228..6d9749dcd10 100644 --- a/cpp/pybind/t/geometry/voxel_block_grid.cpp +++ b/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 diff --git a/cpp/pybind/t/pipelines/slam/slam.cpp b/cpp/pybind/t/pipelines/slam/slam.cpp index ec6aa01fb46..3abc8d72977 100644 --- a/cpp/pybind/t/pipelines/slam/slam.cpp +++ b/cpp/pybind/t/pipelines/slam/slam.cpp @@ -38,8 +38,7 @@ static const std::unordered_map {"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 "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(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(), - "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(), + "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){ + 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 "