JP2018147095A - カメラ位置姿勢推定装置、方法およびプログラム - Google Patents

Abstract

【課題】カメラの位置姿勢の推定精度を向上させる。【解決手段】記憶部１ａは、キー画像２ａ，２ｂ，・・・と、キー画像２ａ，２ｂ，・・・それぞれの撮像時におけるカメラの位置姿勢を示す姿勢値３ａ，３ｂ，・・・とを記憶する。演算部１ｂは、キー画像２ａ，２ｂ，・・・の中から現画像４に類似するキー画像２ａを特定し、特定されたキー画像２ａから複数画素を含む領域５ａ，５ｂを特定するとともに、現画像４から領域５ａ，５ｂに対応する領域６ａ，６ｂを推定し、領域５ａ，５ｂと領域６ａ，６ｂとの間の画素値の比較結果と、キー画像２ａに対応する姿勢値３ａとに基づいて、現画像４の撮像時におけるカメラの位置姿勢を推定する。【選択図】図１

Description

本発明は、カメラ位置姿勢推定装置、方法およびプログラムに関する。

現実空間を撮像した画像上の所定位置に仮想的な画像を重畳して表示する拡張現実感（Augmented Reality，ＡＲ）技術が知られている。このＡＲ技術は、例えば、工場などの作業空間において、作業内容や作業対象箇所などを示す作業支援情報を撮像画像上に重畳表示することによって作業者の作業を支援する、といった用途で普及しつつある。

ＡＲ技術では、撮像画像上の適切な位置に仮想的な画像を重畳するために、実空間でのカメラの位置姿勢を推定する技術が用いられる。また、ユーザが作業中の状況では、カメラの位置姿勢が頻繁に変化する。このため、位置姿勢の推定に一時的に失敗する状態になりやすい。このことから、位置姿勢の推定に失敗した状態から、その推定を再開する復帰処理の技術が求められている。

復帰処理手法の一例として、現画像内の特徴点とキーフレーム内の特徴点とを対応付けし、対応付けられたキーフレーム中の特徴点についての三次元マップの座標情報に基づいて、現画像の撮像時におけるカメラの位置姿勢を推定する手法が提案されている。

また、位置姿勢の算出が破綻することを防止する次のような位置姿勢計測装置も提案されている。この位置姿勢計測装置は、前フレームから得られる複数の位置姿勢を基に複数の位置姿勢を発生させ、それぞれについて非線形最適化によりカメラの位置姿勢を求め、その中から最も妥当な位置姿勢を選択する。

特開２００８−１７６５０９号公報

J. Straub et al., Fast relocalization for visual odometry using binary features, In Proc. ICIP, pp.2548-2552, 2013

ところで、前述の復帰処理手法で用いられる位置姿勢の推定処理では、現画像とキーフレームとの間で特徴点の対応関係が特定される。しかし、この処理では、例えば、対象物を撮像したときに画像上に類似する画像パターンが多数現れる場合に、特徴点の対応関係に誤りが発生しやすい。このため、位置姿勢の推定精度が低下するという問題がある。

１つの側面では、本発明は、カメラの位置姿勢の推定精度を向上させたカメラ位置姿勢推定装置、方法およびプログラムを提供することを目的とする。

１つの案では、記憶部と演算部とを有する次のようなカメラ位置姿勢推定装置が提供される。このカメラ位置姿勢推定装置において、記憶部は、複数の第１の画像と、複数の第１の画像それぞれの撮像時におけるカメラの位置姿勢を示す複数の第１の姿勢値とを記憶する。演算部は、複数の第１の画像の中から第２の画像に類似する第３の画像を特定し、第３の画像から複数画素を含む第１の領域を特定するとともに、第２の画像から第１の領域に対応する第２の領域を推定し、第１の領域と第２の領域との間の画素値の比較結果と、複数の第１の姿勢値のうち第３の画像に対応する第２の姿勢値とに基づいて、第２の画像の撮像時におけるカメラの位置姿勢を示す第３の姿勢値を推定する。

また、１つの案では、上記のカメラ位置姿勢推定装置と同様の処理をコンピュータが実行するカメラ位置姿勢推定方法が提供される。
さらに、１つの案では、上記のカメラ位置姿勢推定装置と同様の処理をコンピュータに実行させるカメラ位置姿勢推定プログラムが提供される。

１つの側面では、カメラの位置姿勢の推定精度を向上させることができる。

第１の実施の形態に係るカメラ位置姿勢推定装置の構成例および処理例を示す図である。 第２の実施の形態に係る端末装置のハードウェア構成例を示す図である。 端末装置が備える処理機能の構成例を示すブロック図である。 三次元マップのデータ構成例を示す図である。 キーフレーム情報テーブルのデータ構成例を示す図である。 撮像画像に類似画像パターンが現れる場合における特徴点とマップ点との対応付けの例を示す図である。 復帰処理部の内部構成例を示す図である。 詳細相対位置姿勢の算出処理について説明するための第１の図である。 詳細相対位置姿勢の算出処理について説明するための第２の図である。 トラッキング処理部の処理手順の例を示すフローチャート（その１）である。 トラッキング処理部の処理手順の例を示すフローチャート（その２）である。 第１の変形例での注目領域の特定方法を説明するための図である。 第１の変形例における復帰処理手順の例を示すフローチャートである。 画像ピラミッドの構成例を示す図である。 第２の変形例における復帰処理手順の例を示すフローチャートである。

以下、本発明の実施の形態について図面を参照して説明する。
〔第１の実施の形態〕
図１は、第１の実施の形態に係るカメラ位置姿勢推定装置の構成例および処理例を示す図である。図１に示すカメラ位置姿勢推定装置１は、記憶部１ａと演算部１ｂを有する。記憶部１ａは、例えば、カメラ位置姿勢推定装置１が備える記憶装置（図示せず）の記憶領域として実装される。演算部１ｂは、例えば、カメラ位置姿勢推定装置１が備えるプロセッサ（図示せず）として実装される。

記憶部１ａは、キー画像（第１の画像）２ａ，２ｂ，・・・を記憶する。キー画像２ａ，２ｂ，・・・は、それぞれが図示しないカメラによって撮像されたときのカメラの位置姿勢が正しく推定されている画像である。記憶部１ａには、キー画像２ａ，２ｂ，・・・のそれぞれについて推定された位置姿勢を示す姿勢値３ａ，３ｂ，・・・も記憶されている。

演算部１ｂは、キー画像２ａ，２ｂ，・・・の中から現画像（第２の画像）４に類似するキー画像（第３の画像）を特定する（ステップＳ１）。現画像４は、カメラで撮像され、その撮像時におけるカメラの位置姿勢の推定対象となっている画像である。図１の例では、類似するキー画像としてキー画像２ａが特定されたものとする。

次に、演算部１ｂは、特定されたキー画像２ａから、複数画素を含む領域（第１の領域）を特定する。図１の例では、このような領域として領域５ａ，５ｂが特定されている。また、演算部１ｂは、特定された領域５ａ，５ｂにそれぞれ対応する領域（第２の領域）６ａ，６ｂを、現画像４から推定する（ステップＳ２）。

次に、演算部１ｂは、領域５ａ，５ｂと領域６ａ，６ｂとの間の画素値の比較結果と、キー画像２ａに対応する姿勢値３ａとに基づいて、現画像４の撮像時におけるカメラの位置姿勢を推定する（ステップＳ３）。

