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

Description

本発明は、複数の画像からそれらを撮影したカメラの相対的な位置姿勢を推定する技術に関する。

カメラで撮影した複数の画像から、被写体の３次元形状復元や背景のパノラマ画像を生成するには、画像を撮影したカメラ位置姿勢の高精度な推定が必要である。

カメラ位置姿勢を推定するには最低２枚の画像が必要である。３枚以上の画像がある場合は、２枚ずつ組みにすることによって姿勢の推定に対応可能である。以下では２枚の画像間の相対的な位置姿勢を推定する方法について述べる。相対的な位置姿勢を推定する理由は、入力が画像のみの場合、２画像間の位置姿勢の絶対的な変化量は推定不可能だからである。相対的な位置姿勢を推定するためには、２枚の画像の内どちらかを３次元空間の原点と仮定する。原点や距離の指標が既知な場合は、推定した相対的な位置姿勢に適切な座標変換を適用すればよい。以下では１枚目の画像を座標原点とし、２枚目の画像の姿勢を自由度３で表される３次元の回転行列、スケール不定の位置を自由度２で表される３次元ベクトルとする。

焦点距離、光学中心座標、レンズ歪みなどの内部パラメータが既知の場合にカメラ位置姿勢を推定する１つの方法として、エピポーラ方程式を用いる方法がある。エピポーラ方程式は、２枚の画像上で同一の３次元座標を表す画像座標（以下、対応点と呼ぶ）とカメラ位置姿勢の幾何学的関係を表す方程式である。

例えば、非特許文献１には、２枚の画像上の８組以上の対応点を用いてエピポーラ方程式を解き、カメラ位置姿勢を推定する方法が記載されている。非特許文献１に記載されている方法では、まず、対応点をカメラ位置姿勢により表される３次元のＥ行列（Ｅｓｓｅｎｔｉａｌ Ｍａｔｒｉｘ）と呼ばれるパラメータに変換する。次に、Ｅ行列の拘束条件を無視して自由度８の独立な変数とすることで、８組以上の対応点が満たす線形なエピポーラ方程式を解き、Ｅ行列を計算する。拘束条件とは、Ｅ行列を特異値分解して得られる３つの特異値の内、１つがゼロで、２つが等しいことである。最後に、拘束条件を満たすように事後にＥ行列を補正し、補正されたＥ行列を分解して位置と姿勢をそれぞれ推定する。補正とは、推定されたＥ行列を特異値分解し、特異値を前述のように変更することである。

非特許文献１に記載されている方法によるカメラ位置姿勢の推定精度は対応点組の抽出精度に依存するため、カメラの動きが微小な場合やカメラの光軸方向と移動方向が近い場合など、対応点組の画像座標の差分が少ないときは、精度が大きく低下することが知られている。

そのような場合にも高精度にカメラ位置姿勢を推定する方法として、対応点組とカメラ以外のセンサ情報を併用する方法がある。

例えば、非特許文献２には、加速度センサや１つの消失点より姿勢を表す自由度３のうち２つの自由度を既知（以下、既知姿勢と呼ぶ）として、３組の対応点が満たすエピポーラ方程式を解いて位置と姿勢を直接推定する方法が記載されている。推定すべきパラメータが、位置を表す２自由度と残りの姿勢を表す１自由度（以下、未知姿勢と呼ぶ）の合計３自由度に削減されるため、非特許文献１よりも高精度である。

しかし、非特許文献２に記載された技術では、パラメータ間の拘束条件を考慮したエピポーラ方程式は複雑な非線形連立方程式になるため、対応点の入力が３組に限られ、４組以上の最小自乗解を求めることができない。

最小自乗解を求める方法として、非特許文献３には、対応点が３組、４組、５組以上に場合分けして位置姿勢を推定する方法が記載されている。どの場合も、非特許文献１と同様に、線形なエピポーラ方程式からＥ行列を求め、拘束条件を満たすようにＥ行列を補正し、補正されたＥ行列を分解して位置と姿勢を推定する。

上記の各方法を用いてカメラ位置姿勢を推定することで、対応点の３次元形状復元や背景のパノラマ画像が生成できることが知られている。カメラの相対的な位置と姿勢を元に対応点の３次元座標を復元する方法は、例えば、非特許文献４や非特許文献５に記載されている。

R.Hartley and A.Zisserman、 "Multiple View Geometry in Computer Vision Second Edition、" Cambridge University Press、2004、p257-259、p294. M. Kalantari、 A. Hashemi、 F. Jung and J-P. Guedon、 "A new solution to the relative orientation problem using only 3 points and the vertical direction、" Journal of Mathematical Imaging and Vision、 2011、 vol.39、 Issue.3、 pp.259-268. F. Fraundorfer、 P. Tanskanen and M. Pollefeys、 "A minimal case solution to the calibrated relative pose problem for the case of two known orientation angles、" Proceedings of the 11th European conference on Computer vision: Part IV (ECCV’10)、 Heraklion、 Greece、 2010、 pp.269-282. 徐剛、「写真から作る３次元ＣＧ」、近代科学社、2001年、p69-73. 佐藤淳、「コンピュータビジョン 視覚の幾何学」、コロナ社、1999年、p122-123.

非特許文献２に記載されているように、姿勢のうち２つが既知であれば、最少３組の対応点からカメラ位置姿勢を推定できる。しかし、対応点には量子化やカメラ内部のセンサノイズによる観測誤差が生じるため、３組のみでは高精度にカメラ位置姿勢を推定できない。また、３組では一般に複数のカメラ位置姿勢が推定され、どのカメラ位置姿勢を選択すべきかを数学的に区別することができない。唯一のカメラ位置姿勢を高精度に推定するためには、４組以上の対応点を用いる必要がある。しかし、非特許文献２に記載されている位置姿勢推定方法は、対応点が３組のみの場合であり、４組以上については適用できない。

非特許文献３に記載されている位置姿勢推定方法は、位置と姿勢とを同時に算出するため、補正手段が必要となり計算量が大きい。具体的には補正するための特異値分解などの行列操作を行う必要がある。

