Added ability to transform not only point positions, but also other 3D-data channels.

tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h

@@ -63,10 +63,50 @@ template <>
 inline
 const std::string& getFrameId(const sensor_msgs::PointCloud2 &p) {return p.header.frame_id;}
 
-// this method needs to be implemented by client library developers
-template <>
-inline
-void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 &p_out, const geometry_msgs::TransformStamped& t_in)
+/** \brief Apply the given isometry transform to an xyz-channel in the pointcloud.
+ * \param p_in Input pointcloud.
+ * \param p_out Output pointcloud (can be the same as input).
+ * \param t The transform to apply.
+ * \param channelPrefix Channel name prefix. If prefix is e.g. "vp_", then channels "vp_x", "vp_y" and "vp_z" will be considered.
+ *                      Empty prefix denotes the "x", "y" and "z" channels of point positions.
+ * \param onlyRotation If true, only rotation will be applied (i.e. the channel is just a directional vector).
+ */
+inline void transformChannel(const sensor_msgs::PointCloud2 &p_in,
+    sensor_msgs::PointCloud2 &p_out, const Eigen::Isometry3f& t,
+    const std::string& channelPrefix, bool onlyRotation = false)
+{
+  sensor_msgs::PointCloud2ConstIterator<float> x_in(p_in, channelPrefix + "x");
+  sensor_msgs::PointCloud2ConstIterator<float> y_in(p_in, channelPrefix + "y");
+  sensor_msgs::PointCloud2ConstIterator<float> z_in(p_in, channelPrefix + "z");
+
+  sensor_msgs::PointCloud2Iterator<float> x_out(p_out, channelPrefix + "x");
+  sensor_msgs::PointCloud2Iterator<float> y_out(p_out, channelPrefix + "y");
+  sensor_msgs::PointCloud2Iterator<float> z_out(p_out, channelPrefix + "z");
+
+  Eigen::Vector3f point;
+  for (; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) {
+    if (!onlyRotation)
+      point = t * Eigen::Vector3f(*x_in, *y_in, *z_in);
+    else
+      point = t.linear() * Eigen::Vector3f(*x_in, *y_in, *z_in);
+
+    *x_out = point.x();
+    *y_out = point.y();
+    *z_out = point.z();
+  }
+}
+
+/** \brief Transform given 3D-data channels in the pointcloud using the given transformation.
+ * \param p_in Input point cloud.
+ * \param p_out Output pointcloud (can be the same as input).
+ * \param t_in The transform to apply.
+ * \param n_channels Number of channels to transform.
+ * \param ... Channels are given as pairs (char* channelPrefix, int onlyRotation),
+ *            where the channelPrefix is the channel prefix passed further to transformChannel()
+ *            and onlyRotation specifies whether the whole transform should be applied or only
+ *            its rotation part (useful for transformation of directions, i.e. normals).
+ */
+inline void doTransformChannels(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 &p_out, const geometry_msgs::TransformStamped& t_in, int n_channels, ...)
 {
   p_out = p_in;
   p_out.header = t_in.header;
@@ -75,22 +115,35 @@ void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2
                                                                      t_in.transform.rotation.w, t_in.transform.rotation.x,
                                                                      t_in.transform.rotation.y, t_in.transform.rotation.z);
 