このようなカメラ位置姿勢推定装置１によれば、それぞれ複数画素を有する領域５ａ，５ｂと領域６ａ，６ｂとの間の画素値の比較結果に基づいて、現画像４に対応する位置姿勢が推定される。これにより、例えば、キー画像２ａと現画像４との間での局所的な特徴点同士の対応関係に基づいて推定する場合と比較して、より大域的な画像領域の対応関係に基づいて位置姿勢を推定することができる。そのため、画像上に類似する画像パターンが多数写っている場合において、画像間で対応する位置の推定を誤り、その誤りによって位置姿勢の推定精度が低下するという事態の発生可能性を抑制できる。したがって、位置姿勢の推定精度を向上させることができる。

〔第２の実施の形態〕
次に、図１のカメラ位置姿勢推定装置１が備える位置姿勢の推定処理を、位置姿勢の推定に失敗した状態から推定を再開する復帰処理に利用した端末装置の例について説明する。

図２は、第２の実施の形態に係る端末装置のハードウェア構成例を示す図である。第２の実施の形態に係る端末装置１００は、画像を撮像してそのときのカメラの位置姿勢を推定し、推定された位置姿勢の情報を用いて撮像画像上に仮想的な画像を重畳表示させる機能を備える。この端末装置１００は、例えば、図２に示すような携帯型のコンピュータとして実現される。

図２に示す端末装置１００は、プロセッサ１０１によって装置全体が制御されている。プロセッサ１０１は、マルチプロセッサであってもよい。プロセッサ１０１は、例えば、ＣＰＵ（Central Processing Unit）、ＭＰＵ（Micro Processing Unit）、ＤＳＰ（Digital Signal Processor）、ＡＳＩＣ（Application Specific Integrated Circuit）、またはＰＬＤ（Programmable Logic Device）である。また、プロセッサ１０１は、ＣＰＵ、ＭＰＵ、ＤＳＰ、ＡＳＩＣ、ＰＬＤのうちの２以上の要素の組み合わせであってもよい。

プロセッサ１０１には、バス１０９を介して、ＲＡＭ（Random Access Memory）１０２と複数の周辺機器が接続されている。
ＲＡＭ１０２は、端末装置１００の主記憶装置として使用される。ＲＡＭ１０２には、プロセッサ１０１に実行させるＯＳ（Operating System）プログラムやアプリケーションプログラムの少なくとも一部が一時的に格納される。また、ＲＡＭ１０２には、プロセッサ１０１による処理に必要な各種データが格納される。

バス１０９に接続されている周辺機器としては、ＳＳＤ（Solid State Drive）１０３、表示装置１０４、入力装置１０５、読み取り装置１０６、無線通信インタフェース１０７およびカメラ１０８がある。

ＳＳＤ１０３は、端末装置１００の補助記憶装置として使用される。ＳＳＤ１０３には、ＯＳプログラム、アプリケーションプログラム、および各種データが格納される。なお、補助記憶装置としては、ＨＤＤ（Hard Disk Drive）などの他の種類の不揮発性記憶装置を使用することもできる。

表示装置１０４は、プロセッサ１０１からの命令に従って、画像をその画面に表示する。表示装置１０４としては、液晶ディスプレイや、有機ＥＬ（ElectroLuminescence）ディスプレイなどがある。

入力装置１０５は、ユーザによる入力操作に応じた信号をプロセッサ１０１に送信する。入力装置１０５としては、例えば、表示装置１０４の表示面に配置されるタッチパネルや、タッチパッド、マウス、トラックボール、操作キーなどがある。

読み取り装置１０６には、可搬型記録媒体１０６ａが脱着される。読み取り装置１０６は、可搬型記録媒体１０６ａに記録されたデータを読み取ってプロセッサ１０１に送信する。可搬型記録媒体１０６ａとしては、光ディスク、光磁気ディスク、半導体メモリなどがある。

無線通信インタフェース１０７は、無線通信により他の装置との間でデータの送受信を行う。
カメラ１０８は、撮像素子によって得られた画像信号をデジタル化して、プロセッサ１０１に送信する。

以上のようなハードウェア構成によって、端末装置１００の処理機能を実現することができる。
＜端末装置の処理機能＞
図３は、端末装置が備える処理機能の構成例を示すブロック図である。端末装置１００は、記憶部１１０、トラッキング処理部１２０、マップ作成部１３０および重畳表示制御部１４０を有する。記憶部１１０は、端末装置１００が備える記憶装置（例えば、ＲＡＭ１０２、ＳＳＤ１０３など）の記憶領域として実装される。トラッキング処理部１２０、マップ作成部１３０および重畳表示制御部１４０の処理は、例えば、プロセッサ１０１が所定のプログラムを実行することで実現される。

記憶部１１０には、三次元マップ１１１とキーフレーム情報テーブル１１２が記憶される。三次元マップ１１１には、対象物上の特徴点の三次元座標が記憶される。キーフレーム情報テーブル１１２には、キーフレームに関するキーフレーム情報が記憶される。

ここで、図４は、三次元マップのデータ構成例を示す図である。三次元マップ１１１の各レコードには、マップ点番号、三次元座標および特徴量が記憶される。マップ点番号は、対象物上の特徴点（マップ点）の識別番号を示す。三次元座標は、世界座標系におけるマップ点のＸ軸、Ｙ軸、Ｚ軸それぞれに対する座標を示す。各座標は、例えば、ミリメートル（ｍｍ）で表される。特徴量は、マップ点の特徴を示す情報である。特徴量としては、例えば、ＳＩＦＴ（Scale-Invariant Feature Transform）、ＳＵＲＦ（Speeded Up Robust Features）、ＯＲＢ（Oriented FAST and Rotated BRIEF，FAST：Features from Accelerated Segment Test，BRIEF：Binary Robust Independent Elementary Features）などが用いられる。

三次元マップ１１１は、後述するように、トラッキング処理部１２０によってカメラ１０８の位置姿勢が推定される際に参照される。また、三次元マップ１１１には、マップ作成部１３０によって、位置姿勢が正しく推定された撮像画像から新たに抽出された特徴点についてのレコードが追加される。

図５は、キーフレーム情報テーブルのデータ構成例を示す図である。キーフレームとは、位置姿勢が正しく推定された撮像画像の中から所定の条件に基づいて選択された画像である。キーフレーム情報は、キーフレーム情報テーブル１１２の１つのレコードに含まれる情報に対応し、キーフレーム番号、位置姿勢、画像情報、特徴点群および対応マップ点番号を含む。

キーフレーム番号は、キーフレームの識別番号を示す。位置姿勢は、キーフレームの撮像時におけるカメラの位置姿勢を示す。位置姿勢は、（ｒ₁，ｒ₂，ｒ₃，ｔ₁，ｔ₂，ｔ₃）という６次元の情報として表される。（ｒ₁，ｒ₂，ｒ₃）は、世界座標におけるカメラ１０８の姿勢（回転移動成分）を示し、（ｔ₁，ｔ₂，ｔ₃）は、世界座標におけるカメラ１０８の位置（平行移動成分）を示す。

画像情報は、キーフレームの各画素の画素値を示す。本実施の形態では、画像情報は、各画素の輝度値を示すものとする。特徴点群は、キーフレームから抽出され、かつ、三次元マップ１１１内のマップ点と対応付けられた特徴点それぞれについてのキーフレーム上の座標を示す。対応マップ点番号は、特徴点それぞれに対応するマップ点のマップ点番号を示す。

以下、図３を用いて説明を続ける。
トラッキング処理部１２０は、カメラ１０８から撮像画像を取得するたびに、その撮像時のカメラ１０８の位置姿勢を推定する。また、トラッキング処理部１２０は、位置姿勢が正しく推定された撮像画像の中からキーフレームを選択し、そのキーフレームに関するキーフレーム情報をキーフレーム情報テーブル１１２に登録する。

