transformPointCloud with indices loses point info (eg, rgb)

[phil0stine]

The fix to this issue is actually already in the code, but commented. Currently, only XYZ fields are copied to the output cloud.
This would also apply to calls to pcl::transformPointCloudWithNormals.
From transforms.hpp

pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, const std::vector<int> &indices, pcl::PointCloud<PointT> &cloud_out, const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)

  if (cloud_in.is_dense)
  {
    // If the dataset is dense, simply transform it!
    for (size_t i = 0; i < npts; ++i)
    {
      // Copy fields first, then transform xyz data
      //cloud_out.points[i] = cloud_in.points[indices[i]];
      //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[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));
    }
  }
  else
  {
    // Dataset might contain NaNs and Infs, so check for them first,
    // otherwise we get errors during the multiplication (?)
    for (size_t i = 0; i < npts; ++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]];

[taketwo]

@rbrusu It was you who commented out those lines (0266f79). Could you recall the motivation?

[rbrusu]

@taketwo which lines do we refer to?

[taketwo]

@rbrusu I added inline notes at corresponding lines in that commit.

[rbrusu]

Ah, I see - thanks! I don't remember exactly why, but I think we were doing tests with double precision, and didn't want to have a core function like transform be the bottleneck. If you look at Scalar, that's now a template parameter, which means you can do transformPointCloud < PointT , double > (...). What we could do (though at the cost of some extra copy operations), is maybe enable

  cloud_out.points[i] = cloud_in.points[indices[i]];

back, but still allow the transform to be done separately?

[taketwo]

What if we just start each transform... function with a call to copyPointCloud()?

[rbrusu]

Well it's all about efficiency. Since transformPointCloud might be called from loops (such as ICP's), it would be best to keep it as tight as possible. I understand the reasons for wanting that, but this might be an extra parameter that we could add to the method without breaking the API such as (copyAllFields = false)?

[taketwo]

Shouldn't that be a concern of those users who want to achieve a better performance to use "stripped down" point cloud (with irrelevant fields removed) in their ICP algorithms?

[taketwo]

And by the way, copy point cloud would be invoked only if the input and output clouds are different, so cheap in-place transform will still be possible.

[rbrusu]

They probably should, but I for one have lots of code where I don't ;) That's why I suggested the parameter because then we're making it clear to the user when we're copying data and when we're not.

[taketwo]

Hm, what about the version of transformPointCloud() that does not have indices argument? It does not conform your "tight" ideology in that it does a complete copy (line 54), even though later in the loop XYZ fields are overwritten with transformed coordinates.

My point is that the behaviors of different transform overrides are inconsistent. If you want the tight behavior plus the parameter then I am fine with it. But the question is do we want to alter the behavior of that first transform function (which is currently non-tight).

[rbrusu]

Yup, that seems to be an outlier. That's inconsistent.

[taketwo]

I am all in favor of consistency, but such a minor change will suddenly break code for those who rely on complete data copying. I guess making it is like calling for bugs and complaints.