そこで、本発明は、２枚の画像を撮影したカメラの相対的な位置姿勢を、高精度かつ高速に推定できる位置姿勢推定装置、位置姿勢推定方法および位置姿勢推定プログラムを提供することを目的とする。

本発明による位置姿勢推定装置は、２枚の画像に含まれる３組以上の対応点と、当該２枚の画像を撮影したカメラの相対的な姿勢を表す３つの姿勢パラメータのうち既知の２つの姿勢パラメータである既知姿勢とを入力し、入力された前記対応点と前記既知姿勢とを用いて、前記２つの既知姿勢を共通の固定値として含む連立多項式であり、かつ、前記姿勢パラメータのうちの１つであって未知の姿勢パラメータである未知姿勢と前記対応点と前記既知姿勢とから構成され、前記未知姿勢を変数とする行列であって、固有値ゼロに対応する固有ベクトルが前記カメラの相対的な位置を表す第１行列または最小固有値に対応する固有ベクトルが前記カメラの相対的な位置を表す第２行列について、前記第１行列の行列式をゼロにするまたは前記第２行列の行列式が極値をとる所定の関数を満たすすべての前記未知姿勢の解を未知姿勢の候補として出力する未知姿勢候補計算部と、前記未知姿勢の候補それぞれに対する前記カメラの相対的な位置の候補を計算する位置候補計算部と、すべての前記未知姿勢の候補、前記未知姿勢の候補それぞれに対する前記カメラの相対的な位置の候補、および前記対応点を入力し、前記未知姿勢の候補および前記位置の候補の中から、前記対応点と前記カメラの相対的な位置と前記未知姿勢との幾何関係を表す所定の誤差関数が最小となる１つまたは複数の前記未知姿勢、および前記カメラの相対的な位置を抽出する誤差最小候補抽出部とを備えたことを特徴とする。

本発明による位置姿勢推定方法は、２枚の画像に含まれる３組以上の対応点と、当該２枚の画像を撮影したカメラの相対的な姿勢を表す３つの姿勢パラメータのうち既知の２つの姿勢パラメータである既知姿勢とを入力し、入力された前記対応点と前記既知姿勢とを用いて、前記２つの既知姿勢を共通の固定値として含む連立多項式であり、かつ、前記姿勢パラメータのうちの１つであって未知の姿勢パラメータである未知姿勢と前記対応点と前記既知姿勢とから構成され、前記未知姿勢を変数とする行列であって、固有値ゼロに対応する固有ベクトルが前記カメラの相対的な位置を表す第１行列または最小固有値に対応する固有ベクトルが前記カメラの相対的な位置を表す第２行列について、前記第１行列の行列式をゼロにするまたは前記第２行列の行列式が極値をとる所定の関数を満たすすべての前記未知姿勢の解を未知姿勢の候補として出力し、前記未知姿勢の候補それぞれに対する前記カメラの相対的な位置の候補を計算し、すべての前記未知姿勢の候補、前記未知姿勢の候補それぞれに対する前記カメラの相対的な位置の候補、および前記対応点を入力し、前記未知姿勢の候補および前記位置の候補の中から、前記対応点と前記カメラの相対的な位置と前記未知姿勢との幾何関係を表す所定の誤差関数が最小となる１つまたは複数の前記未知姿勢、および前記カメラの相対的な位置を抽出することを特徴とする。

本発明による位置姿勢推定プログラムは、コンピュータに、２枚の画像に含まれる３組以上の対応点と、当該２枚の画像を撮影したカメラの相対的な姿勢を表す３つの姿勢パラメータのうち既知の２つの姿勢パラメータである既知姿勢とを入力し、入力された前記対応点と前記既知姿勢とを用いて、前記２つの既知姿勢を共通の固定値として含む連立多項式であり、かつ、前記姿勢パラメータのうちの１つであって未知の姿勢パラメータである未知姿勢と前記対応点と前記既知姿勢とから構成され、前記未知姿勢を変数とする行列であって、固有値ゼロに対応する固有ベクトルが前記カメラの相対的な位置を表す第１行列または最小固有値に対応する固有ベクトルが前記カメラの相対的な位置を表す第２行列について、前記第１行列の行列式をゼロにするまたは前記第２行列の行列式が極値をとる所定の関数を満たすすべての前記未知姿勢の解を未知姿勢の候補として出力する未知姿勢候補計算処理と、前記未知姿勢の候補それぞれに対する前記カメラの相対的な位置の候補を計算する位置候補計算処理と、すべての前記未知姿勢の候補、前記未知姿勢の候補それぞれに対する前記カメラの相対的な位置の候補、および前記対応点を入力し、前記未知姿勢の候補および前記位置の候補の中から、前記対応点と前記カメラの相対的な位置と前記未知姿勢との幾何関係を表す所定の誤差関数が最小となる１つまたは複数の前記未知姿勢、および前記カメラの相対的な位置を抽出する誤差最小候補抽出処理とを実行させることを特徴とする。

本発明によれば、２枚の画像を撮影したカメラの相対的な位置姿勢を、高精度かつ高速に推定できる。

本発明による位置姿勢推定装置の第１の実施形態の構成例を示すブロック図である。 本発明による位置姿勢推定装置の第１の実施形態の動作の一例を示すフローチャートである。 本発明による位置姿勢推定装置の第２の実施形態の構成例を示すブロック図である。 本発明による位置姿勢推定装置の第２の実施形態の動作の一例を示すフローチャートである。 本発明による位置姿勢推定装置の第３の実施形態の構成例を示すブロック図である。 本発明による位置姿勢推定装置の第３の実施形態の動作の一例を示すフローチャートである。 本発明による位置姿勢推定装置を情報処理システムに実装した場合のブロック図である。 本発明による位置姿勢推定装置の主要部の構成を示すブロック図である。

次に、本発明の実施の形態について図面を参照して詳細に説明する。

実施形態１．
図１は、本発明による位置姿勢推定装置の第１の実施形態（実施形態１）の構成例を示すブロック図である。図１に示す位置姿勢推定装置は、未知姿勢候補計算部１と、位置候補計算部２と、誤差最小候補抽出部３とを備える。未知姿勢候補計算部１は、係数計算部１１と、連立多項式求解部１２と、実数解抽出部１３と、未知姿勢候補変換部１４とを含む。

