feat: update depth colorizer implementation and configuration options

README.MD

@@ -334,8 +334,8 @@ Launch camera node
 source ~/ros2_ws/install/setup.bash
 ros2 run orbbec_camera list_devices_node # Check if the camera is connected
 ros2 launch orbbec_camera gemini_330_series.launch.py # Or other launch file, see below table
-# Optional: enable Jet colorized depth on /camera/depth/image_raw
+# Optional: publish OrbbecViewer-style colorized depth on /camera/depth/image_raw
-ros2 launch orbbec_camera gemini_330_series.launch.py enable_depth_colorizer:=true
+ros2 launch orbbec_camera gemini_330_series.launch.py depth_colorizer_mode:=jet
 ```
 
 - On terminal 2
@@ -369,9 +369,14 @@ ros2 service call /camera/get_sdk_version orbbec_camera_msgs/srv/GetString '{}'
 
 For more usage details, please refer to the official [OrbbecSDK ROS2 documentation](https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/4_application_guide/application_guide.html)
 
-When `enable_depth_colorizer` is enabled, `depth/image_raw` directly publishes an `rgb8` image
+`depth_colorizer_mode` is a string render mode:
-(OpenCV Jet). When disabled, the topic continues to publish the raw `16UC1` depth image; the
+
-colorized image uses a 0--10 m depth range for mapping.
+- `none`: disable colorization and publish the raw `16UC1` depth image;
+- `jet`: use the OrbbecViewer-style dynamic histogram, gamma, and Jet mapping;
+- `jet_inv`: use the OrbbecViewer-style inverse Jet mapping;
+- `gray`: use the OrbbecViewer-style grayscale mapping;
+
+The `jet` and `jet_inv` modes publish `rgb8`; `gray` publishes `mono8` on `depth/image_raw`.
 
 ## Supported Devices
 

README_CN.MD

@@ -333,8 +333,8 @@ sudo bash install_udev_rules.sh
 source ~/ros2_ws/install/setup.bash
 ros2 run orbbec_camera list_devices_node #检查相机是否已连接
 ros2 launch orbbec_camera gemini_330_series.launch.py # 或其他启动文件，见下表
-# 可选：开启 Jet 伪彩色深度图，直接发布到 /camera/depth/image_raw
+# 可选：使用 OrbbecViewer 风格发布伪彩色深度图到 /camera/depth/image_raw
-ros2 launch orbbec_camera gemini_330_series.launch.py enable_depth_colorizer:=true
+ros2 launch orbbec_camera gemini_330_series.launch.py depth_colorizer_mode:=jet
 ```
 
 - 终端 2
