単眼広角カメラを用いた屋内三次元環境モデリングと姿勢推定MonoSLAM in indoor enviroment by wide-angle Camera

○正　山崎 公俊　学　矢口 裕明　学　畑尾 直孝学　ザオプトラ ニコラウス　正　稲葉 雅幸（東大）

*Kimitoshi YAMAZAKI, Hiroaki YAGUCHI, Naotaka HATAO,

Zaoputra NIKOLAUS and Masayuki INABA (The Univ. of Tokyo)

yamazaki@jsk.t.u-tokyo.ac.jp

This paper describes 3D environment modeling and camera pose estimation system by using a singlecamera. Because a wide angle camera provides a wide field of view at once, it enables the system toacquire reconstruction results stably. Moreover, a camera which has advantages of small size and lightweight, is easily considered to apply the system to a portable sensor. We constructed environmentmodeling and camera pose estimation system by means of SFM approach (8 point algorithm, bundleadjustment and motion stereo). Graph slam is also adapted to smooth the reconstruction results.Experiments were performed in indoor environment, we comfirmed the capability of the reconstructionability of a wide angle camera.

Key words: Wide angle camera, MonoSLAM

1 はじめにカメラは，得られる情報量が多いため，環境情報を取

得するセンサとしてきわめて有用である．本稿では，広視野画像が得られる小型カメラを用いて，環境の三次元形状とカメラ自身の動きを得るシステムについて述べる．屋内で動き回る移動体の姿勢推定，もしくは環境モデ

リングを考えたとき，単眼カメラはシンプルなシステム構成でこの目的を達成できる．この利点に着目し，上述の課題を解決する手法が従来から提案されてきている [1][6][8].姿勢推定を安定して行うための一つの解決策は，画角

の広いカメラを用いることである．なぜなら，数十度程度の視野しか持たない通常のカメラでは，屋内のように視覚特徴が多い環境であっても，常にそれらの特徴を捕らえ続けておくことが難しいからである．例えば，廊下などの環境では通常は長手方向が進行方向かつ注視領域となるべきだが，画像特徴を多く得やすいのはむしろ左右の壁面である．これらより，屋内環境で姿勢認識を考える場合，カメラは広視野のものを利用するのが望ましい．本稿では，球面レンズを装着した一台のカメラで，周

囲環境の三次元復元とカメラの姿勢推定を同時に行うシステムについて述べる．提案システムは，小型・軽量に構成され，一度に広視野の情報を獲得でき，動き自由度の制限も特にないため，様々に動く対象，例えば人間の頭などへ搭載する目的に適している．形状復元と姿勢推定の実現方法としては，画像から抽出した画像特徴点からSFM(Structure from motion)を逐次的に行う．具体的には，8点法とモーションステレオ，バンドル調整を組み合わせる．また，特徴点の対応付け誤差や基線長が定まらないことから生じる復元誤差を，GraphSLAMのスムージング手法を適用することにより軽減する．

図 1: Wide angle camera

2 アプローチ2.1 小型広角カメラ

周囲環境から多くの情報を途切れなく得るためには，一度に広い視野で画像を取得できることが望まれる．本研究では，図 1に示すような球面レンズを搭載したカメラを利用し，そこから得られる半球視野画像から姿勢推定と環境モデリングを行う．なお，このような画像にピンホールカメラモデルを仮定したキャリブレーションを行うと，視野が無駄に切り落とされたり画質を損なう恐れがあるため，本研究では球面カメラモデル（r−θモデル）を仮定し，光線ベクトルをベースとした復元計算を行う．

2.2 環境特徴の取得

画像からロバストに情報を得る方法としては，特徴点がよく知られている．ある画像から，エッジのコーナーなど特徴量の大きな小領域を抽出し，その特徴を別の画像上へ対応付けることで，復元処理の鍵となる情報が得

られる．SIFT[4]，KLT[5]などが提案されている．ただし，これらの方法は，小領域の平行移動のみ，も

