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

Add depth to point cloud conversion using util functions

Browse Source
This commit is contained in:
RBT22
2024-04-03 12:58:34 +02:00
parent caab21ec1b
commit 746c317157
2 changed files with 41 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+2
View File
@@ -46,6 +46,7 @@
#include <image_transport/publisher.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include "libobsensor/ObSensor.hpp"
#include "libobsensor/hpp/Utils.hpp"
#include "orbbec_camera_msgs/msg/device_info.hpp"
#include "orbbec_camera_msgs/srv/get_device_info.hpp"
@@ -396,6 +397,7 @@ class OBCameraNode {
std::string color_info_url_;
std::string ir_info_url_;
std::optional<OBCameraParam> camera_param_;
std::optional<OBCalibrationParam> calibration_param_;
bool enable_d2c_viewer_ = false;
std::unique_ptr<D2CViewer> d2c_viewer_ = nullptr;
std::map<stream_index_pair, std::atomic_bool> save_images_;
orbbec_camera/src/ob_camera_node.cpp
+39 -29
View File
@@ -743,6 +743,13 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
RCLCPP_ERROR_STREAM(logger_, "camera_param_ is null");
return;
}
if (!calibration_param_) {
calibration_param_ = pipeline_->getCalibrationParam(pipeline_config_);
}
if (!calibration_param_) {
RCLCPP_ERROR_STREAM(logger_, "calibration_param_ is null");
return;
}
auto depth_frame = frame_set->depthFrame();
if (!depth_frame) {
return;
@@ -753,16 +760,27 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
if (depth_data == nullptr) {
return;
}
float fdx =
camera_param_->depthIntrinsic.fx * ((float)(width) / camera_param_->depthIntrinsic.width);
float fdy =
camera_param_->depthIntrinsic.fy * ((float)(height) / camera_param_->depthIntrinsic.height);
fdx = 1 / fdx;
fdy = 1 / fdy;
float u0 =
camera_param_->depthIntrinsic.cx * ((float)(width) / camera_param_->depthIntrinsic.width);
float v0 =
camera_param_->depthIntrinsic.cy * ((float)(height) / camera_param_->depthIntrinsic.height);
uint32_t tableSize = width * height * 2; // one for x-coordinate and one for y-coordinate LUT
float * data = new float[tableSize];
OBXYTables xyTables;
if (!ob::CoordinateTransformHelper::transformationInitXYTables(
*calibration_param_, OB_SENSOR_DEPTH, data,
&tableSize, &xyTables)) {
return;
}
uint32_t pointcloudSize = width * height * sizeof(OBPoint3f);
uint8_t * pointcloudData = new uint8_t[pointcloudSize];
memset(pointcloudData, 0, pointcloudSize);
OBPoint * pointPixel = (OBPoint *)pointcloudData;
ob::CoordinateTransformHelper::transformationDepthToPointCloud(
&xyTables,
depth_data,
pointPixel);
sensor_msgs::PointCloud2Modifier modifier(point_cloud_msg_);
modifier.setPointCloud2FieldsByString(1, "xyz");
modifier.resize(width * height);
@@ -777,25 +795,17 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
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_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 vaild_point = true;
if (depth_data[y * width + x] < min_depth || depth_data[y * width + x] > max_depth) {
vaild_point = false;
}
if (vaild_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++;
}
for (uint32_t i = 0; i < width * height; i++) {
bool valid_point = true;
if (pointPixel[i].z<MIN_DISTANCE || pointPixel[i].z> MAX_DISTANCE) {
valid_point = false;
}
if (valid_point || ordered_pc_) {
*iter_x = pointPixel[i].x / 1000.0;
*iter_y = pointPixel[i].y / 1000.0;
*iter_z = pointPixel[i].z / 1000.0;
++iter_x, ++iter_y, ++iter_z;
valid_count++;
}
}
auto timestamp = use_hardware_time_ ? fromUsToROSTime(depth_frame->timeStampUs())