三次元点群処理ライブラリPCLと 統合ロボットシステム研究での 利用例の紹介

1. 三次元点群処理ライブラリPCLと
統合ロボットシステム研究での
利用例の紹介
@第25回コンピュータビジョン勉強会
植田亮平

2. 自己紹介
● 名前
– 植田亮平 a.k.a. がらえもん
– @garaemon_coder
● 所属
– 東京大学情報理工学系研究科・創造情報学専攻
– JSK/情報システム工学研究室
● 専門
– ロボットシステム
● 最近の興味
– Shared Autonomy, Supervised Teleoperation

3. 略歴
2010.4 博士課程スタート
2011.8 WillowGarageインターン
2012.4 大学休学/スタートアップに参加
2014.4 大学復帰
現在

4. アジェンダ
1. 前置き
– ロボット屋さんにとっての3次元点群
2. pcl::tracking
– PCLにおけるトラッキングライブラリの紹介
3. ロボットシステムの中の点群処理
– tabletop manipulationを例にとって

5. ロボット屋さんにとっての3次元点群

6. ロボットって何?
?
PR2 [willowgarage]

7. ロボットって何?
?
センサアクチュエータ
計算機

8. ロボットって何?
?
= 計算機
+ センサ
+ アクチュエータ

9. ロボット屋さんからみた3次元点群
● 実環境でアクチュエートするには3次元の情報が
必須
– 自分がどこにいるのか?
– 対象物はどこにあるのか?
– どういう経路で動けばいいか?
全部3次元の情報

10. 様々なセンサ – kinectだけじゃないよ!

11. レーザレンジファインダ
hokuyo
SICK

12. レーザ + 回転
PR2 hokuyo-tilt
multisense SL/hokuyo-spindle

13. ToFカメラ
swissranger sr4000
softkinetic DepthSense 311
sr4000 + stereo coloring

14. ステレオカメラ
PS4 camera [SCE]
multisense s7 [Carnegie Robotics]

15. PCLの登場(とその背景) 1/2
● ロボットに3次元点群は大事
● でも良いライブラリってないね(2009)
– WillowGarageはPR2で２つの良い点群センサを持っ
ていた
1. チルトレーザ
2. プロジェクタ付きステレオ
– 画像処理ではOpenCVがあった

16. PCLの登場(とその背景) 2/2
● TUMのRadu RusuがWGにjoin
– RaduはもともとWG intern出身
● ROS (Robot Operating System)の一部として
PCL(PointCloud Library)を公開
– OpenCVの姉妹プロジェクト
– PCLは後にROSから独立したライブラリとして発展

17. kinectの登場
kinectすげー
こんなに綺麗な点群とれるのか
30fpsでるなら点群で
6自由度の物体追跡できそうじゃないか

18. pcl::tracking
Ryohei Ueda (Radu Rusu – Mentor)
Univ. of Tokyo
14/10/04

19. 1mm downsampled

20. agenda
►イントロダクション
►パーティクルフィルタ
►尤度関数
►並列化による高速化
►さらなる高速化のためのチューニング
►実装
►実験

21. イントロダクション
なぜトラッキング?
►安価で30fpsとれるデプスセンサの登場
►リアルタイムで物体認識をするのに役立つ
1mm downsampled

22. パーティクルフィルタ
►トラッキングアルゴリズムはいろいろあるが、、、
►パーティクルフィルタによるトラッキングを採用
●尤度関数を自由に設計できる
●処理が重たい場合は並列化による最適化
●近年CPUコア数は増えてる

23. パーティクルフィルタ
[T.Higuchi, Particle Filter, ‘05]
prediction weighting resampling

24. パーティクルフィルタ
[T.Higuchi, Particle Filter, ‘05]
prediction weighting resampling
対象の位置+姿勢 (6自由度)
センサからの点群尤度関数

25. 尤度関数

26. 尤度関数
もしも仮説の場所にモデル
があったとしたら、
尤度 – センサデータとの一致度
はどのくらいか?
仮説=位置+姿勢

27. 尤度関数
モデル

28. 尤度関数
モデル
モデル点群を仮説の位置に移動する

29. 尤度関数
移動したモデル点群の一点
から最も近いセンサ点群
を探索する

30. 尤度関数
と のデータ(位置、色etc)から
尤度を計算する

31. 尤度関数
モデル点群の一点 を
全ての点、もしくはサンプリングして
和をとり尤度とする

32. 尤度関数
モデル点群の一点 を
全ての点、もしくはサンプリングして
和をとり尤度とする

33. 尤度関数
モデル点群の一点 を
全ての点、もしくはサンプリングして
和をとり尤度とする

34. 並列化
パーティクルフィルタを採用した利点
►各パーティクルの計算は独立
►容易に並列化可能(OpenMPを利用)
8 cores single core (hyperthreaded)
2~6x faster
1mm downsampled

35. モデル点群のサイズ
パーティクルの数
センサ点群の数
高速化
計算量

36. モデル点群のサイズ
パーティクルの数
センサ点群の数
高速化
計算量
最近傍探索(kdtree)のコスト

37. 高速化/Downsampling
►サンプリングしちゃおう!
1mm毎のサンプリング2mm毎のサンプリング
4~5x faster

38. 高速化/Adaptive Particle Filter
►Adaptive Particle Filterを利用しよう
►KLD Sampling [D. Fox, IJRR03]

