From 9b5369fe54a46544960c4dd4b9ca37ff872af7ae Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Thu, 21 Dec 2023 17:28:56 +0200 Subject: [PATCH 1/3] support 2 res for LPC --- .../include/base_realsense_node.h | 2 +- realsense2_camera/src/base_realsense_node.cpp | 26 ++++++++++++++----- 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 506bafcf54..32a29fe4ad 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -204,7 +204,7 @@ namespace realsense2_camera void startDynamicTf(); void publishDynamicTransforms(); void publishPointCloud(rs2::points f, const rclcpp::Time& t, const rs2::frameset& frameset); - void publishLabeledPointCloud(rs2::labeled_points points, const rclcpp::Time& t); + void publishLabeledPointCloud(rs2::labeled_points lpc, const rclcpp::Time& t); bool shouldPublishCameraInfo(const stream_index_pair& sip); Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics) const; IMUInfo getImuInfo(const rs2::stream_profile& profile); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 092a6b1d06..5dc5b5bbc7 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -841,7 +841,7 @@ bool BaseRealSenseNode::shouldPublishCameraInfo(const stream_index_pair& sip) return (stream != RS2_STREAM_SAFETY && stream != RS2_STREAM_OCCUPANCY && stream != RS2_STREAM_LABELED_POINT_CLOUD); } -void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points pc, const rclcpp::Time& t){ +void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points lpc, const rclcpp::Time& t){ if(!_labeled_pointcloud_publisher || 0 == _labeled_pointcloud_publisher->get_subscription_count()) return; @@ -857,22 +857,34 @@ void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points pc, const r "y", 1, sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, "label", 1, sensor_msgs::msg::PointField::UINT8); - modifier.resize(pc.size()); + modifier.resize(lpc.size()); // Fill the PointCloud message with data sensor_msgs::PointCloud2Iterator iter_x(*msg_pointcloud, "x"); sensor_msgs::PointCloud2Iterator iter_y(*msg_pointcloud, "y"); sensor_msgs::PointCloud2Iterator iter_z(*msg_pointcloud, "z"); sensor_msgs::PointCloud2Iterator iter_label(*msg_pointcloud, "label"); - const rs2::vertex* vertex = pc.get_vertices(); - const uint8_t* label = pc.get_labels(); - msg_pointcloud->width = 320; - msg_pointcloud->height = 180; + const rs2::vertex* vertex = lpc.get_vertices(); + const uint8_t* label = lpc.get_labels(); + + // Calculate label point cloud width & height from the size + // We know the aspect ratio is the same for both resolutions and we know that w * h = vertices_count + // so: + // aspect ratio is 320/180 == 640/360 == 0.5625 + // vertices_count = w * h --> w * 0.5625w --> w = sqrt(size / 0.5625) + auto lpc_size = lpc.size(); + static const auto aspect_ratio = 0.5625f; // 640/360 or 320/180 + int lpc_width = static_cast< int >( std::sqrt( lpc_size / aspect_ratio ) ); + lpc_width = ( lpc_width == 0 ) ? 1 : lpc_width; // protect dev by 0 + int lpc_height = static_cast< int >( lpc_size / lpc_width ); + + msg_pointcloud->width = lpc_width; + msg_pointcloud->height = lpc_height; msg_pointcloud->point_step = 3*sizeof(float) + sizeof(uint8_t); msg_pointcloud->row_step = msg_pointcloud->width * msg_pointcloud->point_step; msg_pointcloud->data.resize(msg_pointcloud->height * msg_pointcloud->row_step); - for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++, label++) + for (size_t point_idx=0; point_idx < lpc.size(); point_idx++, vertex++, label++) { *iter_x = vertex->x; *iter_y = vertex->y; From 1160f2dda4b3d53a74853c7ea40fb9a6eff541ad Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Sun, 24 Dec 2023 16:51:04 +0200 Subject: [PATCH 2/3] get LPC w & h from API --- realsense2_camera/src/base_realsense_node.cpp | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 5dc5b5bbc7..55c9584ca2 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -867,19 +867,8 @@ void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points lpc, const const rs2::vertex* vertex = lpc.get_vertices(); const uint8_t* label = lpc.get_labels(); - // Calculate label point cloud width & height from the size - // We know the aspect ratio is the same for both resolutions and we know that w * h = vertices_count - // so: - // aspect ratio is 320/180 == 640/360 == 0.5625 - // vertices_count = w * h --> w * 0.5625w --> w = sqrt(size / 0.5625) - auto lpc_size = lpc.size(); - static const auto aspect_ratio = 0.5625f; // 640/360 or 320/180 - int lpc_width = static_cast< int >( std::sqrt( lpc_size / aspect_ratio ) ); - lpc_width = ( lpc_width == 0 ) ? 1 : lpc_width; // protect dev by 0 - int lpc_height = static_cast< int >( lpc_size / lpc_width ); - - msg_pointcloud->width = lpc_width; - msg_pointcloud->height = lpc_height; + msg_pointcloud->width = lpc.get_width(); + msg_pointcloud->height = lpc.get_height(); msg_pointcloud->point_step = 3*sizeof(float) + sizeof(uint8_t); msg_pointcloud->row_step = msg_pointcloud->width * msg_pointcloud->point_step; msg_pointcloud->data.resize(msg_pointcloud->height * msg_pointcloud->row_step); From b929cb39fb12a1b502cae56a0649b96cd397eb57 Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Mon, 25 Dec 2023 17:18:16 +0200 Subject: [PATCH 3/3] use new LPC API get_bits_per_pixel --- realsense2_camera/src/base_realsense_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 55c9584ca2..a3cbec4a5d 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -869,7 +869,7 @@ void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points lpc, const msg_pointcloud->width = lpc.get_width(); msg_pointcloud->height = lpc.get_height(); - msg_pointcloud->point_step = 3*sizeof(float) + sizeof(uint8_t); + msg_pointcloud->point_step = lpc.get_bits_per_pixel() / 8; msg_pointcloud->row_step = msg_pointcloud->width * msg_pointcloud->point_step; msg_pointcloud->data.resize(msg_pointcloud->height * msg_pointcloud->row_step);