pcl_ros::transformPointCloud的矢量化

Vectorization of pcl_ros::transformPointCloud

本文关键字：矢量化 transformPointCloud pcl ros      更新时间：2023-10-16

我刚刚注意到函数pcl_ros：：transformPointCloud没有矢量化。下面是从这里复制的代码片段。

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;
}

上面的代码遍历了点云中的每个点，并用它乘以变换。

我想知道是否可以对此进行矢量化，以最大程度地减少经过的时间。

我正在寻找实施/合并的建议。我在 Ubuntu 14.04 LTS PC 上使用 ROS Indigo (PCL 1.7.1(。

假设x_idx、y_idx和z_idx是0、4和8，并且你不关心非有限数据的所有特殊情况处理等，你可以将内部循环简化为这样：

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将是in.width * in.height和out_step，in_step将是相应的point_step成员。可能的微小改进：您可以将T复制到局部变量中，这样就不需要每次都从内存中加载它。

如果point_step是sizeof(float)的倍数，您也可以使用out_stride = out.point_step / sizeof(float)等将其简化为单个作业。但是，这通常会生成比上述版本效率更低的代码(在 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

相关文章：