i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 03:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

add filter params

Browse Source
This commit is contained in:
Joe Dong
2024-04-07 15:30:59 +08:00
parent 9a1c6cdec6
commit 9b6b90d9cb
5 changed files with 169 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+89
View File
@@ -142,6 +142,66 @@ void OBCameraNode::setupDevices() {
}
}
try {
device_->loadPreset(device_preset_.c_str());
auto depth_sensor = device_->getSensor(OB_SENSOR_DEPTH);
// set depth sensor to filter
auto filter_list = depth_sensor->getRecommendedFilters();
for (size_t i = 0; i < filter_list->count(); i++) {
auto filter = filter_list->getFilter(i);
std::map<std::string, bool> filter_params = {
{"DecimationFilter", enable_decimation_filter_},
{"HdrMerge", enable_hdr_merge_},
{"SequencedFilter", enable_sequence_id_filter_},
{"ThresholdFilter", enable_threshold_filter_},
{"NoiseRemovalFilter", enable_noise_removal_filter_},
{"SpatialAdvancedFilter", enable_spatial_filter_},
{"TemporalFilter", enable_temporal_filter_},
{"HoleFillingFilter", enable_hole_filling_filter_},
};
std::string filter_name = filter->type();
RCLCPP_INFO_STREAM(logger_, "Setting " << filter_name << "......");
if (filter_params.find(filter_name) != filter_params.end()) {
std::string value = filter_params[filter_name] ? "true" : "false";
RCLCPP_INFO_STREAM(logger_, "set " << filter_name << " to " << value);
filter->enable(filter_params[filter_name]);
filter_status_[filter_name] = filter_params[filter_name];
}
if (filter_name == "DecimationFilter") {
auto decimation_filter = filter->as<ob::DecimationFilter>();
decimation_filter->setScaleValue(decimation_filter_scale_range_);
} else if (filter_name == "ThresholdFilter") {
auto threshold_filter = filter->as<ob::ThresholdFilter>();
threshold_filter->setValueRange(threshold_filter_min_, threshold_filter_max_);
} else if (filter_name == "SpatialAdvancedFilter") {
auto spatial_filter = filter->as<ob::SpatialAdvancedFilter>();
OBSpatialAdvancedFilterParams params{};
params.alpha = spatial_filter_alpha_;
params.magnitude = spatial_filter_magnitude_;
params.radius = spatial_filter_radius_;
params.disp_diff = spatial_filter_diff_threshold_;
spatial_filter->setFilterParams(params);
} else if (filter_name == "TemporalFilter") {
auto temporal_filter = filter->as<ob::TemporalFilter>();
temporal_filter->setDiffScale(temporal_filter_diff_threshold_);
temporal_filter->setWeight(temporal_filter_weight_);
} else if (filter_name == "HoleFillingFilter") {
auto hole_filling_filter = filter->as<ob::HoleFillingFilter>();
OBHoleFillingMode hole_filling_mode = holeFillingModeFromString(hole_filling_filter_mode_);
hole_filling_filter->setFilterMode(hole_filling_mode);
} else if (filter_name == "SequenceIdFilter") {
auto sequenced_filter = filter->as<ob::SequenceIdFilter>();
sequenced_filter->selectSequenceId(sequence_id_filter_id_);
} else if (filter_name == "NoiseRemovalFilter") {
auto noise_removal_filter = filter->as<ob::NoiseRemovalFilter>();
OBNoiseRemovalFilterParams params{};
params.disp_diff = noise_removal_filter_min_diff_;
params.max_size = noise_removal_filter_max_size_;
noise_removal_filter->setFilterParams(params);
} else {
RCLCPP_ERROR_STREAM(logger_, "Unsupported filter: " << filter_name);
}
}
if (!depth_work_mode_.empty()) {
device_->switchDepthWorkMode(depth_work_mode_.c_str());
}
@@ -604,6 +664,30 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<int>(max_save_images_count_, "max_save_images_count", 10);
setAndGetNodeParameter<bool>(use_hardware_time_, "use_hardware_time", true);
setAndGetNodeParameter<bool>(enable_depth_scale_, "enable_depth_scale", true);
setAndGetNodeParameter<std::string>(device_preset_, "device_preset", "Default");
setAndGetNodeParameter<bool>(enable_decimation_filter_, "enable_decimation_filter", false);
setAndGetNodeParameter<bool>(enable_hdr_merge_, "enable_hdr_merge", false);
setAndGetNodeParameter<bool>(enable_sequence_id_filter_, "enable_sequence_id_filter", false);
setAndGetNodeParameter<bool>(enable_threshold_filter_, "enable_threshold_filter", false);
setAndGetNodeParameter<bool>(enable_noise_removal_filter_, "enable_noise_removal_filter", true);
setAndGetNodeParameter<bool>(enable_spatial_filter_, "enable_spatial_filter", true);
setAndGetNodeParameter<bool>(enable_temporal_filter_, "enable_temporal_filter", false);
setAndGetNodeParameter<bool>(enable_hole_filling_filter_, "enable_hole_filling_filter", false);
setAndGetNodeParameter<int>(decimation_filter_scale_range_, "decimation_filter_scale_range", 2);
setAndGetNodeParameter<int>(sequence_id_filter_id_, "sequence_id_filter_id", 1);
setAndGetNodeParameter<int>(threshold_filter_max_, "threshold_filter_max", 16000);
setAndGetNodeParameter<int>(threshold_filter_min_, "threshold_filter_min", 0);
setAndGetNodeParameter<int>(noise_removal_filter_min_diff_, "noise_removal_filter_min_diff", 8);
setAndGetNodeParameter<int>(noise_removal_filter_max_size_, "noise_removal_filter_max_size", 80);
setAndGetNodeParameter<float>(spatial_filter_alpha_, "spatial_filter_alpha", 0.5);
setAndGetNodeParameter<int>(spatial_filter_diff_threshold_, "spatial_filter_diff_threshold", 8);
setAndGetNodeParameter<int>(spatial_filter_magnitude_, "spatial_filter_magnitude", 1);
setAndGetNodeParameter<int>(spatial_filter_radius_, "spatial_filter_radius", 1);
setAndGetNodeParameter<float>(temporal_filter_diff_threshold_, "temporal_filter_diff_threshold",
0.1);
setAndGetNodeParameter<float>(temporal_filter_weight_, "temporal_filter_weight", 0.4);
setAndGetNodeParameter<std::string>(hole_filling_filter_mode_, "hole_filling_filter_mode",
"FILL_TOP");
}
void OBCameraNode::setupTopics() {
@@ -727,6 +811,11 @@ void OBCameraNode::setupPublishers() {
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_right_ir", rclcpp::QoS(1).transient_local());
}
filter_status_pub_ = node_->create_publisher<std_msgs::msg::String>(
"depth_filter_status", rclcpp::QoS(1).transient_local());
std_msgs::msg::String msg;
msg.data = filter_status_.dump(2);
filter_status_pub_->publish(msg);
}
void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
orbbec_camera/src/utils.cpp
+11
View File
@@ -662,4 +662,15 @@ std::string metaDataTypeToString(const OBFrameMetadataType &meta_data_type) {
return "unknown";
}
}
OBHoleFillingMode holeFillingModeFromString(const std::string &hole_filling_mode) {
if (hole_filling_mode == "FILL_TOP") {
return OB_HOLE_FILL_TOP;
} else if (hole_filling_mode == "FILL_NEAREST") {
return OB_HOLE_FILL_NEAREST;
} else if (hole_filling_mode == "FILL_FAREST") {
return OB_HOLE_FILL_FAREST;
} else {
return OB_HOLE_FILL_NEAREST;
}
}
} // namespace orbbec_camera