マップ作成部１３０は、キーフレーム情報テーブル１１２に登録されたキーフレーム情報に基づいて、マップ点の三次元座標および特徴量を三次元マップ１１１に登録する。例えば、マップ作成部１３０は、新たに抽出された特徴点を含むキーフレームのペアを選択し、キーフレーム間で対応する所定個数以上の特徴点の二次元座標を用いて、三角測量の原理によって各特徴点の三次元座標を算出する。三角測量の原理による三次元座標の算出方法としては、例えば、非特許文献「R. I. Hartley et al., Triangulation, Computer Vision and Image Understanding, Vol. 68, No.2, pp.146-157, 1997」に記載の方法を用いることができる。

重畳表示制御部１４０は、三次元マップ１１１と、トラッキング処理部１２０によって推定された位置姿勢とに基づいて、撮像画像上に所定の作業支援情報を重畳表示させる。例えば、重畳表示制御部１４０は、撮像画像からマーカを認識し、そのマーカの内部パターンの認識結果から作業段階を特定する。重畳表示制御部１４０は、作業段階ごとに対応付けて記憶された作業支援情報（図示せず）の中から、特定された作業段階に対応する作業支援情報を読み出し、読み出した作業支援情報を撮像画像上の適切な位置に重畳して表示させる。重畳表示制御部１４０は、撮像画像について推定された位置姿勢に基づいて、作業支援情報の表示状態（例えば、回転角度）を調整する。

次に、トラッキング処理部１２０の内部構成について説明する。トラッキング処理部１２０は、画像取得部１２１、位置姿勢推定部１２２、品質判定部１２３、キーフレーム登録処理部１２４および復帰処理部１２５を有する。

画像取得部１２１は、カメラ１０８によって撮像された撮像画像を一定間隔で取得して、位置姿勢推定部１２２に供給する。なお、以下の説明では、画像取得部１２１によって取得された撮像画像、すなわち、位置姿勢の推定対象の撮像画像を「現画像」と記載する場合がある。

位置姿勢推定部１２２は、現画像の撮像時におけるカメラ１０８の位置姿勢を、次のような方法によって推定する。
位置姿勢推定部１２２は、まず、現画像から特徴点を抽出する。次に、位置姿勢推定部１２２は、キーフレーム情報テーブル１１２に登録されたキーフレームの中から、現画像に類似するキーフレームを近傍キーフレームとして特定する。例えば、位置姿勢推定部１２２は、現画像と、キーフレーム情報テーブル１１２に登録されたキーフレームのそれぞれを所定サイズに縮小し、さらにガウシアンフィルタでぼかす。そして、位置姿勢推定部１２２は、現画像とキーフレームそれぞれとの間で輝度値のＳＳＤ（Sum of Squared Distance）を計算し、ＳＳＤが最小のキーフレームを近傍キーフレームとして特定する。

次に、位置姿勢推定部１２２は、近傍キーフレームに含まれるマップ点（特徴点）の二次元座標を近傍キーフレームのキーフレーム情報から取得する。位置姿勢推定部１２２は、現画像から抽出された特徴点に対応する、近傍キーフレーム上のマップ点を特定し、特徴点とマップ点とのペアを複数組抽出する。なお、特徴点に対応するマップ点は、各点の特徴量の類似度に基づいて特定される。

次に、位置姿勢推定部１２２は、特徴点とマップ点との４組以上のペアを用いて、ＰｎＰアルゴリズムによって現画像の撮像時におけるカメラ１０８の位置姿勢を推定する。ＰｎＰアルゴリズムによる位置姿勢の推定方法としては、例えば、非特許文献「V. Lepetit et al., EPnP: An Accurate O(n) Solution to the PnP Problem, International Journal of Computer Vision, Vol.81, No.2, pp.155-166(2008)」に記載の方法を用いることができる。

なお、以下の説明では、後述する「相対位置姿勢」と区別するために、カメラ１０８の絶対的な位置姿勢を「絶対位置姿勢」と記載する場合がある。
品質判定部１２３は、位置姿勢推定部１２２によって推定された絶対位置姿勢の推定品質を判定する。例えば、位置姿勢推定部１２２によって現画像から抽出された特徴点の総数をＦ１、それらの特徴点のうち、位置姿勢推定部１２２の処理によって近傍キーフレーム上のマップ点と対応付けられた特徴点の数をＦ２とする。品質判定部１２３は、Ｆ２／Ｆ１が所定の閾値（例えば、０．３）以上の場合に推定品質が高いと判定し、閾値未満の場合に推定品質が低いと判定する。

ここで、推定品質が高いと判定された場合、位置姿勢の推定に成功したことになり、キーフレーム登録処理部１２４の処理が実行された後、画像取得部１２１によって次に取得された撮像画像を用いて位置姿勢推定部１２２の処理が実行される。一方、推定品質が低いと判定された場合、位置姿勢の推定に失敗したことになり、復帰処理部１２５による復帰処理が実行される。

キーフレーム登録処理部１２４は、キーフレーム情報の登録条件にしたがって、現画像をキーフレームとして登録するか否かを判定する。キーフレーム登録処理部１２４は、例えば、前回キーフレームとして登録された撮像画像から現画像までのフレーム数が所定値（例えば、２０フレーム）に達しているとき、現画像をキーフレームとして登録すると判定する。その場合、キーフレーム登録処理部１２４は、現画像についてのキーフレーム情報をキーフレーム情報テーブル１１２に登録する。

復帰処理部１２５は、位置姿勢の推定に失敗した状態から、絶対位置姿勢の推定を再開する復帰処理を実行する。ここで、まず、復帰処理の比較例について説明し、その後に本実施の形態での復帰処理について説明する。

＜復帰処理の比較例＞
まず、復帰処理の第１の比較例として、本実施の形態と同様の三次元マップとキーフレーム情報を用いた方法を例示する。この方法では、現画像内の特徴点とキーフレーム内のマップ点とが対応付けされ、対応付けられたキーフレーム中のマップ点についての三次元マップの座標情報に基づき、特徴点とマップ点との対応関係から絶対位置姿勢が推定される。

この方法では、現画像上の特徴点とキーフレーム上のマップ点とのすべての組み合わせについて、特徴点とマップ点の各特徴量が比較される。しかし、現画像とキーフレームとの間で局所的な比較が行われることから、撮像画像に類似画像パターンが多数現れる場合に、特徴点とマップ点との対応付けに誤りが発生しやすくなり、その結果、絶対位置姿勢の推定精度が低下するという問題がある。

図６は、撮像画像に類似画像パターンが現れる場合における特徴点とマップ点との対応付けの例を示す図である。図６において、キーフレーム２１０には、互いに類似する画像パターン２１１ａ〜２１１ｄが写っている。一方、現画像３１０にも、互いに類似する画像パターン３１１ａ〜３１１ｄが写っている。そして、画像パターン３１１ａ，３１１ｂ，３１１ｃ，３１１ｄは、それぞれキーフレーム２１０上の画像パターン２１１ａ，２１１ｂ，２１１ｃ，２１１ｄと同一の対象物領域を撮像して得られた領域である。

また、キーフレーム２１０には、マップ点２１２ａ〜２１２ｅが含まれているものとする。一方、現画像３１０からは、特徴点３１２ａ〜３１２ｅが抽出されたものとする。上記の第１の比較例を用いた処理では、特徴点３１２ａ〜３１２ｅのそれぞれがマップ点２１２ａ〜２１２ｅのうちのどれに対応するかが、特徴量のマッチングによって判定される。

