3D-LiDARで取得した3次元点群から法線群を生成し可視化するソースコードをご紹介します。

【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();
}
Ad.

解説

  • 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を変えたい

さいごに

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


以上です。

Ad.