ROSでTF2を使ってPointCloud2をdoTransformしたら遅かった件

なんか、doTransform遅くない？

ROSを使ってシコシコとLIDARの点群を処理するプログラムを書いていたところ、どうにもFPSが出なかった。
どうせ、自分が雑に書いたコードが遅いんだろ？と、思って処理時間を計測してみたら、そうでもないらしい。

順番に実行速度を計測していったら犯人を発見。

sensor_msgs::PointCloud2 pc2_output;
tf2::doTransform(*pc2_input, pc2_output, transform_stamped);

elapsed : 40.112 ms

おいおい、doTransformかよ。そこだけで40msも使ってくれるなよ。。。

他の手段を探してみる

そもそも、TFの座標系変換なんてやってることは回してズラしてるだけなんだから、たかだか10000個くらいの点群処理しても、そんなに時間掛かるはず無いよなー。
とか思いながら色々調べてみると、 pcl_ros::transformPointCloud を使う手もあるらしい。

参考 ： pcl_ros Namespace Reference

とりあえず処理時間計測してみた

計測用のコード

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>

#include <chrono>

#include <pcl_ros/transforms.h>

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>

#include <tf2/transform_datatypes.h>
#include <tf2_eigen/tf2_eigen.h>

tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener* tf2_listener;

ros::Publisher testPublisher1;
ros::Publisher testPublisher2;

// 速度計測用
std::chrono::system_clock::time_point  start, end;
double elapsed;
std::string title;
auto __START__ = [&](std::string t) {
    start = std::chrono::system_clock::now();
    title = t;
};
auto __END__ = [&]() {
    end = std::chrono::system_clock::now();
    elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
    std::cout << title << " : " << (elapsed / 1000.0) << " ms" << std::endl;
};

void pointsCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) {

    geometry_msgs::TransformStamped transform_stamped = tfBuffer.lookupTransform("lidar", "map", ros::Time(0), ros::Duration(2.0));

    // tf2::doTransform を利用した座標の変換
    sensor_msgs::PointCloud2 conv_cloud_msg_1;
    __START__("lookupTransform - doTransform");
    {
        tf2::doTransform(*msg, conv_cloud_msg_1, transform_stamped);
    }
    __END__();
    testPublisher1.publish(conv_cloud_msg_1);

    // pcl_ros::transformPointCloudを利用した座標の変換
    sensor_msgs::PointCloud2 conv_cloud_msg_2;
    __START__("pcl_ros::transformPointCloud ");
    {
        Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
        pcl_ros::transformPointCloud(mat, *msg, conv_cloud_msg_2);
    }
    __END__();
    testPublisher2.publish(conv_cloud_msg_2);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "point_bench");
    ros::NodeHandle nh, pnh("~");

    testPublisher1 = nh.advertise<sensor_msgs::PointCloud2>("doTransform", 1);
    testPublisher2 = nh.advertise<sensor_msgs::PointCloud2>("transformPointCloud", 1);

    ros::Subscriber sub = nh.subscribe("points", 1000, pointsCallback);
    tf2_listener = new tf2_ros::TransformListener(tfBuffer);

    ros::spin();
    return 0;
}

実行結果

lookupTransform - doTransform : 39.033 ms
pcl_ros::transformPointCloud  : 0.295 ms

まとめ

• pcl_ros::transformPointCloud の方が爆速だった
• tfではなくtf2で使う時は、一旦EigenのMatrixに変換する
• Matrix4fへ cast する所で無駄に悩んだ
  • stackoverflow : Error mixing types with Eigen matrices