未知姿勢候補計算部１は、２枚の画像の３組以上の対応点と、２枚の画像を撮影したカメラの相対的な姿勢（以下、「姿勢」と記載することがある）を表す３つの姿勢パラメータのうち既知である２つの姿勢パラメータを既知姿勢として入力する。そして、未知姿勢候補計算部１は、３つの姿勢パラメータのうちの１つであって未知の姿勢パラメータである未知姿勢とカメラの相対的位置（以下、「位置」と記載することがある）とを変数として、対応点と未知姿勢を用いて表される所定の関数を満たすすべての未知姿勢を計算する。以下の説明では、対応点と未知姿勢により表される所定の関数を、単に連立多項式と呼ぶ。

係数計算部１１は、３組以上の対応点と２つの既知姿勢とを入力し、連立多項式の係数を計算して連立多項式求解部１２に出力する。

連立多項式求解部１２は、係数計算部１１によって算出された連立多項式の係数を入力し、連立多項式を解き、すべての解を出力する。

実数解抽出部１３は、連立多項式求解部１２によって算出された連立多項式のすべての解を入力し、その中からすべての実数解を抽出して出力する。実数解抽出部１３は、実数解が１つも抽出できない場合、以降の処理を打ち切って解なしのフラグを出力する。

未知姿勢候補変換部１４は、実数解抽出部１３によって抽出されたすべての実数解を入力し、未知姿勢の候補を計算して出力する。未知姿勢候補変換部１４は、各実数解をそれぞれ、以降の計算において１つの未知姿勢の候補を表すものとして扱われる値に変換する。

位置候補計算部２は、未知姿勢候補計算部１によって算出された未知姿勢の候補を入力し、カメラの相対的な位置の候補を計算して出力する処理部である。

誤差最小候補抽出部３は、未知姿勢の候補と位置の候補と対応点とを入力し、対応点と未知姿勢と位置の幾何関係を表す所定の誤差関数が最小となる１つまたは複数の未知姿勢と位置を計算し、出力する。また、誤差最小候補抽出部３は、後述するように既知姿勢を用いて対応点を置き換える場合、既知姿勢を入力してもよい。

本実施形態において、未知姿勢候補計算部１（より具体的には、係数計算部１１、連立多項式求解部１２、実数解抽出部１３、未知姿勢候補変換部１４）、位置候補計算部２、誤差最小候補抽出部３は、例えば、特定の演算処理等を行うよう設計されたハードウェア、またはプログラムに従って動作するＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等の情報処理装置によって実現される。

次に、本実施形態の位置姿勢推定装置の動作を説明する。図２は、本発明による位置姿勢推定装置の第１の実施形態の動作の一例を示すフローチャートである。

まず、係数計算部１１は、３組以上の対応点と２つの既知姿勢が入力されると、未知姿勢とカメラの相対位置とを変数として、対応点と未知姿勢を用いて表される所定の関数（以下、連立多項式と呼ぶ）の係数を計算し、連立多項式求解部１２に出力する（ステップＳ１１）。

ここで、カメラの姿勢は、３自由度の回転行列または回転行列を表現するために必要な３つの変数により表される。姿勢の自由度は、カメラの姿勢を表す３×３行列の９個の要素がそれぞれ何個の変数で表現されているかを示す指標である。２つの既知姿勢とは、３つの変数のうちの既知の２つの変数、または、２自由度で表現される回転行列のことである。未知姿勢とは、残りの１つの変数、または、１自由度で表現される回転行列のことである。

本実施形態において、係数計算部１１が、係数を算出する対象となる連立多項式は、カメラ姿勢の自由度をどのように表現するかで一意に定まる。そのため、係数計算部１１は、例えば、予め定義付けられたカメラ姿勢の自由度の表現方法または連立多項式の型に従い、３組以上の対応点と２つの既知姿勢を用いて、連立多項式における係数を算出すればよい。また、係数計算部１１は、例えば、カメラ姿勢の自由度の表現方法に応じた複数の連立多項式の型を予め定義づけておき、起動時等に読み出した設定パラメータの値に応じて用いる連立多項式の型を選択することも可能である。

次いで、連立多項式求解部１２は、係数計算部１１が算出した連立多項式の係数を入力し、ステップＳ１１で入力された対応点と既知姿勢とを用いて連立多項式を解く（ステップＳ１２）。また、連立多項式求解部１２は、連立多項式を満たすすべての解を実数解抽出部１３に出力する。ここでの連立多項式の解は、未知姿勢として不適切な値も含まれる。例えば、その一部またはすべてが複素数である場合がある。

実数解抽出部１３は、連立多項式求解部１２が解いた連立多項式のすべての解が入力されると、それらの解の中に実数解がある場合（ステップＳ１３のＹｅｓ）、すべての実数解を抽出して未知姿勢候補変換部１４に出力する。連立多項式のすべての解から実数解が１つも抽出できない場合には、実数解抽出部１３は、位置姿勢の推定結果として「解なし」のフラグを出力して動作を終了する（ステップＳ１３のＮｏ、ステップＳ１７）。例えば、実数解抽出部１３は、すべての解が複素数であれば「解なし」のフラグを出力し、動作を終了する。「解なし」のフラグは、例えば、真偽値であってもよいし、事前に決定した解なしを表す位置姿勢の値であってもよい。

連立多項式に１つでも実数解があった場合、未知姿勢候補変換部１４は、すべての実数解を入力し、未知姿勢の候補へ変換する（ステップＳ１４）。計算の結果得られた未知姿勢の候補は、位置候補計算部２と誤差最小候補抽出部３に出力される。未知姿勢候補変換部１４は、得られた実数解をそれぞれ未知姿勢の候補とされる値（未知姿勢を表す１つの数値または自由度１の３×３回転行列）に変換する処理を行えばよい。

位置候補計算部２は、ステップＳ１４で得られたすべての未知姿勢の候補を入力し、それぞれの未知姿勢に対応するカメラの相対的な位置を計算し、位置の候補として誤差最小候補抽出部３に出力する（ステップＳ１５）。

