点群の並進移動および回転移動の実装をご紹介します。

【ROS+PCL】点群の並進移動&回転

点群を扱うときに、形を崩さずに並進/回転移動させたいことはよくあります。

この記事では、ROSとPCLを使った実装例をご紹介します。

仕様

Subscribeした点群に対して、並進/回転移動を適用してPublishする仕様とします。

ただし、回転の考え方は複数あり、本記事では、以下の2種類を実装します。

  • ロール・ピッチ・ヨーをもとにする回転:
    回転前の座標に固定されたx、y、z軸回りに回転させる
  • オイラー角をもとにする回転:
    ロボット座標に固定されたx、y、z軸回りに回転させる(i.e. 各軸の回転ごとに回転軸は回転する)

以下に、実装における、入出力とパラメータを記載します。

入力(Subscribe)

  • sensor_msgs::PointCloud2:入力点群

出力(Publish)

  • sensor_msgs::PointCloud2:変換された点群

パラメータ

  • double:x軸方向の移動距離 [m]
  • double:y軸方向の移動距離 [m]
  • double:z軸方向の移動距離 [m]
  • double:ロール(またはx軸回りの)回転角度 [deg]
  • double:ピッチ(またはy軸回りの)回転角度 [deg]
  • double:ヨー(またはz軸回りの)回転角度 [deg]

環境構築

今回のプログラムを動かすためには、以下をインストールする必要があります。

  • ROS
  • PCL

(Dockerを使う場合)

Dockerを使う場合は、以下のDockerfileを例として参考にしてください。

########## Pull ##########
FROM ros:noetic
########## ROS setup ##########
RUN mkdir -p ~/catkin_ws/src && \
	cd ~/catkin_ws && \
	/bin/bash -c "source /opt/ros/noetic/setup.bash; catkin_make" && \
	echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc && \
	echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc && \
	echo "export ROS_WORKSPACE=~/catkin_ws" >> ~/.bashrc
########## Requirements ##########
RUN apt-get update && \
apt-get install -y \
		libpcl-dev \
		ros-noetic-pcl-conversions \
		ros-noetic-rviz

実装

ディレクトリ構造

~/catkin_ws/src/pc_transform/
┣ src/
┃ ┣ rpy_transform.cpp
┃ ┗ eular_transform.cpp
┣ launch/
┃ ┗ transform.launch
┗ CMakeLists.txt

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(pc_transform)

add_compile_options(-std=c++14 -O2 -g -Wall)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  tf
)
find_package(PCL)

catkin_package(
)

include_directories(
  ${catkin_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
)

add_executable(tf_transform src/tf_transform.cpp)
target_link_libraries(tf_transform
  ${catkin_LIBRARIES}
)

add_executable(eular_transform src/eular_transform.cpp)
target_link_libraries(eular_transform
  ${catkin_LIBRARIES}
  ${PCL_LIBRARIES}
)

add_executable(rpy_transform src/rpy_transform.cpp)
target_link_libraries(rpy_transform
  ${catkin_LIBRARIES}
  ${PCL_LIBRARIES}
)

rpy_transform.cpp

ロール・ピッチ・ヨーをもとに回転させる実装です。

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>

class PcRpyTransform{
	private:
		/*node handle*/
		ros::NodeHandle nh_;
		ros::NodeHandle nh_private_;
		/*subscriber*/
		ros::Subscriber sub_;
		/*publisher*/
		ros::Publisher pub_;
		/*parameter*/
		std::string publish_frame_;
		double x_m_, y_m_, z_m_;
		double r_deg_, p_deg_, y_deg_;

	public:
		PcRpyTransform();
		void callbackPC(const sensor_msgs::PointCloud2ConstPtr& msg);
		void transformPC(pcl::PointCloud<pcl::PointXYZI>::Ptr pc);
		void publication(pcl::PointCloud<pcl::PointXYZI>::Ptr pc);
		double degToRad(double deg);
};

PcRpyTransform::PcRpyTransform()
	: nh_private_("~")
{
	std::cout << "--- pc_rpy_transform ---" << std::endl;
	/*parameter*/
	nh_private_.param("publish_frame", publish_frame_, std::string(""));
	std::cout << "publish_frame_ = " << publish_frame_ << std::endl;
	nh_private_.param("x_m", x_m_, 0.0);
	std::cout << "x_m_ = " << x_m_ << std::endl;
	nh_private_.param("y_m", y_m_, 0.0);
	std::cout << "y_m_ = " << y_m_ << std::endl;
	nh_private_.param("z_m", z_m_, 0.0);
	std::cout << "z_m_ = " << z_m_ << std::endl;
	nh_private_.param("r_deg", r_deg_, 0.0);
	std::cout << "r_deg_ = " << r_deg_ << std::endl;
	nh_private_.param("p_deg", p_deg_, 0.0);
	std::cout << "p_deg_ = " << p_deg_ << std::endl;
	nh_private_.param("y_deg", y_deg_, 0.0);
	std::cout << "y_deg_ = " << y_deg_ << std::endl;
	/*subscriber*/
	sub_ = nh_.subscribe("/point_cloud", 1, &PcRpyTransform::callbackPC, this);
	/*publisher*/
	pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/point_cloud/transformed", 1);
}

void PcRpyTransform::callbackPC(const sensor_msgs::PointCloud2ConstPtr &msg)
{
	pcl::PointCloud<pcl::PointXYZI>::Ptr pc (new pcl::PointCloud<pcl::PointXYZI>);
	pcl::fromROSMsg(*msg, *pc);
	transformPC(pc);
	publication(pc);
}

void PcRpyTransform::transformPC(pcl::PointCloud<pcl::PointXYZI>::Ptr pc)
{
	Eigen::Affine3f transformatoin = pcl::getTransformation(x_m_, y_m_, z_m_, degToRad(r_deg_), degToRad(p_deg_), degToRad(y_deg_));
	pcl::transformPointCloud(*pc, *pc, transformatoin);
}

void PcRpyTransform::publication(pcl::PointCloud<pcl::PointXYZI>::Ptr pc)
{
	sensor_msgs::PointCloud2 ros_pc;
	pcl::toROSMsg(*pc, ros_pc);
	if(publish_frame_ != "")	ros_pc.header.frame_id = publish_frame_;
	pub_.publish(ros_pc);
}

double PcRpyTransform::degToRad(double deg)
{
	double rad = deg / 180.0 * M_PI;
	rad = atan2(sin(rad), cos(rad));
	return rad;
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "pc_rpy_transform");
	
	PcRpyTransform pc_rpy_transform;

	ros::spin();
}

