Skip to content

Commit

Permalink
Update default params
Browse files Browse the repository at this point in the history
  • Loading branch information
@jian-dong
jian-dong committed May 15, 2024
1 parent 3242031 commit 25cf0c7
Show file tree
Hide file tree
Showing 11 changed files with 120 additions and 64 deletions.
32 changes: 18 additions & 14 deletions include/orbbec_camera/ob_camera_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -419,7 +419,7 @@ class OBCameraNode {
// Only for Gemini2 device
bool enable_hardware_d2d_ = true;
std::string depth_work_mode_;
OBMultiDeviceSyncMode sync_mode_ = OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN;
OBMultiDeviceSyncMode sync_mode_ = OB_MULTI_DEVICE_SYNC_MODE_STANDALONE;
std::string sync_mode_str_;
int depth_delay_us_ = 0;
int color_delay_us_ = 0;
Expand Down Expand Up @@ -470,19 +470,23 @@ class OBCameraNode {
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";
int decimation_filter_scale_range_ = -1;
int sequence_id_filter_id_ = -1;
int threshold_filter_max_ = -1;
int threshold_filter_min_ = -1;
int noise_removal_filter_min_diff_ = -1;
int noise_removal_filter_max_size_ = -1;
float spatial_filter_alpha_ = -1.0;
int spatial_filter_diff_threshold_ = -1;
int spatial_filter_magnitude_ = -1;
int spatial_filter_radius_ = -1;
float temporal_filter_diff_threshold_ = -1.0;
float temporal_filter_weight_ = -1.0;
int hdr_merge_exposure_1_ = -1;
int hdr_merge_gain_1_ = -1;
int hdr_merge_exposure_2_ = -1;
int hdr_merge_gain_2_ = -1;
std::string hole_filling_filter_mode_;
ros::Publisher filter_status_pub_;
nlohmann::json filter_status_;
std::shared_ptr<diagnostic_updater::Updater> diagnostic_updater_ = nullptr;
Expand Down
2 changes: 1 addition & 1 deletion launch/dabai_dcl.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@
<arg name="enable_ldp" default="true"/>
<!-- Configure the path for depth filter file, for example: /config/depthfilter/Gemini2_v1.7.json -->
<arg name="depth_filter_config" default="" />
<arg name="sync_mode" default="free_run"/>
<arg name="sync_mode" default="standalone"/>
<arg name="depth_delay_us" default="0"/>
<arg name="color_delay_us" default="0"/>
<arg name="trigger2image_delay_us" default="0"/>
Expand Down
2 changes: 1 addition & 1 deletion launch/femto_bolt.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@
<arg name="log_level" default="none"/>
<arg name="enable_d2c_viewer" default="false"/>
<arg name="enable_soft_filter" default="true"/>
<arg name="sync_mode" default="free_run"/>
<arg name="sync_mode" default="standalone"/>
<arg name="depth_delay_us" default="0"/>
<arg name="color_delay_us" default="0"/>
<arg name="trigger2image_delay_us" default="0"/>
Expand Down
2 changes: 1 addition & 1 deletion launch/femto_mega.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@
<arg name="port" default="0"/>
<arg name="enable_d2c_viewer" default="false"/>
<arg name="enable_soft_filter" default="true"/>
<arg name="sync_mode" default="free_run"/>
<arg name="sync_mode" default="standalone"/>
<arg name="depth_delay_us" default="0"/>
<arg name="color_delay_us" default="0"/>
<arg name="trigger2image_delay_us" default="0"/>
Expand Down
2 changes: 1 addition & 1 deletion launch/gemini2.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@
<arg name="enable_ldp" default="true"/>
<!-- Configure the path for depth filter file, for example: /config/depthfilter/Gemini2_v1.7.json -->
<arg name="depth_filter_config" default="" />
<arg name="sync_mode" default="free_run"/>
<arg name="sync_mode" default="standalone"/>
<arg name="depth_delay_us" default="0"/>
<arg name="color_delay_us" default="0"/>
<arg name="trigger2image_delay_us" default="0"/>
Expand Down
2 changes: 1 addition & 1 deletion launch/gemini2L.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@
<arg name="enable_soft_filter" default="true"/>
<arg name="soft_filter_max_diff" default="-1"/>
<arg name="soft_filter_speckle_size" default="-1"/>
<arg name="sync_mode" default="free_run"/>
<arg name="sync_mode" default="standalone"/>
<arg name="depth_delay_us" default="0"/>
<arg name="color_delay_us" default="0"/>
<arg name="trigger2image_delay_us" default="0"/>
Expand Down
2 changes: 1 addition & 1 deletion launch/gemini2XL.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@
<arg name="enable_soft_filter" default="true"/>
<arg name="soft_filter_max_diff" default="-1"/>
<arg name="soft_filter_speckle_size" default="-1"/>
<arg name="sync_mode" default="free_run"/>
<arg name="sync_mode" default="standalone"/>
<arg name="depth_delay_us" default="0"/>
<arg name="color_delay_us" default="0"/>
<arg name="frame_sync" default="true"/>
Expand Down
2 changes: 1 addition & 1 deletion launch/gemini2_nodelet.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@
<arg name="enable_soft_filter" default="true"/>
<arg name="soft_filter_max_diff" default="-1"/>
<arg name="soft_filter_speckle_size" default="-1"/>
<arg name="sync_mode" default="free_run"/>
<arg name="sync_mode" default="standalone"/>
<arg name="depth_delay_us" default="0"/>
<arg name="color_delay_us" default="0"/>
<arg name="trigger2image_delay_us" default="0"/>
Expand Down
30 changes: 19 additions & 11 deletions launch/gemini_330_series.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
<arg name="diagnostics_frequency" default="1.0"/>
<arg name="enable_laser" default="true"/>
<arg name="laser_on_off_mode" default="0"/>
<arg name="sync_mode" default="free_run"/>
<arg name="sync_mode" default="standalone"/>


<!-- Color camera parameters -->
Expand Down Expand Up @@ -58,7 +58,7 @@
<arg name="enable_ir_long_exposure" default="false"/>

<!-- Point cloud parameters -->
<arg name="enable_point_cloud" default="true"/>
<arg name="enable_point_cloud" default="false"/>
<arg name="enable_colored_point_cloud" default="true"/>
<arg name="ordered_pc" default="false"/>
<!-- Gemini 335/335L only support SW align mode, Please DO NOT change it -->
Expand All @@ -74,17 +74,21 @@
<arg name="enable_temporal_filter" default="false"/>
<arg name="enable_hole_filling_filter" default="false"/>
<arg name="sequence_id_filter_id" default="1"/>
<arg name="threshold_filter_max" default="16000"/>
<arg name="threshold_filter_min" default="0"/>
<arg name="threshold_filter_max" default="-1"/>
<arg name="threshold_filter_min" default="-1"/>
<arg name="noise_removal_filter_min_diff" default="256"/>
<arg name="noise_removal_filter_max_size" default="80"/>
<arg name="spatial_filter_alpha" default="0.5"/>
<arg name="spatial_filter_diff_threshold" default="8"/>
<arg name="spatial_filter_magnitude" default="1"/>
<arg name="spatial_filter_radius" default="1"/>
<arg name="temporal_filter_diff_threshold" default="0.1"/>
<arg name="temporal_filter_weight" default="0.4"/>
<arg name="hole_filling_filter_mode" default="FILL_TOP"/>
<arg name="spatial_filter_alpha" default="-1.0"/>
<arg name="spatial_filter_diff_threshold" default="-1"/>
<arg name="spatial_filter_magnitude" default="-1"/>
<arg name="spatial_filter_radius" default="-1"/>
<arg name="temporal_filter_diff_threshold" default="-1.-"/>
<arg name="temporal_filter_weight" default="-1.0"/>
<arg name="hole_filling_filter_mode" default=""/>
<arg name="hdr_merge_exposure_1" default="-1"/>
<arg name="hdr_merge_gain_1" default="-1"/>
<arg name="hdr_merge_exposure_2" default="-1"/>
<arg name="hdr_merge_gain_2" default="-1"/>
<!-- IMU parameters -->
<arg name="enable_sync_output_accel_gyro" default="true"/>
<arg name="enable_accel" default="true"/>
Expand Down Expand Up @@ -175,6 +179,10 @@
<param name="temporal_filter_diff_threshold" value="$(arg temporal_filter_diff_threshold)"/>
<param name="temporal_filter_weight" value="$(arg temporal_filter_weight)"/>
<param name="hole_filling_filter_mode" value="$(arg hole_filling_filter_mode)"/>
<param name="hdr_merge_exposure_1" value="$(arg hdr_merge_exposure_1)"/>
<param name="hdr_merge_gain_1" value="$(arg hdr_merge_gain_1)"/>
<param name="hdr_merge_exposure_2" value="$(arg hdr_merge_exposure_2)"/>
<param name="hdr_merge_gain_2" value="$(arg hdr_merge_gain_2)"/>

<param name="enable_sync_output_accel_gyro" value="$(arg enable_sync_output_accel_gyro)"/>
<param name="enable_accel" value="$(arg enable_accel)"/>
Expand Down
34 changes: 19 additions & 15 deletions src/ob_camera_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -141,7 +141,7 @@ void OBCameraNode::getParameters() {
enable_ir_auto_exposure_ = nh_private_.param<bool>("enable_ir_auto_exposure", true);
ir_exposure_ = nh_private_.param<int>("ir_exposure_", -1);
enable_ir_long_exposure_ = nh_private_.param<bool>("enable_ir_long_exposure", false);
sync_mode_str_ = nh_private_.param<std::string>("sync_mode", "free_run");
sync_mode_str_ = nh_private_.param<std::string>("sync_mode", "standalone");
std::transform(sync_mode_str_.begin(), sync_mode_str_.end(), sync_mode_str_.begin(), ::toupper);
sync_mode_ = OBSyncModeFromString(sync_mode_str_);
depth_delay_us_ = nh_private_.param<int>("depth_delay_us", 0);
Expand Down Expand Up @@ -192,23 +192,27 @@ void OBCameraNode::getParameters() {
enable_sequenced_filter_ = nh_private_.param<bool>("enable_sequenced_filter", false);
enable_threshold_filter_ = nh_private_.param<bool>("enable_threshold_filter", false);
enable_noise_removal_filter_ = nh_private_.param<bool>("enable_noise_removal_filter", true);
enable_spatial_filter_ = nh_private_.param<bool>("enable_spatial_filter", true);
enable_spatial_filter_ = nh_private_.param<bool>("enable_spatial_filter", false);
enable_temporal_filter_ = nh_private_.param<bool>("enable_temporal_filter", false);
enable_hole_filling_filter_ = nh_private_.param<bool>("enable_hole_filling_filter", false);
decimation_filter_scale_range_ = nh_private_.param<int>("decimation_filter_scale_range", 2);
sequence_id_filter_id_ = nh_private_.param<int>("sequence_id_filter_id", 1);
threshold_filter_max_ = nh_private_.param<int>("threshold_filter_max", 16000);
threshold_filter_min_ = nh_private_.param<int>("threshold_filter_min", 0);
noise_removal_filter_min_diff_ = nh_private_.param<int>("noise_removal_filter_min_diff", 8);
decimation_filter_scale_range_ = nh_private_.param<int>("decimation_filter_scale_range", -1);
sequence_id_filter_id_ = nh_private_.param<int>("sequence_id_filter_id", -1);
threshold_filter_max_ = nh_private_.param<int>("threshold_filter_max", -1);
threshold_filter_min_ = nh_private_.param<int>("threshold_filter_min", -1);
noise_removal_filter_min_diff_ = nh_private_.param<int>("noise_removal_filter_min_diff", 250);
noise_removal_filter_max_size_ = nh_private_.param<int>("noise_removal_filter_max_size", 80);
spatial_filter_alpha_ = nh_private_.param<float>("spatial_filter_alpha", 0.5);
spatial_filter_diff_threshold_ = nh_private_.param<int>("spatial_filter_diff_threshold", 8);
spatial_filter_magnitude_ = nh_private_.param<int>("spatial_filter_magnitude", 1);
spatial_filter_radius_ = nh_private_.param<int>("spatial_filter_radius", 1);
temporal_filter_diff_threshold_ = nh_private_.param<float>("temporal_filter_diff_threshold", 0.1);
temporal_filter_weight_ = nh_private_.param<float>("temporal_filter_weight", 0.4);
hole_filling_filter_mode_ =
nh_private_.param<std::string>("hole_filling_filter_mode", "FILL_TOP");
spatial_filter_alpha_ = nh_private_.param<float>("spatial_filter_alpha", -1.0);
spatial_filter_diff_threshold_ = nh_private_.param<int>("spatial_filter_diff_threshold", -1);
spatial_filter_magnitude_ = nh_private_.param<int>("spatial_filter_magnitude", -1);
spatial_filter_radius_ = nh_private_.param<int>("spatial_filter_radius", -1);
temporal_filter_diff_threshold_ =
nh_private_.param<float>("temporal_filter_diff_threshold", -1.0);
temporal_filter_weight_ = nh_private_.param<float>("temporal_filter_weight", -1.0);
hole_filling_filter_mode_ = nh_private_.param<std::string>("hole_filling_filter_mode", "");
hdr_merge_exposure_1_ = nh_private_.param<int>("hdr_merge_exposure_1", -1);
hdr_merge_gain_1_ = nh_private_.param<int>("hdr_merge_gain_1", -1);
hdr_merge_exposure_2_ = nh_private_.param<int>("hdr_merge_exposure_2", -1);
hdr_merge_gain_2_ = nh_private_.param<int>("hdr_merge_gain_2", -1);
diagnostics_frequency_ = nh_private_.param<double>("diagnostics_frequency", 1.0);
enable_laser_ = nh_private_.param<bool>("enable_laser", true);
laser_on_off_mode_ = nh_private_.param<int>("laser_on_off_mode", 0);
Expand Down
74 changes: 57 additions & 17 deletions src/ros_setup.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,19 @@ void OBCameraNode::setupDevices() {
if (!device_preset_.empty()) {
device_->loadPreset(device_preset_.c_str());
}
if (sync_mode_ != OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN) {
auto sync_config = device_->getMultiDeviceSyncConfig();
sync_config.syncMode = sync_mode_;
sync_config.depthDelayUs = depth_delay_us_;
sync_config.colorDelayUs = color_delay_us_;
sync_config.trigger2ImageDelayUs = trigger2image_delay_us_;
sync_config.triggerOutDelayUs = trigger_out_delay_us_;
sync_config.triggerOutEnable = trigger_out_enabled_;
device_->setMultiDeviceSyncConfig(sync_config);
if (device_->isPropertySupported(OB_PROP_SYNC_SIGNAL_TRIGGER_OUT_BOOL,
OB_PERMISSION_READ_WRITE)) {
}
}
auto depth_sensor = device_->getSensor(OB_SENSOR_DEPTH);
device_->setBoolProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, enable_color_auto_exposure_);
Expand Down Expand Up @@ -190,37 +203,65 @@ void OBCameraNode::setupDevices() {
}
if (filter_name == "DecimationFilter" && enable_decimation_filter_) {
auto decimation_filter = filter->as<ob::DecimationFilter>();
decimation_filter->setScaleValue(decimation_filter_scale_range_);
if (decimation_filter_scale_range_ != -1) {
decimation_filter->setScaleValue(decimation_filter_scale_range_);
}
} else if (filter_name == "ThresholdFilter" && enable_threshold_filter_) {
auto threshold_filter = filter->as<ob::ThresholdFilter>();
threshold_filter->setValueRange(threshold_filter_min_, threshold_filter_max_);
if (threshold_filter_min_ != -1 && threshold_filter_max_ != -1) {
threshold_filter->setValueRange(threshold_filter_min_, threshold_filter_max_);
}
} else if (filter_name == "SpatialAdvancedFilter" && enable_spatial_filter_) {
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);
if (spatial_filter_alpha_ != -1.0 && spatial_filter_magnitude_ != -1 &&
spatial_filter_radius_ != -1 && spatial_filter_diff_threshold_ != -1) {
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" && enable_temporal_filter_) {
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") {
if (temporal_filter_diff_threshold_ != -1 && temporal_filter_weight_ != -1) {
temporal_filter->setDiffScale(temporal_filter_diff_threshold_);
temporal_filter->setWeight(temporal_filter_weight_);
}
} else if (filter_name == "HoleFillingFilter" && enable_hole_filling_filter_ &&
!hole_filling_filter_mode_.empty()) {
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" && enable_sequenced_filter_) {
auto sequenced_filter = filter->as<ob::SequenceIdFilter>();
sequenced_filter->selectSequenceId(sequence_id_filter_id_);
if (sequence_id_filter_id_ != -1) {
sequenced_filter->selectSequenceId(sequence_id_filter_id_);
}
} else if (filter_name == "NoiseRemovalFilter" && enable_noise_removal_filter_) {
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 if (filter_name == "HDRMerge") {
// do nothing
if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) {
params.disp_diff = noise_removal_filter_min_diff_;
params.max_size = noise_removal_filter_max_size_;
noise_removal_filter->setFilterParams(params);
}
} else if (filter_name == "HDRMerge" && enable_hdr_merge_) {
auto hdr_merge_filter = filter->as<ob::HdrMerge>();
OBHdrConfig hdr_config{};
if (hdr_merge_exposure_1_ != -1 && hdr_merge_exposure_2_ != -1 && hdr_merge_gain_1_ != -1 &&
hdr_merge_gain_2_ != -1) {
hdr_config.exposure_1 = hdr_merge_exposure_1_;
hdr_config.exposure_2 = hdr_merge_exposure_2_;
hdr_config.gain_1 = hdr_merge_gain_1_;
hdr_config.gain_2 = hdr_merge_gain_2_;
ROS_INFO_STREAM("set HDRMerge exposure_1 to " << hdr_merge_exposure_1_);
ROS_INFO_STREAM("set HDRMerge exposure_2 to " << hdr_merge_exposure_2_);
ROS_INFO_STREAM("set HDRMerge gain_1 to " << hdr_merge_gain_1_);
ROS_INFO_STREAM("set HDRMerge gain_2 to " << hdr_merge_gain_2_);
device_->setStructuredData(OB_STRUCT_DEPTH_HDR_CONFIG, &hdr_config, sizeof(OBHdrConfig));
}

} else {
ROS_INFO_STREAM("Skip setting " << filter_name);
}
Expand All @@ -236,7 +277,6 @@ void OBCameraNode::setupDevices() {
if (device_->isPropertySupported(OB_PROP_IR_LONG_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(OB_PROP_IR_LONG_EXPOSURE_BOOL, enable_ir_long_exposure_);
}

} catch (const ob::Error& e) {
ROS_ERROR_STREAM("Failed to setup devices: " << e.getMessage());
} catch (const std::exception& e) {
Expand Down

0 comments on commit 25cf0c7

Please sign in to comment.