誤差最小候補抽出部３は、未知姿勢の候補と位置の候補と対応点とを入力し、対応点と未知姿勢と位置の幾何関係を表す所定の誤差関数が最小となる１つまたは複数の未知姿勢と位置を計算し、出力する（ステップＳ１６）。また、誤差最小候補抽出部３は、後述するように既知姿勢を用いて対応点を置き換える場合、既知姿勢を入力する必要はなく、置き換えない場合、既知姿勢を入力してもよい。

誤差最小候補抽出部３は、入力された対応点数が３組の場合、入力されたすべての未知姿勢の候補と位置の候補を、未知姿勢と位置として出力する。また、入力した対応点数が４組以上の場合、誤差最小候補抽出部３は、入力したすべての未知姿勢の候補と位置の候補の中から、所定の誤差関数を最小化する１つまたは複数の位置と未知姿勢とを計算し、出力する。対応点数が３組の場合にこの上記計算を行わない理由は、すべての未知姿勢の候補における誤差が０となることから、どの候補が正解に最も近いかを判別することができないからである。一方、対応点数が４組以上の場合には、各候補における誤差が異なることから、ただ一つの誤差最小の候補を選択できる。

（実施例１）
以下、具体的な例を用いて第１の実施形態を説明する。まず、未知姿勢とカメラの相対位置を変数とするエピポーラ方程式について説明する。以下では、Ｉは単位行列、ｄｅｔは行列式、‖‖はベクトルのＬ２ノルム、［ ］_×は、３次元ベクトルのクロス積の行列表現を表す。また、入力対応点数をｎ組、１枚目の画像のｉ番目の対応点座標をｖ_ｉ、２枚目の画像のｉ番目の対応点座標をｖ’_ｉ、焦点距離や光学中心などのカメラの内部パラメータは校正済みとし、ｖ_ｉとｖ’_ｉは斉次化されているとする。また、ｖ_ｉとｖ’_ｉの個数は等しいとする。また、２つの既知姿勢をα、β、未知姿勢をθとすると、カメラの相対的な姿勢は回転行列として式（１）で表される。

本実施例では、姿勢は式（１）のように表現されるが、軸方向と回転方向と乗算の順番の定義の仕方により、任意に表現可能である。また、本実施形態では既知姿勢はα、βという２つのパラメータで表現されるが、単位四元数を用いて表現されてもよいし、非特許文献２に記載されているように回転軸と回転量を用いて表現されてもよい。

カメラの相対的な位置を自由度２の３次元ベクトルｔで表すと、対応点ｖ_ｉ、ｖ’_ｉ、位置ｔ、姿勢Ｒは、エピポーラ方程式と呼ばれる式（２）のように表される。

ｔの自由度を２とするのは、スケール倍の不定性による。αとβは既知であるため、ｖ_ｉをＲ_αＲ_βｖ_ｉと置き換えることにより、変形するとエピポーラ方程式は式（３）のように表される。

式（３）の未知数はｔを表す２つとθの３つであるため、最少３組の対応点があれば式（３）は解け、４組以上ならば最小自乗解を計算できる。まず、３組の場合について説明する。３組の対応点が入力された場合、式（３）は式（４）のように表される。

ここで、Ａは、θを変数として含む３×３の行列である。式（４）は、ｔがゼロベクトルまたはＡの零空間であることを示す。ｔはゼロベクトルではないため、Ａの固有値ゼロに対応する固有ベクトルとしてｔが表される。すなわち、θは、θを変数としてＡの１つの固有値をゼロとした関数の解である。

例えば、Ａの固有値は３次式の解であるため、連立多項式求解部１２は、θを変数として固有値を閉形式で書き下した式を用いてもよい。この場合、連立多項式求解部１２は、適当な初期値のもとニュートン法などの反復解法によりθを求める。また、固有値がゼロであるため、連立多項式求解部１２は、ｄｅｔ（Ａ）＝０の解を求めてもよい。連立多項式求解部１２は、ｄｅｔ（Ａ）＝０を満たすθを求めるために、例えば、ｄｅｔ（Ａ）のｃｏｓθとｓｉｎθを２つの独立変数ｃ、ｓと置き換えることにより、新たな条件式としてｃ^２＋ｓ^２=１を加えた式（５）を解いてもよい。

また、非特許文献２に記載されているように、ｃｏｓθとｓｉｎθとを半角の公式を用いて１変数で表現することもできる。この場合、式（５）は連立多項式ではなく、１変数多項式となる。連立多項式求解部１２は、求められたθをＡに代入し、Ａの固有値ゼロに対応する固有ベクトルとしてｔを計算する。このとき、ｔの絶対的なスケールは不明で自由度２であるため、ｔのノルムを１に正規化する。

次に、４組以上の対応点が入力された場合について説明する。４組以上の対応点が入力された場合、式（３）を拡張すると式（６）のように表される。

ここで、Ｂは、式（７）のように表され、θを変数として含むｎ×３の行列である。

非特許文献１や非特許文献３で知られているように、式（６）が示す誤差はＢ^ＴＢの最小固有値であり、ｔは、ゼロベクトルではなくＢ^ＴＢの最小固有値に対応する固有ベクトルである。すなわち、θは、θを変数としてＢ^ＴＢの１つの固有値を最小化する関数の解である。例えば、Ｂ^ＴＢは３×３の行列であるため、Ｂ^ＴＢの固有値はθを変数として書き下すことができ、式（４）と同様に反復解法により求めることができる。また、１つの固有値が最小であるため、連立多項式求解部１２は、以下の式（８）の解を求めてもよい。

式（８）を満たすθを求めるために、連立多項式求解部１２は、例えば、式（８）のｃｏｓθとｓｉｎθを２つの独立変数ｃ、ｓと置き換え、新たな条件式としてｃ^２+ｓ^２=１を加えた以下の式（９）を解いてもよい。

また、式（５）と同様に、非特許文献２に記載されているように、ｃｏｓθとｓｉｎθを半角の公式を用いて１変数で表現することもできる。この場合、式（９）は連立多項式ではなく、１変数多項式となる。連立多項式求解部１２は、求められたθをＢ^ＴＢに代入し、Ｂ^ＴＢの最小固有値に対応する固有ベクトルとしてｔを計算する。３組の場合と同様、ｔのノルムを１に正規化する。

