【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型の回転を解説）

▶【ROS＋PCL】点群の並進移動＆回転