@@ -368,8 +368,14 @@ ros2 service call /camera/get_sdk_version orbbec_camera_msgs/srv/GetString '{}'
 
 更多使用详情，请参考官方 [OrbbecSDK ROS2 文档](https://orbbec.github.io/OrbbecSDK_ROS2/zh/source/camera_devices/4_application_guide/application_guide.html)
 
-开启 `enable_depth_colorizer` 后，`depth/image_raw` 直接发布 `rgb8` 彩色图（OpenCV Jet）。
+`depth_colorizer_mode` 是字符串渲染模式：
-关闭时该话题仍发布原始 `16UC1` 深度图，彩色图使用 0--10 m 深度范围进行映射。
+
+- `none`：关闭彩色化，`depth/image_raw` 发布原始 `16UC1` 深度图；
+- `jet`：使用 OrbbecViewer 的动态直方图、伽马和 Jet 映射；
+- `jet_inv`：使用 OrbbecViewer 风格的反向 Jet 映射；
+- `gray`：使用 OrbbecViewer 风格的灰度映射；
+
+`jet` 和 `jet_inv` 模式发布 `rgb8`，`gray` 模式发布 `mono8` 到 `depth/image_raw`。
 
 ## 支持的设备
 

orbbec_camera/config/camera_params.yaml

@@ -1,7 +1,7 @@
 # common params
 depth_registration: false
 enable_d2c_viewer: false
-enable_depth_colorizer: false
+depth_colorizer_mode: "none"
 enable_point_cloud: false
 enable_colored_point_cloud: false
 enable_ldp: false

orbbec_camera/config/camera_secondary_params.yaml

@@ -1,7 +1,7 @@
 # common params
 depth_registration: false
 enable_d2c_viewer: false
-enable_depth_colorizer: false
+depth_colorizer_mode: "none"
 enable_point_cloud: false
 enable_colored_point_cloud: false
 enable_ldp: false

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -801,7 +801,7 @@ class OBCameraNode {
   std::string color_info_url_;
   std::string ir_info_url_;
   std::optional<OBCameraParam> camera_param_;
-  bool colorizer_enabled_ = false;
+  std::string colorizer_mode_ = "none";
   bool enable_d2c_viewer_ = false;
   std::unique_ptr<D2CViewer> d2c_viewer_ = nullptr;
   std::map<stream_index_pair, std::atomic_bool> save_images_;

orbbec_camera/launch/astra.launch.py

@@ -68,7 +68,7 @@ def generate_launch_description():
         DeclareLaunchArgument('log_file_name', default_value=''),
         DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
         DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
-        DeclareLaunchArgument('enable_depth_colorizer', default_value='false'),
+        DeclareLaunchArgument('depth_colorizer_mode', default_value='none'),
         DeclareLaunchArgument('enable_ldp', default_value='true'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
         DeclareLaunchArgument('use_hardware_time', default_value='false'),

orbbec_camera/launch/astra2.launch.py

@@ -100,7 +100,7 @@ def generate_launch_description():
         DeclareLaunchArgument('temporal_filter_weight', default_value='-1.0'),
         DeclareLaunchArgument('hole_filling_filter_mode', default_value=''),
         DeclareLaunchArgument("enable_d2c_viewer", default_value="false"),
-        DeclareLaunchArgument("enable_depth_colorizer", default_value="false"),
+        DeclareLaunchArgument("depth_colorizer_mode", default_value="none"),
         DeclareLaunchArgument("enable_ldp", default_value="true"),
         DeclareLaunchArgument("ordered_pc", default_value="false"),
         DeclareLaunchArgument("enable_depth_scale", default_value="true"),

orbbec_camera/launch/dabai_a.launch.py

@@ -42,7 +42,8 @@ def load_parameters(context, args):
     if config_file_path:
         yaml_params = load_yaml(config_file_path)
         default_params = merge_params(default_params, yaml_params)
-    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
+    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename',
+                    'depth_colorizer_mode'}
 
     result = {}
     for key, value in default_params.items():
@@ -158,7 +159,7 @@ def generate_launch_description():
         DeclareLaunchArgument('log_file_name', default_value=''),
         DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
         DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
-        DeclareLaunchArgument('enable_depth_colorizer', default_value='false'),
+        DeclareLaunchArgument('depth_colorizer_mode', default_value='none'),
         DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'),
         DeclareLaunchArgument('enable_ldp', default_value='true'),
         DeclareLaunchArgument('ldp_power_level', default_value='-1'),

orbbec_camera/launch/dabai_al.launch.py

@@ -42,7 +42,8 @@ def load_parameters(context, args):
     if config_file_path:
         yaml_params = load_yaml(config_file_path)
         default_params = merge_params(default_params, yaml_params)
-    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
+    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename',
+                    'depth_colorizer_mode'}
 
     result = {}
     for key, value in default_params.items():
@@ -158,7 +159,7 @@ def generate_launch_description():
         DeclareLaunchArgument('log_file_name', default_value=''),
         DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
         DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
-        DeclareLaunchArgument('enable_depth_colorizer', default_value='false'),
+        DeclareLaunchArgument('depth_colorizer_mode', default_value='none'),
         DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'),
         DeclareLaunchArgument('enable_ldp', default_value='true'),
         DeclareLaunchArgument('ldp_power_level', default_value='-1'),

orbbec_camera/launch/dabai_dcw2.launch.py

@@ -77,7 +77,7 @@ def generate_launch_description():
         DeclareLaunchArgument('log_level', default_value='info'),
         DeclareLaunchArgument('log_file_name', default_value=''),
         DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
-        DeclareLaunchArgument('enable_depth_colorizer', default_value='false'),
+        DeclareLaunchArgument('depth_colorizer_mode', default_value='none'),
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('enable_ldp', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),

orbbec_camera/launch/dabai_max_pro.launch.py

@@ -86,7 +86,7 @@ def generate_launch_description():
         DeclareLaunchArgument('log_file_name', default_value=''),
         DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
         DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
-        DeclareLaunchArgument('enable_depth_colorizer', default_value='false'),
+        DeclareLaunchArgument('depth_colorizer_mode', default_value='none'),
         DeclareLaunchArgument('enable_noise_removal_filter', default_value='true'),
         DeclareLaunchArgument('noise_removal_filter_min_diff', default_value='256'),
         DeclareLaunchArgument('noise_removal_filter_max_size', default_value='80'),

orbbec_camera/launch/femto.launch.py

@@ -77,7 +77,7 @@ def generate_launch_description():
         DeclareLaunchArgument("log_file_name", default_value=""),
         DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"),
         DeclareLaunchArgument("enable_d2c_viewer", default_value="false"),
-        DeclareLaunchArgument("enable_depth_colorizer", default_value="false"),
+        DeclareLaunchArgument("depth_colorizer_mode", default_value="none"),
         DeclareLaunchArgument("enable_ldp", default_value="true"),
         DeclareLaunchArgument("enable_frame_sync", default_value="false"),
         DeclareLaunchArgument("ordered_pc", default_value="false"),

orbbec_camera/launch/femto_bolt.launch.py

@@ -80,7 +80,7 @@ def generate_launch_description():
         DeclareLaunchArgument("log_file_name", default_value=""),
         DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"),
         DeclareLaunchArgument("enable_d2c_viewer", default_value="false"),
-        DeclareLaunchArgument("enable_depth_colorizer", default_value="false"),
+        DeclareLaunchArgument("depth_colorizer_mode", default_value="none"),
         DeclareLaunchArgument("enable_ldp", default_value="true"),
         DeclareLaunchArgument('enable_noise_removal_filter', default_value='false'),
         DeclareLaunchArgument('enable_decimation_filter', default_value='false'),

orbbec_camera/launch/femto_mega.launch.py

@@ -86,7 +86,7 @@ def generate_launch_description():
         DeclareLaunchArgument("log_file_name", default_value=""),
         DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"),
         DeclareLaunchArgument("enable_d2c_viewer", default_value="false"),
-        DeclareLaunchArgument("enable_depth_colorizer", default_value="false"),
+        DeclareLaunchArgument("depth_colorizer_mode", default_value="none"),
         DeclareLaunchArgument("enable_ldp", default_value="true"),
         DeclareLaunchArgument('enable_noise_removal_filter', default_value='false'),
         DeclareLaunchArgument('enable_decimation_filter', default_value='false'),

orbbec_camera/launch/gemini2.launch.py

@@ -92,7 +92,7 @@ def generate_launch_description():
         DeclareLaunchArgument("log_file_name", default_value=""),
         DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"),
         DeclareLaunchArgument("enable_d2c_viewer", default_value="false"),
-        DeclareLaunchArgument("enable_depth_colorizer", default_value="false"),
+        DeclareLaunchArgument("depth_colorizer_mode", default_value="none"),
         DeclareLaunchArgument('disparity_to_depth_mode', default_value='SW'),
         DeclareLaunchArgument("enable_ldp", default_value="true"),
         DeclareLaunchArgument('enable_decimation_filter', default_value='false'),

orbbec_camera/launch/gemini210.launch.py

@@ -90,7 +90,7 @@ def generate_launch_description():
         DeclareLaunchArgument("log_file_name", default_value=""),
         DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"),
         DeclareLaunchArgument("enable_d2c_viewer", default_value="false"),
-        DeclareLaunchArgument("enable_depth_colorizer", default_value="false"),
+        DeclareLaunchArgument("depth_colorizer_mode", default_value="none"),
         DeclareLaunchArgument('disparity_to_depth_mode', default_value='SW'),
         DeclareLaunchArgument("enable_ldp", default_value="true"),
         DeclareLaunchArgument('enable_decimation_filter', default_value='false'),

orbbec_camera/launch/gemini2L.launch.py

@@ -64,7 +64,8 @@ def load_parameters(context, args):
     if config_file_path:
         yaml_params = load_yaml(config_file_path)
         default_params = merge_params(default_params, yaml_params)
-    skip_convert = {"config_file_path", "usb_port", "serial_number", 'bag_record_filename', 'bag_filename'}
+    skip_convert = {"config_file_path", "usb_port", "serial_number", 'bag_record_filename', 'bag_filename',
+                    "depth_colorizer_mode"}
 
     return {
         key: (value if key in skip_convert else convert_value(value))
@@ -146,7 +147,7 @@ def generate_launch_description():
         DeclareLaunchArgument("log_file_name", default_value=""),
         DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"),
         DeclareLaunchArgument("enable_d2c_viewer", default_value="false"),
-        DeclareLaunchArgument("enable_depth_colorizer", default_value="false"),
+        DeclareLaunchArgument("depth_colorizer_mode", default_value="none"),
         DeclareLaunchArgument("disparity_to_depth_mode", default_value="SW"),
         DeclareLaunchArgument("enable_ldp", default_value="true"),
         DeclareLaunchArgument("enable_decimation_filter", default_value="false"),

orbbec_camera/launch/gemini345.launch.py

@@ -42,7 +42,8 @@ def load_parameters(context, args):
     if config_file_path:
         yaml_params = load_yaml(config_file_path)
         default_params = merge_params(default_params, yaml_params)
-    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
+    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename',
+                    'depth_colorizer_mode'}
 
     result = {}
     for key, value in default_params.items():
@@ -157,7 +158,7 @@ def generate_launch_description():
         DeclareLaunchArgument('log_file_name', default_value=''),
         DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
         DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
-        DeclareLaunchArgument('enable_depth_colorizer', default_value='false'),
+        DeclareLaunchArgument('depth_colorizer_mode', default_value='none'),
         DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'),
         DeclareLaunchArgument('enable_ldp', default_value='true'),
         DeclareLaunchArgument('ldp_power_level', default_value='-1'),

orbbec_camera/launch/gemini345_lg.launch.py

@@ -42,7 +42,8 @@ def load_parameters(context, args):
     if config_file_path:
         yaml_params = load_yaml(config_file_path)
         default_params = merge_params(default_params, yaml_params)
-    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
+    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename',
+                    'depth_colorizer_mode'}
 
     result = {}
     for key, value in default_params.items():
