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

3D-LiDARで観測される3次元点群を扱う際、特徴量を抽出する目的で、主成分分析（PCA）を用いた法線推定がよく用いられます。

PCLを用いれば、簡単に法線推定を実装することができます。この記事では、ROS対応の3D-LiDARで観測した点群に法線推定を適用するソースコードを公開します。なるべくシンプルにコーディングしてみたつもりです。

実装の概要

• サブスクライブ：3次元点群（sensor_msg::pointCloud2）
• パブリッシュ：法線群（sensor_msg::pointCloud2）
• 可視化：PCLVisualizer
• sensor_msgs::PointCloud2
  ↓
  pcl::PointCloud<pcl::PointXYZ>
  ↓
  pcl::PointCloud<pcl::PointNormal>
  ↓
  sensor_msgs::PointCloud2

ソースコード

#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/features/normal_3d.h>
#include <pcl_conversions/pcl_conversions.h>

class NormalEstimationPCL{
	private:
		ros::NodeHandle nh;
		ros::Subscriber sub;
		ros::Publisher pub;
		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:
		NormalEstimationPCL();
		void Callback(const sensor_msgs::PointCloud2ConstPtr& msg);
		void NormalEstimation(void);
		void Visualization(void);
		void Publication(void);
};

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

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

void NormalEstimationPCL::NormalEstimation(void)
{
	std::cout << "NORMAL ESTIMATION" << std::endl;
	/*法線推定クラスを宣言*/
	pcl::NormalEstimation<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 NormalEstimationPCL::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 NormalEstimationPCL::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");
	std::cout << "Normal Estimation PCL" << std::endl;

	NormalEstimationPCL normal_estimation_pcl;
	ros::spin();
}

解説

• ROSメッセージからPCLの点群に変換

  pcl::fromROSMsg(*msg, *cloud);

• cloudの(x, y, z)情報とheader情報をnormalsにコピー

  copyPointCloud(*cloud, *normals);

• 関数　NormalEstimation(void)
  法線推定を実行するメインパート
• 関数　Visualization(void)
  可視化する関数
• 関数　Publication(void)
  ROSメッセージをパブリッシュする関数
• PCLの点群からROSメッセージに変換

  pcl::toROSMsg(*normals, normals_pub);

可視化が不要なら...

PCLVisualizerによる可視化が不要な場合は、下記の行をコメントアウトしてください。

pcl::visualization::PCLVisualizer viewer{"pc_normals"};

viewer.setBackgroundColor(1, 1, 1);
viewer.addCoordinateSystem(0.5, "axis");

void Visualization(void);

Visualization();

void NormalEstimationPCL::Visualization(void)
{
	～　略　～
}

処理はかなり重い

この実装の処理時間はかなり遅いです。慣れている方は実装を工夫することをオススメします。

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

法線推定の関連記事

法線推定の関連記事のリンクを載せておきます。

▶【PCL, ROS】NormalEstimationで距離に応じてRadiusSearchを変えたい

さいごに

少しでも役に立てれば幸いです。

以上です。