feat: add support for undistortion parameters and filters in camera configuration

orbbec_camera/config/common.yaml

@@ -122,6 +122,8 @@ orbbec_ros:
       depth_flip: false
       # If false, the depth scale will not be enabled
       enable_depth_scale: true
+      # If true, the depth image will be undistorted
+      enable_depth_undistortion: false
       # Depth accuracy, the format should be 1mm. The default value is 1mm.
       depth_precision: ""
 
@@ -260,6 +262,8 @@ orbbec_ros:
       left_ir_fps: 0
       # Format of the left IR image. Supported formats are 'Y8'
       left_ir_format: "ANY"
+      # If true, the left IR image will be undistorted
+      enable_left_ir_undistortion: false
 
     rightir:
       # If false, the right IR image will not be enabled
@@ -278,6 +282,8 @@ orbbec_ros:
       right_ir_fps: 0
       # Format of the right IR image. Supported formats are 'Y8'
       right_ir_format: "ANY"
+      # If true, the right IR image will be undistorted
+      enable_right_ir_undistortion: false
 
     ir:
       # If false, the left IR image will not be enabled
@@ -296,6 +302,8 @@ orbbec_ros:
       ir_fps: 0
       # Format of the IR image. Supported formats are 'Y8'
       ir_format: "Y8"
+      # If true, the IR image will be undistorted
+      enable_ir_undistortion: false
 
     ir_parameters:
       # If false, the IR image auto exposure will not be enabled

orbbec_camera/config/gemini305_dual_color.yaml

@@ -74,6 +74,7 @@ left_color_flip: false
 left_color_mirror: false
 enable_left_color_decimation_filter: false
 left_color_decimation_filter_scale: -1
+enable_left_color_undistortion: false
 
 # right_color
 enable_right_color: true
@@ -88,6 +89,7 @@ right_color_flip: false
 right_color_mirror: false
 enable_right_color_decimation_filter: false
 right_color_decimation_filter_scale: -1
+enable_right_color_undistortion: false
 
 # color_common
 enable_color_auto_exposure_priority: false
@@ -111,6 +113,10 @@ color_backlight_compensation: -1
 color_powerline_freq: ""
 color_denoising_level: -1
 enable_color_undistortion: false
+enable_depth_undistortion: false
+enable_ir_undistortion: false
+enable_left_ir_undistortion: false
+enable_right_ir_undistortion: false
 
 # hdr_params
 hdr_index1_depth_exposure: 1
@@ -268,4 +274,3 @@ laser_index0_depth_exposure: 3000
 laser_index0_depth_gain: 16
 laser_index0_ir_brightness: 60
 laser_index0_ir_ae_max_exposure: 30000
-

orbbec_camera/include/orbbec_camera/constants.h

@@ -134,6 +134,10 @@ const int32_t GEMINI_336LE_PID = 0x0810;                  // Gemini 335Le
 const int32_t CUSTOM_ADVANTECH_GEMINI_336_PID = 0x0816;   // Custom Advantech Gemini 336
 const int32_t CUSTOM_ADVANTECH_GEMINI_336L_PID = 0x0817;  // Custom Advantech Gemini 336L
 const int32_t DABAI_MAX_PID = 0x069a;                     // dabai max
+const int32_t DABAI_A_PID = 0x0A12;                       // DaBai A
+const int32_t DABAI_AL_PID = 0x0A13;                      // DaBai AL
+const int32_t GEMINI_345_PID = 0x0812;                    // Gemini 345
+const int32_t GEMINI_345LG_PID = 0x0813;                  // Gemini 345Lg
 const int32_t GEMINI_338_PID = 0x0818;                    // Gemini 338
 const int32_t GEMINI_435Le_PID = 0x815;                   // Gemini 435Le
 const int32_t GEMINI_305_PID = 0x0840;                    // Gemini 305

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -479,6 +479,8 @@ class OBCameraNode {
 
   std::shared_ptr<ob::Frame> processLeftIrFrameFilter(std::shared_ptr<ob::Frame>& frame);
 
+  std::shared_ptr<ob::Frame> processIrFrameFilter(std::shared_ptr<ob::Frame>& frame);
+
   uint64_t getFrameTimestampUs(const std::shared_ptr<ob::Frame>& frame);
 
   void onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set);
