i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-09-13 19:40:19 +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/SDK/include/libobsensor/hpp/Utils.hpp
+137 -115
View File
@@ -6,133 +6,155 @@
#pragma once #pragma once
#include "Types.hpp" #include "Types.hpp"
#include "Frame.hpp"
namespace ob { namespace ob {
class Device; class Device;
class OB_EXTENSION_API CoordinateTransformHelper { class OB_EXTENSION_API CoordinateTransformHelper {
public: public:
/** /**
* @brief Transform a 3d point of a source coordinate system into a 3d point of the target coordinate system. * @brief Transform a 3d point of a source coordinate system into a 3d point of the target
* * coordinate system.
* @param calibrationParam Device calibration param,see pipeline::getCalibrationParam *
* @param sourcePoint3f Source 3d point value * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
* @param sourceSensorType Source sensor type * @param sourcePoint3f Source 3d point value
* @param targetSensorType Target sensor type * @param sourceSensorType Source sensor type
* @param targetPoint3f Target 3d point value * @param targetSensorType Target sensor type
* * @param targetPoint3f Target 3d point value
* @return bool Transform result *
*/ * @return bool Transform result
static bool calibration3dTo3d(const OBCalibrationParam calibrationParam, const OBPoint3f sourcePoint3f, const OBSensorType sourceSensorType, */
const OBSensorType targetSensorType, OBPoint3f *targetPoint3f); static bool calibration3dTo3d(const OBCalibrationParam calibrationParam,
const OBPoint3f sourcePoint3f, const OBSensorType sourceSensorType,
const OBSensorType targetSensorType, OBPoint3f *targetPoint3f);
/** /**
* @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 3d point of the target coordinate system. * @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into
* * a 3d point of the target coordinate system.
* @param calibrationParam Device calibration param,see pipeline::getCalibrationParam *
* @param sourcePoint2f Source 2d point value * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
* @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters * @param sourcePoint2f Source 2d point value
* @param sourceSensorType Source sensor type * @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters
* @param targetSensorType Target sensor type * @param sourceSensorType Source sensor type
* @param targetPoint3f Target 3d point value * @param targetSensorType Target sensor type
* * @param targetPoint3f Target 3d point value
* @return bool Transform result *
*/ * @return bool Transform result
static bool calibration2dTo3d(const OBCalibrationParam calibrationParam, const OBPoint2f sourcePoint2f, const float sourceDepthPixelValue, */
const OBSensorType sourceSensorType, const OBSensorType targetSensorType, OBPoint3f *targetPoint3f); static bool calibration2dTo3d(const OBCalibrationParam calibrationParam,
const OBPoint2f sourcePoint2f, const float sourceDepthPixelValue,
const OBSensorType sourceSensorType,
const OBSensorType targetSensorType, OBPoint3f *targetPoint3f);
/** /**
* @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 3d point of the target coordinate system. * @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into
* @brief This function uses undistortion, which may result in longer processing time. * a 3d point of the target coordinate system.
* * @brief This function uses undistortion, which may result in longer processing time.
* @param calibrationParam Device calibration param,see pipeline::getCalibrationParam *
* @param sourcePoint2f Source 2d point value * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
* @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters * @param sourcePoint2f Source 2d point value
* @param sourceSensorType Source sensor type * @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters
* @param targetSensorType Target sensor type * @param sourceSensorType Source sensor type
* @param targetPoint3f Target 3d point value * @param targetSensorType Target sensor type
* * @param targetPoint3f Target 3d point value
* @return bool Transform result *
*/ * @return bool Transform result
static bool calibration2dTo3dUndistortion(const OBCalibrationParam calibrationParam, const OBPoint2f sourcePoint2f, const float sourceDepthPixelValue, */
const OBSensorType sourceSensorType, const OBSensorType targetSensorType, OBPoint3f *targetPoint3f); static bool calibration2dTo3dUndistortion(const OBCalibrationParam calibrationParam,
const OBPoint2f sourcePoint2f,
const float sourceDepthPixelValue,
const OBSensorType sourceSensorType,
const OBSensorType targetSensorType,
OBPoint3f *targetPoint3f);
/** /**
* @brief Transform a 3d point of a source coordinate system into a 2d pixel coordinate of the target camera. * @brief Transform a 3d point of a source coordinate system into a 2d pixel coordinate of the
* * target camera.
* @param calibrationParam Device calibration param,see pipeline::getCalibrationParam *
* @param sourcePoint3f Source 3d point value * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
* @param sourceSensorType Source sensor type * @param sourcePoint3f Source 3d point value
* @param targetSensorType Target sensor type * @param sourceSensorType Source sensor type
* @param targetPoint2f Target 2d point value * @param targetSensorType Target sensor type
* * @param targetPoint2f Target 2d point value
* @return bool Transform result *
*/ * @return bool Transform result
static bool calibration3dTo2d(const OBCalibrationParam calibrationParam, const OBPoint3f sourcePoint3f, const OBSensorType sourceSensorType, */
const OBSensorType targetSensorType, OBPoint2f *targetPoint2f); static bool calibration3dTo2d(const OBCalibrationParam calibrationParam,
const OBPoint3f sourcePoint3f, const OBSensorType sourceSensorType,
const OBSensorType targetSensorType, OBPoint2f *targetPoint2f);
/** /**
* @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 2d pixel coordinate of the target camera * @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into
* * a 2d pixel coordinate of the target camera
* @param calibrationParam Device calibration param,see pipeline::getCalibrationParam *
* @param sourcePoint2f Source 2d point value * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
* @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters * @param sourcePoint2f Source 2d point value
* @param sourceSensorType Source sensor type * @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters
* @param targetSensorType Target sensor type * @param sourceSensorType Source sensor type
* @param targetPoint2f Target 2d point value * @param targetSensorType Target sensor type
* * @param targetPoint2f Target 2d point value
* @return bool Transform result *
*/ * @return bool Transform result
static bool calibration2dTo2d(const OBCalibrationParam calibrationParam, const OBPoint2f sourcePoint2f, const float sourceDepthPixelValue, */
const OBSensorType sourceSensorType, const OBSensorType targetSensorType, OBPoint2f *targetPoint2f); static bool calibration2dTo2d(const OBCalibrationParam calibrationParam,
const OBPoint2f sourcePoint2f, const float sourceDepthPixelValue,
const OBSensorType sourceSensorType,
const OBSensorType targetSensorType, OBPoint2f *targetPoint2f);
/** /**
* @brief Transforms the depth frame into the geometry of the color camera. * @brief Transforms the depth frame into the geometry of the color camera.
* *
* @param device Device handle * @param device Device handle
* @param depthFrame Input depth frame * @param depthFrame Input depth frame
* @param targetColorCameraWidth Target color camera width * @param targetColorCameraWidth Target color camera width
* @param targetColorCameraHeight Target color camera height * @param targetColorCameraHeight Target color camera height
* *
* @return std::shared_ptr<ob::Frame> Transformed depth frame * @return std::shared_ptr<ob::Frame> Transformed depth frame
*/ */
static std::shared_ptr<ob::Frame> transformationDepthFrameToColorCamera(std::shared_ptr<ob::Device> device, std::shared_ptr<ob::Frame> depthFrame, static std::shared_ptr<ob::Frame> transformationDepthFrameToColorCamera(
uint32_t targetColorCameraWidth, uint32_t targetColorCameraHeight); std::shared_ptr<ob::Device> device, std::shared_ptr<ob::Frame> depthFrame,
uint32_t targetColorCameraWidth, uint32_t targetColorCameraHeight);
/** /**
* @brief Init transformation tables * @brief Init transformation tables
* *
* @param calibrationParam Device calibration param,see pipeline::getCalibrationParam * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
* @param sensorType sensor type * @param sensorType sensor type
* @param data input data,needs to be allocated externally.During initialization, the external allocation size is 'dataSize', for example, dataSize = 1920 * * @param data input data,needs to be allocated externally.During initialization, the external
* 1080 * 2*sizeof(float) (1920 * 1080 represents the image resolution, and 2 represents two LUTs, one for x-coordinate and one for y-coordinate). * allocation size is 'dataSize', for example, dataSize = 1920 * 1080 * 2*sizeof(float) (1920 *
* @param dataSize input data size * 1080 represents the image resolution, and 2 represents two LUTs, one for x-coordinate and one
* @param xyTables output xy tables * for y-coordinate).
* * @param dataSize input data size
* @return bool Transform result * @param xyTables output xy tables
*/ *
static bool transformationInitXYTables(const OBCalibrationParam calibrationParam, const OBSensorType sensorType, float *data, uint32_t *dataSize, * @return bool Transform result
OBXYTables *xyTables); */
static bool transformationInitXYTables(const OBCalibrationParam calibrationParam,
const OBSensorType sensorType, float *data,
uint32_t *dataSize, OBXYTables *xyTables);
/** /**
* @brief Transform depth image to point cloud data * @brief Transform depth image to point cloud data
* *
* @param xyTables input xy tables,see CoordinateTransformHelper::transformationInitXYTables * @param xyTables input xy tables,see CoordinateTransformHelper::transformationInitXYTables
* @param depthImageData input depth image data * @param depthImageData input depth image data
* @param pointCloudData output point cloud data * @param pointCloudData output point cloud data
* *
*/ */
static void transformationDepthToPointCloud(OBXYTables *xyTables, const void *depthImageData, void *pointCloudData); static void transformationDepthToPointCloud(OBXYTables *xyTables, const void *depthImageData,
void *pointCloudData);
/** /**
* @brief Transform depth image to RGBD point cloud data * @brief Transform depth image to RGBD point cloud data
* *
* @param xyTables input xy tables,see CoordinateTransformHelper::transformationInitXYTables * @param xyTables input xy tables,see CoordinateTransformHelper::transformationInitXYTables
* @param depthImageData input depth image data * @param depthImageData input depth image data
* @param colorImageData input color image data (only RGB888 support) * @param colorImageData input color image data (only RGB888 support)
* @param pointCloudData output RGBD point cloud data * @param pointCloudData output RGBD point cloud data
* *
*/ */
static void transformationDepthToRGBDPointCloud(OBXYTables *xyTables, const void *depthImageData, const void *colorImageData, void *pointCloudData); static void transformationDepthToRGBDPointCloud(OBXYTables *xyTables, const void *depthImageData,
const void *colorImageData, void *pointCloudData);
}; };
} // namespace ob } // namespace ob
orbbec_camera/include/orbbec_camera/constants.h
+1
View File
@@ -103,6 +103,7 @@ const float ROS_DEPTH_SCALE = 0.001;
const int32_t FEMTO_OW_PID = 0x0638; const int32_t FEMTO_OW_PID = 0x0638;
const int32_t FEMTO_BOLT_PID = 0x066b; const int32_t FEMTO_BOLT_PID = 0x066b;
const int32_t FEMTO_LIVE_PID = 0x0668; const int32_t FEMTO_LIVE_PID = 0x0668;
const uint32_t FEMTO_MEGA_PID = 0x0669;
const int32_t FEMTO_PID = 0x0635; const int32_t FEMTO_PID = 0x0635;
const int32_t ASTRA_PLUS_PID = 0x0636; const int32_t ASTRA_PLUS_PID = 0x0636;
const int32_t ASTRA_PLUS_S_PID = 0x0637; const int32_t ASTRA_PLUS_S_PID = 0x0637;
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+10 -2
View File
@@ -139,9 +139,9 @@ class OBCameraNode {
const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor = const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor =
rcl_interfaces::msg::ParameterDescriptor()); // set and get parameter rcl_interfaces::msg::ParameterDescriptor()); // set and get parameter
~OBCameraNode(); ~OBCameraNode() noexcept;
void clean(); void clean() noexcept;
void startStreams(); void startStreams();
@@ -538,5 +538,13 @@ class OBCameraNode {
bool retry_on_usb3_detection_failure_ = false; bool retry_on_usb3_detection_failure_ = false;
std::atomic_bool is_camera_node_initialized_{false}; std::atomic_bool is_camera_node_initialized_{false};
int laser_energy_level_ = -1; int laser_energy_level_ = -1;
ob::PointCloudFilter depth_point_cloud_filter_;
ob::PointCloudFilter colored_point_cloud_filter_;
std::optional<OBCalibrationParam> calibration_param_;
std::optional<OBXYTables> xy_tables_;
float* xy_table_data_ = nullptr;
uint32_t xy_table_data_size_ = 0;
uint8_t* rgb_pint_cloud_buffer_ = nullptr;
uint32_t rgb_pint_cloud_buffer_size_ = 0;
}; };
} // namespace orbbec_camera } // namespace orbbec_camera
orbbec_camera/src/ob_camera_node.cpp
+93 -83
View File
@@ -23,6 +23,7 @@
#include <filesystem> #include <filesystem>
#include <fstream> #include <fstream>
#include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "diagnostic_msgs/msg/diagnostic_status.hpp"
#include "libobsensor/hpp/Utils.hpp"
#if defined(USE_RK_HW_DECODER) #if defined(USE_RK_HW_DECODER)
#include "orbbec_camera/rk_mpp_decoder.h" #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]) { if (enable_stream_[COLOR]) {
rgb_buffer_ = new uint8_t[width_[COLOR] * height_[COLOR] * 3]; 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; 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_); std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBCameraNode"); RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBCameraNode");
is_running_.store(false); is_running_.store(false);
@@ -108,6 +115,14 @@ void OBCameraNode::clean() {
delete[] rgb_buffer_; delete[] rgb_buffer_;
rgb_buffer_ = nullptr; 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() { 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 OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
(void)frame_set; (void)frame_set;
if (!depth_cloud_pub_) { if (!depth_cloud_pub_ || depth_cloud_pub_->get_subscription_count() == 0 ||
return; !enable_point_cloud_ || !depth_frame_) {
}
if (depth_cloud_pub_->get_subscription_count() == 0 || !enable_point_cloud_) {
return;
}
if (!depth_frame_) {
return; return;
} }
std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_); 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"); RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
return; 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 width = depth_frame->width();
auto height = depth_frame->height(); 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>(); auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg); sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
modifier.setPointCloud2FieldsByString(1, "xyz"); 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_x(*point_cloud_msg, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y"); sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z"); sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z");
size_t valid_count = 0; const static float MIN_DISTANCE = 20.0; // 2cm
const static float MIN_DISTANCE = 20.0; const static float MAX_DISTANCE = 10000.0; // 10m
const static float MAX_DISTANCE = 10000.0;
double depth_scale = depth_frame->getValueScale();
const static float min_depth = MIN_DISTANCE / depth_scale; const static float min_depth = MIN_DISTANCE / depth_scale;
const static float max_depth = MAX_DISTANCE / depth_scale; const static float max_depth = MAX_DISTANCE / depth_scale;
for (uint32_t y = 0; y < height; y++) { size_t valid_count = 0;
for (uint32_t x = 0; x < width; x++) { for (size_t i = 0; i < point_size; i++) {
bool valid_point = true; bool valid_point = points[i].z >= min_depth && points[i].z <= max_depth;
if (depth_data[y * width + x] < min_depth || depth_data[y * width + x] > max_depth) { if (valid_point || ordered_pc_) {
valid_point = false; *iter_x = static_cast<float>(points[i].x / 1000.0);
} *iter_y = static_cast<float>(points[i].y / 1000.0);
if (valid_point || ordered_pc_) { *iter_z = static_cast<float>(points[i].z / 1000.0);
float xf = (x - u0) * fdx; ++iter_x, ++iter_y, ++iter_z;
float yf = (y - v0) * fdy; valid_count++;
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++;
}
} }
} }
if (valid_count == 0) { 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) { void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
if (!depth_registration_cloud_pub_) { if (!depth_registration_cloud_pub_ ||
return; depth_registration_cloud_pub_->get_subscription_count() == 0 ||
} !enable_colored_point_cloud_ || !depth_frame_) {
if (depth_registration_cloud_pub_->get_subscription_count() == 0 ||
!enable_colored_point_cloud_) {
return;
}
if (!depth_frame_) {
return; return;
} }
CHECK_NOTNULL(depth_frame_.get()); CHECK_NOTNULL(depth_frame_.get());
std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_); std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
auto depth_frame = depth_frame_->as<ob::DepthFrame>(); 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); depth_height, color_width, color_height);
return; return;
} }
auto camera_params = pipeline_->getCameraParam(); if (!xy_tables_.has_value()) {
auto intrinsics = camera_params.rgbIntrinsic; calibration_param_ = pipeline_->getCalibrationParam(pipeline_config_);
float fdx = intrinsics.fx * ((float)(color_width) / intrinsics.width);
float fdy = intrinsics.fy * ((float)(color_height) / intrinsics.height); uint32_t table_size =
fdx = 1 / fdx; color_width * color_height * 2; // one for x-coordinate and one for y-coordinate LUT
fdy = 1 / fdy; if (xy_table_data_size_ != table_size) {
float u0 = intrinsics.cx * ((float)(color_width) / intrinsics.width); RCLCPP_INFO_STREAM(logger_, "Init xy tables with size " << table_size);
float v0 = intrinsics.cy * ((float)(color_height) / intrinsics.height); xy_table_data_size_ = table_size;
const auto *depth_data = (uint16_t *)depth_frame->data(); 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_); 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>(); auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg); sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
modifier.setPointCloud2FieldsByString(1, "xyz"); modifier.setPointCloud2FieldsByString(1, "xyz");
@@ -1347,31 +1370,17 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
double depth_scale = depth_frame->getValueScale(); double depth_scale = depth_frame->getValueScale();
static float min_depth = MIN_DISTANCE / depth_scale; static float min_depth = MIN_DISTANCE / depth_scale;
static float max_depth = MAX_DISTANCE / depth_scale; static float max_depth = MAX_DISTANCE / depth_scale;
for (uint32_t y = 0; y < color_height; y++) { for (size_t i = 0; i < color_width * color_height; i++) {
for (uint32_t x = 0; x < color_width; x++) { bool valid_point = point_cloud[i].z >= min_depth && point_cloud[i].z <= max_depth;
bool valid_point = true; if (valid_point || ordered_pc_) {
float depth = depth_data[y * depth_width + x]; *iter_x = static_cast<float>(point_cloud[i].x / 1000.0);
if (depth < min_depth || depth > max_depth) { *iter_y = static_cast<float>(point_cloud[i].y / 1000.0);
valid_point = false; *iter_z = static_cast<float>(point_cloud[i].z / 1000.0);
} *iter_r = static_cast<uint8_t>(point_cloud[i].r);
if (valid_point || ordered_pc_) { *iter_g = static_cast<uint8_t>(point_cloud[i].g);
float xf = (x - u0) * fdx; *iter_b = static_cast<uint8_t>(point_cloud[i].b);
float yf = (y - v0) * fdy; ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b;
float zf = depth * depth_scale; ++valid_count;
*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;
}
} }
} }
if (valid_count == 0) { 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) { metadata_publishers_[COLOR]->get_subscription_count() > 0) {
has_subscriber = true; 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; has_subscriber = true;
} }
bool is_decoded = false; bool is_decoded = false;