【ROS】PointCloudで任意の(x,y)でのz座標を得る

はじめに

LiDARもデプスカメラもPointCloudでデータを扱いますが、コストマップや平面検出や法線推定は多くのライブラリがあるのに対して、あるxy座標でのz座標の値をプロットしたいと思った時にちょっと困ったのでその方法をご紹介します。

やりたいこと

z座標を求めるにあたり次のパターンがあると思います。

• (x,y,0)に最も近い点を求め、その点のz座標を使う
• 全ての点をxy平面上に投影したときの最も近い点を求め、その点のz座標を使う
• 周辺の点の平均値をとる
• 周辺の点の中央値をとる

最近傍点を見つける際に3次元で考えるか2次元で考えるかで少し変わってくるケースもあるかと思いますので、みなさんの環境に合わせてどちらが良いか選んでください。

CMakeList.txt

今回の内容にはpcl_rosの追加が必要です。

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  tf
  pcl_ros
)

3次元近傍点探索

近傍点の探索にはPCLライブラリを使います。サンプルコードはこちらです。

3次元で半径rで探索して一番近い点を返すやつ

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>
#include <pcl/kdtree/kdtree_flann.h>


void callback(sensor_msgs::PointCloud2 pc2){
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_nan (new pcl::PointCloud<pcl::PointXYZ>); // NaN値あり
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); // NaN値なし
  
  //sensor_msgs::PointCloud2からpcl::PointXYZに変換
  pcl::fromROSMsg(pc2, *cloud_nan);

  // NaN値が入ってるといろいろ面倒なので除去
  std::vector<int> nan_index;
  pcl::removeNaNFromPointCloud(*cloud_nan, *cloud, nan_index);

  // KD木を作っておく。近傍点探索とかが早くなる。
  pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>);
  tree->setInputCloud(cloud);

  //近傍点探索に使うパラメータと結果が入る変数
  double radius = 0.1;  //半径r
  std::vector<int> k_indices;  //範囲内の点のインデックスが入る
  std::vector<float> k_sqr_distances;  //範囲内の点の距離が入る
  unsigned int max_nn = 1;  //何点見つかったら探索を打ち切るか。0にすると打ち切らない
  
  pcl::PointXYZ p;  //中心座標
  p.x = 0.5;
  p.y = 0.5;
  p.z = 0.0;

  //半径r以内にある点を探索
  tree->radiusSearch(p, radius, k_indices, k_sqr_distances, max_nn);
  
  if(k_indices.size() == 0) return;
  
  pcl::PointXYZ result = cloud->points[k_indices[0]];

  ROS_INFO("A nearest point of (0.5, 0.5) is...\nx: %lf, y:%lf, z:%lf", result.x, result.y, result.z);
  
}

int main(int argc, char** argv){
	ros::init(argc,argv,"z_at_xy");
	ros::NodeHandle nh;
	
	ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("in", 1, callback);
	
	ros::spin();
	
	return 0;
}

このコードは最も近い1点を見つけます。k_indicesに見つかった点のインデックスが入ります。周辺の点の平均値や中央値をとるなどの処理をしたい場合はmax_nnを2以上か0にしておくと指定の個数になるまで探します。

xy平面上での距離で近傍点探索

2次元バージョンはこんな感じで

void callback_2d(sensor_msgs::PointCloud2 pc2){
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_nan (new pcl::PointCloud<pcl::PointXYZ>); // NaN値あり
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); // NaN値なし
  
  pcl::fromROSMsg(pc2, *cloud_nan);

  // NaN値が入ってるといろいろ面倒なので除去
  std::vector<int> nan_index;
  pcl::removeNaNFromPointCloud(*cloud_nan, *cloud, nan_index);

  //x座標とy座標だけをコピーしたPointCloudを作る
  pcl::PointCloud<pcl::PointXY>::Ptr cloud2d (new pcl::PointCloud<pcl::PointXY>); // NaN値なし
  cloud2d->points.resize(cloud->size());
  for(int i=0; i<cloud->points.size(); i++){
    cloud2d->points[i].x = cloud->points[i].x;
    cloud2d->points[i].y = cloud->points[i].y;
  }

  //treeも2Dで作る
  pcl::KdTreeFLANN<pcl::PointXY>::Ptr tree2d (new pcl::KdTreeFLANN<pcl::PointXY>);
  tree2d->setInputCloud(cloud2d);

  //近傍点探索に使うパラメータと結果が入る変数
  double radius = 0.1; //半径r
  std::vector<int> k_indices; //範囲内の点のインデックスが入る
  std::vector<float> k_sqr_distances; //範囲内の点の距離が入る 
  unsigned int max_nn = 1; //何点見つかったら探索を打ち切るか。0にすると打ち切らない
  
  //中心座標
  pcl::PointXY p;
  p.x = 0.5;
  p.y = 0.5;

  //2Dで近傍点探索
  tree2d->radiusSearch(p, radius, k_indices, k_sqr_distances, max_nn);
  
  if(k_indices.size() == 0) return;
  
  //求めたインデックスでもとのPointCloudの点を見る
  ROS_INFO("A nearest point of (0.5, 0.5) is...\nx: %lf, y:%lf, z:%lf", cloud->points[k_indices[0]].x, cloud->points[k_indices[0]].y, cloud->points[k_indices[0]].z);
  
}

int main(int argc, char** argv){
	ros::init(argc,argv,"z_at_xy");
	ros::NodeHandle nh;
	
	ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("in", 1, callback_2d);
	
	ros::spin();
	
	return 0;
}

pcl::PointCloudXYZのxy座標だけをコピーしたpcl::PointXYを作っておいてそちらで近傍点探索した後、得られたインデックスk_indicesでもとのpcl::PointCloudXYZを参照します。

求めた点の中央値を取る

平均値は求めた点のz座標を合計して個数で割るだけなのでたぶん簡単なので書く必要は無いと思いますが、中央値を取る方法は知らなかったので載せておきます。

ちなみに周辺の点の中央値をその点の値とする手法はメディアンフィルタ（中央値フィルタ）と呼ばれ、画像の境界線を残したままノイズを除去する方法として画像処理などでよく用いられるようです。このフィルタは計算コストがちょっと高いらしいので、多用はしないほうが良いかも知れません。

std::vector<float> vals;  //近傍点のz座標
for(int j=0; j<k_indices.size(); j++){
  vals.push_back(depth_cloud->points[k_indices[j]].z);
}

//上位半分までソートする
partial_sort(vals.begin(), vals.begin() + vals.size()/2 + 1, vals.end());

//中央値をとる
float filtered_z = vals[vals.size()/2];

ROSを最初から勉強したい方はこちら。