図６の例では、特徴点３１２ａ，３１２ｂ，３１２ｃは、それぞれマップ点２１２ａ，２１２ｂ，２１２ｃに対応すると判定されたとする。これらの対応付けは正しく行われている。一方、特徴点３１２ｄ，３１２ｅは、それぞれマップ点２１２ｅ，２１２ｄに対応すると判定されたとする。これらの対応付けは間違っている。

このように、類似画像パターンが多数現れる状況では、各画像の中に類似する特徴量を有する特徴点およびマップ点がそれぞれ複数存在するので、局所的な特徴量のマッチングを用いるだけでは特徴点とマップ点との対応付けの誤りが発生しやすい。特徴点とマップ点との対応付けに誤りが発生すると、その対応付けに基づいて推定された絶対位置姿勢の推定精度が低下してしまう。

一方、復帰処理の第２の比較例としては、特徴点とマップ点とのマッチングを行わない次のような方法が考えられる。この方法では、現画像の縮小画像と記憶された各キーフレームの縮小画像との比較によって近傍キーフレームが特定され、現画像と近傍キーフレームとの間の相対的な位置姿勢（相対位置姿勢）が算出される。そして、相対位置姿勢に基づいて絶対位置姿勢が推定される。

この方法では、類似画像パターンが多数現れることによる相対位置姿勢の推定精度の低下は発生しにくい。しかし、縮小画像同士の比較が行われるため、相対位置姿勢の算出精度が低く、その結果、絶対位置姿勢の推定精度が低いという問題がある。

また、第１の比較例と第２の比較例とを組み合わせる方法も考えられる。例えば、第２の比較例で算出された相対位置姿勢の算出結果を用いて、第１の比較例での特徴点とマップ点との対応付け処理に制限を加える方法が考えられる。この方法では、相対位置姿勢の算出結果から明らかに間違いとわかるような特徴点とマップ点との対応付けが行われなくなり、対応付けの処理精度が改善される。しかし、相対位置姿勢の算出精度が低いことから、類似画像パターン同士が近い位置に存在する場合に、特徴点とマップ点との対応付けの誤りが発生しやすい。

また、他の方法として、現画像およびキーフレームに対してオプティカルフローによる特徴点追跡を適用して、第１の比較例における特徴点とマップ点との対応付けに制限を加える方法も考えられる。しかし、オプティカルフローを用いることで、平行移動以外のカメラ１０８の運動（例えば、回転運動）が起こった場合に位置姿勢の推定精度が低下するという問題がある。さらに、オプティカルフローと特徴量のマッチングの両方を実行することで、処理負荷が増大するという問題もある。

＜第２の実施の形態での復帰処理手順＞
本実施の形態では、復帰処理部１２５は、現画像と近傍キーフレームとの間の相対位置姿勢を算出する際に、局所的な特徴量の比較ではなく、特徴点の周囲領域とマップ点の周囲領域との間の画素値の比較を行う。これにより、類似画像パターンが多数現れる場合における相対位置姿勢の算出精度の低下を抑制し、その結果として絶対位置姿勢の推定精度を向上させる。

図７は、復帰処理部の内部構成例を示す図である。復帰処理部１２５は、仮相対位置姿勢算出部１２５ａ、詳細相対位置姿勢算出部１２５ｂおよび絶対位置姿勢算出部１２５ｃを有する。

仮相対位置姿勢算出部１２５ａは、現画像と近傍キーフレームを縮小して同じ所定サイズの縮小画像を作成する。なお、復帰処理部１２５の処理対象の画像を位置姿勢推定部１２２の処理対象の画像より必ず後に撮像された画像とする場合には、仮相対位置姿勢算出部１２５ａは、位置姿勢推定部１２２と同様の手順によってキーフレームの中から近傍キーフレームを特定する。

次に、仮相対位置姿勢算出部１２５ａは、現画像の縮小画像と近傍キーフレームの縮小画像との合わせ込みを行うことで、現画像と近傍キーフレームとの間の相対位置姿勢を算出する。相対位置姿勢は、現画像の撮像時と近傍キーフレームの撮像時のそれぞれにおけるカメラ１０８の相対的な位置姿勢を示す。現画像の撮像時における絶対位置姿勢ξ_nと、近傍キーフレームの撮像時における絶対位置姿勢ξ₁と、相対位置姿勢ξ_rとの関係は、次の式（１）で表される。
ξ_n＝ξ_r・ξ₁ ・・・（１）
仮相対位置姿勢算出部１２５ａによって算出される相対位置姿勢は、現画像と近傍キーフレームの各縮小画像から算出される大まかな算出値であり、詳細相対位置姿勢算出部１２５ｂでの処理の初期値として用いられる。そこで、ここでは、仮相対位置姿勢算出部１２５ａによって算出される相対位置姿勢を「仮相対位置姿勢」と呼び、ξ_r0で表す。

仮相対位置姿勢は、より詳細には、例えば次のような手順で算出される。仮相対位置姿勢算出部１２５ａは、現画像と近傍キーフレームを縮小して、それぞれ例えば４０画素×３０画素の縮小画像を作成する。さらに、仮相対位置姿勢算出部１２５ａは、各縮小画像をガウシアンフィルタによってぼかす。このようにして生成された各縮小画像をＳＢＩ（Small Blurred Image）と呼ぶ。

次に、仮相対位置姿勢算出部１２５ａは、現画像のＳＢＩに含まれる特徴点と、近傍キーフレームのＳＢＩに含まれるマップ点との対応関係に基づいて、各ＳＢＩの間のホモグラフィ行列Ｈを算出する。ホモグラフィ行列は、近傍キーフレームのＳＢＩ上の点の座標を現画像のＳＢＩ上の点の座標に変換するための３行３列の行列である。ホモグラフィ行列Ｈの算出方法としては、例えば、非特許文献「S. Benhimane and E. Malis, Homography-based Visual Tracking and Servoing, The International Journal of Robotics and Research, 26(7):661-676, 2007」に記載の方法を用いることができる。

なお、復帰処理部１２５の処理対象の画像が位置姿勢推定部１２２の処理対象と同じである場合、特徴点とマップ点との対応関係は位置姿勢推定部１２２の処理によって求められている。このため、仮相対位置姿勢算出部１２５ａは、求められている対応関係に基づいてホモグラフィ行列Ｈを算出することができる。

次に、仮相対位置姿勢算出部１２５ａは、近傍キーフレームのＳＢＩから２点の仮想点ｖｐ₁，ｖｐ₂ を選択する。そして、仮相対位置姿勢ξ_r0の回転成分を行列形式で表したものをＲとしたとき、仮相対位置姿勢算出部１２５ａは、次の式（２）を満たすようなＲを求める。

式（２）において、Ａは、カメラ１０８の内部パラメータであり、あらかじめキャリブレーションによって求められているものとする。式（２）によれば、ホモグラフィ行列Ｈを用いて仮想点を射影変換した点と、回転成分Ｒおよび並進成分０で仮想点を座標変換した点との距離が最小になるような回転成分Ｒが求められる。

仮相対位置姿勢算出部１２５ａは、以上の手順によって求められた回転成分Ｒに基づいて、仮相対位置姿勢ξ_r0を出力する。仮相対位置姿勢ξ_r0は、求められた回転成分Ｒの行列のうち、所定の３つの要素を回転成分とし、並進成分を０とした６次元の実数値として算出される。

