Vectorization of pcl_ros::transformPointCloud - c++

Question

I just noticed that the function pcl_ros::transformPointCloud is not vectorized. Below is the code snippet copied from here.
void transformPointCloud(
const Eigen::Matrix4f& transform,
const sensor_msgs::PointCloud2& in,
sensor_msgs::PointCloud2& out)
{
int x_idx = pcl::getFieldIndex(in, "x");
int y_idx = pcl::getFieldIndex(in, "y");
int z_idx = pcl::getFieldIndex(in, "z");
Eigen::Array4i xyz_offset(
in.fields[x_idx].offset,
in.fields[y_idx].offset,
in.fields[z_idx].offset, 0);
// most of the code is not shown here
for (size_t i = 0; i < in.width * in.height; ++i)
{
Eigen::Vector4f pt(*(float*)&in.data[xyz_offset[0]],
*(float*)&in.data[xyz_offset[1]],
*(float*)&in.data[xyz_offset[2]], 1);
Eigen::Vector4f pt_out;
pt_out = transform * pt;
}
memcpy(&out.data[xyz_offset[0]], &pt_out[0], sizeof(float));
memcpy(&out.data[xyz_offset[1]], &pt_out[1], sizeof(float));
memcpy(&out.data[xyz_offset[2]], &pt_out[2], sizeof(float));
xyz_offset += in.point_step;
}
The code above iterated over each point in the point cloud and multiply the transformation with it.
I am wondering if this can be vectorized so as to minimize the elapsed time.
I am looking for suggestions to implement/incorporate the same. I am using ROS Indigo (PCL 1.7.1) on Ubuntu 14.04 LTS PC.

Answer 1

Assuming x_idx, y_idx, and z_idx are 0, 4 and 8 and you don't care about all the special case handling of non-finite data, etc, you can simplify the inner loop to something like this:
void foo(char* data_out, Eigen::Index N, int out_step, const Eigen::Matrix4f& T, const char* data_in, int in_step)
{
for(Eigen::Index i=0; i<N; ++i)
{
Eigen::Vector3f::Map((float*)(data_out + i*out_step)).noalias()
= (T * Eigen::Vector3f::Map((const float*)(data_in + i*in_step)).homogeneous()).head<3>();
}
}
N would be in.width * in.height and out_step and in_step would be the corresponding point_step members. Minor possible improvement: You can copy T into a local variable so it does not need to be loaded from memory every time.
If point_step is a multiple of sizeof(float) you could also reduce this to a single assignment, using out_stride = out.point_step / sizeof(float), etc. However, this usually generates less efficient code than the version above (may change in future versions of Eigen).
void foo2(float* data_out, Eigen::Index N, int out_stride, const Eigen::Matrix4f& T, const float* data_in, int in_stride)
{
Eigen::Matrix3Xf::Map(data_out, 3, N, Eigen::OuterStride<>(out_stride)).noalias()
= (T *
Eigen::Matrix3Xf::Map(data_in, 3, N, Eigen::OuterStride<>(in_stride))
.colwise().homogeneous()
).topRows<3>();
}
Godbolt-Link