三次元点群処理ライブラリ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で２つの良い点群センサを持っていた

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はか

なり容易になった

– 少ない前提知識でのタスク実現