詳細相対位置姿勢算出部１２５ｂは、算出された仮相対位置姿勢ξ_r0を反復計算の初期値として用いて、詳細相対位置姿勢ξ_r1を算出する。この反復計算の処理には、近傍キーフレーム上のマップ点の位置を相対位置姿勢に基づいて移動したときの、現画像における移動先を算出する処理と、特徴点と移動先のそれぞれに対応する注目領域を特定する処理とが含まれる。

図８は、詳細相対位置姿勢の算出処理について説明するための第１の図である。近傍キーフレームＩ₁上のｉ番目のマップ点をｐ_iとする。このとき、詳細相対位置姿勢算出部１２５ｂは、マップ点ｐ_iの位置を相対位置姿勢ξ_rに基づいて移動したときの、現画像Ｉ₂における移動先ｗ（ｐ_i）の座標ｗを算出する。移動先ｗ（ｐ_i）の座標ｗは、例えば、次のようにして算出される。

カメラ１０８の内部パラメータＡを、下記の式（３）のように表す。このとき、詳細相対位置姿勢算出部１２５ｂは、マップ点ｐ_i＝（ｕ_i，ｖ_i）の世界座標系における三次元座標（Ｘ_i，Ｙ_i，Ｚ_i）を、下記の式（４）にしたがって算出する。

また、詳細相対位置姿勢算出部１２５ｂは、相対位置姿勢ξ_rを、ロドリゲス変換により回転成分Ｒと並進成分ｔとに分割する。回転成分Ｒは３行３列の行列であり、並進成分ｔは３行１列の行列である。そして、詳細相対位置姿勢算出部１２５ｂは、移動先ｗ（ｐ_i）の座標ｗを次の式（５）にしたがって算出する。なお、式（５）において、（Ｒ｜ｔ）は、３行３列のＲと３行１列のｔとをまとめた３行４列の行列を指す。

図９は、詳細相対位置姿勢の算出処理について説明するための第２の図である。詳細相対位置姿勢算出部１２５ｂは、近傍キーフレームＩ₁上のマップ点ｐ_iのそれぞれに対応する注目領域を特定する。注目領域は、マップ点を中心とした縦横ｓ画素の矩形領域として特定される。図９の例では、マップ点ｐ₁，ｐ₂，ｐ₃にそれぞれ対応する注目領域２２１，２２２，２２３が特定されている。

また、詳細相対位置姿勢算出部１２５ｂは、現画像Ｉ₂上の移動先ｗ（ｐ_i）のそれぞれに対応する注目領域を特定する。現画像Ｉ₂上の注目領域も、移動先の点を中心とした縦横Ｎ画素の矩形領域として特定される。図９の例では、移動先ｗ（ｐ₁），ｗ（ｐ₂），ｗ（ｐ₃）にそれぞれ対応する注目領域３２１，３２２，３２３が特定されている。

詳細相対位置姿勢算出部１２５ｂは、近傍キーフレームＩ₁上の注目領域とこれに対応する現画像Ｉ₂上の注目領域とのすべての組み合わせについての、輝度差の二乗和Ｓを算出する。ここで、近傍キーフレームＩ₁上のｉ番目のマップ点に対応する注目領域に含まれるｊ番目の画素の輝度値をＩ_1(i,j)と表す。また、現画像Ｉ₂上のｉ番目の移動先に対応する注目領域に含まれるｊ番目の画素の輝度値をＩ_2(i,j)と表す。このとき、輝度差の二乗和Ｓは、次の式（６）にしたがって求められる。
Ｓ＝Σ_iΣ_j[Ｉ_2(i,j)−Ｉ_1(i,j)]² ・・・（６）
詳細相対位置姿勢算出部１２５ｂは、輝度差の二乗和Ｓが最小となるような相対位置姿勢ξ_rを、ガウス・ニュートン法を用いた反復計算によって算出し、その算出結果を詳細相対位置姿勢ξ_r1として出力する。以上の計算によれば、近傍キーフレームＩ₁と現画像Ｉ₂との間で、局所的な特徴点同士の特徴量ではなく、特徴点の周囲画素を含む注目領域同士の画素値が比較されることによって、詳細相対位置姿勢ξ_r1が算出される。これによって、類似する画像パターンが多数現れる場合でも、詳細相対位置姿勢ξ_r1の算出精度が低下しにくくなる。

なお、詳細相対位置姿勢ξ_r1のさらに詳細な計算方法については、図１１において説明する。
最後に、詳細相対位置姿勢算出部１２５ｂは、算出された詳細相対位置姿勢ξ_r1と、近傍キーフレームＩ₁についての絶対位置姿勢ξ₁とに基づいて、現画像Ｉ₂についての絶対位置姿勢ξ_nを推定する。絶対位置姿勢ξ_nは、前述の式（１）から算出することができる。上記のように、詳細相対位置姿勢ξ_r1の算出精度が低下しにくくなっていることから、現画像Ｉ₂についての絶対位置姿勢ξ_nの推定精度を向上させることができる。

＜フローチャート＞
次に、トラッキング処理部１２０の処理についてフローチャートを用いて説明する。
図１０、図１１は、トラッキング処理部の処理手順の例を示すフローチャートである。

［ステップＳ１１］画像取得部１２１は、カメラ１０８によって撮像された撮像画像をカメラ１０８から取得する。
［ステップＳ１２］位置姿勢推定部１２２は、現画像から特徴点を抽出する。

［ステップＳ１３］位置姿勢推定部１２２は、キーフレーム情報テーブル１１２に登録されたキーフレームの中から、現画像に類似するキーフレームを近傍キーフレームとして特定する。そして、位置姿勢推定部１２２は、近傍キーフレームに含まれるマップ点（特徴点）の二次元座標を近傍キーフレームのキーフレーム情報から取得する。位置姿勢推定部１２２は、現画像から抽出された特徴点に対応する、近傍キーフレーム上のマップ点を特定し、特徴点とマップ点とのペアを複数組抽出する。

［ステップＳ１４］位置姿勢推定部１２２は、特徴点とマップ点との４組以上のペアを用いて、ＰｎＰアルゴリズムによって現画像の撮像時におけるカメラ１０８の絶対位置姿勢を推定する。

［ステップＳ１５］品質判定部１２３は、ステップＳ１４で推定された絶対位置姿勢の推定品質を判定する。品質判定部１２３は、絶対位置姿勢の推定品質を示す指標値が所定の閾値以上である場合、推定品質が高いと判定して、ステップＳ１６の処理を実行する。一方、品質判定部１２３は、指標値が閾値未満の場合、推定品質が低いと判定して、図１１のステップＳ２１の処理を実行する。

［ステップＳ１６］キーフレーム登録処理部１２４は、キーフレーム情報の登録条件を満たすかを判定する。キーフレーム登録処理部１２４は、登録条件を満たす場合、現画像をキーフレームとして登録すると判定し、ステップＳ１７の処理を実行する。一方、キーフレーム登録処理部１２４は、登録条件を満たさない場合、処理を終了する。

［ステップＳ１７］キーフレーム登録処理部１２４は、現画像についてのキーフレーム情報をキーフレーム情報テーブル１１２に登録する。キーフレーム情報のうち、位置姿勢の項目には、ステップＳ１４で推定された絶対位置姿勢が登録される。特徴点群の項目には、ステップＳ１３で近傍キーフレーム上のマップ点と対応付けられた現画像上の特徴点の二次元座標が登録される。対応マップ点番号の項目には、現画像上の特徴点に対応するマップ点の識別番号が登録される。