@@ -158,7 +159,7 @@ def generate_launch_description():
         DeclareLaunchArgument('log_file_name', default_value=''),
         DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
         DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
-        DeclareLaunchArgument('enable_depth_colorizer', default_value='false'),
+        DeclareLaunchArgument('depth_colorizer_mode', default_value='none'),
         DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'),
         DeclareLaunchArgument('enable_ldp', default_value='true'),
         DeclareLaunchArgument('ldp_power_level', default_value='-1'),

orbbec_camera/launch/gemini435_le.launch.py

@@ -42,7 +42,8 @@ def load_parameters(context, args):
     if config_file_path:
         yaml_params = load_yaml(config_file_path)
         default_params = merge_params(default_params, yaml_params)
-    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
+    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename',
+                    'depth_colorizer_mode'}
 
     result = {}
     for key, value in default_params.items():
@@ -188,7 +189,7 @@ def generate_launch_description():
         DeclareLaunchArgument('log_file_name', default_value=''),
         DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
         DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
-        DeclareLaunchArgument('enable_depth_colorizer', default_value='false'),
+        DeclareLaunchArgument('depth_colorizer_mode', default_value='none'),
         DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'),
         DeclareLaunchArgument('enable_ldp', default_value='true'),
         DeclareLaunchArgument('ldp_power_level', default_value='-1'),

