【ROS，PCL】並列処理で点群の法線推定を高速化（NormalEstimationOMP）

以前、単スレッドでの法線推定の実装を公開しました。しかし、単スレッドでは法線推定の処理はかなり遅く、リアルタイムでの計算が間に合いません。

▶【ROS, PCL】3D-LiDARの点群から法線群を生成（Normal Estimation）

そこで、この記事では並列処理を使った高速な法線推定の実装をご紹介します。

概要

• 受け取った3次元点群の法線を計算
• 並列処理（OpenMP）によって高速化
• sub：点群（point-cloud,{x, y, z}）
• pub：法線群（normal-cloud{x, y, z, n}）
• 可視化：pcl viewer
• PCLの既存のクラスpcl::NormalEstimationOMPを使うことで簡単に実装可能

ソースコード

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/features/normal_3d_omp.h>

class NormalEstimationPCLOMP{
	private:
		/*node handle*/
		ros::NodeHandle nh;
		/*subscriber*/
		ros::Subscriber sub;
		/*publisher*/
		ros::Publisher pub;
		/*pcl objects*/
		pcl::visualization::PCLVisualizer viewer{"pc_normals"};
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud {new pcl::PointCloud<pcl::PointXYZ>};
		pcl::PointCloud<pcl::PointNormal>::Ptr normals {new pcl::PointCloud<pcl::PointNormal>};
	public:
		NormalEstimationPCLOMP();
		void Callback(const sensor_msgs::PointCloud2ConstPtr& msg);
		void NormalEstimation(void);
		void Visualization(void);
		void Publication(void);
};

NormalEstimationPCLOMP::NormalEstimationPCLOMP()
{
	sub = nh.subscribe("/velodyne_points", 1, &NormalEstimationPCLOMP::Callback, this);
	pub = nh.advertise<sensor_msgs::PointCloud2>("/normals", 1);
	viewer.setBackgroundColor(1, 1, 1);
	viewer.addCoordinateSystem(0.5, "axis");
}

void NormalEstimationPCLOMP::Callback(const sensor_msgs::PointCloud2ConstPtr &msg)
{
	std::cout << "CALLBACK" << std::endl;
	pcl::fromROSMsg(*msg, *cloud);
	copyPointCloud(*cloud, *normals);
	NormalEstimation();
	Visualization();
	Publication();
}

void NormalEstimationPCLOMP::NormalEstimation(void)
{
	std::cout << "NORMAL ESTIMATION" << std::endl;
	/*法線推定クラスを宣言*/
	pcl::NormalEstimationOMP<pcl::PointNormal, pcl::PointNormal> ne;
	/*入力点群を指定*/
	ne.setInputCloud(normals);
	/*kd-treeクラスを宣言*/
	pcl::search::KdTree<pcl::PointNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointNormal> ());
	/*近傍点群の探索法を指定*/
	ne.setSearchMethod(tree);
	/*近傍点群の探索半径を指定*/
	ne.setRadiusSearch(0.5);
	/*法線推定を実行*/
	ne.compute(*normals);
}

void NormalEstimationPCLOMP::Visualization(void)
{
	std::cout << "VISUALIZER" << std::endl;

	viewer.removeAllPointClouds();

	viewer.addPointCloud(cloud, "cloud");
	viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 0.0, "cloud");
	viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");

	viewer.addPointCloudNormals<pcl::PointNormal>(normals, 10, 0.5, "normals");
	viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "normals");
	viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, "normals");

	viewer.spinOnce();
}

void NormalEstimationPCLOMP::Publication(void)
{
	sensor_msgs::PointCloud2 normals_pub;
	pcl::toROSMsg(*normals, normals_pub);
	pub.publish(normals_pub);
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "normal_estimation_pcl_omp");
	std::cout << "Normal Estimation PCL OMP" << std::endl;

	NormalEstimationPCLOMP normal_estimation_pcl_omp;
	ros::spin();
}

解説

• 並列化のためのヘッダファイルをincludeする必要があります

#include <pcl/features/normal_3d_omp.h>

• 並列法線推定クラスの宣言はこの行

pcl::NormalEstimationOMP<pcl::PointNormal, pcl::PointNormal> ne;

• 実際に法線の計算をするのはこの行

ne.compute(*normals);

• pcl::NormalEstimation（単スレッド）と同じAPI（使い方）なので実装しやすいです

補足

点の数を間引くことで、より高速化させることができます。

さいごに

並列処理（OpenMP）による高速な法線の計算を行う実装を公開しました。
参考になれば幸いです。

以上です。

↓距離に応じてRadiusSearchを変えたい場合はコチラ↓