@@ -538,14 +540,21 @@ class OBCameraNode {
 
   static bool isGemini435LePID(uint32_t pid);
   static bool isPublishMetaData(uint32_t pid);
+  static bool isDabaiASeriesForHwD2C(uint32_t pid);
 
   static bool isDepthWorkModeDevices(uint32_t pid);
   static bool isnotLaserDevices(uint32_t pid);
 
   void setupDepthPostProcessFilter();
   void setupColorPostProcessFilter();
+  void setupIrPostProcessFilter();
   void setupRightIrPostProcessFilter();
   void setupLeftIrPostProcessFilter();
+  void setupUndistortionFilters();
+  bool shouldUseHwD2CColorUndistortion() const;
+  void configureHwD2CColorUndistortion(const std::shared_ptr<ob::Frame>& depth_frame);
+  void applyHwD2CColorUndistortion(std::shared_ptr<ob::FrameSet>& frame_set,
+                                   const std::shared_ptr<ob::Frame>& depth_frame);
 
   // interleave AE
   int init_interleave_hdr_param();
@@ -919,10 +928,9 @@ class OBCameraNode {
   std::chrono::milliseconds software_trigger_period_{33};
   bool enable_heartbeat_ = false;
   bool enable_firmware_log_ = false;
-  bool enable_color_undistortion_ = false;
+  std::map<stream_index_pair, bool> enable_undistortion_;
-  std::shared_ptr<image_publisher> color_undistortion_publisher_;
+  std::shared_ptr<ob::UnDistortionFilter> hw_d2c_color_undistortion_filter_;
-  rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr
+  bool hw_d2c_color_undistortion_configured_ = false;
-      color_undistortion_camera_info_publisher_;
   bool has_first_color_frame_ = false;
   bool use_intra_process_ = false;
   std::string cloud_frame_id_;
@@ -931,6 +939,7 @@ class OBCameraNode {
   std::vector<std::shared_ptr<ob::Filter>> color_filter_list_;
   std::vector<std::shared_ptr<ob::Filter>> left_color_filter_list_;
   std::vector<std::shared_ptr<ob::Filter>> right_color_filter_list_;
+  std::vector<std::shared_ptr<ob::Filter>> ir_filter_list_;
   std::vector<std::shared_ptr<ob::Filter>> left_ir_filter_list_;
   std::vector<std::shared_ptr<ob::Filter>> right_ir_filter_list_;
 

orbbec_camera/launch/dabai_a.launch.py

@@ -210,6 +210,12 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_ir_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),

orbbec_camera/launch/dabai_al.launch.py

@@ -212,6 +212,12 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_ir_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),

orbbec_camera/launch/gemini305.launch.py

@@ -275,6 +275,12 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_ir_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),

orbbec_camera/launch/gemini305_g.launch.py

@@ -275,6 +275,12 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_ir_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),

orbbec_camera/launch/gemini345.launch.py

@@ -209,6 +209,12 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_ir_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),

orbbec_camera/launch/gemini345_lg.launch.py

@@ -212,6 +212,12 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_ir_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),

orbbec_camera/launch/gemini435_le.launch.py

@@ -243,6 +243,12 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_ir_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),

orbbec_camera/launch/gemini_330_series.launch.py

@@ -261,6 +261,12 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_ir_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),

orbbec_camera/launch/gemini_330_series_low_cpu.launch.py

@@ -260,6 +260,12 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_ir_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),

orbbec_camera/launch/gemini_intra_process_demo_launch.py

@@ -163,6 +163,12 @@ def generate_launch_description():
         DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
         DeclareLaunchArgument('time_domain', default_value='device'),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_left_ir_undistortion', default_value='false'),
+        DeclareLaunchArgument('enable_right_ir_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),

orbbec_camera/src/ob_camera_node.cpp

@@ -2409,6 +2409,136 @@ void OBCameraNode::setupColorPostProcessFilter() {
     }
   }
 }