以下、図１１を用いて説明を続ける。
［ステップＳ２１］仮相対位置姿勢算出部１２５ａは、現画像の縮小画像と近傍キーフレームの縮小画像との合わせ込みを行うことで、現画像と近傍キーフレームとの間の仮相対位置姿勢ξ_r0を算出する。なお、近傍キーフレームは、図１０のステップＳ１３で特定されたキーフレームである。仮相対位置姿勢算出部１２５ａは、算出された仮相対位置姿勢ξ_r0を、この後の反復計算（ステップＳ２２〜Ｓ２５）で用いる相対位置姿勢ξ_rの初期値として設定する。

［ステップＳ２２］詳細相対位置姿勢算出部１２５ｂは、近傍キーフレーム上のマップ点ｐ_iの位置を相対位置姿勢ξ_rに基づいて移動したときの、現画像における移動先ｗ（ｐ_i）の座標ｗを算出する。

［ステップＳ２３］詳細相対位置姿勢算出部１２５ｂは、近傍キーフレーム上のマップ点ｐ_iのそれぞれに対応する注目領域を特定する。また、詳細相対位置姿勢算出部１２５ｂは、現画像上の移動先ｗ（ｐ_i）のそれぞれに対応する注目領域を特定する。

［ステップＳ２４］詳細相対位置姿勢算出部１２５ｂは、前述の式（６）にしたがって、注目領域間の輝度差の二乗和Ｓを算出する。
［ステップＳ２５］詳細相対位置姿勢算出部１２５ｂは、反復計算の結果、輝度差の二乗和Ｓが収束したかを判定する。この処理は、具体的には次のようにして実行される。

前述のように、近傍キーフレームＩ₁上のｉ番目のマップ点に対応する注目領域に含まれるｊ番目の画素の輝度値をＩ_1(i,j)と表す。また、現画像Ｉ₂上のｉ番目の移動先に対応する注目領域に含まれるｊ番目の画素の輝度値をＩ_2(i,j)と表す。詳細相対位置姿勢算出部１２５ｂは、コスト関数Ｅ（ξ_r）を式（７）のように設定する。また、式（７）中のｅ（ξ_r）は、式（８）のように表される。

詳細相対位置姿勢算出部１２５ｂは、相対位置姿勢がξ_rのときの式（９）および式（１０）の計算を実行する。
Ａ＝Ｊ^TＪ ・・・（９）
ａ＝−Ｊ^Tｅ ・・・（１０）
ここで、Ｊは、ｅ（ξ_r）のヤコビ行列であり、下記の式（１１）のように表される。また、式（１１）中のｅは、式（１２）のように表される。なお、式（８）のＡは、ヘッセ行列をヤコビ行列によって二次近似したものである。

コスト関数Ｅ（ξ_r）を最小化するδξ_rは、式（９），（１０）を用いて式（１３）のように表される。
Ａ・δξ_r＝ａ ・・・（１３）
詳細相対位置姿勢算出部１２５ｂは、式（１３）を解いてδξ_rを計算する。そして、詳細相対位置姿勢算出部１２５ｂは、式（１４）にしたがってδＥを計算する。
δＥ＝Ｅ（ξ_r＋δξ_r）−Ｅ（ξ_r） ・・・（１４）
詳細相対位置姿勢算出部１２５ｂは、ステップＳ２２〜Ｓ２４での変化量｜δＥ｜／Ｅを、収束を判定するための指標値として用いる。あるいは、指標値としては、変化量｜δξ_r｜／ξ_rが用いられてもよい。詳細相対位置姿勢算出部１２５ｂは、指標値が十分小さい場合、輝度差の二乗和Ｓが収束したと判定する。この場合、詳細相対位置姿勢算出部１２５ｂは、このときのξ_rを詳細相対位置姿勢ξ_r1として出力し、ステップＳ２７の処理を実行する。

一方、詳細相対位置姿勢算出部１２５ｂは、指標値が十分小さいとは言えない場合、ステップＳ２６の処理を実行する。
［ステップＳ２６］詳細相対位置姿勢算出部１２５ｂは、ξ_rをξ_r＋δξ_rによって更新する。この後、ステップＳ２２に戻り、更新されたξ_rを用いた処理が実行される。

［ステップＳ２７］詳細相対位置姿勢算出部１２５ｂは、算出された詳細相対位置姿勢ξ_r1と、キーフレーム情報テーブル１１２から取得される、近傍キーフレームについての絶対位置姿勢ξ₁とに基づき、現画像についての絶対位置姿勢ξ_nを式（１）にしたがって推定する。

以上説明した端末装置１００によれば、近傍キーフレームと現画像との間で、局所的な特徴点同士の特徴量ではなく、特徴点の周囲を含む注目領域同士の画素値が比較されることによって、詳細相対位置姿勢が算出され、これに基づいて絶対位置姿勢が推定される。これによって、前述の第１の比較例と比較して、類似する画像パターンが多数現れる場合でも、詳細相対位置姿勢ξ_r1の算出精度が低下しにくくなる。その結果、絶対位置姿勢の推定精度を向上させることができる。したがって、復帰処理を高精度に行うことができる。

また、端末装置１００によれば、近傍キーフレームと現画像の各縮小画像を用いて大まかな仮相対位置姿勢を算出した後、上記のような注目領域同士の画素値の比較によって詳細相対位置姿勢を算出する。そして、詳細相対位置姿勢に基づいて絶対位置姿勢を推定する。このため、前述の第２の比較例と比較して、相対位置姿勢の算出精度を向上させることができ、その結果、絶対位置姿勢の推定精度も向上させることができる。

さらに、端末装置１００によれば、平行移動以外のカメラ１０８の運動時に位置姿勢の推定精度が低下しやすいオプティカルフローによる特徴点追跡も、実行しなくて済む。また、オプティカルフローと特徴量のマッチングの両方を実行することもないので、処理負荷を抑制できる。

次に、第２の実施の形態に係る端末装置１００の処理の一部を変形した変形例について説明する。なお、以下の第１の変形例および第２の変形例では、端末装置の処理機能の基本的な構成は同じなので、端末装置の処理機能の構成について第２の実施の形態と同じ符号を用いて説明する。

〔第１の変形例〕
図１２は、第１の変形例での注目領域の特定方法を説明するための図である。第２の実施の形態では、図９に示したように、近傍キーフレーム上のマップ点および現画像上の特徴点を中心とした縦横Ｎ画素の領域が、注目領域として設定された。これに対して、第１の変形例では、近傍キーフレーム上のマップ点の中から、エッジ２５１上に存在するマップ点２５２ａ，２５２ｂのペアが抽出される。そして、注目領域２５３は、Ｘ軸とＹ軸のうち、一方の方向（図１２ではＸ軸方向）に対する範囲が、マップ点２５２ａ，２５２ｂのペアを包含する範囲となるように設定される。また、他方の方向（図１２ではＹ軸方向）に対する範囲が、マップ点２５２ａ，２５２ｂのペアを結ぶ線（図１２ではエッジ２５１）を中心として、その方向に沿って両側にＭ画素を含むように設定される。第１の変形例では、上記のような注目領域がキーフレームから複数特定される。

また、現画像においては、仮相対位置姿勢に基づく、上記のようなマップ点のペアの移動先が特定される。そして、移動先のペアを結ぶ直線を利用して、近傍キーフレームと同じルールで注目領域が特定される。なお、近傍キーフレーム上のマップ点のペアの位置関係は、現画像上の移動先のペアの位置関係と同じになるとは限らない。このため、キーフレームと現画像との間で対応する注目領域は、互いに同じ大きさや同じ形状になるとは限らない。

上記のようにエッジの検出結果に基づいて注目領域が特定されることで、第２の実施の形態と比較して、対象物に直線的な模様が多い環境における詳細相対位置姿勢の推定精度を向上させることができる。

