Stack Exchange
Vectorization of pcl_ros::transformPointCloud
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.

If I mock the types that I don't know, the compiler will automatically vectorize this for you: https://godbolt.org/g/3hkWmP

@hlt Yes, this happens due to manual vectorization by Eigen, however. The compiler would not be allowed to re-arrange a 'trivial' matrix-vector-product implementation. Not sure what RaviJoshi means by "is not vectorized" ...

Interesting! Well, transform is an Eigen matrix. If we can convert the input point cloud to Eigen matrix, then we can simply write transform * in in one line in Eigen is going to take care of it. This is what I meant by saying vectorization. However, I think converting point cloud to an Eigen matrix would again took some time. In the end, my aim is to minimize the elapsed time of transformPointCloud function.

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

Thanks a lot. I haven't tested your suggestion yet. I have following two points to say: (1) I printed out some variables. ids x_idx: 0, y_idx: 1, z_idx: 2. members of Eigen::Array4i xyz_offset during initialization, i.e., xyz_offset[0]: 0, xyz_offset[1]: 4, xyz_offset[2]: 8. point_step:32 is same for in and out cloud. (2) The variable in.data and out.data are uint8[] derived from sensor_msgs/PointCloud2

I just implemented your suggestion. Unfortunately, it is throwing segmentation fault at runtime. Please see the code snippet here. I am debugging it using gdb. It says following:
Program received signal SIGSEGV, Segmentation fault.
0x0000000000406a70 in Eigen::internal::gemm_pack_rhs<float, long, 4, 0, false, false>::operator()(float*, float const*, long, long, long, long, long) [clone .constprop.525] ()
Any suggestions, please?

I also tried your first suggestion but got segmentation fault again. Please see the code snippet here. Please note that I was getting ‘Index’ is not a member of ‘Eigen’ error with Eigen::Index, so I used size_t instead of Eigen::Index. I am using eigen3 which is shipped along with ROS Indigo on Ubuntu 14.04 LTS 64 Bit PC. Any suggestions, please?

Replace (float*)&out.data by out.data.

Thank you very much. I just realized that the data inside sensor_msgs/PointCloud2 is a vector. Hence, in the first method, i.e., foo, I changed data_out and data_in by &out.data[0] and  &in.data[0] respectively. It worked! However, you mentioned foo2 when point_step is a multiple of sizeof(float). In my case, point_step is 32.  The function foo2 now shows compilation errors. Please see first version and second versions.

In the second version you also need to write (float*)(&out.data[0]) of course.

As always, thank you very much. I did a benchmarking of these two methods together with original implementation. Here are the results of benchmarking- pcl_ros::transformPointCloud took ms: 4, foo took ms: 5, and foo2 took ms: 7. You can see the complete code here.