eular_transform.cpp

オイラー角をもとに回転させる実装です。

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>

class PcEularTransform{
	private:
		/*node handle*/
		ros::NodeHandle nh_;
		ros::NodeHandle nh_private_;
		/*subscriber*/
		ros::Subscriber sub_;
		/*publisher*/
		ros::Publisher pub_;
		/*parameter*/
		std::string publish_frame_;
		double x_m_, y_m_, z_m_;
		double x_deg_, y_deg_, z_deg_;

	public:
		PcEularTransform();
		void callbackPC(const sensor_msgs::PointCloud2ConstPtr& msg);
		void transformPC(pcl::PointCloud<pcl::PointXYZI>::Ptr pc);
		void publication(pcl::PointCloud<pcl::PointXYZI>::Ptr pc);
		double degToRad(double deg);
};

PcEularTransform::PcEularTransform()
	: nh_private_("~")
{
	// rpy_transform.cppと同じ
}

void PcEularTransform::callbackPC(const sensor_msgs::PointCloud2ConstPtr &msg)
{
	// rpy_transform.cppと同じ
}

void PcEularTransform::transformPC(pcl::PointCloud<pcl::PointXYZI>::Ptr pc)
{
	Eigen::Quaternionf rotation =
		Eigen::AngleAxisf(degToRad(x_deg_), Eigen::Vector3f::UnitX())
		* Eigen::AngleAxisf(degToRad(y_deg_), Eigen::Vector3f::UnitY())
    		* Eigen::AngleAxisf(degToRad(z_deg_), Eigen::Vector3f::UnitZ());
	Eigen::Vector3f offset(x_m_, y_m_, z_m_);

	pcl::transformPointCloud(*pc, *pc, offset, rotation);
}

void PcEularTransform::publication(pcl::PointCloud<pcl::PointXYZI>::Ptr pc)
{
	// rpy_transform.cppと同じ
}

double PcEularTransform::degToRad(double deg)
{
	// rpy_transform.cppと同じ
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "pc_eular_transform");
	
	PcEularTransform pc_eular_transform;

	ros::spin();
}

transform.launch

<launch>
	<!-- global rosparam -->
	<param name="use_sim_time" value="true"/>

	<!-- rosbag -->
	<node pkg="rosbag" type="play" name="player" args="--clock $(env HOME)/rosbag/demo.bag"/>

	<!-- rviz -->
	<node pkg="rviz" type="rviz" name="rviz" args="-d $(find pc_transform)/rviz_config/demo.rviz"/>

	<!-- main -->
	<node pkg="pc_transform" type="rpy_transform" name="rpy_transform" output="screen">
	<!-- <node pkg="pc_transform" type="eular_transform" name="eular_transform" output="screen"> -->
		<remap from="/point_cloud" to="/demo_points"/>
		<remap from="/point_cloud/transformed" to="/demo_points/transformed"/>
		<param name="x_m" type="double" value="1.0"/>
		<param name="y_m" type="double" value="2.0"/>
		<param name="z_m" type="double" value="3.0"/>
		<param name="x_deg" type="double" value="10.0"/>
		<param name="y_deg" type="double" value="20.0"/>
		<param name="z_deg" type="double" value="30.0"/>
	</node>
</launch>

使い方

ビルド

cd ~/catkin_ws
catkin_make

実行

roslaunch pc_transform transform.launch

さいごに

ROSとPCLを使って、3次元点群の並進/回転移動を実装してみました。

参考になれば幸いです。

関連記事

本記事では指定した値で点群を並進移動/回転させました。

一方で、tfが設定されており、点群を座標変換したい場合は、tf::TransformListener::transformPointCloudを使うと便利です。

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

Ad.