本実施形態では、３組と４組以上の場合を分けて説明したが、３組の場合も式（９）を解くことで式（５）と同様の解を得ることが出来る。なぜならば、以下の式（１０）の関係があるので、式（９）の解に式（５）の解が含まれるからである。

次に、本実施例における各部の動作を具体的に説明する。ステップＳ１１において、係数計算部１１は、対応点ｖ_ｉとｖ_ｉ’（３≦ｉ≦ｎ）と２つの既知姿勢（α、β）を入力し、対応点の数をｎとすると、ｎ=３の場合は式（５）または式（９）、ｎ≧４の場合は式（９）で表される連立多項式もしくは１変数多項式の係数を計算し、連立多項式求解部１２に出力する。

２つの既知姿勢は、例えば、加速度センサや消失点を利用して取得できる。加速度センサを利用する場合、カメラを静止させて重力方向を計測する。重力方向は、３次元ベクトルで表されるが、ノルムの大きさは無視されるため、その自由度は２である。そのため、２枚の画像を撮影したときの重力方向の違いをαとβの２つのパラメータで表すことができる。消失点とは、３次元空間における２つ以上の平行線が、射影変換により画像上で交わった点のことである。１つの消失点から既知姿勢を計算する方法が、非特許文献２に記載されている。

ステップＳ１２では、連立多項式求解部１２は、係数計算部１１から出力された係数を用いて連立多項式を解き、式（５）または式（９）の解を実数解抽出部１３に出力する。式（５）または式（９）の連立多項式を解くためには、例えば、シルベスター行列などを用いて１変数多項式へ帰着する方法や、グレブナー基底を用いて２変数の解を同時に計算する方法など、様々な方法を用いることができる。

ステップＳ１３では、実数解抽出部１３が、式（５）または式（９）で表される連立多項式の解を入力し、その中からすべての実数解を計算し、実数解の有無を判定する。実数解抽出部１３は、実数解があれば、未知姿勢候補変換部１４に出力する。実数解抽出部１３は、すべての解が複素数であれば、解なしのフラグを出力して動作を終了する（ステップＳ１７）。解なしのフラグは、例えば、真偽値であってもよいし、事前に決定した解なしを表す位置姿勢の値であってもよい。

ステップＳ１４では、未知姿勢候補変換部１４が、全ての実数解を入力し、未知姿勢の候補を計算し、位置候補計算部２と誤差最小候補抽出部３に出力する。未知姿勢の候補は、例えば、式（１）のＲ_θで与えられる。例えば、実数解がｋ個ある場合、θがｋ個あることを意味し、未知姿勢候補変換部１４は、このｋ個のθを１つずつ式（１）に代入し、ｋ個の未知姿勢の候補（３×３行列）を求めればよい。

ステップＳ１５では、位置候補計算部２が、入力されたすべての未知姿勢の候補に対し、それぞれ位置の候補を計算し、誤差最小候補抽出部３に出力する。位置の候補は、Ａの固有値ゼロに対応する固有ベクトル、もしくは、Ｂ^ＴＢの最小固有値に対応する固有ベクトルである。固有ベクトルは、固有値分解や特異値分解により求めることができる。

ステップＳ１６では、まず誤差最小候補抽出部３は、対応点と、未知姿勢の候補と、位置の候補とを入力する。誤差最小候補抽出部３は、入力した対応点数に応じて異なる動作を行う。ｎ＝３の場合、誤差最小候補抽出部３は、入力したすべての未知姿勢の候補と位置の候補を、未知姿勢と位置として出力する。なぜならば、ｄｅｔ（Ａ）＝ｄｅｔ（Ｂ^ＴＢ）＝０であるから‖Ｂｔ‖^２＝０となり、どの候補が正解に最も近いかを判別することができないからである。ｎ≧４で、かつ、未知姿勢の候補と位置の候補が１つ以上入力された場合、誤差最小候補抽出部３は、Ｂ^ＴＢの最小固有値が最小となる未知姿勢の候補と位置の候補のペアを、未知姿勢と位置として出力する。

本実施例では、位置候補計算部２において、すべての実数解に対して一括処理により最適解の候補を絞り込んだが、連立多項式の解を１つずつ逐次的に処理することも可能である。例えば、連立多項式求解部１２が１つ解を求めるたびに、実数解抽出部１３に解を出力する。実数解抽出部１３により実数解と判定されれば、未知姿勢候補変換部１４による姿勢候補への変換処理、誤差最小候補抽出部３による誤差最小候補判定処理がループ処理により実行される。そして、誤差最小候補抽出部３は、誤差最小候補判定処理として、誤差が最小になる候補をその都度記憶しておき、実数解が抽出される度に、誤差計算、誤差最小値の比較処理、最小候補および最小値の上書き処理を行ってもよい。ここで、姿勢候補数および入力点数による分岐処理を、誤差最小候補抽出部３がまとめて行うようにしてもよい。

また、本実施例では、係数計算部１１と誤差最小候補抽出部３のそれぞれで対応点の組数を数えて処理を切り替えたが、係数計算部１１による対応点数の判定結果により、その後の処理をｎ=３とｎ≧４の２つに場合分けして処理してもよい。この場合、ｎ=３であれば、未知姿勢候補計算部１と位置候補計算部２は、式（５）を計算し、ｎ≧４であれば、未知姿勢候補計算部１と位置候補計算部２と誤差最小候補抽出部３とは、式（９）を計算する。

また、本実施例では、誤差最小候補抽出部３は、誤差としてＢ^ＴＢの最小固有値を用いたが、ｄｅｔ（Ｂ^ＴＢ）を用いてもよい。この場合、未知姿勢候補変換部１４の次に誤差最小候補抽出部３がｄｅｔ（Ｂ^ＴＢ）を計算し、最小となる未知姿勢の候補を１つだけ選択して位置候補計算部２に出力する。また、その他の誤差として、エピポーラ線と対応点とのユークリッド距離や、ユークリッド距離の近似であるサンプソン誤差を用いてもよい。

