add filter params

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -62,6 +62,7 @@
 #include "orbbec_camera/d2c_viewer.h"
 #include "magic_enum/magic_enum.hpp"
 #include "jpeg_decoder.h"
+#include <std_msgs/msg/string.hpp>
 
 #define STREAM_NAME(sip)                                                                       \
   (static_cast<std::ostringstream&&>(std::ostringstream()                                      \
@@ -469,5 +470,33 @@ class OBCameraNode {
   bool ordered_pc_ = false;
   bool use_hardware_time_ = true;
   bool enable_depth_scale_ = true;
+  std::shared_ptr<ob::Frame> depth_frame_ = nullptr;
+  std::string device_preset_ = "Default";
+  // filter switch
+  bool enable_decimation_filter_ = false;
+  bool enable_hdr_merge_ = false;
+  bool enable_sequence_id_filter_ = false;
+  bool enable_threshold_filter_ = false;
+  bool enable_noise_removal_filter_ = true;
+  bool enable_spatial_filter_ = true;
+  bool enable_temporal_filter_ = false;
+  bool enable_hole_filling_filter_ = false;
+  // filter params
+  int decimation_filter_scale_range_ = 2;
+  int sequence_id_filter_id_ = 1;
+  int threshold_filter_max_ = 16000;
+  int threshold_filter_min_ = 0;
+  int noise_removal_filter_min_diff_ = 8;
+  int noise_removal_filter_max_size_ = 80;
+  float spatial_filter_alpha_ = 0.5;
+  int spatial_filter_diff_threshold_ = 8;
+  int spatial_filter_magnitude_ = 1;
+  int spatial_filter_radius_ = 1;
+  float temporal_filter_diff_threshold_ = 0.1;
+  float temporal_filter_weight_ = 0.4;
+  std::string hole_filling_filter_mode_ = "FILL_TOP";
+
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr filter_status_pub_;
+  nlohmann::json filter_status_;
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/utils.h

@@ -1,18 +1,18 @@
 /*******************************************************************************
-* Copyright (c) 2023 Orbbec 3D Technology, Inc
+ * Copyright (c) 2023 Orbbec 3D Technology, Inc
-*
+ *
-* Licensed under the Apache License, Version 2.0 (the "License");
+ * Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
+ * you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
+ * You may obtain a copy of the License at
-*
+ *
-*     http://www.apache.org/licenses/LICENSE-2.0
+ *     http://www.apache.org/licenses/LICENSE-2.0
-*
+ *
-* Unless required by applicable law or agreed to in writing, software
+ * Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
+ * distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
+ * See the License for the specific language governing permissions and
-* limitations under the License.
+ * limitations under the License.
-*******************************************************************************/
+ *******************************************************************************/
 
 #pragma once
 #include <ostream>
@@ -34,7 +34,8 @@ sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
 
 void saveRGBPointsToPly(const std::shared_ptr<ob::Frame>& frame, const std::string& fileName);
 
-void saveRGBPointCloudMsgToPly(const sensor_msgs::msg::PointCloud2& msg, const std::string& fileName);
+void saveRGBPointCloudMsgToPly(const sensor_msgs::msg::PointCloud2& msg,
+                               const std::string& fileName);
 
 void saveDepthPointsToPly(const sensor_msgs::msg::PointCloud2& msg, const std::string& fileName);
 
@@ -82,6 +83,8 @@ std::string parseUsbPort(const std::string& line);
 
 bool isValidJPEG(const std::shared_ptr<ob::ColorFrame>& frame);
 
-std::string metaDataTypeToString(const OBFrameMetadataType &meta_data_type);
+std::string metaDataTypeToString(const OBFrameMetadataType& meta_data_type);
+
+OBHoleFillingMode holeFillingModeFromString(const std::string& hole_filling_mode);
 
 }  // namespace orbbec_camera

orbbec_camera/launch/gemini2R.launch.py

@@ -87,6 +87,27 @@ def generate_launch_description():
         DeclareLaunchArgument('ordered_pc', default_value='false'),
         DeclareLaunchArgument('use_hardware_time', default_value='true'),
         DeclareLaunchArgument('enable_depth_scale', default_value='true'),
+        DeclareLaunchArgument('enable_decimation_filter', default_value='false'),
+        DeclareLaunchArgument('enable_hdr_merge', default_value='false'),
+        DeclareLaunchArgument('enable_sequence_id_filter', default_value='false'),
+        DeclareLaunchArgument('enable_threshold_filter', default_value='false'),
+        DeclareLaunchArgument('enable_noise_removal_filter', default_value='true'),
+        DeclareLaunchArgument('enable_spatial_filter', default_value='true'),
+        DeclareLaunchArgument('enable_temporal_filter', default_value='false'),
+        DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'),
+        DeclareLaunchArgument('decimation_filter_scale_range', default_value='2'),
+        DeclareLaunchArgument('sequence_id_filter_id', default_value='1'),
+        DeclareLaunchArgument('threshold_filter_max', default_value='16000'),
+        DeclareLaunchArgument('threshold_filter_min', default_value='0'),
+        DeclareLaunchArgument('noise_removal_filter_min_diff', default_value='8'),
+        DeclareLaunchArgument('noise_removal_filter_max_size', default_value='80'),
+        DeclareLaunchArgument('spatial_filter_alpha', default_value='0.5'),
+        DeclareLaunchArgument('spatial_filter_diff_threshold', default_value='8'),
+        DeclareLaunchArgument('spatial_filter_magnitude', default_value='1'),
+        DeclareLaunchArgument('spatial_filter_radius', default_value='1'),
+        DeclareLaunchArgument('temporal_filter_diff_threshold', default_value='0.1'),
+        DeclareLaunchArgument('temporal_filter_weight', default_value='0.4'),
+        DeclareLaunchArgument('hole_filling_filter_mode', default_value='FILL_TOP'),
     ]
 
     # Node configuration

orbbec_camera/src/ob_camera_node.cpp

@@ -142,6 +142,66 @@ void OBCameraNode::setupDevices() {
     }
   }
   try {
+    device_->loadPreset(device_preset_.c_str());
+    auto depth_sensor = device_->getSensor(OB_SENSOR_DEPTH);
+    // set depth sensor to filter
+    auto filter_list = depth_sensor->getRecommendedFilters();
+    for (size_t i = 0; i < filter_list->count(); i++) {
+      auto filter = filter_list->getFilter(i);
+      std::map<std::string, bool> filter_params = {
+          {"DecimationFilter", enable_decimation_filter_},
+          {"HdrMerge", enable_hdr_merge_},
+          {"SequencedFilter", enable_sequence_id_filter_},
+          {"ThresholdFilter", enable_threshold_filter_},
+          {"NoiseRemovalFilter", enable_noise_removal_filter_},
+          {"SpatialAdvancedFilter", enable_spatial_filter_},
+          {"TemporalFilter", enable_temporal_filter_},
+          {"HoleFillingFilter", enable_hole_filling_filter_},
+
+      };
+      std::string filter_name = filter->type();
+      RCLCPP_INFO_STREAM(logger_, "Setting " << filter_name << "......");
+      if (filter_params.find(filter_name) != filter_params.end()) {
+        std::string value = filter_params[filter_name] ? "true" : "false";
+        RCLCPP_INFO_STREAM(logger_, "set " << filter_name << " to " << value);
+        filter->enable(filter_params[filter_name]);
+        filter_status_[filter_name] = filter_params[filter_name];
+      }
+      if (filter_name == "DecimationFilter") {
+        auto decimation_filter = filter->as<ob::DecimationFilter>();
+        decimation_filter->setScaleValue(decimation_filter_scale_range_);
+      } else if (filter_name == "ThresholdFilter") {
+        auto threshold_filter = filter->as<ob::ThresholdFilter>();
+        threshold_filter->setValueRange(threshold_filter_min_, threshold_filter_max_);
+      } else if (filter_name == "SpatialAdvancedFilter") {
+        auto spatial_filter = filter->as<ob::SpatialAdvancedFilter>();
+        OBSpatialAdvancedFilterParams params{};
+        params.alpha = spatial_filter_alpha_;
+        params.magnitude = spatial_filter_magnitude_;
+        params.radius = spatial_filter_radius_;
+        params.disp_diff = spatial_filter_diff_threshold_;
+        spatial_filter->setFilterParams(params);
+      } else if (filter_name == "TemporalFilter") {
+        auto temporal_filter = filter->as<ob::TemporalFilter>();
+        temporal_filter->setDiffScale(temporal_filter_diff_threshold_);
+        temporal_filter->setWeight(temporal_filter_weight_);
+      } else if (filter_name == "HoleFillingFilter") {
+        auto hole_filling_filter = filter->as<ob::HoleFillingFilter>();
+        OBHoleFillingMode hole_filling_mode = holeFillingModeFromString(hole_filling_filter_mode_);
+        hole_filling_filter->setFilterMode(hole_filling_mode);
+      } else if (filter_name == "SequenceIdFilter") {
+        auto sequenced_filter = filter->as<ob::SequenceIdFilter>();
+        sequenced_filter->selectSequenceId(sequence_id_filter_id_);
+      } else if (filter_name == "NoiseRemovalFilter") {
+        auto noise_removal_filter = filter->as<ob::NoiseRemovalFilter>();
+        OBNoiseRemovalFilterParams params{};
+        params.disp_diff = noise_removal_filter_min_diff_;
+        params.max_size = noise_removal_filter_max_size_;
+        noise_removal_filter->setFilterParams(params);
+      } else {
+        RCLCPP_ERROR_STREAM(logger_, "Unsupported filter: " << filter_name);
+      }
+    }
     if (!depth_work_mode_.empty()) {
       device_->switchDepthWorkMode(depth_work_mode_.c_str());
     }
@@ -604,6 +664,30 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<int>(max_save_images_count_, "max_save_images_count", 10);
   setAndGetNodeParameter<bool>(use_hardware_time_, "use_hardware_time", true);
   setAndGetNodeParameter<bool>(enable_depth_scale_, "enable_depth_scale", true);
+  setAndGetNodeParameter<std::string>(device_preset_, "device_preset", "Default");
+  setAndGetNodeParameter<bool>(enable_decimation_filter_, "enable_decimation_filter", false);
+  setAndGetNodeParameter<bool>(enable_hdr_merge_, "enable_hdr_merge", false);
+  setAndGetNodeParameter<bool>(enable_sequence_id_filter_, "enable_sequence_id_filter", false);
+  setAndGetNodeParameter<bool>(enable_threshold_filter_, "enable_threshold_filter", false);
+  setAndGetNodeParameter<bool>(enable_noise_removal_filter_, "enable_noise_removal_filter", true);
+  setAndGetNodeParameter<bool>(enable_spatial_filter_, "enable_spatial_filter", true);
+  setAndGetNodeParameter<bool>(enable_temporal_filter_, "enable_temporal_filter", false);
+  setAndGetNodeParameter<bool>(enable_hole_filling_filter_, "enable_hole_filling_filter", false);
+  setAndGetNodeParameter<int>(decimation_filter_scale_range_, "decimation_filter_scale_range", 2);
+  setAndGetNodeParameter<int>(sequence_id_filter_id_, "sequence_id_filter_id", 1);
+  setAndGetNodeParameter<int>(threshold_filter_max_, "threshold_filter_max", 16000);
+  setAndGetNodeParameter<int>(threshold_filter_min_, "threshold_filter_min", 0);
+  setAndGetNodeParameter<int>(noise_removal_filter_min_diff_, "noise_removal_filter_min_diff", 8);
+  setAndGetNodeParameter<int>(noise_removal_filter_max_size_, "noise_removal_filter_max_size", 80);
+  setAndGetNodeParameter<float>(spatial_filter_alpha_, "spatial_filter_alpha", 0.5);
+  setAndGetNodeParameter<int>(spatial_filter_diff_threshold_, "spatial_filter_diff_threshold", 8);
+  setAndGetNodeParameter<int>(spatial_filter_magnitude_, "spatial_filter_magnitude", 1);
+  setAndGetNodeParameter<int>(spatial_filter_radius_, "spatial_filter_radius", 1);
+  setAndGetNodeParameter<float>(temporal_filter_diff_threshold_, "temporal_filter_diff_threshold",
+                                0.1);
+  setAndGetNodeParameter<float>(temporal_filter_weight_, "temporal_filter_weight", 0.4);
+  setAndGetNodeParameter<std::string>(hole_filling_filter_mode_, "hole_filling_filter_mode",
+                                      "FILL_TOP");
 }
 
 void OBCameraNode::setupTopics() {
@@ -727,6 +811,11 @@ void OBCameraNode::setupPublishers() {
         node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
             "/" + camera_name_ + "/depth_to_right_ir", rclcpp::QoS(1).transient_local());
   }
+  filter_status_pub_ = node_->create_publisher<std_msgs::msg::String>(
+      "depth_filter_status", rclcpp::QoS(1).transient_local());
+  std_msgs::msg::String msg;
+  msg.data = filter_status_.dump(2);
+  filter_status_pub_->publish(msg);
 }
 
 void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {

orbbec_camera/src/utils.cpp

@@ -662,4 +662,15 @@ std::string metaDataTypeToString(const OBFrameMetadataType &meta_data_type) {
       return "unknown";
   }
 }
+OBHoleFillingMode holeFillingModeFromString(const std::string &hole_filling_mode) {
+  if (hole_filling_mode == "FILL_TOP") {
+    return OB_HOLE_FILL_TOP;
+  } else if (hole_filling_mode == "FILL_NEAREST") {
+    return OB_HOLE_FILL_NEAREST;
+  } else if (hole_filling_mode == "FILL_FAREST") {
+    return OB_HOLE_FILL_FAREST;
+  } else {
+    return OB_HOLE_FILL_NEAREST;
+  }
+}
 }  // namespace orbbec_camera