+
+void OBCameraNode::setupIrPostProcessFilter() {
+  try {
+    auto ir_sensor = device_->getSensor(OB_SENSOR_IR);
+    ir_filter_list_ = ir_sensor->createRecommendedFilters();
+    if (ir_filter_list_.empty()) {
+      RCLCPP_WARN_STREAM(logger_, "Failed to get ir sensor filter list");
+    }
+  } catch (const ob::Error &e) {
+    RCLCPP_WARN_STREAM(logger_, "Failed to setup ir filters: "
+                                    << orbbec_camera::formatObErrorWithStatus(e));
+  }
+}
+
+void OBCameraNode::setupUndistortionFilters() {
+  auto remove_undistortion_filter = [](std::vector<std::shared_ptr<ob::Filter>> &filters) {
+    filters.erase(std::remove_if(filters.begin(), filters.end(),
+                                 [](const std::shared_ptr<ob::Filter> &filter) {
+                                   return filter && std::string(filter->type()) ==
+                                                        "UnDistortionFilter";
+                                 }),
+                  filters.end());
+  };
+
+  auto find_or_create_filter =
+      [&](std::vector<std::shared_ptr<ob::Filter>> &filters,
+          OBStreamType stream_type) -> std::shared_ptr<ob::UnDistortionFilter> {
+    for (auto &filter : filters) {
+      if (filter && std::string(filter->type()) == "UnDistortionFilter") {
+        auto undistortion_filter = filter->as<ob::UnDistortionFilter>();
+        undistortion_filter->setStreamType(stream_type);
+        undistortion_filter->enable(true);
+        return undistortion_filter;
+      }
+    }
+    auto undistortion_filter = std::make_shared<ob::UnDistortionFilter>(stream_type);
+    undistortion_filter->enable(true);
+    filters.push_back(undistortion_filter);
+    return undistortion_filter;
+  };
+
+  auto setup_stream_filter = [&](const stream_index_pair &stream_index,
+                                 std::vector<std::shared_ptr<ob::Filter>> &filters) {
+    if (!enable_stream_[stream_index] || !enable_undistortion_[stream_index]) {
+      return;
+    }
+    if (stream_index == LIDAR) {
+      RCLCPP_WARN_STREAM(logger_, "Undistortion is not supported for lidar stream");
+      return;
+    }
+    if (stream_index == COLOR && shouldUseHwD2CColorUndistortion()) {
+      remove_undistortion_filter(filters);
+      hw_d2c_color_undistortion_filter_ =
+          std::make_shared<ob::UnDistortionFilter>(OB_STREAM_COLOR);
+      hw_d2c_color_undistortion_filter_->enable(true);
+      RCLCPP_INFO_STREAM(logger_,
+                         "Enable color undistortion with HW D2C depth intrinsic projection");
+      return;
+    }
+    auto undistortion_filter = find_or_create_filter(filters, stream_index.first);
+    undistortion_filter->clearNewCameraMatrix();
+    RCLCPP_INFO_STREAM(logger_, "Enable " << stream_name_[stream_index] << " undistortion");
+  };
+
+  setup_stream_filter(COLOR, color_filter_list_);
+  setup_stream_filter(COLOR_LEFT, left_color_filter_list_);
+  setup_stream_filter(COLOR_RIGHT, right_color_filter_list_);
+  setup_stream_filter(DEPTH, depth_filter_list_);
+  setup_stream_filter(INFRA0, ir_filter_list_);
+  setup_stream_filter(INFRA1, left_ir_filter_list_);
+  setup_stream_filter(INFRA2, right_ir_filter_list_);
+}
+
+bool OBCameraNode::shouldUseHwD2CColorUndistortion() const {
+  auto color_undistortion = enable_undistortion_.find(COLOR);
+  return color_undistortion != enable_undistortion_.end() && color_undistortion->second &&
+         isDabaiASeriesForHwD2C(pid_) && depth_registration_ && align_mode_ == "HW" &&
+         align_target_stream_ == OB_STREAM_COLOR && enable_stream_.at(COLOR) &&
+         enable_stream_.at(DEPTH);
+}
+
+void OBCameraNode::configureHwD2CColorUndistortion(const std::shared_ptr<ob::Frame> &depth_frame) {
+  if (!hw_d2c_color_undistortion_filter_ || hw_d2c_color_undistortion_configured_) {
+    return;
+  }
+  if (!depth_frame) {
+    RCLCPP_WARN_ONCE(logger_,
+                     "Skip HW D2C color undistortion setup because depth frame is not available");
+    return;
+  }
+  auto stream_profile = depth_frame->getStreamProfile();
+  if (!stream_profile) {
+    RCLCPP_WARN_ONCE(logger_,
+                     "Skip HW D2C color undistortion setup because depth stream profile is null");
+    return;
+  }
+  auto video_profile = stream_profile->as<ob::VideoStreamProfile>();
+  if (!video_profile) {
+    RCLCPP_WARN_ONCE(logger_,
+                     "Skip HW D2C color undistortion setup because depth profile is not video");
+    return;
+  }
+  hw_d2c_color_undistortion_filter_->setNewCameraMatrix(video_profile->getIntrinsic());
+  hw_d2c_color_undistortion_configured_ = true;
+  RCLCPP_INFO_STREAM(logger_,
+                     "Configured HW D2C color undistortion with depth camera intrinsic");
+}
+
+void OBCameraNode::applyHwD2CColorUndistortion(std::shared_ptr<ob::FrameSet> &frame_set,
+                                               const std::shared_ptr<ob::Frame> &depth_frame) {
+  if (!frame_set || !hw_d2c_color_undistortion_filter_) {
+    return;
+  }
+  configureHwD2CColorUndistortion(depth_frame);
+  if (!hw_d2c_color_undistortion_configured_) {
+    return;
+  }
+  auto undistorted_frame = hw_d2c_color_undistortion_filter_->process(frame_set);
+  if (!undistorted_frame) {
+    RCLCPP_WARN_STREAM(logger_, "HW D2C color undistortion filter returned null frame");
+    return;
+  }
+  auto undistorted_frame_set = undistorted_frame->as<ob::FrameSet>();
+  if (!undistorted_frame_set) {
+    RCLCPP_WARN_STREAM(logger_, "HW D2C color undistortion filter returned non-frameset output");
+    return;
+  }
+  frame_set = undistorted_frame_set;
+}
+
 void OBCameraNode::setupLeftIrPostProcessFilter() {
   auto device_info = device_->getDeviceInfo();
   CHECK_NOTNULL(device_info);
@@ -3341,6 +3471,8 @@ void OBCameraNode::getParameters() {
     setAndGetNodeParameter<std::string>(image_qos_[stream_index], param_name, "default");
     param_name = stream_name_[stream_index] + "_camera_info_qos";
     setAndGetNodeParameter<std::string>(camera_info_qos_[stream_index], param_name, "default");
+    param_name = "enable_" + stream_name_[stream_index] + "_undistortion";
+    setAndGetNodeParameter<bool>(enable_undistortion_[stream_index], param_name, false);
   }
 
   for (auto stream_index : IMAGE_STREAMS) {
@@ -3539,7 +3671,6 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<int>(max_depth_limit_, "max_depth_limit", 0);
   setAndGetNodeParameter<bool>(enable_heartbeat_, "enable_heartbeat", false);
   setAndGetNodeParameter<bool>(enable_firmware_log_, "enable_firmware_log", false);
-  setAndGetNodeParameter<bool>(enable_color_undistortion_, "enable_color_undistortion", false);
   setAndGetNodeParameter<std::string>(time_domain_, "time_domain", "global");
   setAndGetNodeParameter<bool>(enable_frame_timestamp_csv_, "enable_frame_timestamp_csv", false);
   setAndGetNodeParameter<std::string>(frame_timestamp_csv_file_, "frame_timestamp_csv_file", "");
@@ -3570,7 +3701,7 @@ void OBCameraNode::getParameters() {
     depth_registration_ = false;
     enable_d2c_viewer_ = false;
     enable_depth_filter_ = false;
-    enable_color_undistortion_ = false;
+    enable_undistortion_[COLOR] = false;
   }
 
   if (isOpenNIDevice(pid_)) {
@@ -3672,12 +3803,16 @@ void OBCameraNode::setupTopics() {
     if (enable_stream_[COLOR] || enable_stream_[COLOR_LEFT] || enable_stream_[COLOR_RIGHT]) {
       setupColorPostProcessFilter();
     }
+    if (enable_stream_[INFRA0]) {
+      setupIrPostProcessFilter();
+    }
     if (enable_stream_[INFRA2]) {
       setupRightIrPostProcessFilter();
     }
     if (enable_stream_[INFRA1]) {
       setupLeftIrPostProcessFilter();
     }
+    setupUndistortionFilters();
     syncConfigJsonDeviceSettings();
     syncConfigJsonFilterSettings(depth_filter_list_, "depth");
     syncConfigJsonFilterSettings(color_filter_list_, "color");
@@ -3963,19 +4098,6 @@ void OBCameraNode::setupPublishers() {
               rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
                           camera_info_qos_profile));
     }
-    if (stream_index == COLOR && enable_color_undistortion_) {
-      color_undistortion_camera_info_publisher_ = node_->create_publisher<CameraInfo>(
-          "color/camera_info_undistorted",
-          rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
-                      camera_info_qos_profile));
-      if (use_intra_process_) {
-        color_undistortion_publisher_ = std::make_shared<image_rcl_publisher>(
-            *node_, "color/image_undistorted", image_qos_profile);
-      } else {
-        color_undistortion_publisher_ = std::make_shared<image_transport_publisher>(
-            *node_, "color/image_undistorted", image_qos_profile);
-      }
-    }
   }
 
   if (depth_registration_ && align_mode_ == "SW") {
@@ -4370,6 +4492,24 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
 
   depth_registration_cloud_pub_->publish(std::move(point_cloud_msg));
 }
