[transformPointcloud] The fourth entry of point is changed during SE3/SO3 transformation

Describe the bug

In release 1.9.0, After the commit 04ae840eb43c78f0e348c35271932c5d6d350759, "Improve speed of transformPointCloud/WithNormals() functions by SSE2/AVX intrinsic", the behavior of transformPointCloud is changed.

In the old code, the x,y,z component are transformed, and the fourth entry (w) is not touched.

for (size_t i = 0; i < cloud_out.points.size (); ++i)
{
  //cloud_out.points[i].getVector3fMap () = transform * cloud_in.points[i].getVector3fMap ();
  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
  cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
  cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
  cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
}

But in the above commit, the transformation is unified to a so3() / se3() function. And in those functions, the fourth entry of point is changed to zero or one. Why do we need this change ?

/** Apply SO3 transform (top-left corner of the transform matrix).
  * \param[in] src input 3D point (pointer to 3 floats)
  * \param[out] tgt output 3D point (pointer to 4 floats), can be the same as input. The fourth element is set to 0. */
void so3 (const float* src, float* tgt) const
{
  const Scalar p[3] = { src[0], src[1], src[2] };  // need this when src == tgt
  tgt[0] = static_cast<float> (tf (0, 0) * p[0] + tf (0, 1) * p[1] + tf (0, 2) * p[2]);
  tgt[1] = static_cast<float> (tf (1, 0) * p[0] + tf (1, 1) * p[1] + tf (1, 2) * p[2]);
  tgt[2] = static_cast<float> (tf (2, 0) * p[0] + tf (2, 1) * p[1] + tf (2, 2) * p[2]);
  tgt[3] = 0;
}

/** Apply SE3 transform.
  * \param[in] src input 3D point (pointer to 3 floats)
  * \param[out] tgt output 3D point (pointer to 4 floats), can be the same as input. The fourth element is set to 1. */
void se3 (const float* src, float* tgt) const
{
  const Scalar p[3] = { src[0], src[1], src[2] };  // need this when src == tgt
  tgt[0] = static_cast<float> (tf (0, 0) * p[0] + tf (0, 1) * p[1] + tf (0, 2) * p[2] + tf (0, 3));
  tgt[1] = static_cast<float> (tf (1, 0) * p[0] + tf (1, 1) * p[1] + tf (1, 2) * p[2] + tf (1, 3));
  tgt[2] = static_cast<float> (tf (2, 0) * p[0] + tf (2, 1) * p[1] + tf (2, 2) * p[2] + tf (2, 3));
  tgt[3] = 1;
}

Normally if a user use the predefined point type in PCL, there will be no different. But I used a customized point type where I pack the intensity value of point into the fourth entry, instead of using the predefined pcl::PointXYZI type where the intensity is stored separately from the four floats of xyzw.

Context

The behavior of transformPointcloud() function changed at release 1.9.0

Expected behavior

The fourth entry of point should not be changed.

Current Behavior

The fourth entry of point is changed to zero or one

To Reproduce

Provide a link to a live example, or an unambiguous set of steps to reproduce this bug. A reproducible example helps to provide faster answers. If you load data e.g. from a PCD or PLY file, please provide the file.

Screenshots/Code snippets

code

Possible Solution

Just stick to the old behavior ?

Start by comparing transformPointCloud and transformPointCloudWithNormals with the implementation linked in commit 04ae840eb43c78f0e348c35271932c5d6d350759, then inspect the so3() and se3() entry points shown in the issue. Reproduce the behavior with a custom point type that stores intensity in the fourth value; done means xyz transforms correctly while that fourth value remains unchanged.