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

Update: Fixed the issue of point cloud distortion in femto bolt

Browse Source
This commit is contained in:
Joe Dong
2024-06-21 19:56:57 +08:00
parent e13545cb60
commit e1b106c626
4 changed files with 241 additions and 200 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+93 -83
View File
@@ -23,6 +23,7 @@
#include <filesystem>
#include <fstream>
#include "diagnostic_msgs/msg/diagnostic_status.hpp"
#include "libobsensor/hpp/Utils.hpp"
#if defined(USE_RK_HW_DECODER)
#include "orbbec_camera/rk_mpp_decoder.h"
@@ -67,6 +68,12 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
if (enable_stream_[COLOR]) {
rgb_buffer_ = new uint8_t[width_[COLOR] * height_[COLOR] * 3];
}
if (enable_colored_point_cloud_ && enable_stream_[DEPTH] && enable_stream_[COLOR]) {
rgb_pint_cloud_buffer_size_ = width_[COLOR] * height_[COLOR] * sizeof(OBColorPoint);
rgb_pint_cloud_buffer_ = new uint8_t[rgb_pint_cloud_buffer_size_];
xy_table_data_size_ = width_[DEPTH] * height_[DEPTH] * 2;
xy_table_data_ = new float[xy_table_data_size_];
}
is_camera_node_initialized_ = true;
}
@@ -85,9 +92,9 @@ void OBCameraNode::setAndGetNodeParameter(
}
}
OBCameraNode::~OBCameraNode() { clean(); }
OBCameraNode::~OBCameraNode() noexcept { clean(); }
void OBCameraNode::clean() {
void OBCameraNode::clean() noexcept {
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBCameraNode");
is_running_.store(false);
@@ -108,6 +115,14 @@ void OBCameraNode::clean() {
delete[] rgb_buffer_;
rgb_buffer_ = nullptr;
}
if (rgb_pint_cloud_buffer_) {
delete[] rgb_pint_cloud_buffer_;
rgb_pint_cloud_buffer_ = nullptr;
}
if (xy_table_data_) {
delete[] xy_table_data_;
xy_table_data_ = nullptr;
}
}
void OBCameraNode::setupDevices() {
@@ -1189,13 +1204,8 @@ void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_
void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
(void)frame_set;
if (!depth_cloud_pub_) {
return;
}
if (depth_cloud_pub_->get_subscription_count() == 0 || !enable_point_cloud_) {
return;
}
if (!depth_frame_) {
if (!depth_cloud_pub_ || depth_cloud_pub_->get_subscription_count() == 0 ||
!enable_point_cloud_ || !depth_frame_) {
return;
}
std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
@@ -1204,19 +1214,21 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
return;
}
CHECK_NOTNULL(pipeline_);
auto camera_params = pipeline_->getCameraParam();
depth_point_cloud_filter_.setCameraParam(camera_params);
float depth_scale = depth_frame->getValueScale();
depth_point_cloud_filter_.setPositionDataScaled(depth_scale);
depth_point_cloud_filter_.setCreatePointFormat(OB_FORMAT_POINT);
auto result_frame = depth_point_cloud_filter_.process(depth_frame);
if (!result_frame) {
RCLCPP_ERROR_STREAM(logger_, "Failed to process depth frame");
return;
}
auto point_size = result_frame->dataSize() / sizeof(OBPoint);
auto *points = static_cast<OBPoint *>(result_frame->data());
auto width = depth_frame->width();
auto height = depth_frame->height();
auto depth_profile = stream_profile_[DEPTH]->as<ob::VideoStreamProfile>();
CHECK_NOTNULL(depth_profile.get());
auto depth_intrinsics = depth_profile->getIntrinsic();
float fdx = depth_intrinsics.fx * ((float)(width) / depth_intrinsics.width);
float fdy = depth_intrinsics.fy * ((float)(height) / depth_intrinsics.height);
fdx = 1 / fdx;
fdy = 1 / fdy;
float u0 = depth_intrinsics.cx * ((float)(width) / depth_intrinsics.width);
float v0 = depth_intrinsics.cy * ((float)(height) / depth_intrinsics.height);
const auto *depth_data = (uint16_t *)depth_frame->data();
auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
modifier.setPointCloud2FieldsByString(1, "xyz");
@@ -1228,28 +1240,19 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
sensor_msgs::PointCloud2Iterator<float> iter_x(*point_cloud_msg, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z");
size_t valid_count = 0;
const static float MIN_DISTANCE = 20.0;
const static float MAX_DISTANCE = 10000.0;
double depth_scale = depth_frame->getValueScale();
const static float MIN_DISTANCE = 20.0; // 2cm
const static float MAX_DISTANCE = 10000.0; // 10m
const static float min_depth = MIN_DISTANCE / depth_scale;
const static float max_depth = MAX_DISTANCE / depth_scale;
for (uint32_t y = 0; y < height; y++) {
for (uint32_t x = 0; x < width; x++) {
bool valid_point = true;
if (depth_data[y * width + x] < min_depth || depth_data[y * width + x] > max_depth) {
valid_point = false;
}
if (valid_point || ordered_pc_) {
float xf = (x - u0) * fdx;
float yf = (y - v0) * fdy;
float zf = depth_data[y * width + x] * depth_scale;
*iter_x = zf * xf / 1000.0;
*iter_y = zf * yf / 1000.0;
*iter_z = zf / 1000.0;
++iter_x, ++iter_y, ++iter_z;
valid_count++;
}
size_t valid_count = 0;
for (size_t i = 0; i < point_size; i++) {
bool valid_point = points[i].z >= min_depth && points[i].z <= max_depth;
if (valid_point || ordered_pc_) {
*iter_x = static_cast<float>(points[i].x / 1000.0);
*iter_y = static_cast<float>(points[i].y / 1000.0);
*iter_z = static_cast<float>(points[i].z / 1000.0);
++iter_x, ++iter_y, ++iter_z;
valid_count++;
}
}
if (valid_count == 0) {
@@ -1288,16 +1291,12 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
}
void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
if (!depth_registration_cloud_pub_) {
return;
}
if (depth_registration_cloud_pub_->get_subscription_count() == 0 ||
!enable_colored_point_cloud_) {
return;
}
if (!depth_frame_) {
if (!depth_registration_cloud_pub_ ||
depth_registration_cloud_pub_->get_subscription_count() == 0 ||
!enable_colored_point_cloud_ || !depth_frame_) {
return;
}
CHECK_NOTNULL(depth_frame_.get());
std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
auto depth_frame = depth_frame_->as<ob::DepthFrame>();
@@ -1314,16 +1313,40 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
depth_height, color_width, color_height);
return;
}
auto camera_params = pipeline_->getCameraParam();
auto intrinsics = camera_params.rgbIntrinsic;
float fdx = intrinsics.fx * ((float)(color_width) / intrinsics.width);
float fdy = intrinsics.fy * ((float)(color_height) / intrinsics.height);
fdx = 1 / fdx;
fdy = 1 / fdy;
float u0 = intrinsics.cx * ((float)(color_width) / intrinsics.width);
float v0 = intrinsics.cy * ((float)(color_height) / intrinsics.height);
const auto *depth_data = (uint16_t *)depth_frame->data();
if (!xy_tables_.has_value()) {
calibration_param_ = pipeline_->getCalibrationParam(pipeline_config_);
uint32_t table_size =
color_width * color_height * 2; // one for x-coordinate and one for y-coordinate LUT
if (xy_table_data_size_ != table_size) {
RCLCPP_INFO_STREAM(logger_, "Init xy tables with size " << table_size);
xy_table_data_size_ = table_size;
delete[] xy_table_data_;
xy_table_data_ = new float[table_size];
}
xy_tables_ = OBXYTables();
CHECK_NOTNULL(xy_table_data_);
if (!ob::CoordinateTransformHelper::transformationInitXYTables(
*calibration_param_, OB_SENSOR_COLOR, xy_table_data_, &table_size, &(*xy_tables_))) {
RCLCPP_ERROR_STREAM(logger_, "Failed to init xy tables");
return;
}
}
const auto *depth_data = (uint8_t *)depth_frame->data();
const auto *color_data = (uint8_t *)(rgb_buffer_);
CHECK_NOTNULL(rgb_pint_cloud_buffer_);
uint32_t point_cloud_buffer_size = color_width * color_height * sizeof(OBColorPoint);
if (point_cloud_buffer_size > rgb_pint_cloud_buffer_size_) {
delete[] rgb_pint_cloud_buffer_;
rgb_pint_cloud_buffer_ = new uint8_t[point_cloud_buffer_size];
rgb_pint_cloud_buffer_size_ = point_cloud_buffer_size;
}
memset(rgb_pint_cloud_buffer_, 0, rgb_pint_cloud_buffer_size_);
auto *point_cloud = (OBColorPoint *)rgb_pint_cloud_buffer_;
ob::CoordinateTransformHelper::transformationDepthToRGBDPointCloud(&(*xy_tables_), depth_data,
color_data, point_cloud);
auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
modifier.setPointCloud2FieldsByString(1, "xyz");
@@ -1347,31 +1370,17 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
double depth_scale = depth_frame->getValueScale();
static float min_depth = MIN_DISTANCE / depth_scale;
static float max_depth = MAX_DISTANCE / depth_scale;
for (uint32_t y = 0; y < color_height; y++) {
for (uint32_t x = 0; x < color_width; x++) {
bool valid_point = true;
float depth = depth_data[y * depth_width + x];
if (depth < min_depth || depth > max_depth) {
valid_point = false;
}
if (valid_point || ordered_pc_) {
float xf = (x - u0) * fdx;
float yf = (y - v0) * fdy;
float zf = depth * depth_scale;
*iter_x = zf * xf / 1000.0;
*iter_y = zf * yf / 1000.0;
*iter_z = zf / 1000.0;
*iter_r = color_data[(y * color_width + x) * 3];
*iter_g = color_data[(y * color_width + x) * 3 + 1];
*iter_b = color_data[(y * color_width + x) * 3 + 2];
++iter_x;
++iter_y;
++iter_z;
++iter_r;
++iter_g;
++iter_b;
++valid_count;
}
for (size_t i = 0; i < color_width * color_height; i++) {
bool valid_point = point_cloud[i].z >= min_depth && point_cloud[i].z <= max_depth;
if (valid_point || ordered_pc_) {
*iter_x = static_cast<float>(point_cloud[i].x / 1000.0);
*iter_y = static_cast<float>(point_cloud[i].y / 1000.0);
*iter_z = static_cast<float>(point_cloud[i].z / 1000.0);
*iter_r = static_cast<uint8_t>(point_cloud[i].r);
*iter_g = static_cast<uint8_t>(point_cloud[i].g);
*iter_b = static_cast<uint8_t>(point_cloud[i].b);
++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b;
++valid_count;
}
}
if (valid_count == 0) {
@@ -1576,7 +1585,8 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
metadata_publishers_[COLOR]->get_subscription_count() > 0) {
has_subscriber = true;
}
if (camera_info_publishers_[COLOR]->get_subscription_count() > 0) {
if (camera_info_publishers_.count(COLOR) &&
camera_info_publishers_[COLOR]->get_subscription_count() > 0) {
has_subscriber = true;
}
bool is_decoded = false;