なお、本変形例では、図１２に示すようなエッジの検出結果に基づく注目領域とともに、第２の実施の形態で用いられた、マップ点および特徴点を中心とする矩形の注目領域も併用される。特徴点は主にコーナー部で抽出されるので、第２の実施の形態では、コーナー部近傍の形状が類似する画像パターンが多い場合に、詳細相対位置姿勢の推定精度向上効果が高い。したがって、第２の実施の形態で用いられた注目領域も併用することで、様々な環境に対して位置姿勢の推定精度を向上させることができる。

図１３は、第１の変形例における復帰処理手順の例を示すフローチャートである。第１の変形例では、図１１に示した処理が図１３に示すように変形される。なお、図１３では、図１１と同じ処理が実行される処理ステップには同じ符号を付して示し、その説明を省略する。

図１３の処理では、図１１のステップＳ２３の次に、ステップＳ２３ａ，Ｓ２３ｂが実行される。その次に、ステップＳ２４，Ｓ２５の代わりにステップＳ２４ａ，Ｓ２５ａが実行される。

［ステップＳ２３ａ］詳細相対位置姿勢算出部１２５ｂは、近傍キーフレームからエッジを検出する。
［ステップＳ２３ｂ］詳細相対位置姿勢算出部１２５ｂは、近傍キーフレーム内のマップ点の中から、同一のエッジ上に存在するマップ点のペアを特定する。そして、詳細相対位置姿勢算出部１２５ｂは、近傍キーフレーム上に、特定されたマップ点のペアのそれぞれに対応する注目領域を特定する。さらに、詳細相対位置姿勢算出部１２５ｂは、特定されたマップ点のペアに対応する現画像上の移動先のペアを特定し、現画像上に、移動先のペアのそれぞれに対応する注目領域を特定する。

［ステップＳ２４ａ］詳細相対位置姿勢算出部１２５ｂは、近傍キーフレーム上の注目領域と現画像上の注目領域との間の輝度差の二乗和Ｓを算出する。
ここで、ステップＳ２３で特定された注目領域を「第１注目領域」と呼び、ステップＳ２３ｂで特定された注目領域を「第２注目領域」と呼ぶ。また、近傍キーフレームＩ₁上のｉ番目のマップ点に対応する第１注目領域に含まれるｊ番目の画素の輝度値をＩ_1(i,j)と表し、現画像Ｉ₂上のｉ番目の移動先に対応する第１注目領域に含まれるｊ番目の画素の輝度値をＩ_2(i,j)と表す。さらに、近傍キーフレームＩ₁上のｍ番目のマップ点ペアに対応する第２注目領域に含まれるｎ番目の画素の輝度値をＩ_1(m,n)と表し、現画像Ｉ₂上のｍ番目の移動先ペアに対応する第２注目領域に含まれるｎ番目の画素の輝度値をＩ_2(m,n)と表す。このとき、詳細相対位置姿勢算出部１２５ｂは、輝度差の二乗和Ｓを次の式（１５）にしたがって算出する。
Ｓ＝Σ_iΣ_j[Ｉ_2(i,j)−Ｉ_1(i,j)]²＋Σ_mΣ_n[Ｉ_2(m,n)−Ｉ_1(m,n)]² ・・・（１５）
［ステップＳ２５ａ］詳細相対位置姿勢算出部１２５ｂは、ステップＳ２４ａで算出された輝度差の二乗和Ｓを適用して、図１１のステップＳ２５と同様の手順により収束したか否かを判定する。

〔第２の変形例〕
図１４は、画像ピラミッドの構成例を示す図である。図１４に示す画像ピラミッドは、元の画像に対応する階層Ｌ１と、元の画像を縮小した縮小画像の階層Ｌ２〜Ｌ４とを有する。階層Ｌ２の画像は、階層Ｌ１の画像の１／２のサイズを有し、階層Ｌ３の画像は、階層Ｌ２の画像の１／２のサイズを有し、階層Ｌ４の画像は、階層Ｌ３の画像の１／２のサイズを有する。なお、ここでは、階層Ｌ４を最上位階層とし、階層Ｌ１を最下位階層とする。

第２の変形例では、近傍キーフレームおよび現画像のそれぞれについて、図１４のような階層Ｌ１〜Ｌ４の画像が用いられる。例えば、近傍キーフレームに対応する階層Ｌ１〜Ｌ４の画像は、対応するキーフレーム情報に対して予め登録されていればよい。一方、現画像に対応する階層Ｌ１〜Ｌ４の画像は、復帰処理時に生成される。

第２の変形例において、詳細相対位置姿勢算出部１２５ｂは、近傍キーフレームおよび現画像に対応する階層Ｌ４から階層Ｌ１までの画像を順次利用しながら、詳細相対位置姿勢の算出のための反復処理を実行する。その処理の詳細については、図１５を用いて説明する。

図１５は、第２の変形例における復帰処理手順の例を示すフローチャートである。第２の変形例では、図１１に示した処理が図１５に示すように変形される。なお、図１５では、図１１と同じ処理が実行される処理ステップには同じ符号を付して示し、その説明を省略する。

図１５の処理では、図１１のステップＳ２１の次に、ステップＳ２１ａが実行され、その後にステップＳ２２が実行される。また、ステップＳ２５ａ，Ｓ２５ｂが追加される。
［ステップ２１ａ］詳細相対位置姿勢算出部１２５ｂは、ステップＳ２２以降の処理に用いる近傍キーフレームおよび現画像として、最上位階層（階層Ｌ４）の画像を選択する。

［ステップＳ２５ａ］詳細相対位置姿勢算出部１２５ｂは、ステップＳ２５で「Ｙｅｓ」と判定された場合、ステップＳ２５ａの処理を実行する。詳細相対位置姿勢算出部１２５ｂは、画像ピラミッドの全階層を選択済みかを判定する。詳細相対位置姿勢算出部１２５ｂは、未選択の階層がある場合、ステップＳ２５ｂの処理を実行し、全階層を選択済みの場合、ステップＳ２７の処理を実行する。

［ステップＳ２５ｂ］詳細相対位置姿勢算出部１２５ｂは、次回のステップＳ２２以降の処理に用いる近傍キーフレームおよび現画像として、１つ下位の階層の画像を選択する。この後、詳細相対位置姿勢算出部１２５ｂは、ステップＳ２６の処理を実行する。

以上の第２の変形例によれば、詳細相対位置姿勢の算出過程において収束を早めることができる。例えば、画像ピラミッドを利用しない場合、カメラ１０８の動きが速くなると、収束に失敗したり、誤った数値に収束する可能性が高まる。これに対して、画像ピラミッドの上位層側から画像を順次利用することで、相対位置姿勢の収束範囲を階層ごとに徐々に狭めながら計算を進めていくことができる。これにより、常に適切な値に収束させながら推定処理を進めていくことができ、正しい値に収束しやすくなる。また、正しい値に収束しやすくなることで、収束するまでの時間を短縮できる。

なお、上記の第２の変形例では、注目領域として、第２の実施の形態で用いられた、マップ点および特徴点を中心とする注目領域のみ利用した例を示した。しかし、第２の変形例のように画像ピラミッドを利用する場合でも、例えば、エッジの検出結果に基づく注目領域を用いることができるし、あるいは、マップ点および特徴点を中心とする注目領域と、エッジの検出結果に基づく注目領域とを併用することもできる。

