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

[flm8620]

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 ?

[mvieth]

I believe the fourth entry is set to zero/one so the behaviour is consistent with the SSE/AVX implementations of so3/se3.

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

Out of interest: why do you do this?

[flm8620]

Because it can save a lot of memory. I only need x,y,z and intensity, four float values. And I never use the fourth entry as the homogenous coordinate.

Indeed this is pretty hacky. But at version 1.8.0, the SIMD optimization is not here yet and the previous code worked for my project. So I consider this as a breaking change.

I know most SIMD instructions operate on 4 float or 4 double numbers, which makes it convenient if we always store x,y,z,w as a pack.