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

3D-LiDARなどの距離センサでは、センサから遠いエリアほど点群の密度が粗（スパース）になる特徴があります。

これを踏まえると、点群の法線推定をするときは、近傍点探索の半径（RadiusSearch）を、センサからの距離に応じて変化させたくなります。具体的には、センサからの距離が遠いところでは、より大きな探索半径を設定します。

既存のクラスだと痒い所に手が届かない

PCLの既存のクラスだと痒い所に手が届かなく、RadiusSearchが一定で変えることができません。

↓既存のクラスでの実装例はコチラ↓

具体的にはこんな感じです。

void NormalEstimationPCL::NormalEstimation(void)
{
	std::cout << "NORMAL ESTIMATION" << std::endl;
	pcl::NormalEstimation<pcl::PointNormal, pcl::PointNormal> ne;
	ne.setInputCloud(normals);
	pcl::search::KdTree<pcl::PointNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointNormal> ());
	ne.setSearchMethod(tree);
	ne.setRadiusSearch(0.5);	// <-ココ
	ne.compute(*normals);
}

この例では0.5mで一定です。

解決策の概要

• 受け取った3次元点群の法線を推定
• センサから遠いほど近傍点探索の半径を大きくする
• sub：点群（point-cloud,{x, y, z}）
• pub：法線群（normal-cloud{x, y, z, n}）
• 可視化：pcl viewer

ソースコード

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
// #include <omp.h>

class NormalEstimation{
	private:
		/*node handle*/
		ros::NodeHandle nh;
		ros::NodeHandle nhPrivate;
		/*subscribe*/
		ros::Subscriber sub_pc;
		/*publish*/
		ros::Publisher pub_pc;
		/*pcl*/
		pcl::visualization::PCLVisualizer viewer {"Normal Estimation Multi Thread"};
		pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud {new pcl::PointCloud<pcl::PointXYZ>};
		pcl::PointCloud<pcl::PointNormal>::Ptr normals {new pcl::PointCloud<pcl::PointNormal>};
		/*parameters*/
		int skip;
		double search_radius_ratio;
	public:
		NormalEstimation();
		void CallbackPC(const sensor_msgs::PointCloud2ConstPtr &msg);
		void Computation(void);
		double Getdepth(pcl::PointXYZ point);
		std::vector<int> KdtreeSearch(pcl::PointXYZ searchpoint, double search_radius);
		void Visualization(void);
		void Publication(void);
};

NormalEstimation::NormalEstimation()
	:nhPrivate("~")
{
	sub_pc = nh.subscribe("/velodyne_points", 1, &NormalEstimation::CallbackPC, this);
	pub_pc = nh.advertise<sensor_msgs::PointCloud2>("/normals", 1);
	viewer.setBackgroundColor(1, 1, 1);
	viewer.addCoordinateSystem(0.8, "axis");
	viewer.setCameraPosition(-30.0, 0.0, 10.0, 0.0, 0.0, 1.0);

	nhPrivate.param("skip", skip, 3);
	nhPrivate.param("search_radius_ratio", search_radius_ratio, 0.09);
	std::cout << "skip = " << skip << std::endl;
	std::cout << "search_radius_ratio = " << search_radius_ratio << std::endl;
}

void NormalEstimation::CallbackPC(const sensor_msgs::PointCloud2ConstPtr &msg)
{
	/* std::cout << "CALLBACK PC" << std::endl; */

	pcl::fromROSMsg(*msg, *cloud);
	std::cout << "cloud->points.size() = " << cloud->points.size() << std::endl;
	normals->points.clear();
	normals->points.resize((cloud->points.size()-1)/skip + 1);

	kdtree.setInputCloud(cloud);
	Computation();

	Publication();
	Visualization();
}

void NormalEstimation::Computation(void)
{
	std::cout << "omp_get_max_threads() = " << omp_get_max_threads() << std::endl;

	double time_start = ros::Time::now().toSec();

	// #ifdef _OPENMP
	// #pragma omp parallel for
	// #endif
	for(size_t i=0;i<cloud->points.size();i+=skip){
		/*search neighbor points*/
		double laser_distance = Getdepth(cloud->points[i]);
		double search_radius = search_radius_ratio*laser_distance;
		std::vector<int> indices = KdtreeSearch(cloud->points[i], search_radius);
		/*compute normal*/
		float curvature;
		Eigen::Vector4f plane_parameters;
		pcl::computePointNormal(*cloud, indices, plane_parameters, curvature);
		/*input*/
		size_t normal_index = i/skip;
		normals->points[normal_index].x = cloud->points[i].x;
		normals->points[normal_index].y = cloud->points[i].y;
		normals->points[normal_index].z = cloud->points[i].z;
		normals->points[normal_index].normal_x = plane_parameters[0];
		normals->points[normal_index].normal_y = plane_parameters[1];
		normals->points[normal_index].normal_z = plane_parameters[2];
		normals->points[normal_index].curvature = curvature;
		flipNormalTowardsViewpoint(cloud->points[i], 0.0, 0.0, 0.0, normals->points[normal_index].normal_x, normals->points[normal_index].normal_y, normals->points[normal_index].normal_z);
	}
	for(size_t i=0;i<normals->points.size();){
		if(std::isnan(normals->points[i].normal_x) || std::isnan(normals->points[i].normal_y) || std::isnan(normals->points[i].normal_z)){
			std::cout << "deleted NAN normal" << std::endl;
			normals->points.erase(normals->points.begin() + i);
		}
		else	i++;
	}

	std::cout << "computation time [s] = " << ros::Time::now().toSec() - time_start << std::endl;
}

double NormalEstimation::Getdepth(pcl::PointXYZ point)
{
	double depth = sqrt(
		point.x*point.x
		+ point.y*point.y
		+ point.z*point.z
	);
	return depth;
}

std::vector<int> NormalEstimation::KdtreeSearch(pcl::PointXYZ searchpoint, double search_radius)
{
	std::vector<int> pointIdxRadiusSearch;
	std::vector<float> pointRadiusSquaredDistance;
	if(kdtree.radiusSearch(searchpoint, search_radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)<=0)	std::cout << "kdtree error" << std::endl;
	return pointIdxRadiusSearch; 
}

void NormalEstimation::Visualization(void)
{
	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, 1, 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, 1, "normals");

	viewer.spinOnce();
}

void NormalEstimation::Publication(void)
{
	normals->header.stamp = cloud->header.stamp;
	normals->header.frame_id = cloud->header.frame_id;

	sensor_msgs::PointCloud2 pc;
	pcl::toROSMsg(*normals, pc);
	pub_pc.publish(pc);
}

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

	ros::spin();
}

解説

• 流れ
  • 点群をサブスクライブ
  • forループ（各点にアクセス）
  • センサと注目点との距離を計算
  • 距離に比例した探索半径を設定
  • 近傍点を探索
  • 近傍点群に対して主成分分析を適用

    pcl::computePointNormal(*cloud, indices, plane_parameters, curvature);

  • オブジェクトに計算結果を格納
  • パブリッシュ
• パラメータ　skip
  何個飛ばしで法線を計算するか設定します。
• パラメータ　search_radius_ratio
  センサからの距離と探索半径の比率を設定します。
• OpenMPで並列処理できるようにするために、push_backを使わずに書いたので、少しややこしい実装になっています。
• OpenMPを使う場合は、コメントアウトしている部分を解除してください。

さいごに

参考になれば幸いです

以上です。