From ec8eda84a5f424b1a662244dee5db93a1ff17d4e Mon Sep 17 00:00:00 2001 From: alexnavtt Date: Wed, 10 Jul 2024 23:39:13 -0600 Subject: [PATCH 1/9] Added point cloud normals --- open3d_conversions/src/open3d_conversions.cpp | 92 ++++++++++++------- 1 file changed, 61 insertions(+), 31 deletions(-) diff --git a/open3d_conversions/src/open3d_conversions.cpp b/open3d_conversions/src/open3d_conversions.cpp index 78702c6..bfeb207 100644 --- a/open3d_conversions/src/open3d_conversions.cpp +++ b/open3d_conversions/src/open3d_conversions.cpp @@ -54,40 +54,70 @@ void open3dToRos( sensor_msgs::msg::PointCloud2 & ros_pc2) { sensor_msgs::PointCloud2Modifier modifier(ros_pc2); + modifier.clear(); + modifier.setPointCloud2FieldsByString(1, "xyz"); + + ros_pc2.point_step = 12; if (pointcloud.HasColors()) { - modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); - } else { - modifier.setPointCloud2FieldsByString(1, "xyz"); + sensor_msgs::msg::PointField & rgb_field = ros_pc2.fields.emplace_back(); + rgb_field.count = 1; + rgb_field.datatype = sensor_msgs::msg::PointField::FLOAT32; + rgb_field.name = "rgb"; + rgb_field.offset = ros_pc2.point_step; + ros_pc2.point_step += 4; } - modifier.resize(pointcloud.points_.size()); - sensor_msgs::PointCloud2Iterator ros_pc2_x(ros_pc2, "x"); - sensor_msgs::PointCloud2Iterator ros_pc2_y(ros_pc2, "y"); - sensor_msgs::PointCloud2Iterator ros_pc2_z(ros_pc2, "z"); - if (pointcloud.HasColors()) { - sensor_msgs::PointCloud2Iterator ros_pc2_r(ros_pc2, "r"); - sensor_msgs::PointCloud2Iterator ros_pc2_g(ros_pc2, "g"); - sensor_msgs::PointCloud2Iterator ros_pc2_b(ros_pc2, "b"); - for (size_t i = 0; i < pointcloud.points_.size(); i++, ++ros_pc2_x, - ++ros_pc2_y, ++ros_pc2_z, ++ros_pc2_r, ++ros_pc2_g, - ++ros_pc2_b) - { - const Eigen::Vector3d & point = pointcloud.points_[i]; - const Eigen::Vector3d & color = pointcloud.colors_[i]; - *ros_pc2_x = point(0); - *ros_pc2_y = point(1); - *ros_pc2_z = point(2); - *ros_pc2_r = static_cast(255 * color(0)); - *ros_pc2_g = static_cast(255 * color(1)); - *ros_pc2_b = static_cast(255 * color(2)); + + if (pointcloud.HasNormals()) { + sensor_msgs::msg::PointField & nx_field = ros_pc2.fields.emplace_back(); + nx_field.count = 1; + nx_field.datatype = sensor_msgs::msg::PointField::FLOAT32; + nx_field.name = "normal_x"; + nx_field.offset = ros_pc2.point_step; + ros_pc2.point_step += 4; + + sensor_msgs::msg::PointField & ny_field = ros_pc2.fields.emplace_back(); + ny_field.count = 1; + ny_field.datatype = sensor_msgs::msg::PointField::FLOAT32; + ny_field.name = "normal_y"; + ny_field.offset = ros_pc2.point_step; + ros_pc2.point_step += 4; + + sensor_msgs::msg::PointField & nz_field = ros_pc2.fields.emplace_back(); + nz_field.count = 1; + nz_field.datatype = sensor_msgs::msg::PointField::FLOAT32; + nz_field.name = "normal_z"; + nz_field.offset = ros_pc2.point_step; + ros_pc2.point_step += 4; + } + + ros_pc2.height = 1; + ros_pc2.width = pointcloud.points_.size(); + ros_pc2.row_step = ros_pc2.width * ros_pc2.point_step; + ros_pc2.is_bigendian = rcpputils::endian::native == rcpputils::endian::big; + ros_pc2.is_dense = true; + ros_pc2.data.resize(ros_pc2.row_step); + + float * ros_it = reinterpret_cast(ros_pc2.data.data()); // Technically UB + for (std::size_t idx = 0; idx < pointcloud.points_.size(); ++idx) { + const Eigen::Vector3d & point_xyz = pointcloud.points_[idx]; + *ros_it++ = static_cast(point_xyz.x()); + *ros_it++ = static_cast(point_xyz.y()); + *ros_it++ = static_cast(point_xyz.z()); + + if (pointcloud.HasColors()) { + const Eigen::Vector3d & point_rgb = pointcloud.colors_[idx]; + const uint8_t r = static_cast(255 * point_rgb(0)); + const uint8_t g = static_cast(255 * point_rgb(1)); + const uint8_t b = static_cast(255 * point_rgb(2)); + const uint32_t rgb = (rcpputils::endian::native == rcpputils::endian::big) ? (b << 24 | g << 16 | r << 8) : (r << 16 | g << 8 | b); + std::memcpy(ros_it++, &rgb, sizeof(float)); } - } else { - for (size_t i = 0; i < pointcloud.points_.size(); - i++, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z) - { - const Eigen::Vector3d & point = pointcloud.points_[i]; - *ros_pc2_x = point(0); - *ros_pc2_y = point(1); - *ros_pc2_z = point(2); + + if (pointcloud.HasNormals()) { + const Eigen::Vector3d & point_normal = pointcloud.normals_[idx]; + *ros_it++ = static_cast(point_normal.x()); + *ros_it++ = static_cast(point_normal.y()); + *ros_it++ = static_cast(point_normal.z()); } } } From 8cd5b298ffdd7c370fc3bee33639a9d1ef570e35 Mon Sep 17 00:00:00 2001 From: alexnavtt Date: Thu, 8 May 2025 14:25:21 -0600 Subject: [PATCH 2/9] Removed monolithic include --- .../include/open3d_conversions/open3d_conversions.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/open3d_conversions/include/open3d_conversions/open3d_conversions.hpp b/open3d_conversions/include/open3d_conversions/open3d_conversions.hpp index 05d82c3..0128e50 100644 --- a/open3d_conversions/include/open3d_conversions/open3d_conversions.hpp +++ b/open3d_conversions/include/open3d_conversions/open3d_conversions.hpp @@ -18,7 +18,9 @@ #include // Open3D -#include +#include +#include +#include // C++ #include From 868973deee89f3b7239b352af5cdf03ee02f81fb Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Wed, 28 May 2025 22:15:22 +0200 Subject: [PATCH 3/9] fix targets (#34) * remove unused 'ament_cmake_ros' * fix linked targets and replace deprecated 'ament_target_dependencies' --- open3d_conversions/CMakeLists.txt | 23 ++++++++--------------- open3d_conversions/package.xml | 3 ++- 2 files changed, 10 insertions(+), 16 deletions(-) diff --git a/open3d_conversions/CMakeLists.txt b/open3d_conversions/CMakeLists.txt index 4cf8801..c2a7515 100644 --- a/open3d_conversions/CMakeLists.txt +++ b/open3d_conversions/CMakeLists.txt @@ -8,31 +8,24 @@ endif() # system dependencies find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(Eigen3 REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) find_package(Open3D REQUIRED) find_package(rclcpp REQUIRED) find_package(rcpputils REQUIRED) find_package(sensor_msgs REQUIRED) -include_directories( - include - ${EIGEN3_INCLUDE_DIR} - ${Open3D_INCLUDE_DIRS} -) - add_library(open3d_conversions src/open3d_conversions.cpp) target_include_directories(open3d_conversions PUBLIC $ $) -ament_target_dependencies( - open3d_conversions - "rclcpp" - "rcpputils" - "sensor_msgs" +target_link_libraries(open3d_conversions + rclcpp::rclcpp + rcpputils::rcpputils + ${sensor_msgs_TARGETS} + Eigen3::Eigen + Open3D::Open3D ) -target_link_libraries(open3d_conversions ${Open3D_LIBRARIES}) target_compile_definitions(open3d_conversions PUBLIC "OPEN3D_CONVERSIONS_BUILDING_LIBRARY") install( @@ -59,6 +52,6 @@ endif() ament_export_include_directories(include) ament_export_libraries(open3d_conversions) ament_export_targets(export_${PROJECT_NAME}) -ament_export_dependencies(Open3D rclcpp) +ament_export_dependencies(Eigen3 Open3D rclcpp rcpputils sensor_msgs) ament_package() diff --git a/open3d_conversions/package.xml b/open3d_conversions/package.xml index 7416e3d..d33d640 100644 --- a/open3d_conversions/package.xml +++ b/open3d_conversions/package.xml @@ -13,7 +13,8 @@ Pranay Mathur Nikhil Khedekar - ament_cmake_ros + + ament_cmake libopen3d-dev rclcpp From 6f5f6975b01a548338f68fd32d0b49f9054d9f38 Mon Sep 17 00:00:00 2001 From: alexnavtt Date: Thu, 31 Jul 2025 23:43:53 -0600 Subject: [PATCH 4/9] Cleaned up open3dToRos pointcloud and removed UB --- open3d_conversions/src/open3d_conversions.cpp | 82 ++++++++++--------- 1 file changed, 42 insertions(+), 40 deletions(-) diff --git a/open3d_conversions/src/open3d_conversions.cpp b/open3d_conversions/src/open3d_conversions.cpp index bfeb207..ab0492f 100644 --- a/open3d_conversions/src/open3d_conversions.cpp +++ b/open3d_conversions/src/open3d_conversions.cpp @@ -53,43 +53,38 @@ void open3dToRos( const open3d::geometry::PointCloud & pointcloud, sensor_msgs::msg::PointCloud2 & ros_pc2) { - sensor_msgs::PointCloud2Modifier modifier(ros_pc2); - modifier.clear(); - modifier.setPointCloud2FieldsByString(1, "xyz"); + // Clear the input pointcloud to prepare it to be set + std_msgs::msg::Header pc_header = ros_pc2.header; + ros_pc2 = sensor_msgs::msg::PointCloud2{}; + ros_pc2.header = pc_header; - ros_pc2.point_step = 12; - if (pointcloud.HasColors()) { - sensor_msgs::msg::PointField & rgb_field = ros_pc2.fields.emplace_back(); - rgb_field.count = 1; - rgb_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - rgb_field.name = "rgb"; - rgb_field.offset = ros_pc2.point_step; + // We define a lambda for new fields to avoid code repetition + // This only works because all our fields have the same structure + auto add_new_field = [&ros_pc2](const std::string& field_name) { + sensor_msgs::msg::PointField & new_field = ros_pc2.fields.emplace_back(); + new_field.count = 1; + new_field.datatype = sensor_msgs::msg::PointField::FLOAT32; + new_field.name = field_name; + new_field.offset = ros_pc2.point_step; ros_pc2.point_step += 4; - } + }; - if (pointcloud.HasNormals()) { - sensor_msgs::msg::PointField & nx_field = ros_pc2.fields.emplace_back(); - nx_field.count = 1; - nx_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - nx_field.name = "normal_x"; - nx_field.offset = ros_pc2.point_step; - ros_pc2.point_step += 4; + add_new_field("x"); + add_new_field("y"); + add_new_field("z"); - sensor_msgs::msg::PointField & ny_field = ros_pc2.fields.emplace_back(); - ny_field.count = 1; - ny_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - ny_field.name = "normal_y"; - ny_field.offset = ros_pc2.point_step; - ros_pc2.point_step += 4; + if (pointcloud.HasColors()) { + // Note that the RGB field is typically packed as a single FLOAT32 value + add_new_field("rgb"); + } - sensor_msgs::msg::PointField & nz_field = ros_pc2.fields.emplace_back(); - nz_field.count = 1; - nz_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - nz_field.name = "normal_z"; - nz_field.offset = ros_pc2.point_step; - ros_pc2.point_step += 4; + if (pointcloud.HasNormals()) { + add_new_field("normal_x"); + add_new_field("normal_y"); + add_new_field("normal_z"); } + // Set the parameters known from the open3d pointcloud definition ros_pc2.height = 1; ros_pc2.width = pointcloud.points_.size(); ros_pc2.row_step = ros_pc2.width * ros_pc2.point_step; @@ -97,12 +92,19 @@ void open3dToRos( ros_pc2.is_dense = true; ros_pc2.data.resize(ros_pc2.row_step); - float * ros_it = reinterpret_cast(ros_pc2.data.data()); // Technically UB + // For clarity we set up this lambda to cleanly set the value and advance the pointer + auto * ros_it = ros_pc2.data.data(); + auto set_next_value = [&ros_it](const auto& value) { + std::memcpy(ros_it, &value, sizeof(value)); + std::advance(ros_it, sizeof(value)); + }; + for (std::size_t idx = 0; idx < pointcloud.points_.size(); ++idx) { - const Eigen::Vector3d & point_xyz = pointcloud.points_[idx]; - *ros_it++ = static_cast(point_xyz.x()); - *ros_it++ = static_cast(point_xyz.y()); - *ros_it++ = static_cast(point_xyz.z()); + const Eigen::Vector3f point_xyz = pointcloud.points_[idx].cast(); + if (point_xyz.hasNaN()) ros_pc2.is_dense = false; + set_next_value(point_xyz.x()); + set_next_value(point_xyz.y()); + set_next_value(point_xyz.z()); if (pointcloud.HasColors()) { const Eigen::Vector3d & point_rgb = pointcloud.colors_[idx]; @@ -110,14 +112,14 @@ void open3dToRos( const uint8_t g = static_cast(255 * point_rgb(1)); const uint8_t b = static_cast(255 * point_rgb(2)); const uint32_t rgb = (rcpputils::endian::native == rcpputils::endian::big) ? (b << 24 | g << 16 | r << 8) : (r << 16 | g << 8 | b); - std::memcpy(ros_it++, &rgb, sizeof(float)); + set_next_value(rgb); } if (pointcloud.HasNormals()) { - const Eigen::Vector3d & point_normal = pointcloud.normals_[idx]; - *ros_it++ = static_cast(point_normal.x()); - *ros_it++ = static_cast(point_normal.y()); - *ros_it++ = static_cast(point_normal.z()); + const Eigen::Vector3f point_normal = pointcloud.normals_[idx].cast(); + set_next_value(point_normal.x()); + set_next_value(point_normal.y()); + set_next_value(point_normal.z()); } } } From 9f884e782faf3f76416a1da1ef49c4786d7e8d77 Mon Sep 17 00:00:00 2001 From: alexnavtt Date: Wed, 6 Aug 2025 14:33:29 -0600 Subject: [PATCH 5/9] Fixed issue linking with some downstream packages --- open3d_conversions/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/open3d_conversions/CMakeLists.txt b/open3d_conversions/CMakeLists.txt index c2a7515..34981d2 100644 --- a/open3d_conversions/CMakeLists.txt +++ b/open3d_conversions/CMakeLists.txt @@ -5,6 +5,7 @@ project(open3d_conversions) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() +set(CMAKE_POSITION_INDEPENDENT_CODE ON) # system dependencies find_package(ament_cmake REQUIRED) From 8af9f05bda63b69397e8c1576c5d3ea5415edb1a Mon Sep 17 00:00:00 2001 From: alexnavtt Date: Sun, 17 Aug 2025 11:49:21 -0600 Subject: [PATCH 6/9] Formatting changes to satisfy ccplink and uncrustify --- open3d_conversions/src/open3d_conversions.cpp | 30 ++++++++++--------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/open3d_conversions/src/open3d_conversions.cpp b/open3d_conversions/src/open3d_conversions.cpp index ab0492f..8187eda 100644 --- a/open3d_conversions/src/open3d_conversions.cpp +++ b/open3d_conversions/src/open3d_conversions.cpp @@ -60,14 +60,14 @@ void open3dToRos( // We define a lambda for new fields to avoid code repetition // This only works because all our fields have the same structure - auto add_new_field = [&ros_pc2](const std::string& field_name) { - sensor_msgs::msg::PointField & new_field = ros_pc2.fields.emplace_back(); - new_field.count = 1; - new_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - new_field.name = field_name; - new_field.offset = ros_pc2.point_step; - ros_pc2.point_step += 4; - }; + auto add_new_field = [&ros_pc2](const std::string & field_name) { + sensor_msgs::msg::PointField & new_field = ros_pc2.fields.emplace_back(); + new_field.count = 1; + new_field.datatype = sensor_msgs::msg::PointField::FLOAT32; + new_field.name = field_name; + new_field.offset = ros_pc2.point_step; + ros_pc2.point_step += 4; + }; add_new_field("x"); add_new_field("y"); @@ -94,14 +94,14 @@ void open3dToRos( // For clarity we set up this lambda to cleanly set the value and advance the pointer auto * ros_it = ros_pc2.data.data(); - auto set_next_value = [&ros_it](const auto& value) { - std::memcpy(ros_it, &value, sizeof(value)); - std::advance(ros_it, sizeof(value)); - }; + auto set_next_value = [&ros_it](const auto & value) { + std::memcpy(ros_it, &value, sizeof(value)); + std::advance(ros_it, sizeof(value)); + }; for (std::size_t idx = 0; idx < pointcloud.points_.size(); ++idx) { const Eigen::Vector3f point_xyz = pointcloud.points_[idx].cast(); - if (point_xyz.hasNaN()) ros_pc2.is_dense = false; + if (point_xyz.hasNaN()) {ros_pc2.is_dense = false;} set_next_value(point_xyz.x()); set_next_value(point_xyz.y()); set_next_value(point_xyz.z()); @@ -111,7 +111,9 @@ void open3dToRos( const uint8_t r = static_cast(255 * point_rgb(0)); const uint8_t g = static_cast(255 * point_rgb(1)); const uint8_t b = static_cast(255 * point_rgb(2)); - const uint32_t rgb = (rcpputils::endian::native == rcpputils::endian::big) ? (b << 24 | g << 16 | r << 8) : (r << 16 | g << 8 | b); + const uint32_t rgb = (rcpputils::endian::native == rcpputils::endian::big) ? + (b << 24 | g << 16 | r << 8) : + (r << 16 | g << 8 | b); set_next_value(rgb); } From 00940a3d8a2e815e75b5430a5aba83b2319764e7 Mon Sep 17 00:00:00 2001 From: alexnavtt Date: Sun, 14 Dec 2025 23:17:56 -0700 Subject: [PATCH 7/9] Updated ros pointcloud to open3d conversion to include point normals --- open3d_conversions/src/open3d_conversions.cpp | 71 ++++++++++--------- 1 file changed, 38 insertions(+), 33 deletions(-) diff --git a/open3d_conversions/src/open3d_conversions.cpp b/open3d_conversions/src/open3d_conversions.cpp index 8187eda..232ffce 100644 --- a/open3d_conversions/src/open3d_conversions.cpp +++ b/open3d_conversions/src/open3d_conversions.cpp @@ -167,46 +167,51 @@ void rosToOpen3d( const sensor_msgs::msg::PointCloud2::SharedPtr & ros_pc2, open3d::geometry::PointCloud & o3d_pc, bool skip_colors) { + const std::size_t num_points = ros_pc2->height * ros_pc2->width; + + // Standard XYZ coordinate sensor_msgs::PointCloud2ConstIterator ros_pc2_x(*ros_pc2, "x"); sensor_msgs::PointCloud2ConstIterator ros_pc2_y(*ros_pc2, "y"); sensor_msgs::PointCloud2ConstIterator ros_pc2_z(*ros_pc2, "z"); - o3d_pc.points_.reserve(ros_pc2->height * ros_pc2->width); - if (ros_pc2->fields.size() == 3 || skip_colors == true) { - for (size_t i = 0; i < ros_pc2->height * ros_pc2->width; - ++i, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z) - { - o3d_pc.points_.push_back( - Eigen::Vector3d(*ros_pc2_x, *ros_pc2_y, *ros_pc2_z)); + o3d_pc.points_.reserve(num_points); + for (std::size_t i = 0; i < num_points; ++i, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z) { + o3d_pc.points_.push_back(Eigen::Vector3d(*ros_pc2_x, *ros_pc2_y, *ros_pc2_z)); + } + + auto hasField = [&ros_pc2](const std::string& name) { + return std::any_of(ros_pc2->fields.begin(), ros_pc2->fields.end(), [&name](const sensor_msgs::msg::PointField& field){return field.name == name;}); + }; + + // Add normals if the input cloud has normals + if (hasField("normal_x") && hasField("normal_y") && hasField("normal_z")) { + sensor_msgs::PointCloud2ConstIterator ros_pc2_nx(*ros_pc2, "normal_x"); + sensor_msgs::PointCloud2ConstIterator ros_pc2_ny(*ros_pc2, "normal_y"); + sensor_msgs::PointCloud2ConstIterator ros_pc2_nz(*ros_pc2, "normal_z"); + o3d_pc.normals_.reserve(num_points); + for (std::size_t i = 0; i < num_points; ++i, ++ros_pc2_nx, ++ros_pc2_ny, ++ros_pc2_nz) { + o3d_pc.normals_.push_back(Eigen::Vector3d(*ros_pc2_nx, *ros_pc2_ny, *ros_pc2_nz)); } - } else { - o3d_pc.colors_.reserve(ros_pc2->height * ros_pc2->width); - if (ros_pc2->fields[3].name == "rgb") { - sensor_msgs::PointCloud2ConstIterator ros_pc2_r(*ros_pc2, "r"); - sensor_msgs::PointCloud2ConstIterator ros_pc2_g(*ros_pc2, "g"); - sensor_msgs::PointCloud2ConstIterator ros_pc2_b(*ros_pc2, "b"); - - for (size_t i = 0; i < ros_pc2->height * ros_pc2->width; ++i, ++ros_pc2_x, - ++ros_pc2_y, ++ros_pc2_z, ++ros_pc2_r, ++ros_pc2_g, ++ros_pc2_b) - { - o3d_pc.points_.push_back( - Eigen::Vector3d(*ros_pc2_x, *ros_pc2_y, *ros_pc2_z)); - o3d_pc.colors_.push_back( - Eigen::Vector3d( + } + + // Add colors if the input cloud has colors + if (hasField("rgb") || hasField("rgba")) { + sensor_msgs::PointCloud2ConstIterator ros_pc2_r(*ros_pc2, "r"); + sensor_msgs::PointCloud2ConstIterator ros_pc2_g(*ros_pc2, "g"); + sensor_msgs::PointCloud2ConstIterator ros_pc2_b(*ros_pc2, "b"); + o3d_pc.colors_.reserve(num_points); + for (std::size_t i = 0; i < num_points; ++i, ++ros_pc2_r, ++ros_pc2_g, ++ros_pc2_b) { + o3d_pc.colors_.push_back(Eigen::Vector3d( (static_cast(*ros_pc2_r)) / 255.0, (static_cast(*ros_pc2_g)) / 255.0, (static_cast(*ros_pc2_b)) / 255.0)); - } - } else if (ros_pc2->fields[3].name == "intensity") { - sensor_msgs::PointCloud2ConstIterator ros_pc2_i(*ros_pc2, - "intensity"); - for (size_t i = 0; i < ros_pc2->height * ros_pc2->width; - ++i, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z, ++ros_pc2_i) - { - o3d_pc.points_.push_back( - Eigen::Vector3d(*ros_pc2_x, *ros_pc2_y, *ros_pc2_z)); - o3d_pc.colors_.push_back( - Eigen::Vector3d(*ros_pc2_i, *ros_pc2_i, *ros_pc2_i)); - } + } + } + // Add intensity as a color if the input cloud has intesity + else if (hasField("intensity")) { + sensor_msgs::PointCloud2ConstIterator ros_pc2_i(*ros_pc2, "intensity"); + o3d_pc.colors_.reserve(num_points); + for (std::size_t i = 0; i < num_points; ++i, ++ros_pc2_i) { + o3d_pc.colors_.push_back(Eigen::Vector3d(*ros_pc2_i, *ros_pc2_i, *ros_pc2_i)); } } } From 9f7aa93036dc111e2907f7473170f5aacb4e9751 Mon Sep 17 00:00:00 2001 From: alexnavtt Date: Fri, 3 Apr 2026 10:40:07 -0600 Subject: [PATCH 8/9] Fixed tester linting incompatibilities --- open3d_conversions/src/open3d_conversions.cpp | 24 +++++++++++-------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/open3d_conversions/src/open3d_conversions.cpp b/open3d_conversions/src/open3d_conversions.cpp index 232ffce..f6eccee 100644 --- a/open3d_conversions/src/open3d_conversions.cpp +++ b/open3d_conversions/src/open3d_conversions.cpp @@ -178,9 +178,13 @@ void rosToOpen3d( o3d_pc.points_.push_back(Eigen::Vector3d(*ros_pc2_x, *ros_pc2_y, *ros_pc2_z)); } - auto hasField = [&ros_pc2](const std::string& name) { - return std::any_of(ros_pc2->fields.begin(), ros_pc2->fields.end(), [&name](const sensor_msgs::msg::PointField& field){return field.name == name;}); - }; + auto hasField = [&ros_pc2](const std::string & name) { + return std::any_of( + ros_pc2->fields.begin(), + ros_pc2->fields.end(), + [&name](const sensor_msgs::msg::PointField & field) {return field.name == name;} + ); + }; // Add normals if the input cloud has normals if (hasField("normal_x") && hasField("normal_y") && hasField("normal_z")) { @@ -200,14 +204,14 @@ void rosToOpen3d( sensor_msgs::PointCloud2ConstIterator ros_pc2_b(*ros_pc2, "b"); o3d_pc.colors_.reserve(num_points); for (std::size_t i = 0; i < num_points; ++i, ++ros_pc2_r, ++ros_pc2_g, ++ros_pc2_b) { - o3d_pc.colors_.push_back(Eigen::Vector3d( - (static_cast(*ros_pc2_r)) / 255.0, - (static_cast(*ros_pc2_g)) / 255.0, - (static_cast(*ros_pc2_b)) / 255.0)); + o3d_pc.colors_.push_back( + Eigen::Vector3d( + (static_cast(*ros_pc2_r)) / 255.0, + (static_cast(*ros_pc2_g)) / 255.0, + (static_cast(*ros_pc2_b)) / 255.0)); } - } - // Add intensity as a color if the input cloud has intesity - else if (hasField("intensity")) { + } else if (hasField("intensity")) { + // Add intensity as a color if the input cloud has intesity sensor_msgs::PointCloud2ConstIterator ros_pc2_i(*ros_pc2, "intensity"); o3d_pc.colors_.reserve(num_points); for (std::size_t i = 0; i < num_points; ++i, ++ros_pc2_i) { From 3e615ffbbffd55205d277320b46130a125263f34 Mon Sep 17 00:00:00 2001 From: alexnavtt Date: Fri, 3 Apr 2026 17:00:16 -0600 Subject: [PATCH 9/9] Added tests for normals in pointclouds --- .../test/test_open3d_conversions.cpp | 77 +++++++++++++++++++ 1 file changed, 77 insertions(+) diff --git a/open3d_conversions/test/test_open3d_conversions.cpp b/open3d_conversions/test/test_open3d_conversions.cpp index ae19700..6bd13f7 100644 --- a/open3d_conversions/test/test_open3d_conversions.cpp +++ b/open3d_conversions/test/test_open3d_conversions.cpp @@ -49,6 +49,33 @@ TEST(ConversionFunctions, open3dToRos2_uncoloredPointcloud) { } } +TEST(ConversionFunction, open3dToRos2_pointcloudWithNormals) { + open3d::geometry::PointCloud o3d_pc; + for (int i = 0; i < 5; ++i) { + o3d_pc.points_.push_back(Eigen::Vector3d(0.5 * i, i * i, 10.5 * i)); + o3d_pc.normals_.push_back(Eigen::Vector3d(1 + i, i * i, 2 * i).normalized()); + } + sensor_msgs::msg::PointCloud2 ros_pc2; + open3d_conversions::open3dToRos(o3d_pc, ros_pc2); + EXPECT_EQ(ros_pc2.height * ros_pc2.width, o3d_pc.points_.size()); + sensor_msgs::PointCloud2Iterator ros_pc2_x(ros_pc2, "x"); + sensor_msgs::PointCloud2Iterator ros_pc2_y(ros_pc2, "y"); + sensor_msgs::PointCloud2Iterator ros_pc2_z(ros_pc2, "z"); + sensor_msgs::PointCloud2Iterator ros_pc2_nx(ros_pc2, "normal_x"); + sensor_msgs::PointCloud2Iterator ros_pc2_ny(ros_pc2, "normal_y"); + sensor_msgs::PointCloud2Iterator ros_pc2_nz(ros_pc2, "normal_z"); + for (int i = 0; i < 5; + i++, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z, ++ros_pc2_nx, ++ros_pc2_ny, ++ros_pc2_nz) + { + EXPECT_EQ(*ros_pc2_x, 0.5 * i); + EXPECT_EQ(*ros_pc2_y, i * i); + EXPECT_EQ(*ros_pc2_z, 10.5 * i); + EXPECT_EQ(*ros_pc2_nx, o3d_pc.normals_.at(i).x()); + EXPECT_EQ(*ros_pc2_ny, o3d_pc.normals_.at(i).y()); + EXPECT_EQ(*ros_pc2_nz, o3d_pc.normals_.at(i).z()); + } +} + TEST(ConversionFunctions, open3dToRos2_coloredPointcloud) { open3d::geometry::PointCloud o3d_pc; for (int i = 0; i < 5; ++i) { @@ -77,6 +104,56 @@ TEST(ConversionFunctions, open3dToRos2_coloredPointcloud) { } } +TEST(ConversionFunctions, rosToOpen3d_pointcloudWithNormals) { + sensor_msgs::msg::PointCloud2 ros_pc2; + ros_pc2.header.frame_id = "ros"; + ros_pc2.height = 1; + ros_pc2.width = 5; + ros_pc2.is_bigendian = false; + ros_pc2.is_dense = true; + sensor_msgs::PointCloud2Modifier modifier(ros_pc2); + modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.resize(5 * 1); + sensor_msgs::PointCloud2Iterator mod_x(ros_pc2, "x"); + sensor_msgs::PointCloud2Iterator mod_y(ros_pc2, "y"); + sensor_msgs::PointCloud2Iterator mod_z(ros_pc2, "z"); + sensor_msgs::PointCloud2Iterator mod_nx(ros_pc2, "normal_x"); + sensor_msgs::PointCloud2Iterator mod_ny(ros_pc2, "normal_y"); + sensor_msgs::PointCloud2Iterator mod_nz(ros_pc2, "normal_z"); + + std::vector normals; + for (int i = 0; i < 5; ++i, ++mod_x, ++mod_y, ++mod_z) { + const Eigen::Vector3d & normal = normals.emplace_back( + Eigen::Vector3d( + 1 + i, i * i, + 2 * i).normalized()); + *mod_x = 0.5 * i; + *mod_y = i * i; + *mod_z = 10.5 * i; + *mod_nx = normal.x(); + *mod_ny = normal.y(); + *mod_nz = normal.z(); + } + + const sensor_msgs::msg::PointCloud2::SharedPtr & ros_pc2_ptr = + std::make_shared(ros_pc2); + open3d::geometry::PointCloud o3d_pc; + open3d_conversions::rosToOpen3d(ros_pc2_ptr, o3d_pc); + EXPECT_EQ(ros_pc2_ptr->height * ros_pc2_ptr->width, o3d_pc.points_.size()); + EXPECT_EQ(o3d_pc.HasColors(), false); + for (unsigned int i = 0; i < 5; i++) { + const Eigen::Vector3d & point = o3d_pc.points_.at(i); + EXPECT_EQ(point(0), 0.5 * i); + EXPECT_EQ(point(1), i * i); + EXPECT_EQ(point(2), 10.5 * i); + + const Eigen::Vector3d & normal = o3d_pc.normals_.at(i); + EXPECT_EQ(normal(0), normals.at(i).x()); + EXPECT_EQ(normal(1), normals.at(i).y()); + EXPECT_EQ(normal(2), normals.at(i).z()); + } +} + TEST(ConversionFunctions, rosToOpen3d_uncoloredPointcloud) { sensor_msgs::msg::PointCloud2 ros_pc2; ros_pc2.header.frame_id = "ros";