orbbec_camera/launch/gemini_301_series.launch.py

@@ -64,7 +64,8 @@ def load_parameters(context, args):
     if config_file_path:
         yaml_params = load_yaml(config_file_path)
         default_params = merge_params(default_params, yaml_params)
-    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
+    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename',
+                    'depth_colorizer_mode'}
 
     result = {}
     for key, value in default_params.items():
@@ -206,7 +207,7 @@ def generate_launch_description():
         DeclareLaunchArgument('log_file_name', default_value=''),
         DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
         DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
-        DeclareLaunchArgument('enable_depth_colorizer', default_value='false'),
+        DeclareLaunchArgument('depth_colorizer_mode', default_value='none'),
         DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'),
         DeclareLaunchArgument('sync_mode', default_value='standalone'),
         DeclareLaunchArgument('depth_delay_us', default_value='0'),

orbbec_camera/launch/gemini_330_series.launch.py

@@ -49,7 +49,7 @@ def load_parameters(context, args):
         yaml_params = load_yaml(config_file_path)
         default_params = merge_params(default_params, yaml_params)
     skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename',
-                    'enhanced_depth_model_path'}
+                    'enhanced_depth_model_path', 'depth_colorizer_mode'}
 
     result = {}
     for key, value in default_params.items():
