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) 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 diff --git a/open3d_conversions/src/open3d_conversions.cpp b/open3d_conversions/src/open3d_conversions.cpp index 78702c6..f6eccee 100644 --- a/open3d_conversions/src/open3d_conversions.cpp +++ b/open3d_conversions/src/open3d_conversions.cpp @@ -53,41 +53,75 @@ void open3dToRos( const open3d::geometry::PointCloud & pointcloud, sensor_msgs::msg::PointCloud2 & ros_pc2) { - sensor_msgs::PointCloud2Modifier modifier(ros_pc2); + // 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; + + // 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; + }; + + add_new_field("x"); + add_new_field("y"); + add_new_field("z"); + if (pointcloud.HasColors()) { - modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); - } else { - modifier.setPointCloud2FieldsByString(1, "xyz"); + // Note that the RGB field is typically packed as a single FLOAT32 value + add_new_field("rgb"); } - 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()) { + 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; + ros_pc2.is_bigendian = rcpputils::endian::native == rcpputils::endian::big; + ros_pc2.is_dense = true; + ros_pc2.data.resize(ros_pc2.row_step); + + // 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::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]; + 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); + set_next_value(rgb); } - } 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::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()); } } } @@ -133,46 +167,55 @@ 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( - (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 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 (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) { + o3d_pc.colors_.push_back(Eigen::Vector3d(*ros_pc2_i, *ros_pc2_i, *ros_pc2_i)); } } } 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";