-  sensor_msgs::PointCloud2ConstIterator<float> x_in(p_in, "x");
-  sensor_msgs::PointCloud2ConstIterator<float> y_in(p_in, "y");
-  sensor_msgs::PointCloud2ConstIterator<float> z_in(p_in, "z");
+  // transform the positional channels like points, viewpoints and normals
+  va_list vl;
+  va_start(vl, n_channels);
+  for (int i = 0; i < n_channels; ++i) {
+    const std::string prefix(va_arg(vl, char *));
+    const bool onlyRotation = static_cast<bool>(va_arg(vl, int));
+    const auto xField = prefix + "x";
 
-  sensor_msgs::PointCloud2Iterator<float> x_out(p_out, "x");
-  sensor_msgs::PointCloud2Iterator<float> y_out(p_out, "y");
-  sensor_msgs::PointCloud2Iterator<float> z_out(p_out, "z");
-
-  Eigen::Vector3f point;
-  for(; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) {
-    point = t * Eigen::Vector3f(*x_in, *y_in, *z_in);
-    *x_out = point.x();
-    *y_out = point.y();
-    *z_out = point.z();
+    for (const auto &field : p_in.fields) {
+      if (field.name == xField)
+        transformChannel(p_in, p_out, t, prefix, onlyRotation);
+    }
   }
+  va_end(vl);
 }
+
+// this method needs to be implemented by client library developers
+template <>
+inline
+void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 &p_out, const geometry_msgs::TransformStamped& t_in)
+{
+  doTransformChannels(p_in, p_out, t_in,
+      3,
+      "", false,  // points
+      "vp_", false, // viewpoints
+      "normal_", true // normals
+  );
+}
+
 inline
 sensor_msgs::PointCloud2 toMsg(const sensor_msgs::PointCloud2 &in)
 {

tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py

@@ -41,20 +41,46 @@ def from_msg_msg(msg):
 
 tf2_ros.ConvertRegistration().add_from_msg(PointCloud2, from_msg_msg)
 
+def transform_to_kdl_rotation_only(t):
+    return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y,
+                                                 t.transform.rotation.z, t.transform.rotation.w))
+
 def transform_to_kdl(t):
     return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y,
                                                  t.transform.rotation.z, t.transform.rotation.w),
                        PyKDL.Vector(t.transform.translation.x, 
                                     t.transform.translation.y, 
                                     t.transform.translation.z))
 
-# PointStamped
-def do_transform_cloud(cloud, transform):
+def do_transform_cloud_with_channels(cloud, transform, channels):
     t_kdl = transform_to_kdl(transform)
+    t_kdl_rot = transform_to_kdl_rotation_only(transform)
     points_out = []
     for p_in in read_points(cloud):
-        p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2])
-        points_out.append((p_out[0], p_out[1], p_out[2]) + p_in[3:])
+        fi = 0
+        p_out = []
+        while fi < len(cloud.fields):
+            field = cloud.fields[fi]
+            transformed = False
+            for prefix, only_rotation in channels:
+                if field.name == prefix + "x":
+                    t = t_kdl if not only_rotation else t_kdl_rot
+                    p = t * PyKDL.Vector(p_in[fi + 0], p_in[fi + 1], p_in[fi + 2])
+                    p_out.extend(p)
+                    transformed = True
+                    break
+            if transformed:
+                fi += 3  # skip the following two fields since we've already done them
+            else:
+                p_out.append(p_in[fi])
+                fi += 1
+        points_out.append(p_out)
+
     res = create_cloud(transform.header, cloud.fields, points_out)
     return res
+
+# PointStamped
+def do_transform_cloud(cloud, transform):
+    return do_transform_cloud_with_channels(
+        cloud, transform, channels=[("", False), ("vp_", False), ("normal_", True)])
 tf2_ros.TransformRegistration().add(PointCloud2, do_transform_cloud)

tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp

@@ -77,6 +77,125 @@ TEST(Tf2Sensor, PointCloud2)
   EXPECT_NEAR(*iter_z_advanced, 27, EPS);
 }
 