以上のように、本実施形態の位置姿勢推定装置は、まず、位置と未知姿勢の幾何学的関係に基づく誤差が最小となる候補をすべて計算し、その中から誤差が最小となる候補を抽出するため、出力された未知姿勢と位置が大域的最小解であることが保証される。すなわち、非特許文献３のように事後に拘束条件を無視した補正を行う方法とは異なり、未知姿勢と位置の候補を個別に計算するため補正による誤差増大が発生しない。

また、本実施形態の位置姿勢推定装置は、補正手段が不要となり計算量を削減できる。すなわち、非特許文献３のように補正するための特異値分解などの行列操作を行う必要がない。

よって、本実施形態の位置姿勢推定装置によれば、高精度かつ安定的、さらに、高速にカメラの相対的な位置姿勢を推定できる。

実施形態２．
第２の実施形態（実施形態２）の位置姿勢推定装置を、図面を参照して説明する。図３は本発明による位置姿勢推定装置の第２の実施形態の構成例を示すブロック図である。本実施形態の位置姿勢推定装置の構成は、図１に示した第１の実施形態の構成と比べて、未知姿勢候補計算部１がさらに２次最適性検証部４を有する点が異なる。なお、２次最適性検証部４以外の構成に関しては、第１の実施形態と同様であるため説明を省略する。

２次最適性検証部４は、実数解抽出部１３から出力される実数解を入力し、未知姿勢を変数とする方程式を未知姿勢で２階微分した関数の正負を検証し、正である実数解を２次最適性十分条件を満たす候補として未知姿勢候補変換部１４に出力する。また、２次最適性検証部４は、検証結果、正である実数解が存在しない場合、以降の処理を打ち切って解なしのフラグを出力する。

本実施形態において、２次最適性検証部４は、例えば、特定の演算処理等を行うよう設計されたハードウェア、またはプログラムに従って動作するＣＰＵ等の情報処理装置によって実現される。

次に本実施形態の位置姿勢推定装置の動作を説明する。図４は、本実施形態の位置姿勢推定装置の動作の一例を示すフローチャートである。以下、ステップＳ２１以外の動作は第１の実施形態と同様であるため、説明を省略する。

２次最適性検証部４は、実数解抽出部２１により抽出された連立多項式の実数解に対して、未知姿勢を変数とする方程式を未知姿勢で２階微分した関数の正負を検証し（ステップＳ２１）、正である実数解を２次最適性十分条件を満たす解として未知姿勢候補変換部１４に出力する。２次最適性検証部４は、すべての実数解が２次最適性十分条件を満たさない場合、解なしのフラグを出力し、動作を終了する（ステップＳ２１のＮｏ、ステップＳ１７）。解なしのフラグは、例えば、真偽値であってもよく、事前に決定した解なしを表す姿勢と位置の値であってもよい。

（実施例２）
次に、実施形態２における各部の動作を具体的に説明する。なお、２次最適性検証部４以外の動作は第１の実施形態の場合と同様である。２次最適性検証部４は、実数解抽出部１３から出力される実数解を入力し、未知姿勢を変数とする方程式を未知姿勢で２階微分した関数に実数解を代入して正負を検証する。ここで、未知姿勢を変数とする方程式は、例えば、θを変数として書き下した（Ｂ^ＴＢ）の固有値としてもよいし、ｄｅｔ（Ｂ^ＴＢ）としてもよい。

２次最適性検証部４は、実数解を代入した当該関数が正であれば、２次最適性十分条件を満たす解として未知姿勢候補変換部１４に出力する（ステップＳ２１のＹｅｓ）。１つも正とならない場合、２次最適性検証部４は、実数解抽出部１３と同様に解なしのフラグを出力し、終了する（ステップＳ２１のＮｏ、ステップＳ１７）。

本実施形態の位置姿勢推定装置は、２次最適性検証部４により、連立多項式は満たすが局所最適解ではない、鞍点と呼ばれる不適切な解が排除される。鞍点が削除されると、２次最適性検証部４で求めた実数解は必ず局所最適解である。よって、その中で誤差最小となる解は大域的最適解であることが保証される。また、２次最適性検証部４が求めた実数解が存在しない場合に、以降の処理を打ち切ることで計算コストを低減できる。

よって、本実施形態の位置姿勢推定装置によれば、推定結果として出力する位置姿勢の信頼性をより向上させることができ、より高速にカメラの相対的な位置姿勢を推定することができる。

実施形態３．
本発明の第３の実施形態の位置姿勢推定装置を、図面を参照して説明する。図５は、本発明による位置姿勢推定装置の第３の実施形態の構成例を示すブロック図である。本実施形態の位置姿勢推定装置は、図１に示した第１の実施形態または図３に示した第２の実施形態の構成に加えて、さらに３次元形状復元部５を備える。なお、３次元形状復元部５以外の構成に関しては、第１の実施形態または第２の実施形態と同様であるため説明を省略する。

３次元形状復元部５は、対応点および既知姿勢と、誤差最小候補抽出部３により出力された位置および未知姿勢とを入力し、対応点の３次元座標を復元し、出力する。また、３次元形状復元部５は、復元した３次元座標とともに位置と姿勢を出力する。ここで、姿勢は、例えば３つのパラメータでもよいし、回転行列でもよい。

本実施形態において、３次元形状復元部５は、例えば、特定の演算処理等を行うよう設計されたハードウェア、またはプログラムに従って動作するＣＰＵ等の情報処理装置によって実現される。

次に、本実施形態の位置姿勢推定装置の動作を説明する。図６は、本実施形態の位置姿勢推定装置の動作の一例を示すフローチャートである。以下、ステップＳ３１以外の動作については第２の実施形態と同様であるため、説明を省略する。なお、本実施形態の位置姿勢推定装置の構成を、第１の実施形態の構成に３次元形状復元部５を加えたものとした場合、図６のステップＳ２１は不要である。

３次元形状復元部５は、対応点と既知姿勢と、誤差最小候補抽出部３により出力された位置と未知姿勢とを入力し、対応点の３次元座標を復元し、位置と姿勢とともに出力する（ステップＳ３１）。