また、上記の各実施の形態に示した装置（カメラ位置姿勢推定装置１、端末装置１００）の処理機能は、コンピュータによって実現することができる。その場合、各装置が有すべき機能の処理内容を記述したプログラムが提供され、そのプログラムをコンピュータで実行することにより、上記処理機能がコンピュータ上で実現される。処理内容を記述したプログラムは、コンピュータで読み取り可能な記録媒体に記録しておくことができる。コンピュータで読み取り可能な記録媒体としては、磁気記憶装置、光ディスク、光磁気記録媒体、半導体メモリなどがある。磁気記憶装置には、ハードディスク装置（ＨＤＤ）、フレキシブルディスク（ＦＤ）、磁気テープなどがある。光ディスクには、ＤＶＤ（Digital Versatile Disc）、ＤＶＤ−ＲＡＭ、ＣＤ−ＲＯＭ（Compact Disc-Read Only Memory）、ＣＤ−Ｒ（Recordable）／ＲＷ（ReWritable）などがある。光磁気記録媒体には、ＭＯ（Magneto-Optical disk）などがある。

プログラムを流通させる場合には、例えば、そのプログラムが記録されたＤＶＤ、ＣＤ−ＲＯＭなどの可搬型記録媒体が販売される。また、プログラムをサーバコンピュータの記憶装置に格納しておき、ネットワークを介して、サーバコンピュータから他のコンピュータにそのプログラムを転送することもできる。

プログラムを実行するコンピュータは、例えば、可搬型記録媒体に記録されたプログラムまたはサーバコンピュータから転送されたプログラムを、自己の記憶装置に格納する。そして、コンピュータは、自己の記憶装置からプログラムを読み取り、プログラムに従った処理を実行する。なお、コンピュータは、可搬型記録媒体から直接プログラムを読み取り、そのプログラムにしたがった処理を実行することもできる。また、コンピュータは、ネットワークを介して接続されたサーバコンピュータからプログラムが転送されるごとに、逐次、受け取ったプログラムにしたがった処理を実行することもできる。

１ カメラ位置姿勢推定装置
１ａ 記憶部
１ｂ 演算部
２ａ，２ｂ キー画像
３ａ，３ｂ 姿勢値
４ 現画像
５ａ，５ｂ，６ａ，６ｂ 領域

Claims (8)

1. 複数の第１の画像と、前記複数の第１の画像それぞれの撮像時におけるカメラの位置姿勢を示す複数の第１の姿勢値とを記憶する記憶部と、演算部とを有し、
   前記演算部は、
   前記複数の第１の画像の中から第２の画像に類似する第３の画像を特定し、
   前記第３の画像から複数画素を含む第１の領域を特定するとともに、前記第２の画像から前記第１の領域に対応する第２の領域を推定し、
   前記第１の領域と前記第２の領域との間の画素値の比較結果と、前記複数の第１の姿勢値のうち前記第３の画像に対応する第２の姿勢値とに基づいて、前記第２の画像の撮像時における前記カメラの位置姿勢を示す第３の姿勢値を推定する、
   カメラ位置姿勢推定装置。
2. 前記第２の領域の推定では、前記第２の姿勢値が示す位置姿勢に対する、前記第２の画像の撮像時における前記カメラの相対的な位置姿勢を示す相対姿勢値に基づいて前記第２の領域を推定したときに、前記第１の領域と前記第２の領域との間の画素値の差分が最小になるような前記相対姿勢値を算出し、
   前記第３の姿勢値の推定では、前記相対姿勢値と前記第２の姿勢値とに基づいて前記第３の姿勢値を推定する、
   請求項１記載のカメラ位置姿勢推定装置。
3. 前記第２の領域の推定は、
   前記第３の画像を縮小した第１の縮小画像と前記第２の画像を縮小した第２の縮小画像とに基づいて、前記第２の姿勢値が示す位置姿勢に対する、前記第２の画像の撮像時における前記カメラの相対的な位置姿勢を示す第１の相対姿勢値を算出し、
   前記第１の相対姿勢値を第２の相対姿勢値の初期値として設定し、
   前記第２の相対姿勢値に基づいて前記第２の領域を推定したときに前記第１の領域と前記第２の領域との間の画素値の差分が最小になるような前記第２の相対姿勢値を、反復計算によって算出する、
   処理を含み、
   前記第３の姿勢値の推定では、前記反復計算によって算出された前記第２の相対姿勢値と、前記第２の姿勢値とに基づいて、前記第３の姿勢値を推定する、
   請求項２記載のカメラ位置姿勢推定装置。
4. 前記第２の領域の推定では、前記第３の画像と前記第２の画像とそれぞれを変換して得られるサイズの異なる複数の画像ペアを、サイズの小さい順に用いて前記反復計算を繰り返し実行し、
   前記複数の画像ペアのうち、第１の画像ペアを用いた前記反復計算によって算出された前記第２の相対姿勢値が、前記第１の画像ペアの次にサイズが大きい第２の画像ペアを用いた前記反復計算における前記第２の相対姿勢値の初期値として設定される、
   請求項３記載のカメラ位置姿勢推定装置。
5. 前記第３の画像は、複数の特徴点を含み、
   前記第１の領域は、前記複数の特徴点のうち、前記第３の画像上のエッジで結ばれた特徴点ペアを含む３以上の画素を含む領域として特定される、
   請求項１乃至４のいずれか１項に記載のカメラ位置姿勢推定装置。
6. 前記記憶部は、複数の第１の特徴点それぞれについての三次元空間上の座標を示す複数の第１のマップ点座標を記憶し、
   前記演算部は、さらに、前記複数の第１のマップ点座標のうち、第４の画像から抽出された第２の特徴点についての第２のマップ点座標に基づいて、前記第４の画像の撮像時における前記カメラの位置姿勢を示す第４の姿勢値を推定し、
   前記第１の領域の特定および前記第２の領域の推定は、前記第４の姿勢値の推定に失敗した場合に実行され、
   前記第２の画像は、前記第４の姿勢値の推定に失敗した後に前記カメラによって撮像された画像である、
   請求項１乃至５のいずれか１項に記載のカメラ位置姿勢推定装置。
7. コンピュータが、
   複数の第１の画像と、前記複数の第１の画像それぞれの撮像時におけるカメラの位置姿勢を示す複数の第１の姿勢値とを記憶する記憶部を参照し、
   前記複数の第１の画像の中から第２の画像に類似する第３の画像を特定し、
   前記第３の画像から複数画素を含む第１の領域を特定するとともに、前記第２の画像から前記第１の領域に対応する第２の領域を推定し、
   前記第１の領域と前記第２の領域との間の画素値の比較結果と、前記複数の第１の姿勢値のうち前記第３の画像に対応する第２の姿勢値とに基づいて、前記第２の画像の撮像時における前記カメラの位置姿勢を示す第３の姿勢値を推定する、
   カメラ位置姿勢推定方法。
8. コンピュータに、
   複数の第１の画像と、前記複数の第１の画像それぞれの撮像時におけるカメラの位置姿勢を示す複数の第１の姿勢値とを記憶する記憶部を参照し、
   前記複数の第１の画像の中から第２の画像に類似する第３の画像を特定し、
   前記第３の画像から複数画素を含む第１の領域を特定するとともに、前記第２の画像から前記第１の領域に対応する第２の領域を推定し、
   前記第１の領域と前記第２の領域との間の画素値の比較結果と、前記複数の第１の姿勢値のうち前記第３の画像に対応する第２の姿勢値とに基づいて、前記第２の画像の撮像時における前記カメラの位置姿勢を示す第３の姿勢値を推定する、
   処理を実行させるカメラ位置姿勢推定プログラム。

Images