mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-09-11 02:40:18 +08:00
chore: extract setupDepthPostProcessFilter()
This commit is contained in:
Joe Dong
@@ -235,119 +235,6 @@ void OBCameraNode::setupDevices() {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to load device preset");
|
||||
}
|
||||
}
|
||||
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" && enable_decimation_filter_) {
|
||||
auto decimation_filter = filter->as<ob::DecimationFilter>();
|
||||
auto range = decimation_filter->getScaleRange();
|
||||
if (decimation_filter_scale_ != -1 && decimation_filter_scale_ < range.max &&
|
||||
decimation_filter_scale_ > range.min) {
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"Set decimation filter scale value to " << decimation_filter_scale_);
|
||||
decimation_filter->setScaleValue(decimation_filter_scale_);
|
||||
}
|
||||
if (decimation_filter_scale_ != -1 &&
|
||||
(decimation_filter_scale_ < range.min || decimation_filter_scale_ > range.max)) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Decimation filter scale value is out of range "
|
||||
<< range.min << " - " << range.max);
|
||||
}
|
||||
} else if (filter_name == "ThresholdFilter" && enable_threshold_filter_) {
|
||||
auto threshold_filter = filter->as<ob::ThresholdFilter>();
|
||||
if (threshold_filter_min_ != -1 && threshold_filter_max_ != -1) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Set threshold filter value range to "
|
||||
<< threshold_filter_min_ << " - " << threshold_filter_max_);
|
||||
threshold_filter->setValueRange(threshold_filter_min_, threshold_filter_max_);
|
||||
}
|
||||
} else if (filter_name == "SpatialAdvancedFilter" && enable_spatial_filter_) {
|
||||
auto spatial_filter = filter->as<ob::SpatialAdvancedFilter>();
|
||||
if (spatial_filter_alpha_ != -1.0 && spatial_filter_magnitude_ != -1 &&
|
||||
spatial_filter_radius_ != -1 && spatial_filter_diff_threshold_ != -1) {
|
||||
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" && enable_temporal_filter_) {
|
||||
auto temporal_filter = filter->as<ob::TemporalFilter>();
|
||||
if (temporal_filter_diff_threshold_ != -1.0 && temporal_filter_weight_ != -1.0) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Set temporal filter value to "
|
||||
<< temporal_filter_diff_threshold_ << " - "
|
||||
<< temporal_filter_weight_);
|
||||
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>();
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"Default hole filling filter mode: " << hole_filling_filter_mode_);
|
||||
OBHoleFillingMode hole_filling_mode = holeFillingModeFromString(hole_filling_filter_mode_);
|
||||
hole_filling_filter->setFilterMode(hole_filling_mode);
|
||||
} else if (filter_name == "SequenceIdFilter" && enable_sequence_id_filter_) {
|
||||
auto sequenced_filter = filter->as<ob::SequenceIdFilter>();
|
||||
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 = noise_removal_filter->getFilterParams();
|
||||
RCLCPP_INFO_STREAM(
|
||||
logger_, "Default noise removal filter params: " << "disp_diff: " << params.disp_diff
|
||||
<< ", max_size: " << params.max_size);
|
||||
params.disp_diff = noise_removal_filter_min_diff_;
|
||||
params.max_size = noise_removal_filter_max_size_;
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"Set noise removal filter params: " << "disp_diff: " << params.disp_diff
|
||||
<< ", max_size: " << params.max_size);
|
||||
if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) {
|
||||
noise_removal_filter->setFilterParams(params);
|
||||
}
|
||||
} else if (filter_name == "HDRMerge" && enable_hdr_merge_) {
|
||||
if (hdr_merge_exposure_1_ != -1 && hdr_merge_gain_1_ != -1 && hdr_merge_exposure_2_ != -1 &&
|
||||
hdr_merge_gain_2_ != -1) {
|
||||
auto hdr_merge_filter = filter->as<ob::HdrMerge>();
|
||||
hdr_merge_filter->enable(true);
|
||||
RCLCPP_INFO_STREAM(
|
||||
logger_, "Set HDR merge filter params: " << "exposure_1: " << hdr_merge_exposure_1_
|
||||
<< ", gain_1: " << hdr_merge_gain_1_
|
||||
<< ", exposure_2: " << hdr_merge_exposure_2_
|
||||
<< ", gain_2: " << hdr_merge_gain_2_);
|
||||
auto config = OBHdrConfig();
|
||||
config.enable = true;
|
||||
config.exposure_1 = hdr_merge_exposure_1_;
|
||||
config.gain_1 = hdr_merge_gain_1_;
|
||||
config.exposure_2 = hdr_merge_exposure_2_;
|
||||
config.gain_2 = hdr_merge_gain_2_;
|
||||
device_->setStructuredData(OB_STRUCT_DEPTH_HDR_CONFIG, &config, sizeof(config));
|
||||
}
|
||||
} else {
|
||||
RCLCPP_INFO_STREAM(logger_, "Skip setting filter: " << filter_name);
|
||||
}
|
||||
}
|
||||
if (!depth_work_mode_.empty()) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Set depth work mode: " << depth_work_mode_);
|
||||
device_->switchDepthWorkMode(depth_work_mode_.c_str());
|
||||
@@ -530,6 +417,122 @@ void OBCameraNode::setupDevices() {
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::setupDepthPostProcessFilter() {
|
||||
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" && enable_decimation_filter_) {
|
||||
auto decimation_filter = filter->as<ob::DecimationFilter>();
|
||||
auto range = decimation_filter->getScaleRange();
|
||||
if (decimation_filter_scale_ != -1 && decimation_filter_scale_ < range.max &&
|
||||
decimation_filter_scale_ > range.min) {
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"Set decimation filter scale value to " << decimation_filter_scale_);
|
||||
decimation_filter->setScaleValue(decimation_filter_scale_);
|
||||
}
|
||||
if (decimation_filter_scale_ != -1 &&
|
||||
(decimation_filter_scale_ < range.min || decimation_filter_scale_ > range.max)) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Decimation filter scale value is out of range "
|
||||
<< range.min << " - " << range.max);
|
||||
}
|
||||
} else if (filter_name == "ThresholdFilter" && enable_threshold_filter_) {
|
||||
auto threshold_filter = filter->as<ob::ThresholdFilter>();
|
||||
if (threshold_filter_min_ != -1 && threshold_filter_max_ != -1) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Set threshold filter value range to "
|
||||
<< threshold_filter_min_ << " - " << threshold_filter_max_);
|
||||
threshold_filter->setValueRange(threshold_filter_min_, threshold_filter_max_);
|
||||
}
|
||||
} else if (filter_name == "SpatialAdvancedFilter" && enable_spatial_filter_) {
|
||||
auto spatial_filter = filter->as<ob::SpatialAdvancedFilter>();
|
||||
if (spatial_filter_alpha_ != -1.0 && spatial_filter_magnitude_ != -1 &&
|
||||
spatial_filter_radius_ != -1 && spatial_filter_diff_threshold_ != -1) {
|
||||
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" && enable_temporal_filter_) {
|
||||
auto temporal_filter = filter->as<ob::TemporalFilter>();
|
||||
if (temporal_filter_diff_threshold_ != -1.0 && temporal_filter_weight_ != -1.0) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Set temporal filter value to "
|
||||
<< temporal_filter_diff_threshold_ << " - "
|
||||
<< temporal_filter_weight_);
|
||||
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>();
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"Default hole filling filter mode: " << hole_filling_filter_mode_);
|
||||
OBHoleFillingMode hole_filling_mode = holeFillingModeFromString(hole_filling_filter_mode_);
|
||||
hole_filling_filter->setFilterMode(hole_filling_mode);
|
||||
} else if (filter_name == "SequenceIdFilter" && enable_sequence_id_filter_) {
|
||||
auto sequenced_filter = filter->as<ob::SequenceIdFilter>();
|
||||
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 = noise_removal_filter->getFilterParams();
|
||||
RCLCPP_INFO_STREAM(
|
||||
logger_, "Default noise removal filter params: " << "disp_diff: " << params.disp_diff
|
||||
<< ", max_size: " << params.max_size);
|
||||
params.disp_diff = noise_removal_filter_min_diff_;
|
||||
params.max_size = noise_removal_filter_max_size_;
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"Set noise removal filter params: " << "disp_diff: " << params.disp_diff
|
||||
<< ", max_size: " << params.max_size);
|
||||
if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) {
|
||||
noise_removal_filter->setFilterParams(params);
|
||||
}
|
||||
} else if (filter_name == "HDRMerge" && enable_hdr_merge_) {
|
||||
if (hdr_merge_exposure_1_ != -1 && hdr_merge_gain_1_ != -1 && hdr_merge_exposure_2_ != -1 &&
|
||||
hdr_merge_gain_2_ != -1) {
|
||||
auto hdr_merge_filter = filter->as<ob::HdrMerge>();
|
||||
hdr_merge_filter->enable(true);
|
||||
RCLCPP_INFO_STREAM(
|
||||
logger_, "Set HDR merge filter params: " << "exposure_1: " << hdr_merge_exposure_1_
|
||||
<< ", gain_1: " << hdr_merge_gain_1_
|
||||
<< ", exposure_2: " << hdr_merge_exposure_2_
|
||||
<< ", gain_2: " << hdr_merge_gain_2_);
|
||||
auto config = OBHdrConfig();
|
||||
config.enable = true;
|
||||
config.exposure_1 = hdr_merge_exposure_1_;
|
||||
config.gain_1 = hdr_merge_gain_1_;
|
||||
config.exposure_2 = hdr_merge_exposure_2_;
|
||||
config.gain_2 = hdr_merge_gain_2_;
|
||||
device_->setStructuredData(OB_STRUCT_DEPTH_HDR_CONFIG, &config, sizeof(config));
|
||||
}
|
||||
} else {
|
||||
RCLCPP_INFO_STREAM(logger_, "Skip setting filter: " << filter_name);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::selectBaseStream() {
|
||||
if (enable_stream_[DEPTH]) {
|
||||
base_stream_ = DEPTH;
|
||||
@@ -1046,6 +1049,7 @@ void OBCameraNode::getParameters() {
|
||||
void OBCameraNode::setupTopics() {
|
||||
getParameters();
|
||||
setupDevices();
|
||||
setupDepthPostProcessFilter();
|
||||
setupProfiles();
|
||||
selectBaseStream();
|
||||
setupCameraCtrlServices();
|
||||
|
||||
Reference in New Issue
Block a user