（実施例３）
次に、本実施形態における各部の動作を具体的に説明する。なお、３次元形状復元部５以外の動作は第１の実施形態または第２の実施形態の場合と同様である。

３次元形状復元部５は、対応点と既知姿勢と、誤差最小候補抽出部３により出力された位置と未知姿勢とを入力し、対応点の３次元座標を復元し、出力する。また、３次元形状復元部５は、復元した３次元座標とともに位置と姿勢を出力する。ここで、姿勢は、例えば３つのパラメータでもよいし、回転行列でもよい。また、３次元形状復元部５は、位置と姿勢を出力せず、３次元座標のみを出力してもよい。

カメラの相対的な位置と姿勢を元に対応点の３次元座標を復元する方法は、例えば、非特許文献４や非特許文献５に記載されている。

なお、上記の各実施形態の位置姿勢推定装置は、例えば、各部に対応したハードウェア等により実現可能であり、情報処理システムによっても実現可能である。図７は、本発明による位置姿勢推定装置を情報処理システムに実装した場合のブロック図である。図７に示す情報処理システムは、プロセッサ６１と、プログラムメモリ６２と、記憶媒体６３とを備える一般的な情報処理システムである。記憶媒体６３は、別個の記憶媒体からなる記憶領域であってもよいし、同一の記憶媒体からなる記憶領域であってもよい。記憶媒体として、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）や、ハードディスク等の磁気記憶媒体を用いることができる。

プログラムメモリ６２には、上述した未知姿勢候補計算部１（より具体的には、係数計算部１１と、連立多項式求解部１２と、実数解抽出部１３と、未知姿勢候補変換部１４）と、位置候補計算部２と、誤差最小候補抽出部３と、２次最適性検証部４と、３次元形状復元部５の各部の処理を、プロセッサ６１に行わせるためのプログラムが格納されており、このプログラムに従ってプロセッサ６１は動作する。プロセッサ６１は、例えば、ＣＰＵ等のプログラムに従って動作するプロセッサであればよい。このように、本発明は、コンピュータプログラムにより実現することも可能である。なお、プログラムによる動作が可能な部のすべてをプログラムで動作させる必要はなく、一部をハードウェアで構成してもよい。また、それぞれ別々のユニットとして実現されていてもよい。

以上、実施形態および実施例を参照して本発明を説明したが、本発明は、上記実施形態および実施例に限定されない。本発明の構成や詳細には、本発明のスコープ内で当業者が理解し得る様々な変更をすることができる。

図８は、本発明による位置姿勢推定装置の主要部の構成を示すブロック図である。図８に示すように、本発明による位置姿勢推定装置は、主要な構成として、２枚の画像に含まれる３組以上の対応点と、当該２枚の画像を撮影したカメラの相対的な姿勢を表す３つの姿勢パラメータのうち既知の２つの姿勢パラメータである既知姿勢とを入力し、入力された対応点と既知姿勢とを用いて、姿勢パラメータのうちの１つであって未知の姿勢パラメータである未知姿勢とカメラの相対的な位置とを変数として未知姿勢と対応点とを用いて表される所定の関数を満たすすべての未知姿勢の解を未知姿勢の候補として出力する未知姿勢候補計算部１と、未知姿勢の候補それぞれに対するカメラの相対的な位置の候補を計算する位置候補計算部２と、すべての未知姿勢の候補、未知姿勢の候補それぞれに対するカメラの相対的な位置の候補、および対応点を入力し、未知姿勢の候補および位置の候補の中から、対応点とカメラの相対的な位置と未知姿勢との幾何関係を表す所定の誤差関数が最小となる１つまたは複数の未知姿勢、およびカメラの相対的な位置を抽出する誤差最小候補抽出部３とを備える。

また、上記の実施形態では、以下の（１）〜（３）に示すような位置姿勢推定装置も開示されている。

（１）位置姿勢推定装置は、未知姿勢候補計算部（例えば、未知姿勢候補計算部１）が、対応点と既知姿勢とを入力し、未知姿勢と対応点とを用いて表される所定の関数の係数を計算する係数計算部（例えば、係数計算部１１）と、係数を入力し、所定の関数を満たすすべての解を計算する連立多項式求解部（例えば、連立多項式求解部１２）と、連立多項式求解部により得られた解の中からすべての実数解を抽出し、抽出したすべての実数解または解なしを示すフラグを出力する実数解抽出部（例えば、実数解抽出部１３）と、実数解抽出部によって抽出されたすべての未知姿勢の実数解を、それぞれ１つの未知姿勢の候補に変換する未知姿勢候補変換部（例えば、未知姿勢候補変換部１４）とを含むように構成されていてもよい。

（２）位置姿勢推定装置は、未知姿勢候補計算部が、所定の関数を満たすすべての実数解のうち、２次最適性十分条件を満たす実数解を計算して、出力する２次最適性検証部（例えば、２次最適性検証部４）を含むように構成されていてもよい。このような位置姿勢推定装置によれば、推定結果として出力する位置姿勢の信頼性をより向上させることができ、より高速にカメラの相対的な位置姿勢を推定することができる。

（３）位置姿勢推定装置は、対応点と、既知姿勢と、誤差最小候補抽出部により出力されたカメラの相対的な位置およびと未知姿勢とを入力し、対応点の３次元座標を復元し、出力する３次元形状復元部（例えば、３次元形状復元部５）を備えるように構成されていてもよい。

この出願は、２０１２年８月３１日に出願された日本出願特願２０１２−１９１２６２を基礎とする優先権を主張し、その開示の全てをここに取り込む。

以上、実施形態および実施例を参照して本願発明を説明したが、本願発明は上記実施形態および実施例に限定されるものではない。本願発明の構成や詳細には、本願発明のスコープ内で当業者が理解し得る様々な変更をすることができる。

産業上の利用の可能性

本発明は、被写体の３次元形状復元や背景のパノラマ画像の生成等に適用される。

１ 未知姿勢候補計算部
２ 位置候補計算部
３ 誤差最小候補抽出部
４ ２次最適性検証部
５ ３次元形状復元部
１１ 係数計算部
１２ 連立多項式求解部
１３ 実数解抽出部
１４ 未知姿勢候補変換部
２１ 実数解抽出部
６１ プロセッサ
６２ プログラムメモリ
６３ 記憶媒体

