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

Calculate xy LUT only once

Browse Source
This commit is contained in:
RBT22
2024-04-03 15:39:01 +02:00
parent a302f4b66a
commit ccfa6d3c83
2 changed files with 20 additions and 20 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
@@ -398,6 +398,8 @@ class OBCameraNode {
std::string ir_info_url_; std::string ir_info_url_;
std::optional<OBCameraParam> camera_param_; std::optional<OBCameraParam> camera_param_;
std::optional<OBCalibrationParam> calibration_param_; std::optional<OBCalibrationParam> calibration_param_;
std::optional<OBXYTables> xy_tables_;
std::optional<float *> xy_table_data_;
bool enable_d2c_viewer_ = false; bool enable_d2c_viewer_ = false;
std::unique_ptr<D2CViewer> d2c_viewer_ = nullptr; std::unique_ptr<D2CViewer> d2c_viewer_ = nullptr;
std::map<stream_index_pair, std::atomic_bool> save_images_; std::map<stream_index_pair, std::atomic_bool> save_images_;
orbbec_camera/src/ob_camera_node.cpp
+18 -20
View File
@@ -743,31 +743,31 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
RCLCPP_ERROR_STREAM(logger_, "camera_param_ is null"); RCLCPP_ERROR_STREAM(logger_, "camera_param_ is null");
return; 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(); auto depth_frame = frame_set->depthFrame();
if (!depth_frame) { if (!depth_frame) {
return; return;
} }
auto width = depth_frame->width(); auto width = depth_frame->width();
auto height = depth_frame->height(); auto height = depth_frame->height();
const auto *depth_data = (uint16_t *)depth_frame->data();
if (depth_data == nullptr) { if (!xy_tables_.has_value()) {
return; calibration_param_ = pipeline_->getCalibrationParam(pipeline_config_);
uint32_t tableSize = width * height * 2; // one for x-coordinate and one for y-coordinate LUT
xy_table_data_ = new float[tableSize];
xy_tables_ = OBXYTables();
if (!ob::CoordinateTransformHelper::transformationInitXYTables(
*calibration_param_, OB_SENSOR_DEPTH, *xy_table_data_,
&tableSize, &(*xy_tables_))) {
xy_tables_.reset();
RCLCPP_ERROR_STREAM(logger_, "Failed to init xy tables");
return;
}
} }
uint32_t tableSize = width * height * 2; // one for x-coordinate and one for y-coordinate LUT const auto *depth_data = (uint16_t *)depth_frame->data();
float * data = new float[tableSize]; if (depth_data == nullptr) {
OBXYTables xyTables;
if (!ob::CoordinateTransformHelper::transformationInitXYTables(
*calibration_param_, OB_SENSOR_DEPTH, data,
&tableSize, &xyTables)) {
return; return;
} }
@@ -777,7 +777,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
OBPoint * pointPixel = (OBPoint *)pointcloudData; OBPoint * pointPixel = (OBPoint *)pointcloudData;
ob::CoordinateTransformHelper::transformationDepthToPointCloud( ob::CoordinateTransformHelper::transformationDepthToPointCloud(
&xyTables, &(*xy_tables_),
depth_data, depth_data,
pointPixel); pointPixel);
@@ -836,8 +836,6 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
RCLCPP_INFO_STREAM(logger_, "Saving point cloud to " << filename); RCLCPP_INFO_STREAM(logger_, "Saving point cloud to " << filename);
saveDepthPointsToPly(point_cloud_msg_, filename); saveDepthPointsToPly(point_cloud_msg_, filename);
} }
delete[] data;
data = nullptr;
delete[] pointcloudData; delete[] pointcloudData;
pointcloudData = nullptr; pointcloudData = nullptr;
} }