Skip to content

Commit e6483bc

Browse files
jian-dongjian-dong
committed
fixed color point cloud
1 parent b46e4b5 commit e6483bc

File tree

3 files changed

+31
-39
lines changed

3 files changed

+31
-39
lines changed

‎orbbec_camera/SDK/lib/x64/libOrbbecSDK.so‎

-4.16 KB
Binary file not shown.

‎orbbec_camera/include/orbbec_camera/ob_camera_node.h‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -511,7 +511,6 @@ class OBCameraNode {
511511

512512
bool ordered_pc_ = false;
513513
bool enable_depth_scale_ = true;
514-
std::shared_ptr<ob::Frame> depth_frame_ = nullptr;
515514
std::string device_preset_ = "Default";
516515
// filter switch
517516
bool enable_decimation_filter_ = false;

‎orbbec_camera/src/ob_camera_node.cpp‎

Lines changed: 31 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -552,14 +552,14 @@ void OBCameraNode::setupDepthPostProcessFilter() {
552552
} else if (filter_name == "NoiseRemovalFilter" && enable_noise_removal_filter_) {
553553
auto noise_removal_filter = filter->as<ob::NoiseRemovalFilter>();
554554
OBNoiseRemovalFilterParams params = noise_removal_filter->getFilterParams();
555-
RCLCPP_INFO_STREAM(
556-
logger_, "Default noise removal filter params: " << "disp_diff: " << params.disp_diff
557-
<< ", max_size: " << params.max_size);
555+
RCLCPP_INFO_STREAM(logger_, "Default noise removal filter params: "
556+
<< "disp_diff: " << params.disp_diff
557+
<< ", max_size: " << params.max_size);
558558
params.disp_diff = noise_removal_filter_min_diff_;
559559
params.max_size = noise_removal_filter_max_size_;
560-
RCLCPP_INFO_STREAM(logger_,
561-
"Set noise removal filter params: " << "disp_diff: " << params.disp_diff
562-
<< ", max_size: " << params.max_size);
560+
RCLCPP_INFO_STREAM(logger_, "Set noise removal filter params: "
561+
<< "disp_diff: " << params.disp_diff
562+
<< ", max_size: " << params.max_size);
563563
if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) {
564564
noise_removal_filter->setFilterParams(params);
565565
}
@@ -568,11 +568,11 @@ void OBCameraNode::setupDepthPostProcessFilter() {
568568
hdr_merge_gain_2_ != -1) {
569569
auto hdr_merge_filter = filter->as<ob::HdrMerge>();
570570
hdr_merge_filter->enable(true);
571-
RCLCPP_INFO_STREAM(
572-
logger_, "Set HDR merge filter params: " << "exposure_1: " << hdr_merge_exposure_1_
573-
<< ", gain_1: " << hdr_merge_gain_1_
574-
<< ", exposure_2: " << hdr_merge_exposure_2_
575-
<< ", gain_2: " << hdr_merge_gain_2_);
571+
RCLCPP_INFO_STREAM(logger_, "Set HDR merge filter params: "
572+
<< "exposure_1: " << hdr_merge_exposure_1_
573+
<< ", gain_1: " << hdr_merge_gain_1_
574+
<< ", exposure_2: " << hdr_merge_exposure_2_
575+
<< ", gain_2: " << hdr_merge_gain_2_);
576576
auto config = OBHdrConfig();
577577
config.enable = true;
578578
config.exposure_1 = hdr_merge_exposure_1_;
@@ -1389,15 +1389,11 @@ void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_
13891389
void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
13901390
(void)frame_set;
13911391
if (!depth_cloud_pub_ || depth_cloud_pub_->get_subscription_count() == 0 ||
1392-
!enable_point_cloud_ || !depth_frame_) {
1392+
!enable_point_cloud_) {
13931393
return;
13941394
}
13951395
std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
1396-
if (!depth_frame_) {
1397-
RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
1398-
return;
1399-
}
1400-
auto depth_frame = depth_frame_->as<ob::DepthFrame>();
1396+
auto depth_frame = frame_set->depthFrame();
14011397
if (!depth_frame) {
14021398
RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
14031399
return;
@@ -1490,16 +1486,12 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
14901486
void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
14911487
if (!depth_registration_cloud_pub_ ||
14921488
depth_registration_cloud_pub_->get_subscription_count() == 0 ||
1493-
!enable_colored_point_cloud_ || !depth_frame_) {
1489+
!enable_colored_point_cloud_ || !frame_set) {
14941490
return;
14951491
}
1496-
CHECK_NOTNULL(depth_frame_.get());
14971492
std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
1498-
if (!depth_frame_) {
1499-
RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
1500-
return;
1501-
}
1502-
auto depth_frame = depth_frame_->as<ob::DepthFrame>();
1493+
1494+
auto depth_frame = frame_set->depthFrame();
15031495
auto color_frame = frame_set->colorFrame();
15041496

15051497
if (!depth_frame || !color_frame) {
@@ -1526,11 +1518,10 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
15261518
}
15271519

15281520
color_point_cloud_filter_.setCameraParam(camera_params);
1529-
double depth_scale = depth_frame->getValueScale();
1521+
auto depth_scale = depth_frame->getValueScale();
15301522
color_point_cloud_filter_.setPositionDataScaled(depth_scale);
1531-
auto alignedFrameset = align_filter_->process(frame_set);
15321523
color_point_cloud_filter_.setCreatePointFormat(OB_FORMAT_RGB_POINT);
1533-
auto result_frame = color_point_cloud_filter_.process(alignedFrameset);
1524+
auto result_frame = color_point_cloud_filter_.process(frame_set);
15341525
if (!result_frame) {
15351526
RCLCPP_ERROR_STREAM(logger_, "Failed to process depth frame");
15361527
return;
@@ -1665,25 +1656,27 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
16651656
publishStaticTransforms();
16661657
tf_published_ = true;
16671658
}
1668-
depth_frame_ = frame_set->getFrame(OB_FRAME_DEPTH);
1659+
auto depth_frame = frame_set->getFrame(OB_FRAME_DEPTH);
16691660
bool depth_laser_status = false;
1670-
if (depth_frame_ && depth_frame_->hasMetadata(OB_FRAME_METADATA_TYPE_LASER_STATUS)) {
1671-
depth_laser_status = depth_frame_->getMetadataValue(OB_FRAME_METADATA_TYPE_LASER_STATUS) == 1;
1661+
if (depth_frame && depth_frame->hasMetadata(OB_FRAME_METADATA_TYPE_LASER_STATUS)) {
1662+
depth_laser_status = depth_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_LASER_STATUS) == 1;
16721663
}
16731664

16741665
auto device_info = device_->getDeviceInfo();
16751666
CHECK_NOTNULL(device_info.get());
16761667
auto pid = device_info->getPid();
16771668
auto color_frame = frame_set->getFrame(OB_FRAME_COLOR);
1678-
// has_first_color_frame_ = has_first_color_frame_ || color_frame;
16791669
if (isGemini335PID(pid)) {
1680-
depth_frame_ = processDepthFrameFilter(depth_frame_);
1681-
if (depth_registration_ && align_filter_ && depth_frame_ && color_frame) {
1682-
auto new_frame = align_filter_->process(frame_set);
1683-
if (new_frame) {
1670+
bool depth_aligned = false;
1671+
depth_frame = processDepthFrameFilter(depth_frame);
1672+
if(depth_frame)
1673+
{ frame_set->pushFrame(depth_frame);}
1674+
if (depth_registration_ && align_filter_ && depth_frame && color_frame) {
1675+
if (auto new_frame = align_filter_->process(frame_set)) {
16841676
auto new_frame_set = new_frame->as<ob::FrameSet>();
16851677
CHECK_NOTNULL(new_frame_set.get());
1686-
depth_frame_ = new_frame_set->getFrame(OB_FRAME_DEPTH);
1678+
frame_set = new_frame_set;
1679+
depth_aligned = true;
16871680
} else {
16881681
RCLCPP_ERROR(logger_, "Failed to align depth frame to color frame");
16891682
return;
@@ -1693,7 +1686,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
16931686
"Depth registration is disabled or align filter is null or depth frame is "
16941687
"null or color frame is null");
16951688
}
1696-
if (depth_registration_ && align_filter_ && depth_frame_ && !color_frame) {
1689+
if (depth_registration_ && align_filter_ && !depth_aligned) {
16971690
return;
16981691
}
16991692
}
@@ -1719,7 +1712,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
17191712
continue;
17201713
}
17211714
if (stream_index == DEPTH) {
1722-
frame = (enable_3d_reconstruction_mode_ && !depth_laser_status) ? nullptr : depth_frame_;
1715+
frame = (enable_3d_reconstruction_mode_ && !depth_laser_status) ? nullptr : frame;
17231716
}
17241717
auto is_ir_frame = frame_type == OB_FRAME_IR_LEFT || frame_type == OB_FRAME_IR_RIGHT ||
17251718
frame_type == OB_FRAME_IR;

0 commit comments

Comments
 (0)