三次元点群処理ライブラリPCLと 統合ロボットシステム研究での...
-
Upload
ryohei-ueda -
Category
Science
-
view
5.752 -
download
0
description
Transcript of 三次元点群処理ライブラリPCLと 統合ロボットシステム研究での...
三次元点群処理ライブラリPCLと統合ロボットシステム研究での
利用例の紹介@第25回コンピュータビジョン勉強会
植田亮平
自己紹介
● 名前– 植田亮平 a.k.a. がらえもん
– @garaemon_coder
● 所属– 東京大学情報理工学系研究科・創造情報学専攻
– JSK/情報システム工学研究室
● 専門– ロボットシステム
● 最近の興味– Shared Autonomy, Supervised Teleoperation
略歴
2011.8 WillowGarageインターン
2014.4 大学復帰
2012.4 大学休学/スタートアップに参加
現在
2010.4 博士課程スタート
アジェンダ
1. 前置き– ロボット屋さんにとっての3次元点群
2. pcl::tracking– PCLにおけるトラッキングライブラリの紹介
3. ロボットシステムの中の点群処理– tabletop manipulationを例にとって
ロボット屋さんにとっての3次元点群
ロボットって何??
PR2 [willowgarage]
ロボットって何??
センサ アクチュエータ
計算機
ロボットって何??
= 計算機 + センサ + アクチュエータ
ロボット屋さんからみた3次元点群
● 実環境でアクチュエートするには3次元の情報が必須– 自分がどこにいるのか?
– 対象物はどこにあるのか?
– どういう経路で動けばいいか?
全部3次元の情報
様々なセンサ – kinectだけじゃないよ!
レーザレンジファインダ
hokuyo
SICK
レーザ + 回転
PR2 hokuyo-tilt
multisense SL/hokuyo-spindle
ToFカメラ
swissranger sr4000
softkinetic DepthSense 311
sr4000 + stereo coloring
ステレオカメラ
PS4 camera [SCE]
multisense s7 [Carnegie Robotics]
PCLの登場(とその背景) 1/2
● ロボットに3次元点群は大事● でも良いライブラリってないね(2009)
– WillowGarageはPR2で2つの良い点群センサを持っていた
1. チルトレーザ
2. プロジェクタ付きステレオ
– 画像処理ではOpenCVがあった
PCLの登場(とその背景) 2/2
● TUMのRadu RusuがWGにjoin– RaduはもともとWG intern出身
● ROS (Robot Operating System)の一部としてPCL(PointCloud Library)を公開– OpenCVの姉妹プロジェクト
– PCLは後にROSから独立したライブラリとして発展
kinectの登場
kinectすげーこんなに綺麗な点群とれるのか
30fpsでるなら点群で6自由度の物体追跡できそうじゃないか
pcl::tracking
Ryohei Ueda (Radu Rusu – Mentor)
Univ. of Tokyo
14/10/04
1mm downsampled
agenda
►イントロダクション►パーティクルフィルタ►尤度関数►並列化による高速化►さらなる高速化のためのチューニング►実装►実験
イントロダクション
なぜトラッキング?
1mm downsampled
►安価で30fpsとれるデプスセンサの登場►リアルタイムで物体認識をするのに役立つ
パーティクルフィルタ
►トラッキングアルゴリズムはいろいろあるが、、、►パーティクルフィルタによるトラッキングを採用
●尤度関数を自由に設計できる● 処理が重たい場合は並列化による最適化
● 近年CPUコア数は増えてる
パーティクルフィルタ
[T.Higuchi, Particle Filter, ‘05]
prediction weighting resampling
パーティクルフィルタ
[T.Higuchi, Particle Filter, ‘05]
prediction weighting resampling対象の位置+姿勢 (6自由度)
センサからの点群尤度関数
尤度関数
尤度関数
仮説=位置+姿勢
もしも仮説の場所にモデルがあったとしたら、尤度 – センサデータとの一致度はどのくらいか?
尤度関数
モデル
尤度関数
モデル
モデル点群を仮説の位置に移動する
尤度関数
移動したモデル点群の一点 から最も近いセンサ点群を探索する
尤度関数
と のデータ(位置、色etc)から尤度を計算する
尤度関数
モデル点群の一点 を全ての点、もしくはサンプリングして和をとり尤度とする
尤度関数
モデル点群の一点 を全ての点、もしくはサンプリングして和をとり尤度とする
尤度関数
モデル点群の一点 を全ての点、もしくはサンプリングして和をとり尤度とする
並列化
►各パーティクルの計算は独立►容易に並列化可能(OpenMPを利用)
パーティクルフィルタを採用した利点
8 cores (hyperthreaded)single core
2~6x faster1mm downsampled
モデル点群のサイズ
パーティクルの数
センサ点群の数
高速化
計算量
モデル点群のサイズ
パーティクルの数
センサ点群の数
高速化
計算量最近傍探索(kdtree)のコスト
高速化/Downsampling
►サンプリングしちゃおう!
1mm毎のサンプリング 2mm毎のサンプリング
4~5x faster
高速化/Adaptive Particle Filter
►Adaptive Particle Filterを利用しよう► KLD Sampling [D. Fox, IJRR03]
高速化/Adaptive Particle Filter
►Adaptive Particle Filterを利用しよう► KLD Sampling [D. Fox, IJRR03]
簡単に言うと、パーティクルの尤度分布に従ってパーティクルの数を変更する分布のピークが明らかな場合、パーティクルの数は少なくて良い分布のピークが明らかでない場合、パーティクルの数を増やす
高速化/点群のサイズ削減
センサ点群の数をどうやって減らす?
高速化/点群のサイズ削減
高速化/点群のサイズ削減
高速化/点群のサイズ削減
高速化/点群のサイズ削減
高速化/点群のサイズ削減
高速化/点群のサイズ削減
高速化/点群のサイズ削減
高速化/点群のサイズ削減
高速化/点群のサイズ削減
OctreeKdtree
fps
* かなりチューンしています
実装
Tracker
ParticleFilterTracker
KLDAdaptiveParticleFilterTracker
KLDAdaptiveParticleFilterOMPTracker
ParticleFilterOMPTracker
PointCloudCoherence
PointCoherence
DistanceCoherence
HSVColorCoherence
NormalCoherence
NearestPairPointCloudCoherence
ApproxNearestPairPointCloudCoherence
実験
movie movie
movie
実験
実験
まとめ
►点群ベースなトラッキングライブラリpcl::tracking►パーティクルフィルタに基づくアプローチ
►位置・姿勢の6自由度のトラッキング► リアルタイム処理に向けた最適化
実際のロボットでは3次元点群どうやって使われ
てるの?
こんな環境を考えよういわゆる ”tabletop”
こんなふうにものを掴みたい
処理の流れを考えてみる
● テーブルの位置を検出する● テーブルの上の物体をクラスタリングする● 掴めそうな方向を推定する● つかむ
自己位置推定
● その前に、部屋の中でロボットが何処にいるか?
● 水平に設置されたレーザレンジファインダの値と予め作った地図とをマッチング(パーティクルフィルタ/ICP)
移動のためのコストマップ
● 床面にチルトレーザのデータを射影して移動時の障害物回避に利用
点群処理の流れ
ロボット自身の点群除去
入力点群
平面検出
平面上の点群抽出
把持可能な方向の推定
把持
点群処理の流れ
ロボット自身の点群除去
入力点群
平面検出
平面上の点群抽出
把持可能な方向の推定
把持
こういうパイプライン処理がしやすいのが3次元点群処理の良い所!
ロボット自身の点群除去● ロボットの形状とカメラのキャリブレーションが必要
赤: センサからの全点群 色つき: フィルタ済み点群
平面検出
● ロボット的にはすごく重要!!● 3次元点群を利用すると最も嬉しいことの一つ
RANSACによる平面検出
● 最もオーソドックスなやり方● モデルとして平面の方程式を使えば良い
– pcl::SACSegmentation
● 欠点:– 一回で求まるのはひとつの平面
– 「領域」にクラスタリングされない
inlier
ラベリングによる平面検出 1/5
● Connected Component Labeling● 入力が画像の形をしていれば、デプス画像に対し
てラベリングすれば良い!● Kinectの点群はデプス画像から作っているのでこの
前提が成り立つ● PCLではこのような点群を”organized”と呼ぶ● pcl::OrganizedMultiPlaneSegmentation
Ef cient Organized Point Cloud Segmentation with fiConnected Components, Alexander J. B. et al. 3rd Workshop on Semantic Perception Mapping and Exploration, 2013
“Organized” PointCloud
● Organizedと呼ばれるデータ構造を利用する● 点群の並びを画素の順番にしてキープしておく● 近似最近某探索等が高速に実装できる
深さ画像
ラベリングによる平面検出 2/5
ラベリングによる平面検出 3/5
ラベリングによる平面検出 4/5
ラベリングによる平面検出 5/5
● 利点– 速い (法線推定とあわせてQVGAで30fpsでる)
– 複数の平面検出が容易
平面上の点群抽出
● 平面が出たらあとは簡単
● 平面上の点群を点同士の距離でクラスタリングするRadu Rusu [Semantic 3D Object Maps for Everyday Manipulation in Human Living Environments]
物体を把持する方向の推定
● 把持計画は奥が深い...が、今回は簡単に● 主成分分析で物体の方向を推定
2次元の主成分分析
動作生成
● いわゆる「モーションプランニング」と呼ばれる– もちろん奥が深い. 今回は真面目にやってない
Octomap Avoidance [Moveit!]
点群を利用すると障害物回避のための環境のモデル化が容易に
– Octomap
Armin Hornum et al. [OctoMap: An Efficient Probabilistic 3D Mapping Framework Based on Octrees]
実験
まとめ
● ロボット屋さんにとって3次元点群はとても重要● パーティクルフィルタベースなトラッキングライブラ
リ、pcl::trackingの紹介● ロボットPR2で3次元点群を利用したマニピュレー
ションの紹介– 3次元点群のおかげでtabletop manipulationはか
なり容易になった
– 少ない前提知識でのタスク実現