【ROS＋PCL】点群のダウンサンプリング（VoxelGrid）

点群データを扱う際に、点の数を間引くこと（ダウンサンプリング）がよくあります。その理由は主に、計算負荷を減らすためです。

この記事では、点群のダウンサンプリングを、ROSとPCLで実装してみます。

仕様

Subscribeした点群に対して、ダウンサンプリングを適用してPublishする仕様とします。

ダウンサンプリングのアルゴリズムには、VoxelGridフィルターを採用します。このアルゴリズムでは、入力点群を3次元のグリッドに区切って、各グリッドの重心点を出力します。複雑そうに聞こえますが、PCLを使えば簡単に実装することができます。

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

入力（Subscribe）

• sensor_msgs::PointCloud2：入力点群

出力（Publish）

• sensor_msgs::PointCloud2：ダウンサンプリングされた点群

パラメータ

• double：ボクセルの1辺の長さ [m]

環境構築

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

• 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_voxelgrid_filter/
┣　src/
┃　┗　pc_voxelgrid_filter.cpp
┣　launch/
┃　┗　pc_voxelgrid_filter.launch
┗　CMakeLists.txt

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(pc_downsampling)

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

find_package(catkin REQUIRED
    roscpp
)
find_package(PCL)

catkin_package(
)

include_directories(
    ${catkin_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
)

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

pc_voxelgrid_filter.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/filters/voxel_grid.h>

class PcVoxelgridFilter{
	private:
		/*node handle*/
		ros::NodeHandle nh_;
		ros::NodeHandle nh_private_;
		/*subscriber*/
		ros::Subscriber sub_;
		/*publisher*/
		ros::Publisher pub_;
	        /*tool*/
	        pcl::VoxelGrid<pcl::PointXYZI> vg_;
	        /*buffer*/
	        pcl::PointCloud<pcl::PointXYZI>::Ptr pc_ {new pcl::PointCloud<pcl::PointXYZI>};
		/*parameter*/
		double leafsize_;

	public:
		PcVoxelgridFilter();
		void callback(const sensor_msgs::PointCloud2ConstPtr& msg);
        void filter(void);
		void publication(std_msgs::Header header);
};

PcVoxelgridFilter::PcVoxelgridFilter()
	: nh_private_("~")
{
	std::cout << "----- pc_voxelgrid_filter -----" << std::endl;
	/*parameter*/
	nh_private_.param("leafsize", leafsize_, 0.5);
	std::cout << "leafsize_ = " << leafsize_ << std::endl;
	/*subscriber*/
	sub_ = nh_.subscribe("/point_cloud", 1, &PcVoxelgridFilter::callback, this);
	/*publisher*/
	pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/point_cloud/downsampled", 1);
}

void PcVoxelgridFilter::callback(const sensor_msgs::PointCloud2ConstPtr &msg)
{
	pcl::fromROSMsg(*msg, *pc_);
	filter();
	publication(msg->header);
}

void PcVoxelgridFilter::filter(void)
{
	vg_.setInputCloud(pc_);
	vg_.setLeafSize(leafsize_, leafsize_, leafsize_);
	vg_.filter(*pc_);
}

void PcVoxelgridFilter::publication(std_msgs::Header header)
{
	if(!pc_->points.empty()){
		sensor_msgs::PointCloud2 ros_pc;
		pcl::toROSMsg(*pc_, ros_pc);
		ros_pc.header = header;
		pub_.publish(ros_pc);
	}
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "pc_voxelgrid_filter");
	
	PcVoxelgridFilter pc_voxelgrid_filter;

	ros::spin();
}

pc_voxelgrid_filter.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_downsampling)/rviz_config/demo.rviz"/>

	<!-- main -->
	<node pkg="pc_downsampling" type="pc_voxelgrid_filter" name="pc_voxelgrid_filter" output="screen">
		<remap from="/point_cloud" to="/demo_points"/>
		<remap from="/point_cloud/downsampled" to="/demo_points/downsampled"/>
		<param name="leafsize" type="double" value="0.1"/>
	</node>
</launch>

使い方

ビルド

cd ~/catkin_ws
catkin_make

実行

roslaunch pc_downsampling pc_voxelgrid_filter.launch

さいごに

ROSとPCLを使って、3次元点群のダウンサンプリングを実装してみました。

参考になれば幸いです。