25
17

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 5 years have passed since last update.

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

Last updated at Posted at 2019-06-24

なんか、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
25
17
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
25
17

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?