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

はじめに

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

やりたいこと

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

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

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

①3次元での探索と②2次元の探索で異なる場合もあります

CMakeList.txt

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

 

3次元近傍点探索

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

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

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

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

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

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

求めた点の中央値を取る

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

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

 

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

コメントを残す

メールアドレスが公開されることはありません。