diff --git a/common/include/pcl/common/impl/transforms.hpp b/common/include/pcl/common/impl/transforms.hpp index c686da561a2..808e18b019c 100644 --- a/common/include/pcl/common/impl/transforms.hpp +++ b/common/include/pcl/common/impl/transforms.hpp @@ -41,17 +41,20 @@ template void pcl::transformPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, - const Eigen::Transform &transform) + const Eigen::Transform &transform, + bool copy_all_fields) { if (&cloud_in != &cloud_out) { - // Note: could be replaced by cloud_out = cloud_in cloud_out.header = cloud_in.header; cloud_out.is_dense = cloud_in.is_dense; cloud_out.width = cloud_in.width; cloud_out.height = cloud_in.height; - cloud_out.points.reserve (cloud_out.points.size ()); - cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ()); + cloud_out.points.reserve (cloud_in.points.size ()); + if (copy_all_fields) + cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ()); + else + cloud_out.points.resize (cloud_in.points.size ()); cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; cloud_out.sensor_origin_ = cloud_in.sensor_origin_; } @@ -92,7 +95,8 @@ template void pcl::transformPointCloud (const pcl::PointCloud &cloud_in, const std::vector &indices, pcl::PointCloud &cloud_out, - const Eigen::Transform &transform) + const Eigen::Transform &transform, + bool copy_all_fields) { size_t npts = indices.size (); // In order to transform the data, we need to remove NaNs @@ -110,7 +114,8 @@ pcl::transformPointCloud (const pcl::PointCloud &cloud_in, for (size_t i = 0; i < npts; ++i) { // Copy fields first, then transform xyz data - //cloud_out.points[i] = cloud_in.points[indices[i]]; + if (copy_all_fields) + cloud_out.points[i] = cloud_in.points[indices[i]]; //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap (); Eigen::Matrix pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z); cloud_out[i].x = static_cast (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3)); @@ -124,11 +129,12 @@ pcl::transformPointCloud (const pcl::PointCloud &cloud_in, // otherwise we get errors during the multiplication (?) for (size_t i = 0; i < npts; ++i) { + if (copy_all_fields) + cloud_out.points[i] = cloud_in.points[indices[i]]; if (!pcl_isfinite (cloud_in.points[indices[i]].x) || !pcl_isfinite (cloud_in.points[indices[i]].y) || !pcl_isfinite (cloud_in.points[indices[i]].z)) continue; - //cloud_out.points[i] = cloud_in.points[indices[i]]; //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap (); Eigen::Matrix pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z); cloud_out[i].x = static_cast (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3)); @@ -142,7 +148,8 @@ pcl::transformPointCloud (const pcl::PointCloud &cloud_in, template void pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, - const Eigen::Transform &transform) + const Eigen::Transform &transform, + bool copy_all_fields) { if (&cloud_in != &cloud_out) { @@ -152,7 +159,10 @@ pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, cloud_out.height = cloud_in.height; cloud_out.is_dense = cloud_in.is_dense; cloud_out.points.reserve (cloud_out.points.size ()); - cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ()); + if (copy_all_fields) + cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ()); + else + cloud_out.points.resize (cloud_in.points.size ()); cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; cloud_out.sensor_origin_ = cloud_in.sensor_origin_; } @@ -207,7 +217,8 @@ template void pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, const std::vector &indices, pcl::PointCloud &cloud_out, - const Eigen::Transform &transform) + const Eigen::Transform &transform, + bool copy_all_fields) { size_t npts = indices.size (); // In order to transform the data, we need to remove NaNs @@ -224,6 +235,9 @@ pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, { for (size_t i = 0; i < cloud_out.points.size (); ++i) { + // Copy fields first, then transform + if (copy_all_fields) + cloud_out.points[i] = cloud_in.points[indices[i]]; //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap (); Eigen::Matrix pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z); cloud_out[i].x = static_cast (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3)); @@ -243,6 +257,10 @@ pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, { for (size_t i = 0; i < cloud_out.points.size (); ++i) { + // Copy fields first, then transform + if (copy_all_fields) + cloud_out.points[i] = cloud_in.points[indices[i]]; + if (!pcl_isfinite (cloud_in.points[indices[i]].x) || !pcl_isfinite (cloud_in.points[indices[i]].y) || !pcl_isfinite (cloud_in.points[indices[i]].z)) @@ -269,12 +287,13 @@ template inline void pcl::transformPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const Eigen::Matrix &offset, - const Eigen::Quaternion &rotation) + const Eigen::Quaternion &rotation, + bool copy_all_fields) { Eigen::Translation translation (offset); // Assemble an Eigen Transform Eigen::Transform t (translation * rotation); - transformPointCloud (cloud_in, cloud_out, t); + transformPointCloud (cloud_in, cloud_out, t, copy_all_fields); } /////////////////////////////////////////////////////////////////////////////////////////// @@ -282,12 +301,13 @@ template inline void pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const Eigen::Matrix &offset, - const Eigen::Quaternion &rotation) + const Eigen::Quaternion &rotation, + bool copy_all_fields) { Eigen::Translation translation (offset); // Assemble an Eigen Transform Eigen::Transform t (translation * rotation); - transformPointCloudWithNormals (cloud_in, cloud_out, t); + transformPointCloudWithNormals (cloud_in, cloud_out, t, copy_all_fields); } /////////////////////////////////////////////////////////////////////////////////////////// diff --git a/common/include/pcl/common/transforms.h b/common/include/pcl/common/transforms.h index 1f0a85a5eac..fa962c98c3f 100644 --- a/common/include/pcl/common/transforms.h +++ b/common/include/pcl/common/transforms.h @@ -51,20 +51,24 @@ namespace pcl * \param[in] cloud_in the input point cloud * \param[out] cloud_out the resultant output point cloud * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z) should be copied into the new transformed cloud * \note Can be used with cloud_in equal to cloud_out * \ingroup common */ template void transformPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, - const Eigen::Transform &transform); + const Eigen::Transform &transform, + bool copy_all_fields = true); template void transformPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, - const Eigen::Affine3f &transform) + const Eigen::Affine3f &transform, + bool copy_all_fields = true) { - return (transformPointCloud (cloud_in, cloud_out, transform)); + return (transformPointCloud (cloud_in, cloud_out, transform, copy_all_fields)); } /** \brief Apply an affine transform defined by an Eigen Transform @@ -72,21 +76,25 @@ namespace pcl * \param[in] indices the set of point indices to use from the input point cloud * \param[out] cloud_out the resultant output point cloud * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z) should be copied into the new transformed cloud * \ingroup common */ template void transformPointCloud (const pcl::PointCloud &cloud_in, const std::vector &indices, pcl::PointCloud &cloud_out, - const Eigen::Transform &transform); + const Eigen::Transform &transform, + bool copy_all_fields = true); template void transformPointCloud (const pcl::PointCloud &cloud_in, const std::vector &indices, pcl::PointCloud &cloud_out, - const Eigen::Affine3f &transform) + const Eigen::Affine3f &transform, + bool copy_all_fields = true) { - return (transformPointCloud (cloud_in, indices, cloud_out, transform)); + return (transformPointCloud (cloud_in, indices, cloud_out, transform, copy_all_fields)); } /** \brief Apply an affine transform defined by an Eigen Transform @@ -94,43 +102,52 @@ namespace pcl * \param[in] indices the set of point indices to use from the input point cloud * \param[out] cloud_out the resultant output point cloud * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z) should be copied into the new transformed cloud * \ingroup common */ template void transformPointCloud (const pcl::PointCloud &cloud_in, const pcl::PointIndices &indices, pcl::PointCloud &cloud_out, - const Eigen::Transform &transform) + const Eigen::Transform &transform, + bool copy_all_fields = true) { - return (transformPointCloud (cloud_in, indices.indices, cloud_out, transform)); + return (transformPointCloud (cloud_in, indices.indices, cloud_out, transform, copy_all_fields)); } template void transformPointCloud (const pcl::PointCloud &cloud_in, const pcl::PointIndices &indices, pcl::PointCloud &cloud_out, - const Eigen::Affine3f &transform) + const Eigen::Affine3f &transform, + bool copy_all_fields = true) { - return (transformPointCloud (cloud_in, indices, cloud_out, transform)); + return (transformPointCloud (cloud_in, indices, cloud_out, transform, copy_all_fields)); } /** \brief Transform a point cloud and rotate its normals using an Eigen transform. * \param[in] cloud_in the input point cloud * \param[out] cloud_out the resultant output point cloud * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new + * transformed cloud * \note Can be used with cloud_in equal to cloud_out */ template void transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, - const Eigen::Transform &transform); + const Eigen::Transform &transform, + bool copy_all_fields = true); template void transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, - const Eigen::Affine3f &transform) + const Eigen::Affine3f &transform, + bool copy_all_fields = true) { - return (transformPointCloudWithNormals (cloud_in, cloud_out, transform)); + return (transformPointCloudWithNormals (cloud_in, cloud_out, transform, copy_all_fields)); } /** \brief Transform a point cloud and rotate its normals using an Eigen transform. @@ -138,20 +155,25 @@ namespace pcl * \param[in] indices the set of point indices to use from the input point cloud * \param[out] cloud_out the resultant output point cloud * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new + * transformed cloud */ template void transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, const std::vector &indices, pcl::PointCloud &cloud_out, - const Eigen::Transform &transform); + const Eigen::Transform &transform, + bool copy_all_fields = true); template void transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, const std::vector &indices, pcl::PointCloud &cloud_out, - const Eigen::Affine3f &transform) + const Eigen::Affine3f &transform, + bool copy_all_fields = true) { - return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, transform)); + return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, transform, copy_all_fields)); } /** \brief Transform a point cloud and rotate its normals using an Eigen transform. @@ -159,14 +181,18 @@ namespace pcl * \param[in] indices the set of point indices to use from the input point cloud * \param[out] cloud_out the resultant output point cloud * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new + * transformed cloud */ template void transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, const pcl::PointIndices &indices, pcl::PointCloud &cloud_out, - const Eigen::Transform &transform) + const Eigen::Transform &transform, + bool copy_all_fields = true) { - return (transformPointCloudWithNormals (cloud_in, indices.indices, cloud_out, transform)); + return (transformPointCloudWithNormals (cloud_in, indices.indices, cloud_out, transform, copy_all_fields)); } @@ -174,33 +200,38 @@ namespace pcl transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, const pcl::PointIndices &indices, pcl::PointCloud &cloud_out, - const Eigen::Affine3f &transform) + const Eigen::Affine3f &transform, + bool copy_all_fields = true) { - return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, transform)); + return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, transform, copy_all_fields)); } /** \brief Apply a rigid transform defined by a 4x4 matrix * \param[in] cloud_in the input point cloud * \param[out] cloud_out the resultant output point cloud * \param[in] transform a rigid transformation + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z) should be copied into the new transformed cloud * \note Can be used with cloud_in equal to cloud_out * \ingroup common */ template void transformPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, - const Eigen::Matrix &transform) + const Eigen::Matrix &transform, + bool copy_all_fields = true) { Eigen::Transform t (transform); - return (transformPointCloud (cloud_in, cloud_out, t)); + return (transformPointCloud (cloud_in, cloud_out, t, copy_all_fields)); } template void transformPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, - const Eigen::Matrix4f &transform) + const Eigen::Matrix4f &transform, + bool copy_all_fields = true) { - return (transformPointCloud (cloud_in, cloud_out, transform)); + return (transformPointCloud (cloud_in, cloud_out, transform, copy_all_fields)); } /** \brief Apply a rigid transform defined by a 4x4 matrix @@ -208,25 +239,29 @@ namespace pcl * \param[in] indices the set of point indices to use from the input point cloud * \param[out] cloud_out the resultant output point cloud * \param[in] transform a rigid transformation + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z) should be copied into the new transformed cloud * \ingroup common */ template void transformPointCloud (const pcl::PointCloud &cloud_in, const std::vector &indices, pcl::PointCloud &cloud_out, - const Eigen::Matrix &transform) + const Eigen::Matrix &transform, + bool copy_all_fields = true) { Eigen::Transform t (transform); - return (transformPointCloud (cloud_in, indices, cloud_out, t)); + return (transformPointCloud (cloud_in, indices, cloud_out, t, copy_all_fields)); } template void transformPointCloud (const pcl::PointCloud &cloud_in, const std::vector &indices, pcl::PointCloud &cloud_out, - const Eigen::Matrix4f &transform) + const Eigen::Matrix4f &transform, + bool copy_all_fields = true) { - return (transformPointCloud (cloud_in, indices, cloud_out, transform)); + return (transformPointCloud (cloud_in, indices, cloud_out, transform, copy_all_fields)); } /** \brief Apply a rigid transform defined by a 4x4 matrix @@ -234,49 +269,58 @@ namespace pcl * \param[in] indices the set of point indices to use from the input point cloud * \param[out] cloud_out the resultant output point cloud * \param[in] transform a rigid transformation + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z) should be copied into the new transformed cloud * \ingroup common */ template void transformPointCloud (const pcl::PointCloud &cloud_in, const pcl::PointIndices &indices, pcl::PointCloud &cloud_out, - const Eigen::Matrix &transform) + const Eigen::Matrix &transform, + bool copy_all_fields = true) { - return (transformPointCloud (cloud_in, indices.indices, cloud_out, transform)); + return (transformPointCloud (cloud_in, indices.indices, cloud_out, transform, copy_all_fields)); } template void transformPointCloud (const pcl::PointCloud &cloud_in, const pcl::PointIndices &indices, pcl::PointCloud &cloud_out, - const Eigen::Matrix4f &transform) + const Eigen::Matrix4f &transform, + bool copy_all_fields = true) { - return (transformPointCloud (cloud_in, indices, cloud_out, transform)); + return (transformPointCloud (cloud_in, indices, cloud_out, transform, copy_all_fields)); } /** \brief Transform a point cloud and rotate its normals using an Eigen transform. * \param[in] cloud_in the input point cloud * \param[out] cloud_out the resultant output point cloud * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new + * transformed cloud * \note Can be used with cloud_in equal to cloud_out * \ingroup common */ template void transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, - const Eigen::Matrix &transform) + const Eigen::Matrix &transform, + bool copy_all_fields = true) { Eigen::Transform t (transform); - return (transformPointCloudWithNormals (cloud_in, cloud_out, t)); + return (transformPointCloudWithNormals (cloud_in, cloud_out, t, copy_all_fields)); } template void transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, - const Eigen::Matrix4f &transform) + const Eigen::Matrix4f &transform, + bool copy_all_fields = true) { - return (transformPointCloudWithNormals (cloud_in, cloud_out, transform)); + return (transformPointCloudWithNormals (cloud_in, cloud_out, transform, copy_all_fields)); } /** \brief Transform a point cloud and rotate its normals using an Eigen transform. @@ -284,6 +328,9 @@ namespace pcl * \param[in] indices the set of point indices to use from the input point cloud * \param[out] cloud_out the resultant output point cloud * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new + * transformed cloud * \note Can be used with cloud_in equal to cloud_out * \ingroup common */ @@ -291,10 +338,11 @@ namespace pcl transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, const std::vector &indices, pcl::PointCloud &cloud_out, - const Eigen::Matrix &transform) + const Eigen::Matrix &transform, + bool copy_all_fields = true) { Eigen::Transform t (transform); - return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, t)); + return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, t, copy_all_fields)); } @@ -302,9 +350,10 @@ namespace pcl transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, const std::vector &indices, pcl::PointCloud &cloud_out, - const Eigen::Matrix4f &transform) + const Eigen::Matrix4f &transform, + bool copy_all_fields = true) { - return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, transform)); + return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, transform, copy_all_fields)); } /** \brief Transform a point cloud and rotate its normals using an Eigen transform. @@ -312,6 +361,9 @@ namespace pcl * \param[in] indices the set of point indices to use from the input point cloud * \param[out] cloud_out the resultant output point cloud * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new + * transformed cloud * \note Can be used with cloud_in equal to cloud_out * \ingroup common */ @@ -319,10 +371,11 @@ namespace pcl transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, const pcl::PointIndices &indices, pcl::PointCloud &cloud_out, - const Eigen::Matrix &transform) + const Eigen::Matrix &transform, + bool copy_all_fields = true) { Eigen::Transform t (transform); - return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, t)); + return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, t, copy_all_fields)); } @@ -330,9 +383,10 @@ namespace pcl transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, const pcl::PointIndices &indices, pcl::PointCloud &cloud_out, - const Eigen::Matrix4f &transform) + const Eigen::Matrix4f &transform, + bool copy_all_fields = true) { - return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, transform)); + return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, transform, copy_all_fields)); } /** \brief Apply a rigid transform defined by a 3D offset and a quaternion @@ -340,21 +394,25 @@ namespace pcl * \param[out] cloud_out the resultant output point cloud * \param[in] offset the translation component of the rigid transformation * \param[in] rotation the rotation component of the rigid transformation + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z) should be copied into the new transformed cloud * \ingroup common */ template inline void transformPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const Eigen::Matrix &offset, - const Eigen::Quaternion &rotation); + const Eigen::Quaternion &rotation, + bool copy_all_fields = true); template inline void transformPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const Eigen::Vector3f &offset, - const Eigen::Quaternionf &rotation) + const Eigen::Quaternionf &rotation, + bool copy_all_fields = true) { - return (transformPointCloud (cloud_in, cloud_out, offset, rotation)); + return (transformPointCloud (cloud_in, cloud_out, offset, rotation, copy_all_fields)); } /** \brief Transform a point cloud and rotate its normals using an Eigen transform. @@ -362,21 +420,26 @@ namespace pcl * \param[out] cloud_out the resultant output point cloud * \param[in] offset the translation component of the rigid transformation * \param[in] rotation the rotation component of the rigid transformation + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new + * transformed cloud * \ingroup common */ template inline void transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const Eigen::Matrix &offset, - const Eigen::Quaternion &rotation); + const Eigen::Quaternion &rotation, + bool copy_all_fields = true); template void transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const Eigen::Vector3f &offset, - const Eigen::Quaternionf &rotation) + const Eigen::Quaternionf &rotation, + bool copy_all_fields = true) { - return (transformPointCloudWithNormals (cloud_in, cloud_out, offset, rotation)); + return (transformPointCloudWithNormals (cloud_in, cloud_out, offset, rotation, copy_all_fields)); } /** \brief Transform a point with members x,y,z diff --git a/test/test_transforms.cpp b/test/test_transforms.cpp index 4ada8931dd5..de15ab72185 100644 --- a/test/test_transforms.cpp +++ b/test/test_transforms.cpp @@ -45,6 +45,8 @@ #include #include +#include + using namespace pcl; using namespace pcl::io; using namespace std; @@ -189,6 +191,81 @@ TEST (PCL, Transform) EXPECT_EQ (1, points2[3].z); } +////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, TransformCopyFields) +{ + Eigen::Affine3f transform; + transform = Eigen::Translation3f (100, 0, 0); + + PointXYZRGBNormal empty_point; + std::vector indices (1); + + PointCloud cloud (2, 1); + cloud.points[0].rgba = 0xFF0000; + cloud.points[1].rgba = 0x00FF00; + + // Preserve data in all fields + { + PointCloud cloud_out; + transformPointCloud (cloud, cloud_out, transform, true); + ASSERT_EQ (cloud.size (), cloud_out.size ()); + EXPECT_RGBA_EQ (cloud.points[0], cloud_out.points[0]); + EXPECT_RGBA_EQ (cloud.points[1], cloud_out.points[1]); + } + // Preserve data in all fields (with indices) + { + PointCloud cloud_out; + transformPointCloud (cloud, indices, cloud_out, transform, true); + ASSERT_EQ (indices.size (), cloud_out.size ()); + EXPECT_RGBA_EQ (cloud.points[0], cloud_out.points[0]); + } + // Do not preserve data in all fields + { + PointCloud cloud_out; + transformPointCloud (cloud, cloud_out, transform, false); + ASSERT_EQ (cloud.size (), cloud_out.size ()); + EXPECT_RGBA_EQ (empty_point, cloud_out.points[0]); + EXPECT_RGBA_EQ (empty_point, cloud_out.points[1]); + } + // Do not preserve data in all fields (with indices) + { + PointCloud cloud_out; + transformPointCloud (cloud, indices, cloud_out, transform, false); + ASSERT_EQ (indices.size (), cloud_out.size ()); + EXPECT_RGBA_EQ (empty_point, cloud_out.points[0]); + } + // Preserve data in all fields (with normals version) + { + PointCloud cloud_out; + transformPointCloudWithNormals (cloud, cloud_out, transform, true); + ASSERT_EQ (cloud.size (), cloud_out.size ()); + EXPECT_RGBA_EQ (cloud.points[0], cloud_out.points[0]); + EXPECT_RGBA_EQ (cloud.points[1], cloud_out.points[1]); + } + // Preserve data in all fields (with normals and indices version) + { + PointCloud cloud_out; + transformPointCloudWithNormals (cloud, indices, cloud_out, transform, true); + ASSERT_EQ (indices.size (), cloud_out.size ()); + EXPECT_RGBA_EQ (cloud.points[0], cloud_out.points[0]); + } + // Do not preserve data in all fields (with normals version) + { + PointCloud cloud_out; + transformPointCloudWithNormals (cloud, cloud_out, transform, false); + ASSERT_EQ (cloud.size (), cloud_out.size ()); + EXPECT_RGBA_EQ (empty_point, cloud_out.points[0]); + EXPECT_RGBA_EQ (empty_point, cloud_out.points[1]); + } + // Do not preserve data in all fields (with normals and indices version) + { + PointCloud cloud_out; + transformPointCloudWithNormals (cloud, indices, cloud_out, transform, false); + ASSERT_EQ (indices.size (), cloud_out.size ()); + EXPECT_RGBA_EQ (empty_point, cloud_out.points[0]); + } +} + ////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, Matrix4Affine3Transform) {