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

Fixed filter params

Browse Source
This commit is contained in:
Joe Dong
2024-05-14 14:36:22 +08:00
parent c02367f6cc
commit 4249cd126e
2 changed files with 16 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/launch/gemini_330_series.launch.py
+2 -2
View File
@@ -66,7 +66,7 @@ def generate_launch_description():
DeclareLaunchArgument('color_info_url', default_value=''), DeclareLaunchArgument('color_info_url', default_value=''),
DeclareLaunchArgument('log_level', default_value='none'), DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='true'),
DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
@@ -93,7 +93,7 @@ def generate_launch_description():
DeclareLaunchArgument('sequence_id_filter_id', default_value='1'), DeclareLaunchArgument('sequence_id_filter_id', default_value='1'),
DeclareLaunchArgument('threshold_filter_max', default_value='16000'), DeclareLaunchArgument('threshold_filter_max', default_value='16000'),
DeclareLaunchArgument('threshold_filter_min', default_value='0'), DeclareLaunchArgument('threshold_filter_min', default_value='0'),
DeclareLaunchArgument('noise_removal_filter_min_diff', default_value='8'), DeclareLaunchArgument('noise_removal_filter_min_diff', default_value='256'),
DeclareLaunchArgument('noise_removal_filter_max_size', default_value='80'), DeclareLaunchArgument('noise_removal_filter_max_size', default_value='80'),
DeclareLaunchArgument('spatial_filter_alpha', default_value='0.5'), DeclareLaunchArgument('spatial_filter_alpha', default_value='0.5'),
DeclareLaunchArgument('spatial_filter_diff_threshold', default_value='8'), DeclareLaunchArgument('spatial_filter_diff_threshold', default_value='8'),
orbbec_camera/src/ob_camera_node.cpp
+14 -14
View File
@@ -1139,18 +1139,8 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
depth_height, color_width, color_height); depth_height, color_width, color_height);
return; return;
} }
auto device_info = device_->getDeviceInfo(); auto camera_params = pipeline_->getCameraParam();
CHECK_NOTNULL(device_info); auto intrinsics = camera_params.rgbIntrinsic;
auto pid = device_info->pid();
OBCameraIntrinsic intrinsics;
if (isGemini335PID(pid)) {
auto color_profile = stream_profile_[COLOR]->as<ob::VideoStreamProfile>();
CHECK_NOTNULL(color_profile.get());
intrinsics = color_profile->getIntrinsic();
} else {
auto camera_params = pipeline_->getCameraParam();
intrinsics = camera_params.rgbIntrinsic;
}
float fdx = intrinsics.fx * ((float)(color_width) / intrinsics.width); float fdx = intrinsics.fx * ((float)(color_width) / intrinsics.width);
float fdy = intrinsics.fy * ((float)(color_height) / intrinsics.height); float fdy = intrinsics.fy * ((float)(color_height) / intrinsics.height);
fdx = 1 / fdx; fdx = 1 / fdx;
@@ -1255,7 +1245,8 @@ std::shared_ptr<ob::Frame> OBCameraNode::processDepthFrameFilter(
for (size_t i = 0; i < filter_list->count(); i++) { for (size_t i = 0; i < filter_list->count(); i++) {
auto filter = filter_list->getFilter(i); auto filter = filter_list->getFilter(i);
CHECK_NOTNULL(filter.get()); CHECK_NOTNULL(filter.get());
if (filter->isEnabled() && frame != nullptr && frame != nullptr) { if (filter->isEnabled() && frame != nullptr) {
RCLCPP_INFO_STREAM(logger_, "Processing depth frame with filter: " << filter->type());
frame = filter->process(frame); frame = filter->process(frame);
if (frame == nullptr) { if (frame == nullptr) {
RCLCPP_ERROR_STREAM(logger_, "Depth filter process failed"); RCLCPP_ERROR_STREAM(logger_, "Depth filter process failed");
@@ -1288,15 +1279,24 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
auto pid = device_info->pid(); auto pid = device_info->pid();
auto color_frame = frame_set->getFrame(OB_FRAME_COLOR); auto color_frame = frame_set->getFrame(OB_FRAME_COLOR);
if (isGemini335PID(pid)) { if (isGemini335PID(pid)) {
if (depth_frame_) {
auto new_depth_frame = processDepthFrameFilter(depth_frame_);
if (new_depth_frame && frame_set->getFrame(OB_FRAME_DEPTH) &&
frame_set->getFrame(OB_FRAME_DEPTH)->data()) {
memcpy(frame_set->getFrame(OB_FRAME_DEPTH)->data(), new_depth_frame->data(),
new_depth_frame->dataSize());
}
}
if (depth_registration_ && align_filter_ && depth_frame_ && color_frame) { if (depth_registration_ && align_filter_ && depth_frame_ && color_frame) {
auto new_frame = align_filter_->process(frame_set); auto new_frame = align_filter_->process(frame_set);
if (new_frame) { if (new_frame) {
auto new_frame_set = new_frame->as<ob::FrameSet>(); auto new_frame_set = new_frame->as<ob::FrameSet>();
CHECK_NOTNULL(new_frame_set.get()); CHECK_NOTNULL(new_frame_set.get());
depth_frame_ = new_frame_set->getFrame(OB_FRAME_DEPTH); depth_frame_ = new_frame_set->getFrame(OB_FRAME_DEPTH);
} else {
RCLCPP_ERROR(logger_, "Failed to align depth frame to color frame");
} }
} }
depth_frame_ = processDepthFrameFilter(depth_frame_);
} }
if (enable_stream_[COLOR] && color_frame) { if (enable_stream_[COLOR] && color_frame) {
std::unique_lock<std::mutex> lock(color_frame_queue_lock_); std::unique_lock<std::mutex> lock(color_frame_queue_lock_);