Skip to content

Commit d0222c3

Browse files
jian-dongjian-dong
committed
Update: Fixed the issue of point cloud distortion in femto bolt
1 parent 16afaeb commit d0222c3

File tree

2 files changed

+93
-76
lines changed

2 files changed

+93
-76
lines changed

‎include/orbbec_camera/ob_camera_node.h

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,7 @@ class OBCameraNode {
6363

6464
OBCameraNode &operator=(OBCameraNode &&) = delete;
6565

66-
~OBCameraNode();
66+
~OBCameraNode() noexcept;
6767

6868
bool isInitialized() const;
6969

@@ -501,6 +501,13 @@ class OBCameraNode {
501501
bool enable_color_hdr_ = false;
502502
int laser_energy_level_ = -1;
503503
bool enable_ldp_ = true;
504+
ob::PointCloudFilter depth_point_cloud_filter_;
505+
boost::optional<OBCalibrationParam> calibration_param_;
506+
boost::optional<OBXYTables> xy_tables_;
507+
float *xy_table_data_ = nullptr;
508+
uint32_t xy_table_data_size_ = 0;
509+
uint8_t *rgb_point_cloud_buffer_ = nullptr;
510+
uint32_t rgb_point_cloud_buffer_size_ = 0;
504511
};
505512

506513
} // namespace orbbec_camera

‎src/ob_camera_node.cpp

Lines changed: 85 additions & 75 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
*******************************************************************************/
1616