39. 高速化/Adaptive Particle Filter
►Adaptive Particle Filterを利用しよう
►KLD Sampling [D. Fox, IJRR03]
簡単に言うと、パーティクルの尤度分布に従ってパーティ
クルの数を変更する
分布のピークが明らかな場合、パーティクルの数は少な
くて良い
分布のピークが明らかでない場合、パーティクルの数を
増やす

40. 高速化/点群のサイズ削減
センサ点群の数をどうやって減らす?

41. 高速化/点群のサイズ削減

42. 高速化/点群のサイズ削減

43. 高速化/点群のサイズ削減

44. 高速化/点群のサイズ削減

45. 高速化/点群のサイズ削減

46. 高速化/点群のサイズ削減

47. 高速化/点群のサイズ削減

48. 高速化/点群のサイズ削減

49. 高速化/点群のサイズ削減
Octree
Kdtree

50. fps
* かなりチューンしています

51. 実装
Tracker
ParticleFilterTracker
ParticleFilterOMPTracker
KLDAdaptiveParticleFilterTracker
KLDAdaptiveParticleFilterOMPTracker
PointCloudCoherence
PointCoherence
DistanceCoherence
HSVColorCoherence
NormalCoherence
NearestPairPointCloudCoherence
ApproxNearestPairPointCloudCoherence

52. 実験
movie movie
movie

53. 実験

54. 実験

55. まとめ
►点群ベースなトラッキングライブラリpcl::tracking
►パーティクルフィルタに基づくアプローチ
►位置・姿勢の6自由度のトラッキング
►リアルタイム処理に向けた最適化

56. 実際のロボットでは
3次元点群どうやって使われ
てるの?

57. こんな環境を考えよう
いわゆる ”tabletop”

58. こんなふうにものを掴みたい

59. 処理の流れを考えてみる
● テーブルの位置を検出する
● テーブルの上の物体をクラスタリングする
● 掴めそうな方向を推定する
● つかむ

60. 自己位置推定
● その前に、部屋の中でロボットが何処にいるか?
● 水平に設置されたレーザレンジファインダの値と予め
作った地図とをマッチング(パーティクルフィルタ/ICP)

61. 移動のためのコストマップ
● 床面にチルトレーザのデータを射影して移動時の
障害物回避に利用

62. 点群処理の流れ
入力点群
ロボット自身の点群除去
平面検出
平面上の点群抽出
把持可能な方向の推定
把持

63. 点群処理の流れ
入力点群
ロボット自身の点群除去
平面検出
平面上の点群抽出
把持可能な方向の推定
把持
こういうパイプライン処理
がしやすいのが
3次元点群処理の良い所!

64. ロボット自身の点群除去
● ロボットの形状とカメラのキャリブレーションが必要
赤: センサからの全点群 色つき: フィルタ済み点群

65. 平面検出
● ロボット的にはすごく重要!!
● 3次元点群を利用すると最も嬉しいことの一つ

66. RANSACによる平面検出
● 最もオーソドックスなやり方
● モデルとして平面の方程式を使えば良い
– pcl::SACSegmentation
● 欠点:
– 一回で求まるのはひとつの平面
– 「領域」にクラスタリングされない
inlier

67. ラベリングによる平面検出 1/5
● Connected Component Labeling
● 入力が画像の形をしていれば、デプス画像に対し
てラベリングすれば良い!
● Kinectの点群はデプス画像から作っているのでこの
前提が成り立つ
● PCLではこのような点群を”organized”と呼ぶ
● pcl::OrganizedMultiPlaneSegmentation
Ef cient Organized fi Point Cloud Segmentation with
Connected Components, Alexander J. B. et al. 3rd
Workshop on Semantic Perception Mapping and
Exploration, 2013

68. “Organized” PointCloud
● Organizedと呼ばれるデータ構造を利用する
● 点群の並びを画素の順番にしてキープしておく
● 近似最近某探索等が高速に実装できる
深さ画像

69. ラベリングによる平面検出 2/5

70. ラベリングによる平面検出 3/5

71. ラベリングによる平面検出 4/5

72. ラベリングによる平面検出 5/5
● 利点
– 速い (法線推定とあわせてQVGAで30fpsでる)
– 複数の平面検出が容易

73. 平面上の点群抽出
● 平面が出たらあとは簡単
● 平面上の点群を点同士の距離でクラスタリングする
Radu Rusu [Semantic 3D Object Maps for Everyday
Manipulation in Human Living Environments]

74. 物体を把持する方向の推定
● 把持計画は奥が深い...が、今回は簡単に
● 主成分分析で物体の方向を推定
2次元の主成分分析

75. 動作生成
● いわゆる「モーションプランニング」と呼ばれる
– もちろん奥が深い. 今回は真面目にやってない
Octomap Avoidance [Moveit!]
点群を利用すると障害物
回避のための環境のモデ
ル化が容易に
– Octomap
Armin Hornum et al. [OctoMap: An
Efficient Probabilistic 3D Mapping
Framework Based on Octrees]

76. 実験

77. まとめ
● ロボット屋さんにとって3次元点群はとても重要
● パーティクルフィルタベースなトラッキングライブラ
リ、pcl::trackingの紹介
● ロボットPR2で3次元点群を利用したマニピュレー
ションの紹介
– 3次元点群のおかげでtabletop manipulationはか
なり容易になった
– 少ない前提知識でのタスク実現