tfを使った点群の座標変換の実装例をご紹介します。

【ROS】tfを使った点群の座標変換の実装

同じ点群でも、どの座標系を基準として扱うかで、各点の座標(x, y, z)は異なります。

例えば、センサー座標系で観測した点群を、マップ座標系で扱いたいことはよくあります。この場合、センサー座標系とマップ座標系の相対位置関係をもとに、点群に回転と並進移動を与える必要があります。

このような座標変換はとてもややこしいものですが、ROSのtfを使っている場合、簡単に実装することができます。この記事ではその実装例をご紹介します。

概要

  • 点群の座標変換
  • tfが張られていることが前提
  • sub:変換前の点群(sensor_msgs::PointCloud2)
  • pub:変換後の点群(sensor_msgs::PointCloud2)

ソースコード

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <tf/transform_listener.h>

class PCTFTransform{
	private:
		/*node handle*/
		ros::NodeHandle _nh;
		ros::NodeHandle _nhPrivate;
		/*subscriber*/
		ros::Subscriber _sub_pc;
		/*publisher*/
		ros::Publisher _pub_pc;
		/*tf*/
		tf::TransformListener _tflistener;
		/*parameters*/
		std::string _target_frame;

	public:
		PCTFTransform();
		void callbackPC(const sensor_msgs::PointCloud2ConstPtr& msg);
		void transformPC(const sensor_msgs::PointCloud2& pc2_in);
		void publication(void);
};

PCTFTransform::PCTFTransform()
	: _nhPrivate("~")
{
	std::cout << "--- pc_tf_transform ---" << std::endl;
	/*parameter*/
	_nhPrivate.param("target_frame", _target_frame, std::string("/target_frame"));
	std::cout << "_target_frame = " << _target_frame << std::endl;
	/*subscriber*/
	_sub_pc = _nh.subscribe("/cloud", 1, &PCTFTransform::callbackPC, this);
	/*publisher*/
	_pub_pc = _nh.advertise<sensor_msgs::PointCloud2>("/cloud/transformed", 1);
}

void PCTFTransform::callbackPC(const sensor_msgs::PointCloud2ConstPtr &msg)
{
	transformPC(*msg);
}

void PCTFTransform::transformPC(const sensor_msgs::PointCloud2& pc2_in)
{
	sensor_msgs::PointCloud pc1_in;
	sensor_msgs::PointCloud pc1_trans;
	sensor_msgs::PointCloud2 pc2_trans;

	sensor_msgs::convertPointCloud2ToPointCloud(pc2_in, pc1_in);
	try{
		_tflistener.waitForTransform(_target_frame, pc2_in.header.frame_id, pc2_in.header.stamp, ros::Duration(1.0));
		_tflistener.transformPointCloud(_target_frame, pc2_in.header.stamp, pc1_in, pc2_in.header.frame_id, pc1_trans);
		sensor_msgs::convertPointCloudToPointCloud2(pc1_trans, pc2_trans);
		_pub_pc.publish(pc2_trans);
	}
	catch(tf::TransformException ex){
		ROS_ERROR("%s",ex.what());
	}
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "pc_tf_transform");
	
	PCTFTransform pc_tf_transform;

	ros::spin();
}

解説

  • メインはこの行です
  • _tflistener.transformPointCloud(_target_frame, pc2_in.header.stamp, pc1_in, pc2_in.header.frame_id, pc1_trans);
    • 引数1:点群をこの座標系に変換します(to)
    • 引数2:この時刻での座標間の相対位置関係をもとに変換します
    • 引数3:回転させたい点群
    • 引数4:変換前の座標系(from)
    • 引数5:変換させた後の点群を格納する点群
  • transformPointCloudを使うためにPointCloud2をPointCloudへ変換しています
  • tf_listenerを使うのでtry/catchで書いたほうが無難です
  • 変数 _target_frame
    • 変換先の座標の名前(frame_id)
    • 既に存在しているframe_idを設定する必要があります

さいごに

参考になったら幸いです。


以上です。

補足

PCL型の点群を回転したい場合は、pcl_ros::transformPointCloudを使ったら良いそうです。(本記事ではROSのPointCloud2型の回転を解説)

Ad.