+std::shared_ptr<ob::Frame> OBCameraNode::processIrFrameFilter(std::shared_ptr<ob::Frame> &frame) {
+  if (frame == nullptr || frame->getType() != OB_FRAME_IR) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < ir_filter_list_.size(); i++) {
+    auto filter = ir_filter_list_[i];
+    CHECK_NOTNULL(filter.get());
+    if (filter->isEnabled() && frame != nullptr) {
+      frame = filter->process(frame);
+      if (frame == nullptr) {
+        RCLCPP_ERROR_STREAM(logger_, "Ir filter process failed");
+        break;
+      }
+    }
+  }
+  return frame;
+}
+
 std::shared_ptr<ob::Frame> OBCameraNode::processRightIrFrameFilter(
     std::shared_ptr<ob::Frame> &frame) {
   if (frame == nullptr || frame->getType() != OB_FRAME_IR_RIGHT) {
@@ -4644,6 +4784,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
     auto left_color_frame = frame_set->getFrame(OB_FRAME_COLOR_LEFT);
     auto right_color_frame = frame_set->getFrame(OB_FRAME_COLOR_RIGHT);
     auto ir_frame = frame_set->getFrame(OB_FRAME_IR);
+    auto depth_frame_for_hw_d2c_undistortion = depth_frame;
     if (depth_frame) {
       setDisparitySearchOffset();
       setDepthAutoExposureROI();
@@ -4653,6 +4794,11 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
         fps_counter_depth_->tick();
       }
     }
+    if (shouldUseHwD2CColorUndistortion() && color_frame) {
+      applyHwD2CColorUndistortion(frame_set, depth_frame_for_hw_d2c_undistortion);
+      depth_frame = frame_set->getFrame(OB_FRAME_DEPTH);
+      color_frame = frame_set->getFrame(OB_FRAME_COLOR);
+    }
     if (color_frame) {
       setColorAutoExposureROI();
       color_frame = processColorFrameFilter(color_frame);
@@ -4678,6 +4824,12 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
       frame_set->pushFrame(right_ir_frame);
       fps_counter_right_ir_->tick();
     }
+    if (ir_frame) {
+      ir_frame = processIrFrameFilter(ir_frame);
+      if (ir_frame) {
+        frame_set->pushFrame(ir_frame);
+      }
+    }
     if (depth_registration_ && align_filter_ && depth_frame) {
       publishRawDepthImage(depth_frame);
       if (auto new_frame = align_filter_->process(frame_set)) {
@@ -4938,9 +5090,6 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
   if (image_publishers_.count(stream_index) && image_publishers_.at(stream_index)) {
     has_subscriber = image_publishers_.at(stream_index)->get_subscription_count() > 0;
   }
-  if (stream_index == COLOR && enable_color_undistortion_ && color_undistortion_publisher_) {
-    has_subscriber = true;
-  }
   if (save_images_[stream_index]) {
     has_subscriber = true;
   }
@@ -5047,10 +5196,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   const bool has_raw_image_subscriber =
       image_publishers_[stream_index]->get_subscription_count() > 0;
   const bool has_compressed_image_subscriber = hasCompressedImageSubscriber(stream_index);
-  const bool enable_undistortion_publish =
-      (stream_index == COLOR && enable_color_undistortion_ && color_undistortion_publisher_);
   bool has_subscriber = has_raw_image_subscriber || has_compressed_image_subscriber ||
-                        enable_undistortion_publish || save_images_[stream_index];
+                        save_images_[stream_index];
   has_subscriber =
       has_subscriber || camera_info_publishers_[stream_index]->get_subscription_count() > 0;
   has_subscriber =
@@ -5166,7 +5313,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     }
   }
 
-  if (!has_raw_image_subscriber && !enable_undistortion_publish && !save_images_[stream_index]) {
+  if (!has_raw_image_subscriber && !save_images_[stream_index]) {
     CHECK(camera_info_publishers_.count(stream_index) > 0);
     camera_info_publishers_[stream_index]->publish(camera_info);
     publishMetadata(frame, stream_index, camera_info.header);
@@ -5177,8 +5324,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   if (image.empty() || image.cols != width || image.rows != height) {
     image.create(height, width, image_format_[stream_index]);
   }
-  if (isColorFrameDecodeRequired(frame) &&
+  if (isColorFrameDecodeRequired(frame) && (has_raw_image_subscriber || save_images_[stream_index])) {
-      (has_raw_image_subscriber || enable_undistortion_publish || save_images_[stream_index])) {
     if (frame->getType() == OB_FRAME_COLOR && !is_color_frame_decoded_) {
       RCLCPP_ERROR(logger_, "color frame is not decoded");
       return;
@@ -5202,41 +5348,6 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     memcpy(image.data, video_frame->getData(), video_frame->getDataSize());
   }
 
-  if (enable_undistortion_publish) {
-    auto undistort_result = undistortImage(image, intrinsic, distortion);
-    sensor_msgs::msg::Image::UniquePtr undistorted_image_msg(new sensor_msgs::msg::Image());
-    cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], undistort_result.image)
-        .toImageMsg(*undistorted_image_msg);
-    CHECK_NOTNULL(undistorted_image_msg.get());
-    undistorted_image_msg->header.stamp = timestamp;
-    undistorted_image_msg->is_bigendian = false;
-    undistorted_image_msg->step = width * unit_step_size_[stream_index];
-    undistorted_image_msg->header.frame_id = frame_id;
-    color_undistortion_publisher_->publish(std::move(undistorted_image_msg));
-
-    if (color_undistortion_camera_info_publisher_) {
-      auto undistorted_camera_info = camera_info;
-      const auto distortion_coeff_count =
-          undistorted_camera_info.d.empty() ? 5 : undistorted_camera_info.d.size();
-      undistorted_camera_info.d.assign(distortion_coeff_count, 0.0);
-      undistorted_camera_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
-      undistorted_camera_info.roi.do_rectify = false;
-      undistorted_camera_info.k.fill(0.0);
-      undistorted_camera_info.k.at(0) = undistort_result.new_intrinsic.fx;
-      undistorted_camera_info.k.at(4) = undistort_result.new_intrinsic.fy;
-      undistorted_camera_info.k.at(2) = undistort_result.new_intrinsic.cx;
-      undistorted_camera_info.k.at(5) = undistort_result.new_intrinsic.cy;
-      undistorted_camera_info.k.at(8) = 1.0;
-      undistorted_camera_info.p.fill(0.0);
-      undistorted_camera_info.p.at(0) = undistort_result.new_intrinsic.fx;
-      undistorted_camera_info.p.at(5) = undistort_result.new_intrinsic.fy;
-      undistorted_camera_info.p.at(2) = undistort_result.new_intrinsic.cx;
-      undistorted_camera_info.p.at(6) = undistort_result.new_intrinsic.cy;
-      undistorted_camera_info.p.at(10) = 1.0;
-      color_undistortion_camera_info_publisher_->publish(undistorted_camera_info);
-    }
-  }
-
   CHECK(camera_info_publishers_.count(stream_index) > 0);
   camera_info_publishers_[stream_index]->publish(camera_info);
   publishMetadata(frame, stream_index, camera_info.header);
@@ -5891,6 +6002,11 @@ bool OBCameraNode::isPublishMetaData(uint32_t pid) {
   return isGemini335PID(pid) || isGemini435LePID(pid) || isGemini305SeriesPID(pid);
 }
 
+bool OBCameraNode::isDabaiASeriesForHwD2C(uint32_t pid) {
+  return pid == DABAI_A_PID || pid == DABAI_AL_PID || pid == GEMINI_345_PID ||
+         pid == GEMINI_345LG_PID;
+}
+
 bool OBCameraNode::isDepthWorkModeDevices(uint32_t pid) { return pid == GEMINI_435Le_PID; }
 
 bool OBCameraNode::isnotLaserDevices(uint32_t pid) { return isGemini305SeriesPID(pid); }