@@ -212,7 +212,7 @@ def generate_launch_description():
         DeclareLaunchArgument('log_file_name', default_value=''),
         DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
         DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
-        DeclareLaunchArgument('enable_depth_colorizer', default_value='false'),
+        DeclareLaunchArgument('depth_colorizer_mode', default_value='none'),
         DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'),
         DeclareLaunchArgument('enable_ldp', default_value='false'),
         DeclareLaunchArgument('ldp_power_level', default_value='-1'),

orbbec_camera/launch/gemini_330_series_low_cpu.launch.py

@@ -43,7 +43,7 @@ def load_parameters(context, args):
         yaml_params = load_yaml(config_file_path)
         default_params = merge_params(default_params, yaml_params)
     skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename',
-                    'enhanced_depth_model_path'}
+                    'enhanced_depth_model_path', 'depth_colorizer_mode'}
 
     result = {}
     for key, value in default_params.items():
@@ -199,7 +199,7 @@ def generate_launch_description():
         DeclareLaunchArgument('log_file_name', default_value=''),
         DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
         DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
-        DeclareLaunchArgument('enable_depth_colorizer', default_value='false'),
+        DeclareLaunchArgument('depth_colorizer_mode', default_value='none'),
         DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'),
         DeclareLaunchArgument('enable_ldp', default_value='false'),
         DeclareLaunchArgument('ldp_power_level', default_value='-1'),

orbbec_camera/launch/gemini_330_series_sdk_json.launch.py

@@ -41,7 +41,8 @@ def load_parameters(context, args):
     config_file_path = LaunchConfiguration('config_file_path').perform(context)
     if config_file_path:
         default_params = merge_params(default_params, load_yaml(config_file_path))
-    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
+    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename',
+                    'depth_colorizer_mode'}
 
     result = {}
     for key, value in default_params.items():
@@ -113,7 +114,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
-        DeclareLaunchArgument('enable_depth_colorizer', default_value='false'),
+        DeclareLaunchArgument('depth_colorizer_mode', default_value='none'),
         DeclareLaunchArgument('show_fps_enable', default_value='false'),
 
         DeclareLaunchArgument('enumerate_net_device', default_value='true'),

orbbec_camera/launch/gemini_intra_process_demo_launch.py

@@ -42,7 +42,8 @@ def load_parameters(context, args):
     if config_file_path:
         yaml_params = load_yaml(config_file_path)
         default_params = merge_params(default_params, yaml_params)
