Add RealSense post-processing filters to RSBagReader

CHANGELOG.md

@@ -1,5 +1,6 @@
 ## Main
 -   Fix WebRTC prebuilt packaging and CI workflow across Linux, macOS arm64, and Windows runtime variants (PR #7515)
+-   Add RealSense depth post-processing filters for RSBagReader playback (issue #6164).
 -   Add `CorrespondenceCheckerBasedOnSourceRotation` to constrain global orientation priors in RANSAC registration (PR #7461)
 -   Use glfwGetMonitorWorkarea for accurate screen size in GetScreenSize, remove unusable_height estimation hack, and subtract window-decoration extents before clamping auto-sized windows (PR #7469)
 -   Upgrade stdgpu third-party library to commit d7c07d0.

cpp/open3d/t/io/sensor/realsense/RSBagReader.cpp

@@ -25,6 +25,70 @@
 namespace open3d {
 namespace t {
 namespace io {
+namespace {
+
+rs2_option GetPostProcessingOption(const std::string &option_name) {
+    static const std::unordered_map<std::string, rs2_option> options = {
+            {"filter_magnitude", RS2_OPTION_FILTER_MAGNITUDE},
+            {"filter_smooth_alpha", RS2_OPTION_FILTER_SMOOTH_ALPHA},
+            {"filter_smooth_delta", RS2_OPTION_FILTER_SMOOTH_DELTA},
+            {"holes_fill", RS2_OPTION_HOLES_FILL},
+    };
+    const auto it = options.find(option_name);
+    if (it == options.end()) {
+        utility::LogError("Unsupported RealSense post-processing option: {}",
+                          option_name);
+    }
+    return it->second;
+}
+
+rs2::filter ConfigurePostProcessingFilter(
+        const rs2::filter &filter,
+        const RSBagReader::PostProcessingFilter &filter_config) {
+    rs2::filter configured_filter = filter;
+    for (const auto &option : filter_config.options_) {
+        const rs2_option rs_option = GetPostProcessingOption(option.first);
+        if (!configured_filter.supports(rs_option)) {
+            utility::LogError(
+                    "RealSense post-processing filter '{}' does not support "
+                    "option '{}'.",
+                    filter_config.filter_name_, option.first);
+        }
+        configured_filter.set_option(rs_option, option.second);
+    }
+    return configured_filter;
+}
+
+rs2::filter MakePostProcessingFilter(
+        const RSBagReader::PostProcessingFilter &filter_config) {
+    if (filter_config.filter_name_ == "decimation") {
+        return ConfigurePostProcessingFilter(rs2::decimation_filter(),
+                                             filter_config);
+    } else if (filter_config.filter_name_ == "spatial") {
+        return ConfigurePostProcessingFilter(rs2::spatial_filter(),
+                                             filter_config);
+    } else if (filter_config.filter_name_ == "temporal") {
+        return ConfigurePostProcessingFilter(rs2::temporal_filter(),
+                                             filter_config);
+    } else if (filter_config.filter_name_ == "hole_filling") {
+        return ConfigurePostProcessingFilter(rs2::hole_filling_filter(),
+                                             filter_config);
+    }
+    utility::LogError("Unsupported RealSense post-processing filter: {}",
+                      filter_config.filter_name_);
+}
+
+std::vector<rs2::filter> MakePostProcessingFilters(
+        const std::vector<RSBagReader::PostProcessingFilter> &filter_configs) {
+    std::vector<rs2::filter> filters;
+    filters.reserve(filter_configs.size());
+    for (const auto &filter_config : filter_configs) {
+        filters.emplace_back(MakePostProcessingFilter(filter_config));
+    }
+    return filters;
+}
+
+}  // namespace
 
 // If DEFAULT_BUFFER_SIZE is odr-uses, a definition is required.
 // For Fedora33, GCC10, CUDA11.2:
@@ -42,6 +106,24 @@ RSBagReader::~RSBagReader() {
 }
 
 bool RSBagReader::Open(const std::string &filename) {
+    if (IsOpened()) {
+        Close();
+    }
+    post_processing_filters_.clear();
+    return OpenPipeline(filename);
+}
+
+bool RSBagReader::Open(const std::string &filename,
+                       const std::vector<PostProcessingFilter> &filters) {
+    if (IsOpened()) {
+        Close();
+    }
+    MakePostProcessingFilters(filters);
+    post_processing_filters_ = filters;
+    return OpenPipeline(filename);
+}
+
+bool RSBagReader::OpenPipeline(const std::string &filename) {
     if (IsOpened()) {
         Close();
     }
@@ -94,6 +176,8 @@ void RSBagReader::fill_frame_buffer() try {
     tail_fid_ = 0;
     uint64_t next_dev_color_fid = 0;
     uint64_t dev_color_fid = 0;
+    auto post_processing_filters =
+            MakePostProcessingFilters(post_processing_filters_);
 
     while (is_opened_) {
         rs_device.resume();
@@ -123,6 +207,9 @@ void RSBagReader::fill_frame_buffer() try {
                 auto &current_frame =
                         frame_buffer_[head_fid_ % frame_buffer_.size()];
 
+                for (auto &filter : post_processing_filters) {
+                    frames = frames.apply_filter(filter);
+                }
                 frames = align_to_color.process(frames);
                 const auto &color_frame = frames.get_color_frame();
                 // Copy frame data to Tensors
@@ -201,7 +288,7 @@ bool RSBagReader::SeekTimestamp(uint64_t timestamp) {
     }
     seek_to_ = timestamp;  // atomic
     if (is_eof_) {
-        Open(filename_);  // EOF requires restarting pipeline.
+        OpenPipeline(filename_);  // EOF requires restarting pipeline.
     } else {
         need_frames_.notify_one();
     }

cpp/open3d/t/io/sensor/realsense/RSBagReader.h

@@ -47,6 +47,11 @@ namespace io {
 ///
 class RSBagReader : public RGBDVideoReader {
 public:
+    struct PostProcessingFilter {
+        std::string filter_name_;
+        std::unordered_map<std::string, float> options_;
+    };
+
     static const size_t DEFAULT_BUFFER_SIZE = 32;
 
     /// Constructor
@@ -70,6 +75,13 @@ class RSBagReader : public RGBDVideoReader {
     /// \param filename Path to the RSBag file.
     virtual bool Open(const std::string &filename) override;
 
+    /// Open an RGBD Video playback with depth post-processing filters.
+    ///
+    /// \param filename Path to the RSBag file.
+    /// \param filters Ordered depth post-processing filters to apply.
+    bool Open(const std::string &filename,
+              const std::vector<PostProcessingFilter> &filters);
+
     /// Close the opened RSBag playback.
     virtual void Close() override;
 
@@ -132,7 +144,9 @@ class RSBagReader : public RGBDVideoReader {
     std::thread frame_reader_thread_;
 
     std::unique_ptr<rs2::pipeline> pipe_;
+    std::vector<PostProcessingFilter> post_processing_filters_;
 
+    bool OpenPipeline(const std::string &filename);
     Json::Value GetMetadataJson();
     std::string GetTagInMetadata(const std::string &tag_name);
 };

cpp/pybind/t/io/sensor.cpp

@@ -11,6 +11,7 @@
 #include "open3d/geometry/RGBDImage.h"
 #include "open3d/t/io/sensor/RGBDSensor.h"
 #include "open3d/t/io/sensor/RGBDVideoReader.h"
+#include "open3d/utility/Logging.h"
 #ifdef BUILD_LIBREALSENSE
 #include "open3d/t/io/sensor/realsense/RSBagReader.h"
 #include "open3d/t/io/sensor/realsense/RealSenseSensor.h"
@@ -23,6 +24,35 @@ namespace fs = std::filesystem;
 namespace open3d {
 namespace t {
 namespace io {
+namespace {
+
+#ifdef BUILD_LIBREALSENSE
+std::vector<RSBagReader::PostProcessingFilter> PyDictToPostProcessingFilters(
+        const py::dict &filters) {
+    std::vector<RSBagReader::PostProcessingFilter> filter_configs;
+    filter_configs.reserve(py::len(filters));
+    for (const auto &filter_item : filters) {
+        RSBagReader::PostProcessingFilter filter_config;
+        filter_config.filter_name_ = py::cast<std::string>(filter_item.first);
+        if (!py::isinstance<py::dict>(filter_item.second)) {
+            utility::LogError(
+                    "RealSense post-processing filter '{}' options must be a "
+                    "dict.",
+                    filter_config.filter_name_);
+        }
+        const auto options =
+                py::reinterpret_borrow<py::dict>(filter_item.second);
+        for (const auto &option_item : options) {
+            filter_config.options_[py::cast<std::string>(option_item.first)] =
+                    py::cast<float>(option_item.second);
+        }
+        filter_configs.emplace_back(std::move(filter_config));
+    }
+    return filter_configs;
+}
+#endif
+
+}  // namespace
 
 // RGBD video reader trampoline
 class PyRGBDVideoReader : public RGBDVideoReader {
@@ -131,7 +161,10 @@ void pybind_sensor_definitions(py::module &m) {
                      "(default video length) Save frames till this time (us)"},
                     {"buffer_size",
                      "Size of internal frame buffer, increase this if you "
-                     "experience frame drops."}};
+                     "experience frame drops."},
+                    {"filters",
+                     "Ordered dict of RealSense depth post-processing filters "
+                     "and their options."}};
 
     // Class RGBD video metadata
     auto rgbd_video_metadata = static_cast<py::class_<RGBDVideoMetadata>>(
@@ -205,9 +238,20 @@ void pybind_sensor_definitions(py::module &m) {
                  "buffer_size"_a = RSBagReader::DEFAULT_BUFFER_SIZE)
             .def("is_opened", &RSBagReader::IsOpened,
                  "Check if the RS bag file  is opened.")
-            .def("open", &RSBagReader::Open,
+            .def("open",
+                 py::overload_cast<const std::string &>(&RSBagReader::Open),
                  py::call_guard<py::gil_scoped_release>(), "filename"_a,
                  "Open an RS bag playback.")
+            .def(
+                    "open",
+                    [](RSBagReader &reader, const fs::path &filename,
+                       const py::dict &filters) {
+                        const auto filter_configs =
+                                PyDictToPostProcessingFilters(filters);
+                        py::gil_scoped_release release;
+                        return reader.Open(filename.string(), filter_configs);
+                    },
+                    "filename"_a, "filters"_a, "Open an RS bag playback.")
             .def("close", &RSBagReader::Close,
                  "Close the opened RS bag playback.")
             .def("is_eof", &RSBagReader::IsEOF,

docs/tutorial/sensor/realsense.rst

@@ -77,6 +77,22 @@ Here is the corresponding Python code:
 
     bag_reader.close()
 
+Depth post-processing filters from ``librealsense`` can be enabled while
+opening a bag file. Filters are applied in dictionary order before the depth
+frame is aligned to the color frame. Supported filters are ``decimation``,
+``spatial``, ``temporal``, and ``hole_filling``.
+
+.. code-block:: Python
+
+    import open3d as o3d
+    bag_reader = o3d.t.io.RSBagReader()
+    bag_reader.open(
+        bag_filename,
+        {"decimation": {"filter_magnitude": 2}},
+    )
+    im_rgbd = bag_reader.next_frame()
+    bag_reader.close()
+
 Examples
 ^^^^^^^^
 

python/test/t/io/test_realsense.py

@@ -81,6 +81,34 @@ def test_RSBagReader():
     shutil.rmtree("L515_test_s")
 
 
+@pytest.mark.xfail(strict=False, reason="May fail depending on test state.")
+@pytest.mark.skipif(os.getenv('GITHUB_SHA') is not None or
+                    not hasattr(o3d.t.io, 'RSBagReader'),
+                    reason="Hangs in Github Actions, succeeds locally or "
+                    "not built with librealsense")
+def test_RSBagReader_post_processing_filters():
+    sample_l515_bag = o3d.data.SampleL515Bag()
+
+    bag_reader = o3d.t.io.RSBagReader()
+    bag_reader.open(sample_l515_bag.path)
+    raw_rgbd = bag_reader.next_frame()
+    bag_reader.close()
+
+    bag_reader = o3d.t.io.RSBagReader()
+    bag_reader.open(sample_l515_bag.path,
+                    {"decimation": {
+                        "filter_magnitude": 2
+                    }})
+    filtered_rgbd = bag_reader.next_frame()
+    bag_reader.close()
+
+    assert not filtered_rgbd.is_empty() and filtered_rgbd.are_aligned()
+    assert filtered_rgbd.depth.rows == raw_rgbd.depth.rows
+    assert filtered_rgbd.depth.columns == raw_rgbd.depth.columns
+    assert np.any(filtered_rgbd.depth.as_tensor().numpy() !=
+                  raw_rgbd.depth.as_tensor().numpy())
+
+
 # Test recording from a RealSense camera, if one is connected
 @pytest.mark.skipif(not hasattr(o3d.t.io, 'RealSenseSensor'),
                     reason="Not built with librealsense")