Add support for pointcloud normals

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)

open3d_conversions/include/open3d_conversions/open3d_conversions.hpp

@@ -18,7 +18,9 @@
 #include <Eigen/Dense>
 
 // Open3D
-#include <open3d/Open3D.h>
+#include <open3d/geometry/Image.h>
+#include <open3d/geometry/PointCloud.h>
+#include <open3d/camera/PinholeCameraIntrinsic.h>
 
 // C++
 #include <string>

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<float> ros_pc2_x(ros_pc2, "x");
-  sensor_msgs::PointCloud2Iterator<float> ros_pc2_y(ros_pc2, "y");
-  sensor_msgs::PointCloud2Iterator<float> ros_pc2_z(ros_pc2, "z");
-  if (pointcloud.HasColors()) {
-    sensor_msgs::PointCloud2Iterator<uint8_t> ros_pc2_r(ros_pc2, "r");
-    sensor_msgs::PointCloud2Iterator<uint8_t> ros_pc2_g(ros_pc2, "g");
-    sensor_msgs::PointCloud2Iterator<uint8_t> 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<int>(255 * color(0));
-      *ros_pc2_g = static_cast<int>(255 * color(1));
-      *ros_pc2_b = static_cast<int>(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<float>();
+    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<uint8_t>(255 * point_rgb(0));
+      const uint8_t g = static_cast<uint8_t>(255 * point_rgb(1));
+      const uint8_t b = static_cast<uint8_t>(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<float>();
+      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<float> ros_pc2_x(*ros_pc2, "x");
   sensor_msgs::PointCloud2ConstIterator<float> ros_pc2_y(*ros_pc2, "y");
   sensor_msgs::PointCloud2ConstIterator<float> 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<float> ros_pc2_nx(*ros_pc2, "normal_x");
+    sensor_msgs::PointCloud2ConstIterator<float> ros_pc2_ny(*ros_pc2, "normal_y");
+    sensor_msgs::PointCloud2ConstIterator<float> 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<uint8_t> ros_pc2_r(*ros_pc2, "r");
-      sensor_msgs::PointCloud2ConstIterator<uint8_t> ros_pc2_g(*ros_pc2, "g");
-      sensor_msgs::PointCloud2ConstIterator<uint8_t> 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<int>(*ros_pc2_r)) / 255.0,
-            (static_cast<int>(*ros_pc2_g)) / 255.0,
-            (static_cast<int>(*ros_pc2_b)) / 255.0));
-      }
-    } else if (ros_pc2->fields[3].name == "intensity") {
-      sensor_msgs::PointCloud2ConstIterator<uint8_t> 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<uint8_t> ros_pc2_r(*ros_pc2, "r");
+    sensor_msgs::PointCloud2ConstIterator<uint8_t> ros_pc2_g(*ros_pc2, "g");
+    sensor_msgs::PointCloud2ConstIterator<uint8_t> 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<int>(*ros_pc2_r)) / 255.0,
+          (static_cast<int>(*ros_pc2_g)) / 255.0,
+          (static_cast<int>(*ros_pc2_b)) / 255.0));
+    }
+  } else if (hasField("intensity")) {
+    // Add intensity as a color if the input cloud has intesity
+    sensor_msgs::PointCloud2ConstIterator<uint8_t> 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));
     }
   }
 }

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<float> ros_pc2_x(ros_pc2, "x");
+  sensor_msgs::PointCloud2Iterator<float> ros_pc2_y(ros_pc2, "y");
+  sensor_msgs::PointCloud2Iterator<float> ros_pc2_z(ros_pc2, "z");
+  sensor_msgs::PointCloud2Iterator<float> ros_pc2_nx(ros_pc2, "normal_x");
+  sensor_msgs::PointCloud2Iterator<float> ros_pc2_ny(ros_pc2, "normal_y");
+  sensor_msgs::PointCloud2Iterator<float> 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<float> mod_x(ros_pc2, "x");
+  sensor_msgs::PointCloud2Iterator<float> mod_y(ros_pc2, "y");
+  sensor_msgs::PointCloud2Iterator<float> mod_z(ros_pc2, "z");
+  sensor_msgs::PointCloud2Iterator<float> mod_nx(ros_pc2, "normal_x");
+  sensor_msgs::PointCloud2Iterator<float> mod_ny(ros_pc2, "normal_y");
+  sensor_msgs::PointCloud2Iterator<float> mod_nz(ros_pc2, "normal_z");
+
+  std::vector<Eigen::Vector3d> 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<sensor_msgs::msg::PointCloud2>(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";