Векторизация pcl_ros :: transformPointCloud - PullRequest
Новая
       20

Векторизация pcl_ros :: transformPointCloud

0 голосов
/ 13 мая 2018

Я только что заметил, что функция 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;
}

Приведенный выше код повторяется для каждой точки в облаке точек и умножает преобразование на него.

Мне интересно, может ли это быть векторизовано, чтобы минимизировать затраченное время.

Я ищу предложения для реализации / включения того же самого. Я использую ROS Indigo (PCL 1.7.1) на ПК с Ubuntu 14.04 LTS.

1 Ответ

0 голосов
/ 13 мая 2018

Предполагая, что 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

...