しくは二次元アフィン変換までしか想定していないため，広角カメラから得られる歪みの大きな画像では，対応付け誤差が大きくなる恐れがある．これは，カメラ姿勢の復元精度に直接的に影響する．本研究では，復元後のカメラ姿勢列にスムージング処理を施すことで，この問題を軽減する．

2.3 環境モデリングとカメラ姿勢推定

単眼カメラから得られる連続画像で SFMを行うには種々の方法があるが，本研究では，二枚の画像を用いる方法としてよく知られた 8点法を適用する．8点法により，カメラの相対的な位置ベクトルと回転行列を算出する．また，その結果からモーションステレオを行い，環境形状を獲得する．個々の手法については次章で述べる．なお，本システムでは実際のカメラ移動量が不明なた

め，復元のスケールを厳密に合わせることはしない．本研究の応用として小型の姿勢センサなどがあるが，そこではシステム内部で構築した環境モデルでの相対姿勢が分かれば済む場合もあり，スケール不定のアプローチでも適用場面は多いと考えられる．

3 環境形状とカメラ姿勢の逐次復元3.1 8点法

8点法は，2つの視点で撮影された画像から，その視点の相対姿勢を求める手法である．二枚の画像間で特徴の対応付けができているとき，下記のような基礎方程式が成り立つことが知られている [2]．

vT1 Ev2 = 0 (1)

ここで，vi = (vix, viy, viz)は，i番目の画像特徴点を光線ベクトルに変換したもの，Eは基本行列である．8点法では，式 (1)から得た以下のような連立方程式を解くことで，基本行列を推定する．

Me = 0M = (v1xv2x v1xv2y v1xv2z ... v1zv2z)e = (e11 e12 e13 ... e33)

(2)

eij は，3 × 3の基本行列の各要素である．こうして求めた Eは，下記のように分解できる [7]．

E = TR (3)

Tは，二画像間の平行移動成分に関する歪み対称行列，Rは回転行列である．なお，実際の画像では特徴点の追跡誤差などが起きう

るため，全ての特徴点を用いて 8点法を行うと解が安定しない．そこで，実装では RANSACにより特徴点を選び出す枠組みを導入する．

3.2 モーションステレオ

8点法により，二画像間の相対姿勢が既知となるので，モーションステレオを行い環境形状を求める [3]．

C1 = |X − s1v1|2 + |X − s2Rv2 − t|2 (4)

ここで，v1，v2 は二枚の画像における対応点の光線ベクトルであり，X は点の三次元位置である．tは式 (3)から求めたカメラの平行移動ベクトルである．s1，s2 は，それぞれ v1，v2 の長さを表すスカラー定数である．式 (4)を最小二乗法によりXについて解き，画像特徴点の三次元座標を求める．

3.3 バンドル調整

8点法は，二つのカメラ間の距離が大きくなければ，安定した解を得ることが難しい．そこで，8点法で一度カメラ姿勢を求めた後は，そのカメラ姿勢とモーションステレオで求めた既知の三次元点から，現在のカメラ姿勢を推定する．これは，以下の式を最小化することで行う．

C2 =p∑

i=1

|ui − f(X)| (5)

ここで，uiは，新しい画像上で追跡されている i番目の特徴点の座標である．Xは，その特徴点の三次元座標であり，f(·)はその点を画像上に逆投影する演算を表す．f(·)の計算には，カメラ姿勢を表すR，tが内包されている．推定計算では，三次元座標が既知の全ての点について式(5)の計算を行い，その結果のメディアン値からカメラ姿勢の変化量を求めることを繰り返す．これは非線形関数の最小化問題となり，本研究では収束計算には最急降下法を用いている．

3.4 復元したカメラ姿勢列のスムージング

上述のアルゴリズムは逐次処理であるため，オンラインで姿勢推定する目的に適している．しかし，姿勢推定の誤差が逐次的に伝播するため，得られる解が次第に真値から外れていく恐れがある．これに対処するため，カメラ姿勢のスムージングを行う．スムージングの手法は，LuとMiliosの手法 [9]を三次元

