sensor_msgs::PointCloud2ConstIterator<T> iterX(rosMsg, fieldNames[0]);
sensor_msgs::PointCloud2ConstIterator<T> iterY(rosMsg, fieldNames[1]);
sensor_msgs::PointCloud2ConstIterator<T> iterZ(rosMsg, fieldNames[2]);
for (size_t i = 0; i < pointCount; ++i, ++iterX, ++iterY, ++iterZ)
{
view(0, i) = *iterX;
view(1, i) = *iterY;
view(2, i) = *iterZ;
}
In the above code snippet, view is a block of an Eigen matrix, pointCloudMsg is a ROS message of type sensor_msgs/PointCloud2. I'm filling the data from the buffer into the matrix.
Note that even though the above code uses the * operator to access the value, I have also used the [ ] operator, and the results are requite similar.
I would expect the above loop to be auto-vectorized by my compiler (gcc 9.3.0). However, it seems to be running in scalar mode. I'm building in RelWithDebInfo with O3.
The iterator is a really cool component that is shipped with sensor_msgs, but I might be using it wrong, or there might be some element in the code that makes it difficult for the compiler to figure out how to simplify the above construct and vectorize it.
The loop presented above runs in scalar mode, it's fast. But I would still like to still try to vectorize it, because I have to fulfill a resource limit.