1717
#include "orbbec_camera/ob_camera_node.h"
18+
#include "libobsensor/hpp/Utils.hpp"
1819
#if defined(USE_RK_HW_DECODER)
1920
#include "orbbec_camera/rk_mpp_decoder.h"
2021
#elif defined(USE_NV_HW_DECODER)
@@ -64,6 +65,13 @@ void OBCameraNode::init() {
6465
CHECK(width_[COLOR] > 0 && height_[COLOR] > 0);
6566
rgb_buffer_ = new uint8_t[width_[COLOR] * height_[COLOR] * 3];
6667
}
68+
if (enable_colored_point_cloud_ && enable_stream_[COLOR] && enable_stream_[DEPTH]) {
69+
CHECK(width_[COLOR] > 0 && height_[COLOR] > 0);
70+
rgb_point_cloud_buffer_size_ = width_[COLOR] * height_[COLOR] * sizeof(OBColorPoint);
71+
rgb_point_cloud_buffer_ = new uint8_t[rgb_point_cloud_buffer_size_];
72+
xy_table_data_size_ = width_[COLOR] * height_[COLOR] * 2;
73+
xy_table_data_ = new float[xy_table_data_size_];
74+
}
6775
rgb_is_decoded_ = false;
6876
if (diagnostics_frequency_ > 0.0) {
6977
diagnostics_thread_ = std::make_shared<std::thread>([this]() { setupDiagnosticUpdater(); });
@@ -73,7 +81,7 @@ void OBCameraNode::init() {
7381

7482
bool OBCameraNode::isInitialized() const { return is_initialized_; }
7583

76-
OBCameraNode::~OBCameraNode() {
84+
OBCameraNode::~OBCameraNode() noexcept {
7785
ROS_INFO_STREAM("OBCameraNode::~OBCameraNode() start");
7886
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
7987
is_running_ = false;
@@ -94,6 +102,8 @@ OBCameraNode::~OBCameraNode() {
94102
stopStreams();
95103
ROS_INFO_STREAM("OBCameraNode::~OBCameraNode() delete rgb_buffer");
96104
delete[] rgb_buffer_;
105+
delete[] rgb_point_cloud_buffer_;
106+
delete[] xy_table_data_;
97107
ROS_INFO_STREAM("OBCameraNode::~OBCameraNode() end");
98108
}
99109

@@ -532,25 +542,33 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& f
532542
if (!enable_point_cloud_ || depth_cloud_pub_.getNumSubscribers() == 0) {
533543
return;
534544
}
535-
auto depth_frame = frame_set->depthFrame();
545+
if (!depth_frame_) {
546+
return;
547+
}
548+
std::lock_guard<decltype(cloud_mutex_)> cloud_lock(cloud_mutex_);
549+
auto depth_frame = depth_frame_->as<ob::DepthFrame>();
536550
if (!depth_frame) {
537551
ROS_ERROR_STREAM("depth frame is null");
538552
return;
539553
}
540-
std::lock_guard<decltype(cloud_mutex_)> cloud_lock(cloud_mutex_);
554+
CHECK_NOTNULL(pipeline_);
555+
auto camera_params = pipeline_->getCameraParam();
556+
if (depth_registration_) {
557+
camera_params.depthIntrinsic = camera_params.rgbIntrinsic;
558+
}
559+
depth_point_cloud_filter_.setCameraParam(camera_params);
560+
float depth_scale = depth_frame->getValueScale();
561+
depth_point_cloud_filter_.setPositionDataScaled(depth_scale);
562+
depth_point_cloud_filter_.setCreatePointFormat(OB_FORMAT_POINT);
563+
auto result_frame = depth_point_cloud_filter_.process(depth_frame);
564+
if (!result_frame) {
565+
ROS_DEBUG("Failed to create point cloud");
566+
return;
567+
}
568+
auto point_size = result_frame->dataSize() / sizeof(OBPoint);
569+
auto* points = static_cast<OBPoint*>(result_frame->data());
541570
auto width = depth_frame->width();
542571
auto height = depth_frame->height();
543-
auto depth_profile = stream_profile_[DEPTH]->as<ob::VideoStreamProfile>();
544-
CHECK_NOTNULL(depth_profile.get());
545-
auto depth_intrinsics = depth_profile->getIntrinsic();
546-
float fdx = depth_intrinsics.fx * ((float)(width) / static_cast<float>(depth_intrinsics.width));
547-
float fdy = depth_intrinsics.fy * ((float)(height) / static_cast<float>(depth_intrinsics.height));
548-
fdx = 1 / fdx;
549-
fdy = 1 / fdy;
550-
float u0 = depth_intrinsics.cx * ((float)(width) / static_cast<float>(depth_intrinsics.width));
551-
float v0 = depth_intrinsics.cy * ((float)(height) / static_cast<float>(depth_intrinsics.height));
552-
553-
const auto* depth_data = (uint16_t*)depth_frame->data();
554572
sensor_msgs::PointCloud2Modifier modifier(cloud_msg_);
555573
modifier.setPointCloud2FieldsByString(1, "xyz");
556574
modifier.resize(width * height);
@@ -561,28 +579,19 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& f
561579
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg_, "x");
562580
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg_, "y");
563581
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg_, "z");
564-
size_t valid_count = 0;
565582
const static double MIN_DISTANCE = 20.0;
566583
const static double MAX_DISTANCE = 10000.0;
567-
double depth_scale = depth_frame->getValueScale();
568584
const static double min_depth = MIN_DISTANCE / depth_scale;
569585
const static double max_depth = MAX_DISTANCE / depth_scale;
570-
for (int y = 0; y < height; y++) {
571-
for (int x = 0; x < width; x++) {
572-
bool valid_point = true;
573-
if (depth_data[y * width + x] < min_depth || depth_data[y * width + x] > max_depth) {
574-
valid_point = false;
575-
}
576-
if (valid_point || ordered_pc_) {
577-
float xf = (x - u0) * fdx;
578-
float yf = (y - v0) * fdy;
579-
float zf = depth_data[y * width + x] * depth_scale;
580-
*iter_x = zf * xf / 1000.0;
581-
*iter_y = zf * yf / 1000.0;
582-
*iter_z = zf / 1000.0;
583-
++iter_x, ++iter_y, ++iter_z;
584-
valid_count++;
585-
}
586+
size_t valid_count = 0;
587+
for (size_t i = 0; i < point_size; i++) {
588+
bool valid_point = points[i].z >= min_depth && points[i].z <= max_depth;
589+
if (valid_point || ordered_pc_) {
590+
*iter_x = static_cast<float>(points[i].x / 1000.0);
591+
*iter_y = static_cast<float>(points[i].y / 1000.0);
592+
*iter_z = static_cast<float>(points[i].z / 1000.0);
593+
++iter_x, ++iter_y, ++iter_z;
594+
valid_count++;
586595
}
587596
}
588597
if (!ordered_pc_) {
@@ -641,26 +650,41 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>&
641650
color_width, color_height);
642651
return;
643652
}
653+
if (!xy_tables_.has_value()) {
654+
calibration_param_ = pipeline_->getCalibrationParam(pipeline_config_);
644655

645-
OBCameraIntrinsic intrinsics;
646-
CHECK_NOTNULL(device_info_.get());
647-
if (isGemini335PID(device_info_->pid())) {
648-
auto color_profile = stream_profile_[COLOR]->as<ob::VideoStreamProfile>();
649-
CHECK_NOTNULL(color_profile.get());
650-
intrinsics = color_profile->getIntrinsic();
651-
} else {
652-
CHECK_NOTNULL(pipeline_.get());
653-
auto camera_params = pipeline_->getCameraParam();
654-
intrinsics = camera_params.rgbIntrinsic;
655-
}
656-
float fdx = intrinsics.fx * ((float)(color_width) / intrinsics.width);
657-
float fdy = intrinsics.fy * ((float)(color_height) / intrinsics.height);
658-
fdx = 1 / fdx;
659-
fdy = 1 / fdy;
660-
float u0 = intrinsics.cx * ((float)(color_width) / intrinsics.width);
661-
float v0 = intrinsics.cy * ((float)(color_height) / intrinsics.height);
662-
const auto* depth_data = (uint16_t*)depth_frame->data();
656+
uint32_t table_size =
657+
color_width * color_height * 2; // one for x-coordinate and one for y-coordinate LUT
658+
if (xy_table_data_size_ != table_size) {
659+
ROS_INFO_STREAM("Init xy tables with size " << table_size);
660+
xy_table_data_size_ = table_size;
661+
delete[] xy_table_data_;
662+
xy_table_data_ = new float[table_size];
663+
}
664+
665+
xy_tables_ = OBXYTables();
666+
CHECK_NOTNULL(xy_table_data_);
667+
if (!ob::CoordinateTransformHelper::transformationInitXYTables(
668+
*calibration_param_, OB_SENSOR_COLOR, xy_table_data_, &table_size, &(*xy_tables_))) {
669+
ROS_ERROR("Failed to init xy tables");
670+
return;
671+
}
672+
}
673+
674+
const auto* depth_data = (uint8_t*)depth_frame->data();
663675
const auto* color_data = (uint8_t*)(rgb_buffer_);
676+
CHECK_NOTNULL(rgb_point_cloud_buffer_);
677+
uint32_t point_cloud_buffer_size = color_width * color_height * sizeof(OBColorPoint);
678+
if (point_cloud_buffer_size > rgb_point_cloud_buffer_size_) {
679+
delete[] rgb_point_cloud_buffer_;
680+
rgb_point_cloud_buffer_ = new uint8_t[point_cloud_buffer_size];
681+
rgb_point_cloud_buffer_size_ = point_cloud_buffer_size;
682+
}
683+
memset(rgb_point_cloud_buffer_, 0, rgb_point_cloud_buffer_size_);
684+
auto* point_cloud = (OBColorPoint*)rgb_point_cloud_buffer_;
685+
ob::CoordinateTransformHelper::transformationDepthToRGBDPointCloud(&(*xy_tables_), depth_data,
686+
color_data, point_cloud);
687+
664688
sensor_msgs::PointCloud2Modifier modifier(cloud_msg_);
665689
modifier.setPointCloud2FieldsByString(1, "xyz");
666690
cloud_msg_.width = color_frame->width();
@@ -682,31 +706,17 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>&
682706
double depth_scale = depth_frame->getValueScale();
683707
static float min_depth = MIN_DISTANCE / depth_scale;
684708
static float max_depth = MAX_DISTANCE / depth_scale;
685-
for (int y = 0; y < color_height; y++) {
686-
for (int x = 0; x < color_width; x++) {
687-
bool valid_point = true;
688-
float depth = depth_data[y * depth_width + x];
689-
if (depth < min_depth || depth > max_depth) {
690-
valid_point = false;
691-
}
692-
if (valid_point || ordered_pc_) {
693-
float xf = (x - u0) * fdx;
694-
float yf = (y - v0) * fdy;
695-
float zf = depth * depth_scale;
696-
*iter_x = zf * xf / 1000.0;
697-
*iter_y = zf * yf / 1000.0;
698-
*iter_z = zf / 1000.0;
699-
*iter_r = color_data[(y * color_width + x) * 3];
700-
*iter_g = color_data[(y * color_width + x) * 3 + 1];
701-
*iter_b = color_data[(y * color_width + x) * 3 + 2];
702-
++iter_x;
703-
++iter_y;
704-
++iter_z;
705-
++iter_r;
706-
++iter_g;
707-
++iter_b;
708-
++valid_count;
709-
}
709+
for (size_t i = 0; i < color_width * color_height; i++) {
710+
bool valid_point = point_cloud[i].z >= min_depth && point_cloud[i].z <= max_depth;
711+
if (valid_point || ordered_pc_) {
712+
*iter_x = static_cast<float>(point_cloud[i].x / 1000.0);
713+
*iter_y = static_cast<float>(point_cloud[i].y / 1000.0);
714+
*iter_z = static_cast<float>(point_cloud[i].z / 1000.0);
715+
*iter_r = static_cast<uint8_t>(point_cloud[i].r);
716+
*iter_g = static_cast<uint8_t>(point_cloud[i].g);
717+
*iter_b = static_cast<uint8_t>(point_cloud[i].b);
718+
++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b;
719+
++valid_count;
710720
}
711721
}
712722
if (!ordered_pc_) {

0 commit comments

Comments
 (0)