に拡張したもの [10]を用いる．これは，GraphSLAMと呼ばれる SLAM(Simultaneous Localization and Mapping)の一種である．GraphSLAMでは，推定されたカメラの座標をノードとし，ノード間の相対的な姿勢推定が成功したところにアークを張る．そして，最終的に作られたグラフ構造から姿勢推定結果のスムージングを行う．ノードの個数を n + 1 個，各ノードを d 次元のベク

トル Xi とする (i = 0, 1, ..., n)．ノード i,j のアークはDij = Xi − Xj で表し，その観測値をDij，誤差共分散行列を Cij とする．なお，X0 は原点とする．このとき，マハラノビス距離の二乗和W は，以下の式

�

��

�

�� �

�

�

��

�

�� �

�

�

��

� � ��

�

���� ����� � ��������� ����� ��� �����! � ��"$# �&%('�'�� ��� ���

図 2: Camera poses smoothing by GraphSLAM

図 3: Reconstruction Algorithm

によって与えられる．

W =∑(i.j)

(Dij − Dij)tC−1ij (Dij − Dij) (6)

このW を最小化する各Xiを求めることにより，スムージングを行う．図 2に，この概念図を示す．

3.5 逐次復元アルゴリズム

環境モデルとカメラ姿勢を求める流れは以下のようである．

1. 画像特徴点を追跡していき，所定の条件を満たしたところで 8点法を行う．最初と現在の二画像間で，カメラの回転・平行移動成分を求める．

2. 求めたカメラ姿勢を基にして，特徴点の三次元位置をモーションステレオの式 (4)により算出する．

3. 特徴点を次の画像で追跡し，既知の三次元点のうち画像上で観測が続いている点を用いて，バンドル調整によりカメラ姿勢を求める．

4. 既知の特徴を再度捉えられる位置にカメラが戻ってきた場合には，以前のに復元したカメラ姿勢列をスムージング処理により補正する．

基本的には，1.を最初に一度だけ行った後，2.，3.を繰り返していく．図 3に，この概念図を示す．

8点法を二枚の画像ごとに行い続ける方式でも環境形状は復元可能であるが，本研究はカメラの実変移量が未知の条件で行っているため，各復元結果でスケールが変わってしまうという問題がある．また，8点法はカメラの基線長がある程度大きくないと復元結果が安定しない．一方，上記の方法であれば基線長の問題は最初の一度だけ考えればよく，バンドル調整をベースとした処理では基線長を考慮する必要がない．また，環境形状のスケールは最初の復元時に固定されるので，後は統一されたスケールの基で復元処理を行うことができる．

4 実験4.1 カメラの歪みパラメータ算出

図 1は，本システムで用いたカメラ (NM30,オプト株式会社)である．33x36x33mm，60gと小型軽量であり，USB

図 4: Smoothing simulation

図 5: Feature tracking results

接続で JPEG画像を PCに取り込むことができる．本実験では，3 fps程度で 640x480の画像を取得して行った．このカメラの画角はほぼ 180度であり，球面カメラモ

デルが仮定できる．ただし若干の歪みを含んでいるため，事前に下記の六次多項式に従って歪みパラメータを求め，キャリブレーションを行った．

φ = k0 + k1r + k2r2 + k3r

3 + k4r4 + k5r

5 + k6r6 (7)

ここで，rは画像中心からの距離，φはレンズ中心からの角度である．本実験で用いたカメラでは, k0 = −9.922×10−4, k1 = 8.149 × 10−3, k2 = −2.185 × 10−6, k3 =4.7205 × 10−8, k4 = −5.6129 × 10−10, k5 = 1.9295 ×10−12, k6 = −2.7635 × 10−15 であった．

4.2 スムージングのシミュレーション

図 4は，3.4節で述べたスムージング処理のシミュレーション結果を示している．青い点列は，逐次復元しているカメラ姿勢に正規分布の誤差を入れたものである．本来ならば，最後の復元時に最初のカメラ姿勢に戻るはずだが，逐次誤差により達成できていない．一方，「最初のカメラ姿勢＝最後のカメラ姿勢」の情報を与えスムージングを行うと，中間のカメラ姿勢全てに修正が加えられ，赤い点列で示すような，理想値とほぼ同等の四角の経路が得られた．

