diff --git a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h index 9e16e0d2e..e77383052 100644 --- a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h +++ b/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 x_in(p_in, channelPrefix + "x"); + sensor_msgs::PointCloud2ConstIterator y_in(p_in, channelPrefix + "y"); + sensor_msgs::PointCloud2ConstIterator z_in(p_in, channelPrefix + "z"); + + sensor_msgs::PointCloud2Iterator x_out(p_out, channelPrefix + "x"); + sensor_msgs::PointCloud2Iterator y_out(p_out, channelPrefix + "y"); + sensor_msgs::PointCloud2Iterator 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 x_in(p_in, "x"); - sensor_msgs::PointCloud2ConstIterator y_in(p_in, "y"); - sensor_msgs::PointCloud2ConstIterator 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(va_arg(vl, int)); + const auto xField = prefix + "x"; - sensor_msgs::PointCloud2Iterator x_out(p_out, "x"); - sensor_msgs::PointCloud2Iterator y_out(p_out, "y"); - sensor_msgs::PointCloud2Iterator 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) { diff --git a/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py b/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py index 74cacd30b..3eb8b8582 100644 --- a/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py +++ b/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py @@ -41,6 +41,10 @@ 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), @@ -48,13 +52,35 @@ def transform_to_kdl(t): 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) diff --git a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp index 860750133..7911e6de2 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp +++ b/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 iter_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); + + sensor_msgs::PointCloud2Iterator iter_vp_x(cloud, "vp_x"); + sensor_msgs::PointCloud2Iterator iter_vp_y(cloud, "vp_y"); + sensor_msgs::PointCloud2Iterator iter_vp_z(cloud, "vp_z"); + + sensor_msgs::PointCloud2Iterator iter_normal_x(cloud, "normal_x"); + sensor_msgs::PointCloud2Iterator iter_normal_y(cloud, "normal_y"); + sensor_msgs::PointCloud2Iterator 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 iter_x_after(cloud_simple, "x"); + sensor_msgs::PointCloud2Iterator iter_y_after(cloud_simple, "y"); + sensor_msgs::PointCloud2Iterator iter_z_after(cloud_simple, "z"); + sensor_msgs::PointCloud2Iterator iter_vp_x_after(cloud_simple, "vp_x"); + sensor_msgs::PointCloud2Iterator iter_vp_y_after(cloud_simple, "vp_y"); + sensor_msgs::PointCloud2Iterator iter_vp_z_after(cloud_simple, "vp_z"); + sensor_msgs::PointCloud2Iterator iter_normal_x_after(cloud_simple, "normal_x"); + sensor_msgs::PointCloud2Iterator iter_normal_y_after(cloud_simple, "normal_y"); + sensor_msgs::PointCloud2Iterator 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 iter_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); + + sensor_msgs::PointCloud2Iterator iter_vp_x(cloud, "vp_x"); + sensor_msgs::PointCloud2Iterator iter_vp_y(cloud, "vp_y"); + sensor_msgs::PointCloud2Iterator iter_vp_z(cloud, "vp_z"); + + sensor_msgs::PointCloud2Iterator iter_normal_x(cloud, "normal_x"); + sensor_msgs::PointCloud2Iterator iter_normal_y(cloud, "normal_y"); + sensor_msgs::PointCloud2Iterator 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 iter_x_after(cloud_simple, "x"); + sensor_msgs::PointCloud2Iterator iter_y_after(cloud_simple, "y"); + sensor_msgs::PointCloud2Iterator iter_z_after(cloud_simple, "z"); + sensor_msgs::PointCloud2Iterator iter_vp_x_after(cloud_simple, "vp_x"); + sensor_msgs::PointCloud2Iterator iter_vp_y_after(cloud_simple, "vp_y"); + sensor_msgs::PointCloud2Iterator iter_vp_z_after(cloud_simple, "vp_z"); + sensor_msgs::PointCloud2Iterator iter_normal_x_after(cloud_simple, "normal_x"); + sensor_msgs::PointCloud2Iterator iter_normal_y_after(cloud_simple, "normal_y"); + sensor_msgs::PointCloud2Iterator 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"); diff --git a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py index b797b7a92..dbcf64bb6 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py +++ b/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)