Claims (6)

1. ２枚の画像に含まれる３組以上の対応点と、当該２枚の画像を撮影したカメラの相対的な姿勢を表す３つの姿勢パラメータのうち既知の２つの姿勢パラメータである既知姿勢とを入力し、入力された前記対応点と前記既知姿勢とを用いて、前記２つの既知姿勢を共通の固定値として含む連立多項式であり、かつ、前記姿勢パラメータのうちの１つであって未知の姿勢パラメータである未知姿勢と前記対応点と前記既知姿勢とから構成され、前記未知姿勢を変数とする行列であって、固有値ゼロに対応する固有ベクトルが前記カメラの相対的な位置を表す第１行列または最小固有値に対応する固有ベクトルが前記カメラの相対的な位置を表す第２行列について、前記第１行列の行列式をゼロにするまたは前記第２行列の行列式が極値をとる所定の関数を満たすすべての前記未知姿勢の解を未知姿勢の候補として出力する未知姿勢候補計算部と、
   前記未知姿勢の候補それぞれに対する前記カメラの相対的な位置の候補を計算する位置候補計算部と、
   すべての前記未知姿勢の候補、前記未知姿勢の候補それぞれに対する前記カメラの相対的な位置の候補、および前記対応点を入力し、前記未知姿勢の候補および前記位置の候補の中から、前記対応点と前記カメラの相対的な位置と前記未知姿勢との幾何関係を表す所定の誤差関数が最小となる１つまたは複数の前記未知姿勢、および前記カメラの相対的な位置を抽出する誤差最小候補抽出部とを備えた
   ことを特徴とする位置姿勢推定装置。
2. 未知姿勢候補計算部は、
   対応点と既知姿勢とを入力し、前記所定の関数の係数を計算する係数計算部と、
   前記係数を入力し、前記所定の関数を満たすすべての解を計算する連立多項式求解部と、
   前記連立多項式求解部により得られた解の中からすべての実数解を抽出し、抽出したすべての実数解または解なしを示すフラグを出力する実数解抽出部と、
   前記実数解抽出部によって抽出されたすべての前記未知姿勢の実数解を、それぞれ１つの未知姿勢の候補に変換する未知姿勢候補変換部とを含む
   請求項１記載の位置姿勢推定装置。
3. 未知姿勢候補計算部は、
   前記所定の関数を満たすすべての実数解のうち、２次最適性十分条件を満たす実数解を計算して、出力する２次最適性検証部を含む
   請求項１または請求項２に記載の位置姿勢推定装置。
4. 対応点と、既知姿勢と、誤差最小候補抽出部により出力されたカメラの相対的な位置および未知姿勢とを入力し、前記対応点の３次元座標を復元し、出力する３次元形状復部を備えた
   請求項１から請求項３のうちのいずれか１項に記載の位置姿勢推定装置。
5. ２枚の画像に含まれる３組以上の対応点と、当該２枚の画像を撮影したカメラの相対的な姿勢を表す３つの姿勢パラメータのうち既知の２つの姿勢パラメータである既知姿勢とを入力し、入力された前記対応点と前記既知姿勢とを用いて、前記２つの既知姿勢を共通の固定値として含む連立多項式であり、かつ、前記姿勢パラメータのうちの１つであって未知の姿勢パラメータである未知姿勢と前記対応点と前記既知姿勢とから構成され、前記未知姿勢を変数とする行列であって、固有値ゼロに対応する固有ベクトルが前記カメラの相対的な位置を表す第１行列または最小固有値に対応する固有ベクトルが前記カメラの相対的な位置を表す第２行列について、前記第１行列の行列式をゼロにするまたは前記第２行列の行列式が極値をとる所定の関数を満たすすべての前記未知姿勢の解を未知姿勢の候補として出力し、
   前記未知姿勢の候補それぞれに対する前記カメラの相対的な位置の候補を計算し、
   すべての前記未知姿勢の候補、前記未知姿勢の候補それぞれに対する前記カメラの相対的な位置の候補、および前記対応点を入力し、前記未知姿勢の候補および前記位置の候補の中から、前記対応点と前記カメラの相対的な位置と前記未知姿勢との幾何関係を表す所定の誤差関数が最小となる１つまたは複数の前記未知姿勢、および前記カメラの相対的な位置を抽出する
   ことを特徴とする位置姿勢推定方法。
6. コンピュータに、
   ２枚の画像に含まれる３組以上の対応点と、当該２枚の画像を撮影したカメラの相対的な姿勢を表す３つの姿勢パラメータのうち既知の２つの姿勢パラメータである既知姿勢とを入力し、入力された前記対応点と前記既知姿勢とを用いて、前記２つの既知姿勢を共通の固定値として含む連立多項式であり、かつ、前記姿勢パラメータのうちの１つであって未知の姿勢パラメータである未知姿勢と前記対応点と前記既知姿勢とから構成され、前記未知姿勢を変数とする行列であって、固有値ゼロに対応する固有ベクトルが前記カメラの相対的な位置を表す第１行列または最小固有値に対応する固有ベクトルが前記カメラの相対的な位置を表す第２行列について、前記第１行列の行列式をゼロにするまたは前記第２行列の行列式が極値をとる所定の関数を満たすすべての前記未知姿勢の解を未知姿勢の候補として出力する未知姿勢候補計算処理と、
   前記未知姿勢の候補それぞれに対する前記カメラの相対的な位置の候補を計算する位置候補計算処理と、
   すべての前記未知姿勢の候補、前記未知姿勢の候補それぞれに対する前記カメラの相対的な位置の候補、および前記対応点を入力し、前記未知姿勢の候補および前記位置の候補の中から、前記対応点と前記カメラの相対的な位置と前記未知姿勢との幾何関係を表す所定の誤差関数が最小となる１つまたは複数の前記未知姿勢、および前記カメラの相対的な位置を抽出する誤差最小候補抽出処理と
   を実行させるための位置姿勢推定プログラム。

Images