+TEST(Tf2Sensor, PointCloud2WithChannels)
+{
+  sensor_msgs::PointCloud2 cloud;
+  sensor_msgs::PointCloud2Modifier modifier(cloud);
+  modifier.setPointCloud2Fields(10,
+      "x", 1, sensor_msgs::PointField::FLOAT32,
+      "y", 1, sensor_msgs::PointField::FLOAT32,
+      "z", 1, sensor_msgs::PointField::FLOAT32,
+      "rgb", 1, sensor_msgs::PointField::FLOAT32,
+      "vp_x", 1, sensor_msgs::PointField::FLOAT32,
+      "vp_y", 1, sensor_msgs::PointField::FLOAT32,
+      "vp_z", 1, sensor_msgs::PointField::FLOAT32,
+      "normal_x", 1, sensor_msgs::PointField::FLOAT32,
+      "normal_y", 1, sensor_msgs::PointField::FLOAT32,
+      "normal_z", 1, sensor_msgs::PointField::FLOAT32
+  );
+  modifier.resize(1);
+
+  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
+
+  sensor_msgs::PointCloud2Iterator<float> iter_vp_x(cloud, "vp_x");
+  sensor_msgs::PointCloud2Iterator<float> iter_vp_y(cloud, "vp_y");
+  sensor_msgs::PointCloud2Iterator<float> iter_vp_z(cloud, "vp_z");
+
+  sensor_msgs::PointCloud2Iterator<float> iter_normal_x(cloud, "normal_x");
+  sensor_msgs::PointCloud2Iterator<float> iter_normal_y(cloud, "normal_y");
+  sensor_msgs::PointCloud2Iterator<float> iter_normal_z(cloud, "normal_z");
+
+  *iter_x = *iter_vp_x = *iter_normal_x = 1;
+  *iter_y = *iter_vp_y = *iter_normal_y = 2;
+  *iter_z = *iter_vp_z = *iter_normal_z = 3;
+
+  cloud.header.stamp = ros::Time(2);
+  cloud.header.frame_id = "A";
+
+  // simple api
+  sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", ros::Duration(2.0));
+  sensor_msgs::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z_after(cloud_simple, "z");
+  sensor_msgs::PointCloud2Iterator<float> iter_vp_x_after(cloud_simple, "vp_x");
+  sensor_msgs::PointCloud2Iterator<float> iter_vp_y_after(cloud_simple, "vp_y");
+  sensor_msgs::PointCloud2Iterator<float> iter_vp_z_after(cloud_simple, "vp_z");
+  sensor_msgs::PointCloud2Iterator<float> iter_normal_x_after(cloud_simple, "normal_x");
+  sensor_msgs::PointCloud2Iterator<float> iter_normal_y_after(cloud_simple, "normal_y");
+  sensor_msgs::PointCloud2Iterator<float> iter_normal_z_after(cloud_simple, "normal_z");
+  EXPECT_NEAR(*iter_x_after, -9, EPS);
+  EXPECT_NEAR(*iter_y_after, 18, EPS);
+  EXPECT_NEAR(*iter_z_after, 27, EPS);
+  EXPECT_NEAR(*iter_vp_x_after, -9, EPS);
+  EXPECT_NEAR(*iter_vp_y_after, 18, EPS);
+  EXPECT_NEAR(*iter_vp_z_after, 27, EPS);
+  EXPECT_NEAR(*iter_normal_x_after, 1, EPS);
+  EXPECT_NEAR(*iter_normal_y_after, -2, EPS);
+  EXPECT_NEAR(*iter_normal_z_after, -3, EPS);
+}
+
+TEST(Tf2Sensor, PointCloud2WithSomeChannels)
+{
+  sensor_msgs::PointCloud2 cloud;
+  sensor_msgs::PointCloud2Modifier modifier(cloud);
+  modifier.setPointCloud2Fields(10,
+                                "x", 1, sensor_msgs::PointField::FLOAT32,
+                                "y", 1, sensor_msgs::PointField::FLOAT32,
+                                "z", 1, sensor_msgs::PointField::FLOAT32,
+                                "rgb", 1, sensor_msgs::PointField::FLOAT32,
+                                "vp_x", 1, sensor_msgs::PointField::FLOAT32,
+                                "vp_y", 1, sensor_msgs::PointField::FLOAT32,
+                                "vp_z", 1, sensor_msgs::PointField::FLOAT32,
+                                "normal_x", 1, sensor_msgs::PointField::FLOAT32,
+                                "normal_y", 1, sensor_msgs::PointField::FLOAT32,
+                                "normal_z", 1, sensor_msgs::PointField::FLOAT32
+  );
+  modifier.resize(1);
+
+  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
+
+  sensor_msgs::PointCloud2Iterator<float> iter_vp_x(cloud, "vp_x");
+  sensor_msgs::PointCloud2Iterator<float> iter_vp_y(cloud, "vp_y");
+  sensor_msgs::PointCloud2Iterator<float> iter_vp_z(cloud, "vp_z");
+
+  sensor_msgs::PointCloud2Iterator<float> iter_normal_x(cloud, "normal_x");
+  sensor_msgs::PointCloud2Iterator<float> iter_normal_y(cloud, "normal_y");
+  sensor_msgs::PointCloud2Iterator<float> iter_normal_z(cloud, "normal_z");
+
+  *iter_x = *iter_vp_x = *iter_normal_x = 1;
+  *iter_y = *iter_vp_y = *iter_normal_y = 2;
+  *iter_z = *iter_vp_z = *iter_normal_z = 3;
+
+  cloud.header.stamp = ros::Time(2);
+  cloud.header.frame_id = "A";
+
+  const auto tf = tf_buffer->lookupTransform("B", "A", ros::Time(2));
+  sensor_msgs::PointCloud2 cloud_simple;
+  tf2::doTransformChannels(cloud, cloud_simple, tf, 2, "", false, "normal_", true);
+  sensor_msgs::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z_after(cloud_simple, "z");
+  sensor_msgs::PointCloud2Iterator<float> iter_vp_x_after(cloud_simple, "vp_x");
+  sensor_msgs::PointCloud2Iterator<float> iter_vp_y_after(cloud_simple, "vp_y");
+  sensor_msgs::PointCloud2Iterator<float> iter_vp_z_after(cloud_simple, "vp_z");
+  sensor_msgs::PointCloud2Iterator<float> iter_normal_x_after(cloud_simple, "normal_x");
+  sensor_msgs::PointCloud2Iterator<float> iter_normal_y_after(cloud_simple, "normal_y");
+  sensor_msgs::PointCloud2Iterator<float> iter_normal_z_after(cloud_simple, "normal_z");
+  EXPECT_NEAR(*iter_x_after, -9, EPS);
+  EXPECT_NEAR(*iter_y_after, 18, EPS);
+  EXPECT_NEAR(*iter_z_after, 27, EPS);
+  EXPECT_NEAR(*iter_vp_x_after, 1, EPS);
+  EXPECT_NEAR(*iter_vp_y_after, 2, EPS);
+  EXPECT_NEAR(*iter_vp_z_after, 3, EPS);
+  EXPECT_NEAR(*iter_normal_x_after, 1, EPS);
+  EXPECT_NEAR(*iter_normal_y_after, -2, EPS);
+  EXPECT_NEAR(*iter_normal_z_after, -3, EPS);
+}
+
 int main(int argc, char **argv){
   testing::InitGoogleTest(&argc, argv);
   ros::init(argc, argv, "test");

tf2_sensor_msgs/test/test_tf2_sensor_msgs.py

@@ -96,8 +96,78 @@ def test_simple_transform_multichannel(self):
         assert(old_data == self.point_cloud_in.data)  # checking no modification in input cloud
 
 
+## A simple unit test for tf2_sensor_msgs.do_transform_cloud_with_channels
+class PointCloudConversionsMulti3Dchannel(unittest.TestCase):
+    TRANSFORM_OFFSET_DISTANCE = 300
+
+    def setUp(self):
+        self.point_cloud_in = point_cloud2.PointCloud2()
+        self.point_cloud_in.fields = [PointField('x', 0, PointField.FLOAT32, 1),
+                                PointField('y', 4, PointField.FLOAT32, 1),
+                                PointField('z', 8, PointField.FLOAT32, 1),
+                                PointField('index', 12, PointField.INT32, 1),
+                                PointField('vp_x', 16, PointField.FLOAT32, 1),
+                                PointField('vp_y', 20, PointField.FLOAT32, 1),
+                                PointField('vp_z', 24, PointField.FLOAT32, 1),
+                                PointField('normal_x', 28, PointField.FLOAT32, 1),
+                                PointField('normal_y', 32, PointField.FLOAT32, 1),
+                                PointField('normal_z', 36, PointField.FLOAT32, 1)]
+
+        self.point_cloud_in.point_step = len(self.point_cloud_in.fields) * 4
+        self.point_cloud_in.height = 1
+        # we add two points (with x, y, z to the cloud)
+        self.point_cloud_in.width = 2
+        self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width
+
+        self.points = [(1.0, 2.0, 0.0, 123, 1.0, 2.0, 0.0, 1.0, 2.0, 0.0),
+                       (10.0, 20.0, 30.0, 456, 10.0, 20.0, 30.0, 10.0, 20.0, 30.0)]
+        for point in self.points:
+            self.point_cloud_in.data += struct.pack('3fi6f', *point)
+
+        self.transform_translate_xyz_300 = TransformStamped()
+        self.transform_translate_xyz_300.transform.translation.x = self.TRANSFORM_OFFSET_DISTANCE
+        self.transform_translate_xyz_300.transform.translation.y = self.TRANSFORM_OFFSET_DISTANCE
+        self.transform_translate_xyz_300.transform.translation.z = self.TRANSFORM_OFFSET_DISTANCE
+        self.transform_translate_xyz_300.transform.rotation.w = 1  # no rotation so we only set w
+
+        self.transform_translate_xyz_300_flip_x = TransformStamped()
+        self.transform_translate_xyz_300_flip_x.transform.translation.x = self.TRANSFORM_OFFSET_DISTANCE
+        self.transform_translate_xyz_300_flip_x.transform.translation.y = self.TRANSFORM_OFFSET_DISTANCE
+        self.transform_translate_xyz_300_flip_x.transform.translation.z = self.TRANSFORM_OFFSET_DISTANCE
+        self.transform_translate_xyz_300_flip_x.transform.rotation.x = 1
+
+        assert(list(point_cloud2.read_points(self.point_cloud_in)) == self.points)
+
+    def test_simple_transform_multichannel(self):
+        old_data = copy.deepcopy(self.point_cloud_in.data)  # deepcopy is not required here because we have a str for now
+        point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300)
+
+        # only translation
+
+        expected_coordinates = [(301.0, 302.0, 300.0, 123, 301.0, 302.0, 300.0, 1.0, 2.0, 0.0),
+                                (310.0, 320.0, 330.0, 456, 310.0, 320.0, 330.0, 10.0, 20.0, 30.0)]
+
+        new_points = list(point_cloud2.read_points(point_cloud_transformed))
+        print("new_points are %s" % new_points)
+        assert(expected_coordinates == new_points)
+        assert(old_data == self.point_cloud_in.data)  # checking no modification in input cloud
+
+        # both translation and rotation
+
+        point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300_flip_x)
+
+        expected_coordinates = [(301.0, 298.0, 300.0, 123, 301.0, 298.0, 300.0, 1.0, -2.0, 0.0),
+                                (310.0, 280.0, 270.0, 456, 310.0, 280.0, 270.0, 10.0, -20.0, -30.0)]
+
+        new_points = list(point_cloud2.read_points(point_cloud_transformed))
+        print("new_points are %s" % new_points)
+        assert(expected_coordinates == new_points)
+        assert(old_data == self.point_cloud_in.data)  # checking no modification in input cloud
+
+
 if __name__ == '__main__':
     import rosunit
     rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversions)
     rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversionsMultichannel)
+    rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversionsMulti3Dchannel)