-    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'}
+    skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename',
+                    'depth_colorizer_mode'}
     return {
         key: (value if key in skip_convert else convert_value(value))
         for key, value in default_params.items()
@@ -117,7 +118,7 @@ def generate_launch_description():
         DeclareLaunchArgument('log_file_name', default_value=''),
         DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
         DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
-        DeclareLaunchArgument('enable_depth_colorizer', default_value='false'),
+        DeclareLaunchArgument('depth_colorizer_mode', default_value='none'),
         DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'),
         DeclareLaunchArgument('enable_ldp', default_value='true'),
         DeclareLaunchArgument('sync_mode', default_value='standalone'),

orbbec_camera/src/ob_camera_node.cpp

@@ -19,6 +19,7 @@
 #include <thread>
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <sstream>
+#include <array>
 #include <algorithm>
 #include <cctype>
 #include <cmath>
@@ -66,7 +67,10 @@ namespace {
 
 constexpr char kEnhancedDepthSupportedTargetResolutions[] = "640x480/1280x720/1280x800";
 constexpr char kEnhancedDepthSupportedDepthFormats[] = "Y10/Y11/Y12/Y14/Y16/Z16";
-constexpr double kColorizerMaxDistanceMm = 10000.0;
+constexpr double kViewerColorizerGamma = 0.65;
+constexpr uint16_t kViewerColorizerMaxDistanceMm = 10000;
+constexpr uint16_t kViewerColorizerDefaultMinDistanceMm = 100;
+constexpr uint16_t kViewerColorizerG305MinDistanceMm = 40;
 
 std::string getDepthFilterStatusName(const std::string &filter_name) {
   if (filter_name == "SpatialAdvancedFilter") {
@@ -4467,7 +4471,10 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<int>(point_cloud_decimation_filter_factor_,
                               "point_cloud_decimation_filter_factor", 1);
   setAndGetNodeParameter<std::string>(point_cloud_qos_, "point_cloud_qos", "default");
-  setAndGetNodeParameter<bool>(colorizer_enabled_, "enable_depth_colorizer", false);
+  setAndGetNodeParameter<std::string>(colorizer_mode_, "depth_colorizer_mode", "none");
+  colorizer_mode_ = lowerParameterValue(colorizer_mode_);
+  colorizer_mode_ = normalizeClosedSetParameterValue(
+      logger_, "depth_colorizer_mode", colorizer_mode_, {"none", "jet", "jet_inv", "gray"}, "none");
   setAndGetNodeParameter<bool>(enable_d2c_viewer_, "enable_d2c_viewer", false);
   setAndGetNodeParameter<std::string>(disparity_to_depth_mode_, "disparity_to_depth_mode", "");
   disparity_to_depth_mode_ =
@@ -5430,9 +5437,10 @@ void OBCameraNode::setupPublishers() {
     }
   }
 
-  if (colorizer_enabled_ && enable_stream_[DEPTH]) {
+  if (colorizer_mode_ != "none" && enable_stream_[DEPTH]) {
-    RCLCPP_INFO(logger_,
+    RCLCPP_INFO_STREAM(logger_, "Depth colorizer mode '"
-                "Depth colorizer enabled, publishing Jet RGB images on depth/image_raw (0-10 m)");
+                                    << colorizer_mode_
+                                    << "' enabled, publishing colorized images on depth/image_raw");
   }
 
   syncSoftwareAlignment();
@@ -5613,22 +5621,78 @@ cv::Mat OBCameraNode::colorizeDepthImage(const cv::Mat &depth_image) {
     return {};
   }
 
-  cv::Mat depth_8u;
+  if (colorizer_mode_ == "none") {
-  if (depth_image.type() == CV_16UC1 || depth_image.type() == CV_32FC1) {
+    return {};
-    depth_image.convertTo(depth_8u, CV_8UC1, 255.0 / kColorizerMaxDistanceMm);
+  }
-  } else if (depth_image.type() == CV_8UC1) {
+
-    depth_image.convertTo(depth_8u, CV_8UC1);
+  if (depth_image.type() != CV_16UC1 && depth_image.type() != CV_32FC1 &&
-  } else {
+      depth_image.type() != CV_8UC1) {
     RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 5000,
                          "Unsupported depth image type for colorizer: %d", depth_image.type());
     return {};
   }
 
-  cv::Mat colorized_bgr;
+  cv::Mat depth_16u;
-  cv::applyColorMap(depth_8u, colorized_bgr, cv::COLORMAP_JET);
+  depth_image.convertTo(depth_16u, CV_16UC1);
+
+  const uint16_t min_depth = isGemini305SeriesPID(pid_) ? kViewerColorizerG305MinDistanceMm
+                                                        : kViewerColorizerDefaultMinDistanceMm;
+  const uint16_t max_depth = kViewerColorizerMaxDistanceMm;
+  const uint32_t value_range = static_cast<uint32_t>(max_depth) - min_depth + 1;
+  std::array<uint32_t, static_cast<size_t>(kViewerColorizerMaxDistanceMm) + 1> histogram{};
+  uint32_t valid_pixel_count = 0;
+
+  for (int row = 0; row < depth_16u.rows; ++row) {
+    const auto *depth_row = depth_16u.ptr<uint16_t>(row);
+    for (int col = 0; col < depth_16u.cols; ++col) {
+      const uint16_t depth_value = depth_row[col];
+      if (depth_value >= min_depth && depth_value <= max_depth) {
+        ++histogram[depth_value];
+        ++valid_pixel_count;
+      }
+    }
+  }
+  for (uint32_t depth_value = 1; depth_value <= max_depth; ++depth_value) {
+    histogram[depth_value] += histogram[depth_value - 1];
+  }
+
+  cv::Mat depth_8u(depth_16u.size(), CV_8UC1);
+  for (int row = 0; row < depth_16u.rows; ++row) {
+    const auto *depth_row = depth_16u.ptr<uint16_t>(row);
+    auto *mapped_row = depth_8u.ptr<uint8_t>(row);
+    for (int col = 0; col < depth_16u.cols; ++col) {
+      uint16_t depth_value = depth_row[col];
+      if (depth_value > max_depth) {
+        depth_value = max_depth;
+      }
+      if (valid_pixel_count != 0 && depth_value >= min_depth) {
+        depth_value = static_cast<uint16_t>(static_cast<float>(value_range) *
+                                                histogram[depth_value] / valid_pixel_count +
+                                            min_depth);
+      }
+      const double normalized_depth = std::max(
+          0.0,
+          std::min(1.0, (static_cast<double>(depth_value) - min_depth) / (max_depth - min_depth)));
+      const double scale_value = 255.0 * std::pow(normalized_depth, kViewerColorizerGamma);
+      mapped_row[col] = static_cast<uint8_t>(std::max(0.0, std::min(255.0, scale_value)));
+    }
+  }
 
   cv::Mat invalid_depth_mask;
   cv::compare(depth_image, cv::Scalar(0), invalid_depth_mask, cv::CMP_EQ);
+  depth_8u.setTo(cv::Scalar::all(0), invalid_depth_mask);
+
+  if (colorizer_mode_ == "gray") {
+    return depth_8u;
+  }
+  if (colorizer_mode_ == "jet_inv") {
+    depth_8u = 255 - depth_8u;
+    depth_8u.setTo(cv::Scalar::all(0), invalid_depth_mask);
+  }
+
+  cv::Mat colorized_bgr;
+  cv::applyColorMap(depth_8u, colorized_bgr, cv::COLORMAP_JET);
+
   colorized_bgr.setTo(cv::Scalar::all(0), invalid_depth_mask);
 
   cv::Mat colorized_rgb;
@@ -6827,12 +6891,13 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     auto depth_scale = video_frame->as<ob::DepthFrame>()->getValueScale();
     image = image * depth_scale;
     image_to_publish = image;
-    if (colorizer_enabled_ && has_raw_image_subscriber) {
+    if (colorizer_mode_ != "none" && has_raw_image_subscriber) {
       auto colorized_image = colorizeDepthImage(image);
       if (!colorized_image.empty()) {
         image_to_publish = std::move(colorized_image);
-        image_encoding = sensor_msgs::image_encodings::RGB8;
+        image_encoding = colorizer_mode_ == "gray" ? sensor_msgs::image_encodings::MONO8
-        image_step = static_cast<uint32_t>(image_to_publish.cols * 3 * sizeof(uint8_t));
+                                                   : sensor_msgs::image_encodings::RGB8;
+        image_step = static_cast<uint32_t>(image_to_publish.cols * image_to_publish.elemSize());
       }
     }
   }