図 6: Experimental environment

図 7: Reconstructed shape

4.3 実画像による実験

図 5は，長方形の部屋内で上向きに設置したカメラを直進させながら撮影した画像列の一部である．カメラを1ｍ強動かす間に，100枚の画像を撮影した．1枚目の画像から 100枚目の画像まで 400点の特徴点追跡を行ったところ，追跡ミスにより消去された点は 5点のみであった．残った 395点に対し，RANSACによる特徴点選択を行いながら 8点法を適用したところ，安定した復元解が得られた．このように，多くの特徴点が長期間追跡可能な点で，広視野画像は優れていると言える．図 6は，幅十数メートルの部屋内の概観図を示してい

る．図 7は，左が天井にカメラを向けた場合の撮影画像，右が部屋形状を復元した結果である．ここでは，カメラが 1ｍ程度動いたあとに 8点法を行った．部屋の形状が復元できていることが分かる．また，図 8は，復元された三次元形状を基に，バンドル調整によってカメラ姿勢を逐次求めていった結果である．カメラは一辺 1500mmの正方形を描くように移動され，その間に撮影された画像は約 500枚であった．図はその途中経過を示しており，画像の枚数が変わるにつれ，三本の矢印で示されるカメラ姿勢が，正方形軌道で移動していることが判る.

5 おわりに本稿では，広視野画像を得られる小型カメラを用いて，

三次元形状とカメラの姿勢推定を同時に行うシステムについて述べた．8点法とモーションステレオ，バンドル調整を組み合わせ，さらにGraphSLAMのスムージング手法を適用することで，環境形状とカメラ姿勢を獲得できることを示した．今後は，復元精度を向上させ，その評価を行う．また，

人の頭などへ搭載するセンサシステムの構築，屋外への

図 8: Reconstructed camera poses

適用などの応用研究を行っていく予定である．

謝辞この研究は、文部科学省「先端融合領域イノベーショ

ン創出拠点の形成：　少子高齢社会と人を支える IRT基盤の創出」で行われたものである。

参考文献[1] A. J. Davison, Y. G. Cid, and N. Kita: ”Real-time 3D

SLAM with Wide-angle Vision,” Proc. of IFAC Sympo-sium On Intelligent Autonomous Vehicle, 2004.

[2] 出口：「ロボットビジョンの基礎」, コロナ社, 2000.

[3] 徐：「写真から作る 3次元 CG」, 近代科学社, 2001.

[4] D. G. Lowe, ”Distinctive image features from scale-invariant keypoints,” Int’l Journal of Computer Vision,60, 2 (2004), pp. 91-110.

[5] J. Shi and C. Tomasi:”Good Features to Track,” Proc.of the 1994 IEEE Computer Society Conference onComputer Vision and Pattern Recognition, pp.593.600,1994.

[6] M. Tomono:“3-D Localization and Mapping Using aSingle Camera Based on Structure-from-Motion withAutomatic Baseline Selection,” Proc. of IEEE Int’l conf.on Robotics and Automation, pp.3342–3347, 2005.

[7] R. Hartley and A.Zisserman:“Multiple View Geometryin computer vision,” Cambridge Univ., 2004.

[8] P. Chang and M. Hebert:”Omni-directional structurefrom motion,” Proc. of the 2000 IEEE Workshop onOmnidirectional Vision, June, 2000, pp. 127 - 133.

[9] F. Lu and E. Milios: “Globally consistent rangescan alignment for environment mapping,” AutonomousRobots, Vol. 4, No. 4, pp.333–349, 1997.

[10] D. Borrmann, J. Elseberg, K. Lingemann, A. Nuchter,and J. Hertzberg: “Globally consistent 3D mappingwith scan matching,” Robotics and Autonomous Sys-tems, Vol. 56, No. 2, pp.130–142, 2008.