画像処理プログラムおよび画像処理装置
【課題】カメラの取り付け位置や取り付け姿勢の情報を迅速かつ精度良く得ることを課題とする。
【解決手段】設定部２３２は、４個のマーカーが映し出された画像平面上の４個に対応するカメラ座標系における仮想的な４つの対応点の中から基準マーカーとする点Ｍを選択する。そして、設定部２３２は、基準点Ｍまでの距離Ｌ０に所定の値を設定する。作成部２３３は、基準点Ｍ以外の点、つまり基準マーカー以外のマーカーに対応する点１〜点３までの各距離Ｌ１，Ｌ２，Ｌ３を表す数式を作成する。導出部２３４は、設定部２３２により設定された基準マーカーで採用する全ての符号パターンを導出する。棄却部２３５は、導出部２３４により導出された符号パターンの中から、距離Ｌ１、Ｌ２、Ｌ３の値の少なくとも１つが相応しい値となり得えない符号パターンを棄却する。
【解決手段】設定部２３２は、４個のマーカーが映し出された画像平面上の４個に対応するカメラ座標系における仮想的な４つの対応点の中から基準マーカーとする点Ｍを選択する。そして、設定部２３２は、基準点Ｍまでの距離Ｌ０に所定の値を設定する。作成部２３３は、基準点Ｍ以外の点、つまり基準マーカー以外のマーカーに対応する点１〜点３までの各距離Ｌ１，Ｌ２，Ｌ３を表す数式を作成する。導出部２３４は、設定部２３２により設定された基準マーカーで採用する全ての符号パターンを導出する。棄却部２３５は、導出部２３４により導出された符号パターンの中から、距離Ｌ１、Ｌ２、Ｌ３の値の少なくとも１つが相応しい値となり得えない符号パターンを棄却する。
【発明の詳細な説明】
【技術分野】
【０００１】
本願の開示する技術は、画像処理プログラムおよび画像処理装置に関する。
【背景技術】
【０００２】
近年、カメラにより撮影された画像を３次元的に認識する技術が実用化されている。この技術は、例えば、車両に搭載された車載カメラにより撮影された画像を用いて、車両の全周囲を見渡すことができる全周囲画像などを合成する場合に利用されている。そのほか、ロボットに搭載されたカメラにより撮影された画像を用いて、対象物体に対するロボットの相対位置を把握可能な画像を合成する場合などにも利用されている。
【０００３】
ところで、カメラにより撮影された画像を３次元的に認識する技術においては、カメラにより撮影された画像を用いて３次元の画像を合成する場合に、カメラの取り付け姿勢や取り付け位置の情報を正しく把握しておくことが求められる場合がある。カメラの取り付け姿勢や取り付け位置の情報を推定する技術には、以下に説明するような公知技術１や公知技術２がある。
【０００４】
上述した公知技術１について図１７を用いて説明する。図１７は、公知技術１を説明するための図である。公知技術１は、まず、マーカーなどの物体を実空間上の所定位置、言い換えれば、世界座標系における所定位置に配置してカメラで撮影する。続いて、公知技術１は、図１７に示すように、マーカーを撮影して得られる画像平面Ｈ上の３つの点（ｘ０，ｘ１，ｘ２）に対応する３つの点（Ｐ０，Ｐ１，Ｐ２）を抽出する。この３点（Ｐ０，Ｐ１，Ｐ２）は、画像平面Ｈ上の３つの点（ｘ０，ｘ１，ｘ２）に対応するカメラ座標系における３次元位置である。そして、公知技術１は、図１７に示すように、カメラ座標系の原点Ｃと、３点（Ｐ０，Ｐ１，Ｐ２）とを結んでできる三角錐の各面を定式化した連立方程式をそれぞれ立てる。例えば、原点Ｃと、３点（Ｐ０，Ｐ１，Ｐ２）とを結んでできる三角錐の各面は余弦定理を用いて定式化した数式で表すことができる。そして、各数式から以下に示す連立方程式（１）が成立する。なお、原点Ｃと点Ｐ０との距離をＬ０、原点Ｃと点Ｐ１との距離をＬ１、原点Ｃと点Ｐ２との距離をＬ２、原点Ｃから点Ｐ０への視線ベクトルをＶ０、原点Ｃから点Ｐ１への視線ベクトルをＶ１、原点Ｃから点Ｐ２への視線ベクトルをＶ２とする。視線ベクトル（Ｖ０，Ｖ１，Ｖ２）は、カメラの光軸位置や焦点距離などの内部パラメータから得られる。なお、以下の式（１）に示す＜Ｖｉ，Ｖｊ＞（ｉ，ｊ＝０，１，２、ｉ≠ｊ）はベクトルＶｉとベクトルＶｊとの内積を表す。また、以下の式（１）に示すＤ０，１、Ｄ０，２、Ｄ１，２は、点Ｐ０〜点Ｐ２のうちの各２点間の実距離をそれぞれ表している。
【０００５】
【数１】
【０００６】
次に、公知技術１は、上述した連立方程式（１）から導出される４次方程式の解を求めることにより、図１７に示すカメラ座標系の原点Ｃから３点（Ｐ０，Ｐ１，Ｐ２）までの距離Ｌ０、Ｌ１、Ｌ２を求める。そして、公知技術１は、原点Ｃから３点（Ｐ０，Ｐ１，Ｐ２）までの距離と、カメラの内部パラメータから得られる視線ベクトル（Ｖ０，Ｖ１，Ｖ２）とを用いて、逆算的にカメラの位置および姿勢を導出する。
【０００７】
また、例えば、公知技術２は、マーカーなどの物体をカメラにより撮影した画像上の特徴点を抽出し、画像上の特徴点と物体の見え方のパターンとの対応関係から、カメラの概略的な推定位置に応じた探索領域を設定する。そして、公知技術２は、カメラの位置や姿勢に関する合計６つのパラメータについて、要求誤差精度を満足する最適解を探索領域の中から探索することによりカメラの位置や姿勢を検出する。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開平６−２５８０２８号公報
【非特許文献】
【０００９】
【非特許文献１】Ｑｕａｎ ａｎｄ Ｌａｎ，「Ｌｉｎｅａｒ Ｎ−Ｐｏｉｎｔ Ｃａｍｅｒａ Ｐｏｓｅ Ｄｅｔｅｒｍｉｎａｔｉｏｎ」，ＩＥＥＥ Ｔｒａｎｓ．ｏｎ ＰＡＭＩ，１９９９
【発明の概要】
【発明が解決しようとする課題】
【００１０】
上述した公知技術１は、抽出した３点からカメラ座標系の原点までの距離を求めるために４次方程式の解を求める必要があるので、計算量が多くなる結果、カメラの取り付け姿勢や取り付け位置の情報を迅速に得られないという問題がある。さらに、公知技術１では、カメラ座標系の原点までの距離を求めるために４次方程式の解を求めるが、４次方程式は特異値分解を用いて解を導き出すので、専門的な数値計算ライブラリ等を格納するための領域が必要となり、実用性に欠けるという問題もある。
【００１１】
また、上述した公知技術２は、カメラの位置や姿勢に関する合計６つのパラメータについて最適解の探索を行うので、局所解に陥りやすい結果、カメラの取り付け姿勢や取り付け位置の情報を必ずしも精度良く得ることができないという問題があった。
【００１２】
開示の技術は、上記に鑑みてなされたものであって、カメラの取り付け位置や取り付け姿勢の情報を迅速かつ精度良く得ることが可能な画像処理プログラムおよび画像処理装置を提供することを目的とする。
【課題を解決するための手段】
【００１３】
本願の開示する画像処理プログラムは、一つの態様において、コンピュータに、次のような処理を実行させる。すなわち、コンピュータに、実空間に設けられた４個以上のマーカーをカメラにより撮影した画像を取得する処理を実行させる。また、コンピュータに、前記カメラの焦点距離および光軸位置の情報を含む内部パラメータおよび前記画像における前記４個以上のマーカーの各々の位置に基づいて、前記カメラから該４個以上のマーカーの各々に対応するベクトルを生成する処理を実行させる。また、コンピュータに、前記４個以上のマーカーの中から基準マーカーを一つ選択する処理を実行させる。また、コンピュータに、前記基準マーカー以外の残りのマーカーのうちのそれぞれに対するベクトルと、該基準マーカーのベクトルとの内積値を算出する処理を実行させる。また、コンピュータに、前記内積値の符号が負となる前記基準マーカー以外のマーカーについて次の処理を実行させる。例えば、コンピュータに、該基準マーカーを用いて該基準マーカー以外のマーカーと前記カメラとの距離を求める二次方程式の解を表す数式に含まれる符号のうち、負の符号の採用を棄却する処理を実行させる。また、コンピュータに、棄却した結果に基づいて、前記基準マーカー以外のマーカーのそれぞれに対する前記数式に含まれる符号を、全ての該基準マーカー以外のマーカーについて組み合わせた符号パターンを生成する処理を実行させる。また、コンピュータに、前記基準マーカーと前記カメラとの距離として第１の値を設定する処理を実行させる。また、コンピュータに、前記第１の値と前記二次方程式と棄却した前記結果とを用いて、該第１の値における、前記基準マーカー以外のマーカーの各々と前記カメラとの距離の候補値を、１つまたは２つ算出する処理を実行させる。また、コンピュータに、前記符号パターンごとに、該符号パターン及び前記１つまたは２つの候補値に基づいて算出されるマーカー間距離と、前記実空間におけるマーカー間の距離との誤差を算出する処理を実行させる。また、コンピュータに、前記第１の値とは異なる第２の値を設定した場合に、前記符号パターンごとに他の誤差を算出する処理を実行させる。また、コンピュータに、前記第１の値を選択した場合に算出された前記誤差および前記第２の値を選択した場合に算出された前記他の誤差に基づいて、前記４個以上のマーカーの各々と前記カメラとの距離を決定する処理を実行させる。また、コンピュータに、決定した前記４個以上のマーカーの各々と前記カメラとの距離に基づいて、該カメラの世界座標系における位置および姿勢を演算する処理を実行させる。
【発明の効果】
【００１４】
本願の開示する技術の一つの態様によれば、カメラの取り付け位置や取り付け姿勢の情報を迅速かつ精度良く得ることができる。
【図面の簡単な説明】
【００１５】
【図１】図１は、実施例１に係る画像処理装置の説明に用いる図である。
【図２】図２は、実施例１に係る画像処理装置の構成を示す機能ブロック図である。
【図３】図３は、実施例１に係る生成部の説明に用いる図である。
【図４】図４は、実施例１に係る作成部の説明に用いる図である。
【図５】図５は、実施例１に係る導出部の説明に用いる図である。
【図６】図６は、実施例１に係る棄却部の処理に関する概念的な説明に用いる図である。
【図７】図７は、実施例１に係る算出部の処理に関する概念的な説明に用いる図である。
【図８】図８は、実施例１に係る算出部の処理に関する概念的な説明に用いる図である。
【図９】図９は、実施例１に係る算出部の処理に関する概念的な説明に用いる図である。
【図１０】図１０は、実施例１に係る算出部の処理に関する概念的な説明に用いる図である。
【図１１】図１１は、実施例１に係る位置姿勢推定部の説明に用いる図である。
【図１２】図１２は、実施例１に係る最適解算出処理の流れを示す図である。
【図１３】図１３は、実施例１に係る最適解候補算出処理の流れを示す図である。
【図１４】図１４は、実施例１に係る符号パターン決定処理の流れを示す図である。
【図１５】図１５は、実施例２に係る画像処理装置の構成を示す機能ブロック図である。
【図１６】図１６は、画像処理プログラムを実行する電子機器の一例を示す図である。
【図１７】図１７は、公知技術１を説明するための図である。
【発明を実施するための形態】
【００１６】
以下に、図面を参照しつつ、本願の開示する画像処理プログラムおよび画像処理装置の一実施形態について詳細に説明する。なお、以下の実施例では、実空間である世界座標系の既知の３次元位置に位置する４個のマーカーを車両やロボットなどに搭載されたカメラにより撮影して、撮影された画像を用いてカメラの位置および姿勢を推定する技術に関する一実施形態について説明する。なお、マーカーは、４個以上であれば何個であっても構わない。以下の実施例では、実空間のことを３次元空間と表記するものとする。本願の開示する画像処理プログラムおよび画像処理装置の一実施形態として後述する実施例により、本願が開示する技術が限定されるものではなく、処理内容に矛盾を生じさせない範囲で各実施例を適宜組み合わせることが可能である。
【実施例１】
【００１７】
実施例１に係る画像処理装置の構成の説明に入る前に、図１を用いて、実施例１に係る画像処理装置を説明する。図１は、実施例１に係る画像処理装置の説明に用いる図である。なお、以下に説明する実施例では、世界座標系の既知の３次元位置に４個のマーカーを配置する場合の一実施形態を説明するが、図１では、説明の便宜上、配置される４個のマーカーのうち３個のマーカーに焦点をあてて説明する。
【００１８】
図１に示すＣはカメラ座標系の原点を示す。なお、図１の説明において、図１に示すＣを「原点Ｃ」と表記するものとする。図１に示すＰ０，Ｐ１，Ｐ２Ａ，Ｐ２Ｂは、４個のマーカーが映し出された画像平面上の点のうち、３個のマーカーに対応するカメラ座標系における仮想的な３次元位置（対応点）を示す。また、Ｐ２Ａ，Ｐ２Ｂは、画像平面上のある点に対応するカメラ座標系における３次元位置となり得る候補に相当する。なお、図１の説明において、図１に示すＰ０，Ｐ１，Ｐ２Ａ，Ｐ２Ｂを、「対応点Ｐ０」，「対応点Ｐ１」，「対応点Ｐ２Ａ」，「対応点Ｐ２Ｂ」とそれぞれ表記するものとする。図１に示すＶ０，Ｖ１，Ｖ２は、原点Ｃから対応点Ｐ０，対応点Ｐ１，対応点Ｐ２Ａ，対応点Ｐ２Ｂへ向かう視線ベクトルを示し、例えば、長さが１の単位ベクトルである。なお、図１の説明において、図１に示すＶ０，Ｖ１，Ｖ２を、「ベクトルＶ０」，「ベクトルＶ１」，「ベクトルＶ２」とそれぞれ表記するものとする。
【００１９】
また、図１に示すＬ０は原点Ｃと対応点Ｐ０との距離を示し、図１に示すＬ１は原点Ｃと対応点Ｐ１との距離の値を示し、図１に示すＬ２Ａは原点Ｃと対応点Ｐ２Ａとの距離の値を示し、図１に示すＬ２Ｂは原点Ｃと対応点Ｐ２Ｂとの距離の値を示す。なお、図１の説明において、図１に示すＬ０を「距離Ｌ０」と表記するものとする。同様に、図１の説明において、図１に示すＬ１，Ｌ２Ａ，Ｌ２Ｂを、「距離Ｌ１」，「距離Ｌ２Ａ」，「距離Ｌ２Ｂ」とそれぞれ表記するものとする。
【００２０】
また、図１に示すＤ（０，１）は、対応点Ｐ０と対応点Ｐ１との３次元空間上の実距離を示し、図１に示すＤ（０，２）は、対応点Ｐ０と対応点Ｐ２との３次元空間上の実距離を示し、図１に示すＤ（１，２）は、対応点Ｐ１と対応点Ｐ２との３次元空間上の実距離を示す。なお、図１の説明において、図１に示すＤ（０，１）を「実距離Ｄ（０，１）」と表記するものとする。同様に、図１の説明において、図１に示すＤ（０，２）を「実距離Ｄ（０，２）」と記し、図１に示すＤ（１，２）を「実距離Ｄ（１，２）」と表記するものとする。
【００２１】
実施例１に係る画像処理装置は、まず、基準点として対応点Ｐ０を選択した場合に、距離Ｌ０を用いて、距離Ｌ１および距離Ｌ２を余弦定理により定式化して表した各数式からなる連立方程式を立てる。この連立方程式は、例えば、以下の式（２）で示される。なお、以下の式（２）に示すＤ０，１、Ｄ０，２、は、上述した図１に示すＤ（０，１）、Ｄ（０，２）にそれぞれ対応する。
【００２２】
【数２】
【００２３】
ここで、距離Ｌ０の値が与えられれば、上述した式（２）の解を求めることにより、２種類の値が距離Ｌ１の値として導出される。つまり、距離Ｌ１の値として、距離Ｌ１を定式化して表した数式の右辺内にある内積の項と根号の項との間にあるプラスの符合を選択した場合と、マイナスの符号を選択した場合の２種類の値が導出される。同様に、上述した式（２）の解を求めることにより、２種類の値が距離Ｌ２の値として導き出される。すなわち、距離Ｌ１を定式化して表した数式の右辺内にある内積の項と根号の項との間にある符合の選択により距離Ｌ１の値が決定付けられる。同様に、距離Ｌ２を定式化して表した数式内の内積の項と根号の項との間にある符合の選択により距離Ｌ２の値が決定付けられる。そこで、実施例１に係る画像処理装置は、距離Ｌ１を定式化して表した数式の右辺内にある根号前の符号の一方と、距離Ｌ２を表す数式の右辺内にある根号前にある符号一方とを組合せた符号パターンを導出する。そして、実施例１に係る画像処理装置は、符号パターンの中から、距離Ｌ１、距離Ｌ２の値として相応しくない値が導き出されるものを棄却する。
【００２４】
例えば、実施例１に係る画像処理装置は、距離Ｌ１およびＬ２が上述した式（２）で表される場合に、（＋、＋）、（−、−）、（＋、−）、（−，＋）の４つの符号パターンを導出できる。次に、実施例１に係る画像処理装置は、導出した符号パターンの中から、少なくとも距離Ｌ１および距離Ｌ２の値の一方が相応しい値となり得えない符号パターンを棄却する。例えば、図１に示すベクトルＶ０とベクトルＶ１とのなす角度「θ」が９０度よりも大きい場合には、距離Ｌ１を表す数式の右辺内にある内積がマイナスの値となる。この場合、式（２）の根号前の符号のうちマイナス符号を選択した場合に計算される距離Ｌ１の値はマイナスとなってしまうので、距離Ｌ１の値として相応しくない。つまり、右辺の第一項は必ず負の値となるため、符号パターンは少なくとも正が採用されないかぎり、右辺は負の値となる。よって、実施例１に係る画像処理装置は、距離Ｌ１を表す数式の右辺内にある根号前の符号のうちマイナスの符号を選択してできる符号パターンを棄却する。また、例えば、図１に示すベクトルＶ０とベクトルＶ２とのなす角度が９０度以下である場合には、距離Ｌ２を表す数式の右辺内にある内積がプラスの値となる。この場合、距離Ｌ２を表す数式に含まれる根号前の各符号をそれぞれ選択した場合に計算される距離Ｌ２の値は、プラスになるかマイナスになるか不明なので、距離Ｌ２の値として相応しいかどうかの判断はできない。よって、実施例１に係る画像処理装置は、距離Ｌ２を表す数式の右辺内にある根号前のプラスの符号およびマイナスの符合を選択してできる各符号パターンを棄却せずに残す。この結果、実施例１に係る画像処理装置は、（−、−）、（−，＋）の２つの符号パターンを棄却し、（＋、−）、（＋，＋）の２つの符号パターンを残すこととなる。
【００２５】
なお、棄却されずに残された符号パターンでは、距離Ｌ１の値の候補は、距離Ｌ１を表す数式の右辺内にある内積の項と根号の項との間にあるプラスの符合を選択した場合に計算される１つの値のみとなる。つまり、図１に示す距離Ｌ１の値の候補は、２種類の符号の選択により与えられる２つの候補のうちの１つの候補に絞り込まれる。一方、距離Ｌ２の値の候補は、距離Ｌ２を表す数式の右辺内にある内積の項と根号の項との間にあるプラスの符合を選択した場合に計算される値、およびマイナスの符号を選択した場合に導出される値の２つとなる。つまり、図１に示す距離Ｌ２の値の候補は、図１に示すＬ２Ａに対応する値と図１に示すＬ２Ｂに対応する値の２つとなり、２種類の符号の選択により与えられる２つの候補がそのまま残される。このように、カメラ座標系の原点からマーカーに対応するカメラ座標系における対応点までの距離を算出する場合に、視線ベクトル同士がなす角度を考慮することにより、２種類の符号の選択により与えられる２つの候補の中から絞り込むことができる場合がある。
【００２６】
次に、実施例１に係る画像処理装置は、棄却されずに残された符号パターンについて、３次元空間上のマーカー間の距離の候補値と３次元空間上のマーカー間の実距離の値との誤差を算出する。例えば、実施例１に係る画像処理装置は、以下に説明するようにして各符号パターンについての誤差を算出する。
【００２７】
まず、実施例１に係る画像処理装置は、距離Ｌ０と、上述した式（２）とに基づいて、例えば、棄却されずに残された符号パターン（＋、＋）について、距離Ｌ１および距離Ｌ２Ａを得られる。また、実施例１に係る画像処理装置は、距離Ｌ０と、上述した式（２）とに基づいて、例えば、棄却されずに残された符号パターン（＋、−）について、距離Ｌ１および距離Ｌ２Ｂを得られる。なお、距離Ｌ０の値は、距離Ｌ０の値が取り得る範囲内で予め設定される。
【００２８】
続いて、実施例１に係る画像処理装置は、符号パターン（＋、＋）について、距離Ｌ１および距離Ｌ２Ａに基づき、３次元空間上のマーカー間の距離の候補値、つまり図１に示す点Ｐ１と点Ｐ２Ａとの距離を算出する。また、実施例１に係る画像処理装置は、符号パターン（＋、−）について、距離Ｌ１および距離Ｌ２Ｂに基づき、３次元空間上のマーカー間の距離の候補値、つまり図１に示す点Ｐ１と点Ｐ２Ｂとの間の距離を算出する。
【００２９】
次に、実施例１に係る画像処理装置は、符号パターン（＋、＋）について算出した点Ｐ１と点Ｐ２Ａとの距離と、３次元空間上のマーカー間の実距離の値との誤差を算出する。同様に、実施例１に係る画像処理装置は、符号パターン（＋、−）について算出した点Ｐ１と点Ｐ２Ｂとの距離と、３次元空間上のマーカー間の実距離の値との誤差を算出する。
【００３０】
続いて、実施例１に係る画像処理装置は、符号パターン（＋、＋）について算出した誤差と、符号パターンである（＋、−）について算出した誤差とを比較する。比較の結果、実施例１に係る画像処理装置は、符号パターン（＋、＋）について算出した誤差の値の方が小さい場合には、距離Ｌ０、距離Ｌ１、距離Ｌ２Ａを、カメラ原点から３次元空間上のマーカーまでの距離に決定する。
【００３１】
実施例１に係る画像処理装置は、上述した符号パターンについての誤差の算出をする処理において、基準点以外に選択した２点間の距離と実距離との誤差が最小となるときの基準点までの距離および基準点以外の点までの距離を検出する処理を行っている。例えば、対応点Ｐ１と対応点Ｐ２との間の距離と実距離との誤差が最小となるときの基準点までの距離および基準点以外の点までの距離を検出する処理は、以下に示す式（３）で表される。なお、以下の式（３）に示すＤ１，２は、上述した図１に示すＤ（１，２）に対応する。以下の式（３）に示すＬ２は、Ｌ２ＡやＬ２Ｂなどに対応する。また、例えば‖Ｖ‖は、ベクトルＶの長さを表わす。また、例えば｜ａ｜は、実数ａの絶対値を表わす。
【００３２】
【数３】
【００３３】
上述してきたように、実施例１に係る画像処理装置は、まず、上述した符号パターンについて高々２種類の符号の選択を行う。そして、実施例１に係る画像処理装置は、選択の結果残された符号パターンについて、基準点以外に選択した２点間の距離と実距離との誤差が最小となるときの基準点までの距離および基準点以外の点までの距離の検出を実行する。このようにして、実施例１に係る画像処理装置は、４個以上のマーカーが映し出された画像平面上の点に対応する３次元空間上の対応点の位置を迅速かつ精度良く算出する。
【００３４】
［画像処理装置の構成（実施例１）］
図２は、実施例１に係る画像処理装置の構成を示す機能ブロック図である。なお、図２には、実施例１に係る画像処理装置の機能を説明する上で必要となる機能ブロックを図示する。以下の実施例中において、単に原点Ｃと表記するものはカメラ座標系の原点を示し、単に対応点と表記するものはカメラ座標系における３次元位置を示すものとする。
【００３５】
図２に戻り、実施例１に係る画像処理装置２００は、例えば、撮影装置１００が接続される。撮影装置１００は、例えば、単眼カメラやステレオカメラなどに該当する。撮影装置１００は、例えば、車両やロボットなどの周囲の実空間上の所定位置に配置された４個のマーカーを撮影する。なお、以下の実施例中、カメラと表記する箇所は撮影装置１００に該当するものとする。
【００３６】
画像処理装置２００は、図２に示すように、取得部２１０、記憶部２２０および制御部２３０を有する。取得部２１０は、撮影装置１００により撮影されたマーカーの画像をそれぞれ取得する。取得部２１０は、撮影装置１００から取得したマーカーの画像を画像記憶部２２１に格納する。
【００３７】
記憶部２２０は、図２に示すように、画像記憶部２２１、カメラパラメータ記憶部２２２およびマーカー距離情報記憶部２２３を有する。なお、記憶部２２０は、例えば、ＲＡＭ（Random Access Memory）やフラッシュメモリ(flash memory)などの半導体メモリ素子である。
【００３８】
画像記憶部２２１は、例えば、取得部２１０により格納されたマーカーの画像をそれぞれ記憶する。カメラパラメータ記憶部２２２は、カメラの焦点距離および光軸位置の情報を含むカメラのパラメータの情報、いわゆるカメラ内部パラメータの情報を記憶する。カメラ内部パラメータの情報は、後述する生成部２３１による視線ベクトルの生成時に用いられる。マーカー距離情報記憶部２２３は、車両やロボットなど周囲に配置されたマーカー間の３次元空間上の実距離の情報をそれぞれ記憶する。なお、記憶部２２０は、さらに距離Ｌｎの値が取り得る範囲を記憶する領域を有する。なお、距離Ｌｎの値が取り得る範囲は予め記憶部に記憶されていても良いし、後述の処理を行う際に、利用者によって入力された数値範囲が記憶部２２０に記憶するとしても良い。また、距離Ｌｎの値が取り得る範囲は、予めカメラの設置位置が予測される環境において、予測値の前後の範囲が設定される。例えば、カメラを備える車体を、マーカーを有する試験場に停車し、詳細なカメラの位置や姿勢を演算する場合においては、試験場における車体を停止する位置や、設計上の車種ごとのカメラ取り付け位置などから推測された数値範囲が設定される。
【００３９】
図２に戻り、制御部２３０は、生成部２３１と、設定部２３２と、作成部２３３と、導出部２３４と、棄却部２３５と、算出部２３６と、決定部２３７と、位置姿勢推定部２３８とを有する。なお、制御部２３０は、例えば、電子回路や集積回路に該当する。電子回路としては、例えば、ＣＰＵ（Central Processing Unit）やＭＰＵ（Micro Processing Unit）がある。また、集積回路としては、例えば、ＡＳＩＣ（Application Specific Integrated Circuit）やＦＰＧＡ（Field Programmable Gate Array)などがある。
【００４０】
生成部２３１は、カメラからマーカーの各々に対応する視線ベクトルを生成する。例えば、生成部２３１は、画像記憶部２２１から４個のマーカーの画像を取得し、カメラの焦点距離および光軸位置の情報を含むカメラ内部パラメータをカメラパラメータ記憶部２２２から取得する。そして、生成部２３１は、４点のマーカーの画像の画像平面上における４個のマーカーそれぞれの位置の情報を取得する。続いて、生成部２３１は、カメラ内部パラメータと、マーカーの画像における４個のマーカーの各々の位置の情報とに基づいて、カメラから４個のマーカーの各々に対応する視線ベクトルを生成する。例えば、視線ベクトルＶｉ（ｉ＝０，１，２，３）は、カメラ内部パラメータを与える正則な３次元正方行列「Ｋ」と、画像の座標「ｘｉ」と、ｘｉの同次座標表現「ｘ´ｉ」を用いて、以下に示す式（４）で表わすことができる。以下に示す式（４）では、視線ベクトルＶｉの大きさが「１」に正規化されている。なお、以下の式（４）に示すＫ−１は、３次元正方行列「Ｋ」の逆行列である。
【００４１】
【数４】
【００４２】
なお、生成部２３１により生成された視線ベクトルを用いて、４個のマーカーが映し出された画像平面上の点に対応するカメラ座標系における仮想的な３次元位置（対応点）は、以下の式（５）で表すことができる。以下の式（５）は、視線ベクトルの延長線上に対応点が位置することを意味している。以下の説明において、単に「対応点」と表記するものは、マーカーが映し出された画像平面上の点に対応するカメラ座標系における仮想的な３次元位置を表すものとする。なお、４個のマーカーが映し出された画像平面上の点に対応する対応点は、以下に説明する設定部２３２〜決定部２３７による処理の結果を用いることにより得ることができる。また、生成部２３１は、マーカーが映し出される画像平面上の各点の位置と、３次元空間上のマーカーの位置との対応関係の情報を予め有するものとする。
【００４３】
【数５】
【００４４】
図３に、生成部２３１が生成する視線ベクトルの一例を示す。図３は、実施例１に係る生成部の説明に用いる図である。図３に示すように、生成部２３１は、カメラ座標系の原点Ｃから対応点の一つである点０に対応するベクトルＶ０、およびカメラ座標系の原点Ｃから対応点の一つである点１に対応するベクトルＶ１をそれぞれ生成する。同様に、図３には示していないが、生成部２３１は、カメラ座標系の原点Ｃから対応点の一つである点２に対応するベクトルＶ２、およびカメラ座標系の原点Ｃから対応点の一つである点３に対応するベクトルＶ３をそれぞれ生成する。
【００４５】
図２に戻り、設定部２３２は、４個のマーカーが映し出された画像平面上の４個に対応する仮想的な４つの対応点の中から基準とするマーカー（以後、基準マーカーと表記する）に対応する点である基準点Ｍ（点０〜点３のいずれか）を選択する。また、設定部２３２は、基準点Ｍまでの距離Ｌ０に所定の値「Ｌ」を設定する。なお、距離Ｌ０に設定される所定の値「Ｌ」は、カメラの設置位置およびマーカーの配置位置から予め推定される。
【００４６】
図２に戻り、作成部２３３は、基準点Ｍ以外の点、つまり基準マーカー以外のマーカーに対応する点１〜点３までの各距離Ｌ１，Ｌ２，Ｌ３を表す数式を作成する。距離Ｌ１，Ｌ２，Ｌ３は、上述した式（２）のように、余弦定理を用いて定式化して表すことができる。図４に、作成部２３３により作成される数式から得られる対応点までの距離の一例を示す。図４は、実施例１に係る作成部の説明に用いる図である。図４に示す点１（＋）は、距離Ｌ１を表す数式の根号前の符号のうちプラスを選択した場合の距離に相当する。図４に示す点１（−）は、距離Ｌ１を表す数式の根号前の符号のうちマイナスを選択した場合の距離に相当する。
【００４７】
図４に示すように、基準点として点０を選択した場合、カメラ座標系の原点Ｃから点１までの距離として、上述した式（２）から、点Ｃから点１（＋）までの距離と点Ｃから点１（−）までの距離の２つの候補が得られる。言い換えれば、カメラ座標系の原点Ｃから点１までの距離は、基準マーカー以外のマーカーに対応する点１〜点３までの各距離Ｌ１，Ｌ２，Ｌ３を表す数式によって、高々２つの候補に限定することができる。
【００４８】
図２に戻り、導出部２３４は、設定部２３２により設定された基準マーカーで採用する符号パターンを全て導出する。図５には、対応点０〜３のうち点０を基準点とした場合に導出される符号パターンを示す。図５は、実施例１に係る導出部の説明に用いる図である。図５に示すように、導出部２３４は、（＋、＋、＋）、（＋、＋、−）、（＋、−、＋）、（＋、−、−）、（−、＋、＋）、（−、＋、−）、（−、−、＋）、（−、−、−）の８つの符号パターンを導出する。
【００４９】
また、導出部２３４は、作成部２３３により作成された各数式の根号内の値を算出し、各数式の根号の値が全て実数であるか否かを判定する。判定の結果、各数式の根号の値の全てが実数である場合には、導出部２３４は、符号パターンを棄却部２３５に送出する。一方、判定の結果、各数式の根号の値の全てが実数ではない場合には、導出部２３４は、後述する決定部２３７に対して処理の終了を送出する。なお、この実施例では、導出部２３４により、各数式の根号の値の全てが実数であるか否かを判定する場合を説明したが、これに限定されるものではなく、例えば、作成部２３３が各数式の根号の値の全てが実数であるか否かを判定してもよい。
【００５０】
棄却部２３５は、導出部２３４により導出された符号パターンの中から、距離Ｌ１、Ｌ２、Ｌ３の少なくとも１つが相応しくない符号パターンを棄却する。まず、棄却部２３５は、基準点Ｍ以外の対応点Ｎを一つ選択する。例えば、点０を基準点Ｍとして選択した場合には、点１〜３のいずれか一つを対応点Ｎとして選択する。次に、棄却部２３５は、原点Ｃから基準点Ｍまでの視線ベクトル、および原点Ｃから対応点Ｎまでの視線ベクトルを生成部２３１から取得する。
【００５１】
続いて、棄却部２３５は、基準点Ｍに対応する視線ベクトルと、選択した対応点Ｎに対応する視線ベクトルとの内積を計算し、視線ベクトルのなす角が９０度よりも大きいか否かを判定する。例えば、棄却部２３５は、計算した内積の値が負の値である場合には、視線ベクトルのなす角が９０度よりも大きいと判定する。判定の結果、棄却部２３５は、視線ベクトルのなす角が９０度よりも大きい場合には、選択した対応点Ｎについてはプラスの符号のみを取り得ることを記録する。ここで、記録するとは、棄却部２３５が、内部的に有する記憶部に、対応点Ｎについてはプラスの符号のみを取り得ることを示すデータを保存することを意味する。
【００５２】
一方、判定の結果、棄却部２３５は、視線ベクトルのなす角が９０度以下である場合には、選択した対応点Ｎについてはプラス、マイナスの両方の符号を取り得ることを記録する。ここで、記録するとは、棄却部２３５が、内部的に有する記憶部に、対応点Ｎについてはプラス、マイナスの両方の符号を取り得ることを示すデータを保存することを意味する。そして、棄却部２３５は、対応点の中から、まだ選択していない他の全ての点についても上述してきたのと同様の処理を行い、全ての対応点が取り得る符号の組合せを列挙することにより符号パターンを決定する。
【００５３】
以下、図６を用いて符号パターンの決定方法の一例を説明する。図６は、実施例１に係る棄却部の処理に関する概念的な説明に用いる図である。棄却部２３５は、図６に示す基準点０以外の点１〜３の中から点１を選択する。次に、棄却部２３５は、基準点０に対応する視線ベクトルＶ０、および点１に対応する視線ベクトルＶ１を生成部２３１から取得する。続いて、棄却部２３５は、視線ベクトルＶ０と視線ベクトルＶ１との内積を計算し、視線ベクトルＶ０と視線ベクトルＶ１とのなす角が９０度よりも大きいか否かを判定する。判定の結果、棄却部２３５は、視線ベクトルＶ０と視線ベクトルＶ１とのなす角が９０度よりも大きい場合には、選択した点１ではプラスの符号のみを取り得ることを記録する。一方、判定の結果、棄却部２３５は、視線ベクトルのなす角が９０度以下である場合には、選択した点１ではプラス、マイナスの両方の符号を取り得ることを記録する。
【００５４】
そして、棄却部２３５は、対応点である点１〜点３の中から、まだ選択していない点２および点３についても上述してきたのと同様の処理を行い、点１〜点３が取り得る符号の組合せを列挙することにより符号パターンを決定する。例えば、棄却部２３５は、点１〜点３が取り得る符号の組合せを列挙する。その結果、棄却部２３５は、図６に示す符号パターンのうち、（−、＋、＋）、（−、＋、−）、（−、−、＋）、（−、−、−）を棄却し、（＋、＋、＋）、（＋、＋、−）、（＋、−、＋）、（＋、−、−）を点１〜点３が取り得る符号の組合せとして残す。よって、棄却部２３５は、棄却されずに残された４つの符号パターン、図６に示す（＋、＋、＋）、（＋、＋、−）、（＋、−、＋）、（＋、−、−）の４つの符号パターンを点１〜点３が取り得る符号パターンとして決定する。
【００５５】
図２に戻り、算出部２３６は、棄却部２３５により決定された各符号パターンについて、基準点以外の各対応点間の距離の候補値と３次元空間上の実距離の値との誤差を演算し、各符号パターンについて最小誤差を算出する。
【００５６】
まず、算出部２３６は、後に用いる最小誤差Ｊの値を初期化する。棄却部２３５により決定された各符号パターンの中から符号パターンを一つ選択し、選択した符号パターンについて距離Ｌ１、Ｌ２およびＬ３の値をそれぞれ算出する。算出部２３６は、設定部２３２により設定された基準点０までの距離Ｌ０の値「Ｌ」と、作成部２３３により作成された数式とを用いて、距離Ｌ１、Ｌ２およびＬ３の値を算出する。
【００５７】
次に、算出部２３６は、距離Ｌ１、Ｌ２およびＬ３の値が正の実数であるか否かを判定する。なお、正の実数とは、実数のうち正の値を意味する。算出部２３６は、判定の結果、距離Ｌ１、Ｌ２およびＬ３の値が正の実数ではない場合には、次の符号パターンを選択する。これとは反対に、算出部２３６は、判定の結果、距離Ｌ１、Ｌ２およびＬ３の値が正の実数である場合には、誤差Ｅの値を、例えば、「０」に初期化する。なお、後に説明するが、誤差Ｅの値は、所定の条件を満足する場合に、基準点０以外の２点の組合せについて算出される誤差の値を加算された値となる。
【００５８】
続いて、算出部２３６は、選択した符号パターン内で、基準点０以外の２点の組合せを一つ取得する。次に、算出部２３６は、取得した２点の組合せについて２点間の距離の候補値を算出する。続いて、算出部２３６は、算出した候補値に対応する３次元空間上のマーカー間の実距離の値をマーカー距離情報記憶部２２３から取得し、この候補値と、この候補値に対応する実距離の値との誤差を演算する。そして、算出部２３６は、算出した誤差の値を誤差Ｅの値に加算して更新し、誤差Ｅの値が最小誤差Ｊの値よりも大きいか否かを判定する。算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値よりも大きい場合には、棄却部２３５により決定された各符号パターンの中から次の符号パターンを選択する。
【００５９】
一方、算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値以下である場合には、選択した符号パターン内で基準点０以外の２点の組合せの全てについて処理を完了したか否かを判定する。算出部２３６は、判定の結果、選択した符号パターン内で基準点０以外の２点の組合せの全てについて処理を完了している場合には、誤差Ｅの値で最小誤差Ｊの値を更新し、棄却部２３５により決定された各符号パターンの中から次の符号パターンを一つ選択する。これとは反対に、算出部２３６は、判定の結果、選択した符号パターン内で基準点０以外の２点の組合せの全てについて処理を完了していない場合には、選択した符号パターン内で、基準点０以外の２点の組合せの中から次の組合せを一つ取得する。以後、算出部２３６は、取得した２点の組合せについて上述したのと同様に処理を行う。
【００６０】
そして、算出部２３６は、全符号パターン内について処理を完了した場合には、「基準点＝点０」および「距離Ｌ０＝Ｌ」に関する処理結果を決定部２３７に送出する。例えば、算出部２３６は、最終的に残った誤差Ｅの値、すなわち該当符号パターン内の最小誤差の値と、最小誤差に対応する符号パターンの距離Ｌ０、Ｌ１、Ｌ２およびＬ３の値を処理結果として送出する。
【００６１】
図７〜図１０を用いて、算出部２３６による処理を概念的に説明する。なお、図７〜図１０では、誤差Ｅの値を初期化した後の算出部２３６の処理について説明する。図７〜図１０は、実施例１に係る算出部の処理に関する概念的な説明に用いる図である。
【００６２】
まず、算出部２３６は、図７に示す各符号パターン（＋、＋、＋）、（＋、＋、−）、（＋、−、＋）、（＋、−、−）の中から、例えば、符号パターン（＋、＋、＋）を選択する。次に、算出部２３６は、選択した符号パターン（＋、＋、＋）内で、基準点０以外の２点の組合せ（点１，点２）を取得する。続いて、算出部２３６は、２点の組合せ（点１，点２）について２点間の距離の候補値を算出する。次に、算出部２３６は、算出した候補値に対応する３次元空間上のマーカー間の実距離の値をマーカー距離情報記憶部２２３から取得し、この候補値と、この候補値に対応する実距離の値との誤差を算出する。続いて、算出部２３６は、算出した誤差の値を誤差Ｅの値に加算して更新し、誤差Ｅの値が最小誤差Ｊの値よりも大きいか否かを判定する。算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値以下である場合には、符号パターン（＋、＋、＋）内で基準点０以外の２点の組合せの全て、（点１，点２）、（点１，点３）および（点２，点３）について処理を完了したか否かを判定する。
【００６３】
算出部２３６は、判定の結果、基準点０以外の２点の組合せの全てについて処理を完了していない場合には、同一の符号パターン（＋、＋、＋）内で基準点０以外の２点の組合せ、例えば（点１，点３）を取得する。そして、算出部２３６は、上述してきた基準点０以外の２点の組合せ（点１，点２）に対して実行した処理と同様の手順で処理を行う。すなわち、算出部２３６は、２点の組合せ（点１，点３）について２点間の距離の候補値を算出し、算出した候補値に対応する３次元空間上の実距離の値を取得し、この候補値と実距離の値との誤差を算出する。続いて、算出部２３６は、算出した誤差の値を誤差Ｅの値に加算して更新し、誤差Ｅの値が最小誤差Ｊの値よりも大きいか否かを判定する。算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値以下である場合には、誤差Ｅの値を最小誤差Ｊの値に加算して更新し、再び、符号パターン（＋、＋、＋）内で基準点０以外の２点の組合せの全てについて処理を完了したか否かを判定する。
【００６４】
なお、算出部２３６により、最初に選択された符号パターン（＋、＋、＋）については、最小誤差Ｊの値が無限大に設定されているので、基準点０以外の２点の組合せの全てについての処理が必ず完了される。よって、図７に示すように、符号パターン（＋、＋、＋）についての誤差Ｅの値「Ｅ０００」が得られる。この誤差Ｅの値「Ｅ０００」は、２点の組合せ（点１，点２）についての誤差の値と、２点の組合せ（点１，点３）についての誤差の値と、２点の組合せ（点２，点３）についての誤差の値が加算されたものに相当する。そして、この誤差Ｅの値「Ｅ０００」は、符号パターン（＋、＋、＋）の後に実行される他の符号パターンに対する処理の中で最小誤差Ｊの値として利用される。このため、上述した手順で処理を行えば、各符号パターンについて算出された誤差の値の中で最も小さい誤差の値を得ることが可能となる。
【００６５】
ここで、符号パターン（＋、＋、＋）内で基準点０以外の２点の組合せの全てについて処理を完了したか否かの判定結果が上述した判定結果と異なり、基準点０以外の２点の組合せの全てについて処理を完了している場合について説明する。算出部２３６は、判定の結果、基準点０以外の２点の組合せの全てについて処理を完了している場合には、誤差Ｅの値で最小誤差Ｊの値を更新する。次に、算出部２３６は、図７に示す各符号パターンの中から次の符号パターン、例えば、符号パターン（＋、＋、−）を選択する。そして、算出部２３６は、上述してきた符号パターン（＋、＋、＋）に対して実行した処理と同様の手順で、符号パターン（＋、＋、−）に対し処理を行う。
【００６６】
ここで、上述した誤差Ｅの値が最小誤差Ｊの値よりも大きいか否かの判定結果が上述した判定結果と異なり、誤差Ｅの値が最小誤差Ｊの値よりも大きい場合について説明する。算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値よりも大きい場合には、現在選択中の符号パターンに対する処理を取りやめ、図７に示す各符号パターンの中から次の符号パターンを選択する。そして、算出部２３６は、上述してきた符号パターン（＋、＋、＋）に対して実行した処理と同様の手順で処理を行う。
【００６７】
例えば、算出部２３６は、符号パターン（＋、＋、−）の処理について、誤差Ｅの値が最小誤差Ｊの値よりも大きいという判定結果が得られた場合には、符号パターン（＋、＋、−）の処理を取りやめ、例えば、未選択の符号パターン（＋、−、−）を選択する。次に、算出部２３６は、符号パターン（＋、−、−）内で基準点０以外の２点の組合せ（点１，点２）を取得する。続いて、算出部２３６は、２点の組合せ（点１，点２）について２点間の距離の候補値を算出し、算出した候補値に対応する３次元空間上の実距離の値を取得し、この候補値と実距離の値との誤差を算出する。次に、算出部２３６は、算出した誤差の値を誤差Ｅの値に加算して更新し、誤差Ｅの値が最小誤差Ｊの値よりも大きいか否かを判定する。算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値以下である場合には、上述してきたのと同様に、符号パターン（＋、−、−）内で基準点０以外の２点の組合せの全てについて処理を完了したか否かを判定する。以後、算出部２３６は、上述してきた同様の手順で処理を行う。一方、算出部２３６は、誤差Ｅの値が最小誤差Ｊの値よりも大きい場合には、誤差Ｅの値を最小誤差Ｊの値に加算して更新し、再び、符号パターン（＋、−、−）内で基準点０以外の２点の組合せの全てについて処理を完了したか否かを判定する。以下、算出部２３６は、上述してきた同様の手順で処理を行う。
【００６８】
上述してきた手順で、算出部２３６は、図７に示す各符号パターンについての演算を行うことにより、図８に示すように、誤差「Ｅ０００」、「Ｅ００１」、「Ｅ００２」、「Ｅ００３」の中で最も値の小さい最小誤差（ｍｉｎＥ０００〜Ｅ００３）の値を得る。そして、算出部２３６は、「基準点＝点０」および「距離Ｌ０＝Ｌ」に関し、最小誤差（ｍｉｎＥ０００〜Ｅ００３）の値と、これに対応するＬ０、Ｌ１、Ｌ２およびＬ３の値を処理結果として後述する決定部２３７に送出する。
【００６９】
また、図９に示すように、算出部２３６は、基準点０までの距離の値として、「Ｌ」とは異なる値「Ｌ＋ΔＬ」が設定部２３２により設定されると、「基準点＝点０」および「距離Ｌ０＝Ｌ＋ΔＬ」に関し、上述してきた手順で処理を実行する。そして、算出部２３６は、「基準点＝点０」および「距離Ｌ０＝Ｌ＋ΔＬ」に関し、最小誤差（ｍｉｎＥ０１０〜Ｅ０１３）の値と、これに対応するＬ０、Ｌ１、Ｌ２およびＬ３の値を処理結果として後述する決定部２３７に送出する。
【００７０】
また、図１０に示すように、算出部２３６は、点０とは異なる点１が新たな基準点として設定部２３２により設定されると、「基準点＝点１」および「距離Ｌ０＝Ｌ´」に関し、上述してきた手順で処理を実行する。そして、算出部２３６は、「基準点＝点１」および「距離Ｌ０＝Ｌ´」に関し、最小誤差ｍｉｎ（Ｅ０２０〜Ｅ０２ｖ：ｖは自然数）の値と、これに対応するＬ０、Ｌ１、Ｌ２およびＬ３の値を処理結果として送出する。また、算出部２３６は、図１０には示していないが、基準点１までの距離の値として、「Ｌ´」とは異なる値「Ｌ´＋ΔＬ´」が設定部２３２により設定されると、「基準点＝点１」および「距離Ｌ０＝Ｌ´＋ΔＬ´」に関し、上述してきた手順で処理を実行する。そして、算出部２３６は、「基準点＝点１」および「距離Ｌ０＝Ｌ´＋ΔＬ´」に関し、最小誤差の値と、これに対応するＬ０、Ｌ１、Ｌ２およびＬ３の値を処理結果として後述する決定部２３７に送出する。
【００７１】
なお、算出部２３６は、基準点までの距離の設定があるたびに、基準点および基準点までの距離に対応する各符号パターンに対応する誤差のうちの最小誤差の値を算出する場合を説明したが、これに限定されるものではない。例えば、算出部２３６は、同一の基準点に対する処理内では、最小誤差Ｊを初期化せずに更新を続け、同一の基準点内の各符号パターンの中で値が最も小さい最小誤差を算出するようにしてもよい。あるいは、算出部２３６は、全ての基準点に対する処理内で最小誤差Ｊを初期化せずに更新を続け、全ての基準点内の各符号パターンに対応する誤差のうちの最小誤差の値を算出するようにしてもよい。
【００７２】
図２に戻り、決定部２３７は、算出部２３６からの処理結果に基づいて、カメラ座標系の原点から、４個以上のマーカーが映し出された画像平面上の点に対応する３次元空間上の対応点の位置までの距離を決定する。
【００７３】
例えば、決定部２３７は、最小誤差の値と、これに対応するＬ０、Ｌ１、Ｌ２およびＬ３の値を処理結果として算出部２３６から取得すると、取得した処理結果を保存する。そして、決定部２３７は、基準点までの距離が取りうる値の全てを設定して処理を完了しているか否かを判定する。例えば、決定部２３７は、現在基準点０が選択されるとすると、基準点までの距離Ｌ０が取りうる値の全てを設定して処理を完了しているか否かを判定することとなる。判定の結果、距離Ｌ０が取りうる値の全てを設定して処理を完了していない場合には、決定部２３７は、設定部２３２に対して、距離Ｌ０の値として「Ｌ」とは異なる別の値を設定するように依頼する。
【００７４】
一方、決定部２３７は、判定の結果、距離Ｌ０が取りうる値の全てを設定して処理を完了している場合には、対応点である点０〜点３の全てを基準点として処理を完了しているか否かを判定する。例えば、決定部２３７は、現在基準点０が選択されるとすると、残りの対応点である点１〜点３の全てを基準点として処理を完了したか否かを判定することとなる。判定の結果、対応点である点０〜点３の全てを基準点として処理を完了していない場合には、決定部２３７は、設定部２３２に対して、新たな基準点および距離の設定を依頼する。
【００７５】
一方、決定部２３７は、判定の結果、対応点である点０〜点３の全てを基準点として処理を完了している場合には、算出部２３６から取得してこれまで保存した処理結果の中で、最小誤差の値が最も小さいものを特定する。例えば、決定部２３７は、図１０に示す最小誤差ｍｉｎ（Ｅ０００〜Ｅ００３）、ｍｉｎ（Ｅ０１０〜Ｅ０１３）、ｍｉｎ（Ｅ０２０〜Ｅ０２ｖ：ｖは自然数）等の中から、値が最も小さい最小誤差を特定する。そして、決定部２３７は、特定された最小誤差に対応付けられているＬ０、Ｌ１、Ｌ２およびＬ３の値を最適解として決定し、位置姿勢推定部２３８に送る。
【００７６】
図２に戻り、位置姿勢推定部２３８は、決定部２３７から最適解として取得したＬ０、Ｌ１、Ｌ２およびＬ３の値を用いて、画像平面上の点に対応するカメラ座標系における３次元位置を算出し、算出した位置を用いてカメラの位置および姿勢を算出する。以下に、図１１を用いて、実施例１に係る位置姿勢推定部２３８について説明する。図１１は、実施例１に係る位置姿勢推定部の説明に用いる図である。図１１に示すαはカメラ座標系を示す。また、図１１に示すβは世界座標系を示す。また、図１１に示すＷ０、Ｗ１、Ｗ２は、世界座標系におけるマーカーの３次元位置を示し、Ｗ０は点０、Ｗ１は点１、Ｗ２は点２にそれぞれ対応するものとする。また、カメラの位置および姿勢を算出する技術自体は公知技術であり、例えば、玉木 徹 他、「姿勢推定と回転行列」（電気情報通信学会研究報告、信学技報,vol.109, no.203, SIS2009-23, pp.59-64, 2009年9月）等を参照されたい。なお、以下では、説明を簡略化するために、マーカーが映し出された画像平面上の点に対応する３つの対応点（点０、点１および点２）を例に挙げて説明するが、対応点の数が４つでも同様に処理できる。
【００７７】
例えば、位置姿勢推定部２３８は、決定部２３７から最適解として取得したＬ０、Ｌ１およびＬ２の値と上述した式（５）とを用いて、画像平面上の点に対応するカメラ座標系における対応点Ｐ０、Ｐ１、Ｐ２を算出する。そして、位置姿勢推定部２３８は、カメラ座標系における３次元位置（Ｐ０、Ｐ１、Ｐ２）と、これらに対応する世界座標系におけるマーカーの位置（Ｗ０、Ｗ１、Ｗ２）とに基づいて、公知技術を利用してカメラの位置および姿勢を算出する。
【００７８】
［画像処理装置による処理（実施例１）］
次に、図１２〜図１４を用いて、実施例１に係る画像処理装置による処理の流れを説明する。図１２は、実施例１に係る最適解算出処理の流れを説明する。図１２は、実施例１に係る最適解算出処理の流れを示す図である。図１２に示す処理は、画像フレームが取得されるたびに実行される。
【００７９】
図１２に示すように、例えば、制御部２３０により取得部２１０による画像取得が検出されると（ステップＳ１０１，ＹＥＳ）、生成部２３１は、視線ベクトルを生成する（ステップＳ１０２）。なお、取得部２１０による画像取得が検出されない場合（ステップＳ１０１，ＮＯ）、制御部２３０は、取得部２１０による画像取得の検出判定を繰り返し実行する。
【００８０】
次に、設定部２３２は、各マーカーに対応する点の中から基準マーカーに対応する基準点Ｍ（点０〜点３のいずれか）を選択する（ステップＳ１０３）。続いて、導出部２３４は、設定部２３２により設定された基準マーカーで採用する符号パターンを全て導出する（ステップＳ１０４）。次に、設定部２３２は、カメラ座標系の原点Ｃから基準点Ｍまでの距離Ｌ０に所定の値を設定する（ステップＳ１０５）。続いて、作成部２３３は、カメラ座標系の原点Ｃから基準点Ｍ以外の各点（点１〜３）までの距離Ｌ１、Ｌ２およびＬ３の値を表す数式を作成する（ステップ１０６）。次に、画像処理装置２００により実行される処理は最適解候補算出処理に移行される（ステップＳ１０７）。最適解候補算出処理は、主に、算出部２３６によりが実行される。なお、最適解候補算出処理については、図１３を用いて後述する。
【００８１】
続いて、決定部２３７は、ステップＳ１０７における最適解候補算出処理の処理結果を取得すると、取得した処理結果を保存する（ステップＳ１０８）。次に、決定部２３７は、基準点Ｍまでの距離Ｌ０が取りうる値の全てを設定して処理を完了しているか否かを判定する（ステップＳ１０９）。
【００８２】
判定の結果、距離Ｌ０が取りうる値の全てを設定して処理を完了していない場合には（ステップＳ１０９，ＮＯ）、決定部２３７は、設定部２３２に対して、距離Ｌ０の値として「Ｌ」とは異なる値を設定するように依頼する。図１２に示す処理は、上述したステップＳ１０５に処理を戻す。
【００８３】
一方、決定部２３７は、判定の結果、距離Ｌ０が取りうる値の全てを設定して処理を完了している場合には（ステップＳ１０９，ＹＥＳ）、点０〜点３の全てを基準点として処理を完了しているか否かを判定する（ステップＳ１１０）。判定の結果、点０〜点３の全てを基準点として処理を完了していない場合には（ステップＳ１１０，ＮＯ）、決定部２３７は、上述したステップＳ１０３に処理を戻す。
【００８４】
一方、決定部２３７は、判定の結果、点０〜点３の全てを基準点として処理を完了している場合には（ステップＳ１１０，ＹＥＳ）、次のように処理する。すなわち、決定部２３７は、ステップＳ１０７にて保存された最小誤差の中から、最小誤差の値が最小のときのＬ０、各Ｌｉ（ｉ＝１〜３）の値を最適解として決定し、位置姿勢推定部２３８に送出する（ステップＳ１１１）。以上で、実施例１に係る最適解算出処理が完了するが、上述してきた図１２に示す最適解算出処理は、処理内容に矛盾を生じさせない範囲でその順序を入れ替えることができる。
【００８５】
なお、上述したステップＳ１０９およびステップＳ１１０の判定処理により、基準点までの距離Ｌの最適化を行われることとなり、図１２に示す処理の内容は以下に示す式（６）で表すことができる。なお、以下の式（６）に示す「ｉ０」は基準点を示し、「Ｌｉ０」は基準点までの距離を示す。
【００８６】
【数６】
【００８７】
続いて、図１３を用いて、実施例１に係る最適解候補算出処理の流れを説明する。図１３は、実施例１に係る最適解候補算出処理の流れを示す図である。図１３に示すように、導出部２３４は、図１２に示すステップＳ１０６にて作成された全数式の根号の値を算出し、全数式の根号の値が実数であるか否かを判定する（ステップＳ２０１）。判定の結果、根号の値が実数ではない場合には（ステップＳ２０１，ＮＯ）、導出部２３４は、図１２に示すステップＳ１０９に処理を戻す。
【００８８】
一方、根号の値が実数であると導出部２３４により判定された場合には（ステップＳ２０１，ＹＥＳ）、画像処理装置２００により実行される処理は符号パターン決定処理に移行される（ステップＳ２０２）。符号パターン決定処理は、棄却部２３５により実行される。符号パターン決定処理については、図１４を用いて後述する。
【００８９】
続いて、算出部２３６は、後に用いる最小誤差Ｊの値を初期化し（ステップＳ２０３）、ステップＳ２０３に示す符号パターン決定処理により得られる各符号パターンについて、以下のステップＳ２０４〜ステップＳ２１５までの処理を実行する。なお、ステップＳ２０４〜ステップＳ２１５は、全符号パターンについてステップＳ２０５〜ステップＳ２１４の処理が完了するまで繰り返し実行することを意味する。算出部２３６は、符号パターンを一つ選択し（ステップＳ２０４）、選択した符号パターンについて距離Ｌ１、Ｌ２およびＬ３の値をそれぞれ算出する（ステップＳ２０５）。次に、算出部２３６は、距離Ｌ１、Ｌ２およびＬ３の値が正の実数であるか否かを判定する（ステップＳ２０６）。判定の結果、算出部２３６は、距離Ｌ１、Ｌ２およびＬ３の値が正の実数ではない場合には（ステップＳ２０６，ＮＯ）、次の符号パターンを選択する。これとは反対に、算出部２３６は、判定の結果、距離Ｌ１、Ｌ２およびＬ３の値が正の実数である場合には（ステップＳ２０６，ＹＥＳ）、誤差Ｅの値を初期化（Ｅ＝０）する（ステップＳ２０７）。
【００９０】
続いて、算出部２３６は、ステップＳ２０４にて選択した符号パターン内で基準点Ｍ以外の２点（点１〜３のうちの２点）の組合せを一つ取得する（ステップＳ２０８）。次に、算出部２３６は、取得した２点の組合せについて２点間の距離の候補値を算出する（ステップＳ２０９）。続いて、算出部２３６は、算出した候補値に対応する３次元空間上のマーカー間の実距離の値をマーカー距離情報記憶部２２３から取得し、この候補値と、この候補値に対応する実距離の値との誤差を演算する（ステップＳ２１０）。そして、算出部２３６は、算出した誤差の値を誤差Ｅの値に加算して更新し（ステップＳ２１１）、誤差Ｅの値が最小誤差Ｊの値よりも大きいか否かを判定する（ステップＳ２１２）。
【００９１】
算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値以下である場合には（ステップＳ２１２，ＮＯ）、選択した符号パターン内で基準点０以外の２点の組合せの全てについて処理を完了したか否かを判定する（ステップＳ２１３）。算出部２３６は、判定の結果、選択した符号パターン内で基準点０以外の２点の組合せの全てについて処理を完了している場合には（ステップＳ２１３，ＹＥＳ）、誤差Ｅの値で最小誤差Ｊの値を更新する（ステップＳ２１４）。続いて、ステップＳ２０２の符号パターン決定処理により得られる全符号パターンについてステップＳ２０５〜ステップＳ２１４の処理が完了するまで（ステップＳ２１５）、上述したステップＳ２０２に処理を戻す。
【００９２】
ここで、ステップＳ２１３の説明に戻る。算出部２３６は、判定の結果、選択した符号パターン内で基準点０以外の２点の組合せの全てについて処理を完了していない場合には（ステップＳ２１３，ＮＯ）、上述したステップＳ２０８に処理を戻す。
【００９３】
ここで、ステップＳ２１２の説明に戻る。算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値よりも大きい場合には（ステップＳ２１３，ＹＥＳ）、次のように処理を実行する。すなわち、算出部２３６は、ステップＳ２０２の符号パターン決定処理により得られる全符号パターンについてステップＳ２０５〜ステップＳ２１４の処理が完了するまで（ステップＳ２１５）、上述したステップＳ２０２に処理を戻す。
【００９４】
そして、算出部２３６は、ステップＳ２０４〜ステップＳ２１５までの処理を完了した場合には、次のステップＳ２１６の処理を行う。すなわち、算出部２３６は、「基準点＝Ｍ（点０〜点３のいずれか）」に関し、最終的に残った誤差Ｅの値、すなわち最小誤差の値と、最小誤差に対応する符号パターンの距離Ｌ０、Ｌ１、Ｌ２およびＬ３の値を処理結果として送出する（ステップＳ２１６）。以上で、実施例１に係る最適解候補算出処理が完了するが、上述してきた図１３に示す最適解算出処理は、処理内容に矛盾を生じさせない範囲でその順序を入れ替えることができる。
【００９５】
なお、上述したステップＳ２１０で用いるマーカー間の実距離の値は、以下の式（７）で表すことができる。
【００９６】
【数７】
【００９７】
なお、上述したステップＳ２０５で算出される距離Ｌ１、Ｌ２およびＬ３の値は、以下に示す式（８）の解を求めることにより算出することができる。
【００９８】
【数８】
【００９９】
なお、上述したステップＳ２１０で算出される誤差の値は、以下の式（９）で表すことができる。
【０１００】
【数９】
【０１０１】
なお、上述したステップＳ２１１の処理は、以下の式（１０）で表すことができる。
【０１０２】
【数１０】
【０１０３】
次に、図１４を用いて、実施例１に係る符号パターン決定処理の流れを説明する。図１４は、実施例１に係る符号パターン決定処理の流れを示す図である。図１４に示すように、符号パターン決定処理では、基準点Ｍ以外の全ての対応点Ｎ（点１〜点３）について、ステップＳ３０１〜ステップＳ３０５までの処理が繰り返し実行される。
【０１０４】
まず、棄却部２３５は、基準点Ｍ以外の対応点Ｎ（点１〜点３のいずれか）を一つ取得し（ステップＳ３０１）、∠ＭＣＮが９０度よりも大きいか否かを判定する（ステップＳ３０２）。なお、∠ＭＣＮとは、原点Ｃ及び基準点Ｍを結ぶ線分と、原点Ｃ及び対応点Ｎを結ぶ線分とがなす角度をいう。∠ＭＣＮが９０度よりも大きいか否かの判定は、上述したように、原点Ｃ及び基準点Ｍを結ぶ線分上の視線ベクトルと、原点Ｃ及び対応点Ｎを結ぶ線分上の視線ベクトルとの内積に基づいて行なう。
【０１０５】
棄却部２３５は、判定の結果、∠ＭＣＮが９０度よりも大きい場合には（ステップＳ３０２，ＹＥＳ）、選択した対応点Ｎの一点ではプラスの符号のみを取り得ることを記録する（ステップＳ３０３）。一方、棄却部２３５は、判定の結果、∠ＭＣＮが９０度以下である場合には（ステップＳ３０２，ＮＯ）、選択した対応点Ｎの一点ではプラス、マイナスの両方の符号を取り得ることを記録する（ステップＳ３０４）。
【０１０６】
そして、棄却部２３５は、基準点Ｍ以外の全ての対応点Ｎについて、ステップＳ３０１〜ステップＳ３０５までの処理を終えると、対応点Ｎが取り得る符号の組合せを列挙して符号パターンを決定する（ステップＳ３０６）。以上で、実施例１に係る符号パターン決定処理が完了する。
【０１０７】
上述してきた図１２〜図１４の処理の意味は、以下の数式（１１）で表すことができる。なお、以下に示す式（１１）の左辺のＪは、上述した式（６）のＪ、つまり、Ｌｉを符号選択によって求めることを含んだ誤差評価関数である。そして、式（１１）から得られる解は、基準点「ｉ０」までの距離を「Ｌｉ０」とした時に、基準点「ｉ０」以外の点ｉまでの距離「Ｌｉ」を符号選択により最適に選んだ場合に残っている誤差となる。よって、式（１１）は、Ｊを最小化する解を探索することにより、基準点「ｉ０」までの最適な距離「Ｌｉ０」と、基準点「ｉ０」以外の点ｉまでの最適な距離「Ｌｉ」を同時に得ることができることを表している。
【０１０８】
【数１１】
【０１０９】
［実施例１による効果］
上述してきたように、実施例１に係る画像処理装置２００は、導出された符号パターンの中から、距離Ｌ１、Ｌ２、Ｌ３の値の少なくとも１つが相応しい値となり得えない符号パターンを棄却する。よって、実施例１に係る画像処理装置２００は、４個以上のマーカーが映し出された画像平面上の点に対応する３次元空間上の対応点の位置を迅速かつ精度良く算出できる。この結果、実施例１によれば、カメラの取り付け位置や取り付け姿勢の情報を迅速かつ精度良く得ることができる。なお、３点の場合は、最少誤差を与える解の組合せである最適解が１つに定まらない。このため、上述してきた実施例１は、４点以上のマーカーを用いて、画像処理装置が最適解算出処理を行う。この結果、実施例１では、最適解を得ることができる。
【０１１０】
また、実施例１に係る画像処理装置は、例えば、図１３に示すように、符号パターンの誤差を算出するときに、従前に算出された符号パターンの誤差にうちの最小誤差よりも誤差が大きくなる符号パターンの処理は取りやめて、次の符号パターンの処理を行う。このため、実施例１によれば、４個以上のマーカーが映し出された画像平面上の点に対応する３次元空間上の対応点の位置をより迅速に算出できる。この結果、実施例１によれば、カメラの取り付け位置や取り付け姿勢の情報をより迅速に得ることができる。
【０１１１】
また、実施例１に係る画像処理装置は、基準点Ｍ以外の点、つまり基準マーカー以外のマーカーに対応する点１〜点３までの各距離Ｌ１，Ｌ２，Ｌ３を表す数式の中の根号内が虚数となる場合には、基準点までの距離や基準点の設定をやり直す。このため、実施例１によれば、４個以上のマーカーが映し出された画像平面上の点に対応する３次元空間上の対応点の位置をより迅速に算出できる。この結果、実施例１によれば、カメラの取り付け位置や取り付け姿勢の情報をより迅速に得ることができる。
【実施例２】
【０１１２】
以下、本願の開示する画像処理プログラムおよび画像処理装置の他の実施形態を説明する。
【０１１３】
（１）装置構成等
例えば、図２に示した画像処理装置２００の機能ブロックの構成は概念的なものであり、必ずしも物理的に図示の如く構成されていることを要しない。例えば、図２に示す制御部２３０を、図１５に示すように、カメラ座標系制御ユニット２３０Ａと位置姿勢推定ユニット２３０Ｂとに機能的または物理的に分散してもよい。このように、画像処理装置２００の機能ブロックの全部または一部を、各種の負荷や使用状況などに応じて、任意の単位で機能的または物理的に分散・統合して構成することができる。図１５は、実施例２に係る画像処理装置の構成を示す機能ブロック図である。
【０１１４】
（２）画像処理プログラム
また、上述の実施例１にて説明した画像処理装置２００により実行される各種の処理は、例えば、車両に搭載されるＥＣＵ（Electronic Control Unit）に実装されたマイコンなどの電子機器で所定のプログラムを実行することによって実現することもできる。あるいは、上述の実施例１にて説明した画像処理装置２００により実行される各種の処理は、ロボットなどに実装されるマイクロプロセッサなどの電子機器で所定のプログラムを実行することによって実現することもできる。そこで、以下では、図１６を用いて、実施例１にて説明した画像処理装置２００により実行される処理と同様の機能を実現する画像処理プログラムを実行する電子機器の一例を説明する。図１６は、画像処理プログラムを実行する電子機器の一例を示す図である。
【０１１５】
図１６に示すように、画像処理装置２００により実行される各種処理を実現する電子機器３００は、各種演算処理を実行するＣＰＵ（Central Processing Unit）３１０を有する。また、図１６に示すように、電子機器３００は、カメラ画像を取得するためのカメラインターフェース３２０、ディスプレイとの間で各種データのやり取りを行うためのディスプレイインターフェース３３０を有する。また、図１６に示すように、電子機器３００は、ハードウェアアクセラレータとして機能するグラフィックエンジン３４０を有する。
【０１１６】
また、図１６に示すように、電子機器３００は、ＣＰＵ３１０により各種処理を実現するためのプログラムやデータ等を記憶するハードディスク装置３５０と、各種情報を一時記憶するＲＡＭ（Random Access Memory）などのメモリ３６０とを有する。そして、各装置３１０〜３６０は、バス３７０に接続される。
【０１１７】
なお、ＣＰＵ３１０の代わりに、例えば、ＭＰＵ（Micro Processing Unit）などの電子回路、ＡＳＩＣ（Application Specific Integrated Circuit）やＦＰＧＡ(Field Programmable Gate Array)などの集積回路を用いることもできる。また、メモリ３６０の代わりに、フラッシュメモリ(flash memory)などの半導体メモリ素子を用いることもできる。また、ハードディスク装置３５０の代わりに、フラッシュメモリなどの不揮発性の半導体メモリ素子用いることもできる。
【０１１８】
ハードディスク装置３５０には、画像処理装置２００の機能と同様の機能を発揮する画像処理プログラム３５１および画像処理用データ３５２が記憶されている。なお、この画像処理プログラム３５１を適宜分散させて、ネットワークを介して通信可能に接続された他のコンピュータの記憶部に記憶させておくこともできる。
【０１１９】
そして、ＣＰＵ３１０が、画像処理プログラム３５１をハードディスク装置３５０から読み出してメモリ３６０に展開することにより、図１６に示すように、画像処理プログラム３５１は画像処理プロセス３６１として機能する。画像処理プロセス３６１は、ハードディスク装置３５０から読み出した画像処理用データ３５２等の各種データを適宜メモリ３６０上の自身に割当てられた領域に展開し、この展開した各種データに基づいて各種処理を実行する。なお、図１６に示すように、画像処理プロセス３６１は、プロセス実行時にメモリ３６０に補助的に格納されるが、画像処理プロセス３６１は、あくまでもＣＰＵ３１０上で動作しているものとする。
【０１２０】
なお、画像処理プロセス３６１は、例えば、実施例１で説明した画像処理装置２００の制御部２３０にて実行される処理、例えば、図１２〜１４に示す処理を含む。
【０１２１】
なお、画像処理プログラム３５１については、必ずしも最初からハードディスク装置３５０に記憶させておく必要はない。例えば、電子機器３００が実装されたＥＣＵへ対応ドライブを接続可能なフレキシブルディスク（ＦＤ）、ＣＤ−ＲＯＭ、ＤＶＤディスク、光磁気ディスク、ＩＣカードなどの「可搬用の物理媒体」に各プログラムを記憶させておく。そして、電子機器３００がこれらから各プログラムを読み出して実行するようにしてもよい。
【０１２２】
さらには、公衆回線、インターネット、ＬＡＮ、ＷＡＮなどを介して、電子機器３００が実装されたＥＣＵに接続される「他のコンピュータ（またはサーバ）」などに各プログラムを記憶させておく。そして、電子機器３００がこれらから各プログラムを読み出して実行するようにしてもよい。
【符号の説明】
【０１２３】
１００ 撮影装置
２００ 画像処理装置
２１０ 取得部
２２０ 記憶部
２２１ 画像記憶部
２２２ カメラパラメータ記憶部
２２３ マーカー距離情報記憶部
２３０ 制御部
２３１ 生成部
２３２ 設定部
２３３ 作成部
２３４ 導出部
２３５ 棄却部
２３６ 算出部
２３７ 決定部
２３８ 位置姿勢推定部
３００ 電子機器
３１０ ＣＰＵ
３２０ カメラインターフェース
３３０ ディスプレイインターフェース
３４０ グラフィックエンジン
３５０ ハードディスク装置
３５１ 画像処理プログラム
３５２ 画像処理用データ
３６０ メモリ
３６１ 画像処理プロセス
【技術分野】
【０００１】
本願の開示する技術は、画像処理プログラムおよび画像処理装置に関する。
【背景技術】
【０００２】
近年、カメラにより撮影された画像を３次元的に認識する技術が実用化されている。この技術は、例えば、車両に搭載された車載カメラにより撮影された画像を用いて、車両の全周囲を見渡すことができる全周囲画像などを合成する場合に利用されている。そのほか、ロボットに搭載されたカメラにより撮影された画像を用いて、対象物体に対するロボットの相対位置を把握可能な画像を合成する場合などにも利用されている。
【０００３】
ところで、カメラにより撮影された画像を３次元的に認識する技術においては、カメラにより撮影された画像を用いて３次元の画像を合成する場合に、カメラの取り付け姿勢や取り付け位置の情報を正しく把握しておくことが求められる場合がある。カメラの取り付け姿勢や取り付け位置の情報を推定する技術には、以下に説明するような公知技術１や公知技術２がある。
【０００４】
上述した公知技術１について図１７を用いて説明する。図１７は、公知技術１を説明するための図である。公知技術１は、まず、マーカーなどの物体を実空間上の所定位置、言い換えれば、世界座標系における所定位置に配置してカメラで撮影する。続いて、公知技術１は、図１７に示すように、マーカーを撮影して得られる画像平面Ｈ上の３つの点（ｘ０，ｘ１，ｘ２）に対応する３つの点（Ｐ０，Ｐ１，Ｐ２）を抽出する。この３点（Ｐ０，Ｐ１，Ｐ２）は、画像平面Ｈ上の３つの点（ｘ０，ｘ１，ｘ２）に対応するカメラ座標系における３次元位置である。そして、公知技術１は、図１７に示すように、カメラ座標系の原点Ｃと、３点（Ｐ０，Ｐ１，Ｐ２）とを結んでできる三角錐の各面を定式化した連立方程式をそれぞれ立てる。例えば、原点Ｃと、３点（Ｐ０，Ｐ１，Ｐ２）とを結んでできる三角錐の各面は余弦定理を用いて定式化した数式で表すことができる。そして、各数式から以下に示す連立方程式（１）が成立する。なお、原点Ｃと点Ｐ０との距離をＬ０、原点Ｃと点Ｐ１との距離をＬ１、原点Ｃと点Ｐ２との距離をＬ２、原点Ｃから点Ｐ０への視線ベクトルをＶ０、原点Ｃから点Ｐ１への視線ベクトルをＶ１、原点Ｃから点Ｐ２への視線ベクトルをＶ２とする。視線ベクトル（Ｖ０，Ｖ１，Ｖ２）は、カメラの光軸位置や焦点距離などの内部パラメータから得られる。なお、以下の式（１）に示す＜Ｖｉ，Ｖｊ＞（ｉ，ｊ＝０，１，２、ｉ≠ｊ）はベクトルＶｉとベクトルＶｊとの内積を表す。また、以下の式（１）に示すＤ０，１、Ｄ０，２、Ｄ１，２は、点Ｐ０〜点Ｐ２のうちの各２点間の実距離をそれぞれ表している。
【０００５】
【数１】
【０００６】
次に、公知技術１は、上述した連立方程式（１）から導出される４次方程式の解を求めることにより、図１７に示すカメラ座標系の原点Ｃから３点（Ｐ０，Ｐ１，Ｐ２）までの距離Ｌ０、Ｌ１、Ｌ２を求める。そして、公知技術１は、原点Ｃから３点（Ｐ０，Ｐ１，Ｐ２）までの距離と、カメラの内部パラメータから得られる視線ベクトル（Ｖ０，Ｖ１，Ｖ２）とを用いて、逆算的にカメラの位置および姿勢を導出する。
【０００７】
また、例えば、公知技術２は、マーカーなどの物体をカメラにより撮影した画像上の特徴点を抽出し、画像上の特徴点と物体の見え方のパターンとの対応関係から、カメラの概略的な推定位置に応じた探索領域を設定する。そして、公知技術２は、カメラの位置や姿勢に関する合計６つのパラメータについて、要求誤差精度を満足する最適解を探索領域の中から探索することによりカメラの位置や姿勢を検出する。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開平６−２５８０２８号公報
【非特許文献】
【０００９】
【非特許文献１】Ｑｕａｎ ａｎｄ Ｌａｎ，「Ｌｉｎｅａｒ Ｎ−Ｐｏｉｎｔ Ｃａｍｅｒａ Ｐｏｓｅ Ｄｅｔｅｒｍｉｎａｔｉｏｎ」，ＩＥＥＥ Ｔｒａｎｓ．ｏｎ ＰＡＭＩ，１９９９
【発明の概要】
【発明が解決しようとする課題】
【００１０】
上述した公知技術１は、抽出した３点からカメラ座標系の原点までの距離を求めるために４次方程式の解を求める必要があるので、計算量が多くなる結果、カメラの取り付け姿勢や取り付け位置の情報を迅速に得られないという問題がある。さらに、公知技術１では、カメラ座標系の原点までの距離を求めるために４次方程式の解を求めるが、４次方程式は特異値分解を用いて解を導き出すので、専門的な数値計算ライブラリ等を格納するための領域が必要となり、実用性に欠けるという問題もある。
【００１１】
また、上述した公知技術２は、カメラの位置や姿勢に関する合計６つのパラメータについて最適解の探索を行うので、局所解に陥りやすい結果、カメラの取り付け姿勢や取り付け位置の情報を必ずしも精度良く得ることができないという問題があった。
【００１２】
開示の技術は、上記に鑑みてなされたものであって、カメラの取り付け位置や取り付け姿勢の情報を迅速かつ精度良く得ることが可能な画像処理プログラムおよび画像処理装置を提供することを目的とする。
【課題を解決するための手段】
【００１３】
本願の開示する画像処理プログラムは、一つの態様において、コンピュータに、次のような処理を実行させる。すなわち、コンピュータに、実空間に設けられた４個以上のマーカーをカメラにより撮影した画像を取得する処理を実行させる。また、コンピュータに、前記カメラの焦点距離および光軸位置の情報を含む内部パラメータおよび前記画像における前記４個以上のマーカーの各々の位置に基づいて、前記カメラから該４個以上のマーカーの各々に対応するベクトルを生成する処理を実行させる。また、コンピュータに、前記４個以上のマーカーの中から基準マーカーを一つ選択する処理を実行させる。また、コンピュータに、前記基準マーカー以外の残りのマーカーのうちのそれぞれに対するベクトルと、該基準マーカーのベクトルとの内積値を算出する処理を実行させる。また、コンピュータに、前記内積値の符号が負となる前記基準マーカー以外のマーカーについて次の処理を実行させる。例えば、コンピュータに、該基準マーカーを用いて該基準マーカー以外のマーカーと前記カメラとの距離を求める二次方程式の解を表す数式に含まれる符号のうち、負の符号の採用を棄却する処理を実行させる。また、コンピュータに、棄却した結果に基づいて、前記基準マーカー以外のマーカーのそれぞれに対する前記数式に含まれる符号を、全ての該基準マーカー以外のマーカーについて組み合わせた符号パターンを生成する処理を実行させる。また、コンピュータに、前記基準マーカーと前記カメラとの距離として第１の値を設定する処理を実行させる。また、コンピュータに、前記第１の値と前記二次方程式と棄却した前記結果とを用いて、該第１の値における、前記基準マーカー以外のマーカーの各々と前記カメラとの距離の候補値を、１つまたは２つ算出する処理を実行させる。また、コンピュータに、前記符号パターンごとに、該符号パターン及び前記１つまたは２つの候補値に基づいて算出されるマーカー間距離と、前記実空間におけるマーカー間の距離との誤差を算出する処理を実行させる。また、コンピュータに、前記第１の値とは異なる第２の値を設定した場合に、前記符号パターンごとに他の誤差を算出する処理を実行させる。また、コンピュータに、前記第１の値を選択した場合に算出された前記誤差および前記第２の値を選択した場合に算出された前記他の誤差に基づいて、前記４個以上のマーカーの各々と前記カメラとの距離を決定する処理を実行させる。また、コンピュータに、決定した前記４個以上のマーカーの各々と前記カメラとの距離に基づいて、該カメラの世界座標系における位置および姿勢を演算する処理を実行させる。
【発明の効果】
【００１４】
本願の開示する技術の一つの態様によれば、カメラの取り付け位置や取り付け姿勢の情報を迅速かつ精度良く得ることができる。
【図面の簡単な説明】
【００１５】
【図１】図１は、実施例１に係る画像処理装置の説明に用いる図である。
【図２】図２は、実施例１に係る画像処理装置の構成を示す機能ブロック図である。
【図３】図３は、実施例１に係る生成部の説明に用いる図である。
【図４】図４は、実施例１に係る作成部の説明に用いる図である。
【図５】図５は、実施例１に係る導出部の説明に用いる図である。
【図６】図６は、実施例１に係る棄却部の処理に関する概念的な説明に用いる図である。
【図７】図７は、実施例１に係る算出部の処理に関する概念的な説明に用いる図である。
【図８】図８は、実施例１に係る算出部の処理に関する概念的な説明に用いる図である。
【図９】図９は、実施例１に係る算出部の処理に関する概念的な説明に用いる図である。
【図１０】図１０は、実施例１に係る算出部の処理に関する概念的な説明に用いる図である。
【図１１】図１１は、実施例１に係る位置姿勢推定部の説明に用いる図である。
【図１２】図１２は、実施例１に係る最適解算出処理の流れを示す図である。
【図１３】図１３は、実施例１に係る最適解候補算出処理の流れを示す図である。
【図１４】図１４は、実施例１に係る符号パターン決定処理の流れを示す図である。
【図１５】図１５は、実施例２に係る画像処理装置の構成を示す機能ブロック図である。
【図１６】図１６は、画像処理プログラムを実行する電子機器の一例を示す図である。
【図１７】図１７は、公知技術１を説明するための図である。
【発明を実施するための形態】
【００１６】
以下に、図面を参照しつつ、本願の開示する画像処理プログラムおよび画像処理装置の一実施形態について詳細に説明する。なお、以下の実施例では、実空間である世界座標系の既知の３次元位置に位置する４個のマーカーを車両やロボットなどに搭載されたカメラにより撮影して、撮影された画像を用いてカメラの位置および姿勢を推定する技術に関する一実施形態について説明する。なお、マーカーは、４個以上であれば何個であっても構わない。以下の実施例では、実空間のことを３次元空間と表記するものとする。本願の開示する画像処理プログラムおよび画像処理装置の一実施形態として後述する実施例により、本願が開示する技術が限定されるものではなく、処理内容に矛盾を生じさせない範囲で各実施例を適宜組み合わせることが可能である。
【実施例１】
【００１７】
実施例１に係る画像処理装置の構成の説明に入る前に、図１を用いて、実施例１に係る画像処理装置を説明する。図１は、実施例１に係る画像処理装置の説明に用いる図である。なお、以下に説明する実施例では、世界座標系の既知の３次元位置に４個のマーカーを配置する場合の一実施形態を説明するが、図１では、説明の便宜上、配置される４個のマーカーのうち３個のマーカーに焦点をあてて説明する。
【００１８】
図１に示すＣはカメラ座標系の原点を示す。なお、図１の説明において、図１に示すＣを「原点Ｃ」と表記するものとする。図１に示すＰ０，Ｐ１，Ｐ２Ａ，Ｐ２Ｂは、４個のマーカーが映し出された画像平面上の点のうち、３個のマーカーに対応するカメラ座標系における仮想的な３次元位置（対応点）を示す。また、Ｐ２Ａ，Ｐ２Ｂは、画像平面上のある点に対応するカメラ座標系における３次元位置となり得る候補に相当する。なお、図１の説明において、図１に示すＰ０，Ｐ１，Ｐ２Ａ，Ｐ２Ｂを、「対応点Ｐ０」，「対応点Ｐ１」，「対応点Ｐ２Ａ」，「対応点Ｐ２Ｂ」とそれぞれ表記するものとする。図１に示すＶ０，Ｖ１，Ｖ２は、原点Ｃから対応点Ｐ０，対応点Ｐ１，対応点Ｐ２Ａ，対応点Ｐ２Ｂへ向かう視線ベクトルを示し、例えば、長さが１の単位ベクトルである。なお、図１の説明において、図１に示すＶ０，Ｖ１，Ｖ２を、「ベクトルＶ０」，「ベクトルＶ１」，「ベクトルＶ２」とそれぞれ表記するものとする。
【００１９】
また、図１に示すＬ０は原点Ｃと対応点Ｐ０との距離を示し、図１に示すＬ１は原点Ｃと対応点Ｐ１との距離の値を示し、図１に示すＬ２Ａは原点Ｃと対応点Ｐ２Ａとの距離の値を示し、図１に示すＬ２Ｂは原点Ｃと対応点Ｐ２Ｂとの距離の値を示す。なお、図１の説明において、図１に示すＬ０を「距離Ｌ０」と表記するものとする。同様に、図１の説明において、図１に示すＬ１，Ｌ２Ａ，Ｌ２Ｂを、「距離Ｌ１」，「距離Ｌ２Ａ」，「距離Ｌ２Ｂ」とそれぞれ表記するものとする。
【００２０】
また、図１に示すＤ（０，１）は、対応点Ｐ０と対応点Ｐ１との３次元空間上の実距離を示し、図１に示すＤ（０，２）は、対応点Ｐ０と対応点Ｐ２との３次元空間上の実距離を示し、図１に示すＤ（１，２）は、対応点Ｐ１と対応点Ｐ２との３次元空間上の実距離を示す。なお、図１の説明において、図１に示すＤ（０，１）を「実距離Ｄ（０，１）」と表記するものとする。同様に、図１の説明において、図１に示すＤ（０，２）を「実距離Ｄ（０，２）」と記し、図１に示すＤ（１，２）を「実距離Ｄ（１，２）」と表記するものとする。
【００２１】
実施例１に係る画像処理装置は、まず、基準点として対応点Ｐ０を選択した場合に、距離Ｌ０を用いて、距離Ｌ１および距離Ｌ２を余弦定理により定式化して表した各数式からなる連立方程式を立てる。この連立方程式は、例えば、以下の式（２）で示される。なお、以下の式（２）に示すＤ０，１、Ｄ０，２、は、上述した図１に示すＤ（０，１）、Ｄ（０，２）にそれぞれ対応する。
【００２２】
【数２】
【００２３】
ここで、距離Ｌ０の値が与えられれば、上述した式（２）の解を求めることにより、２種類の値が距離Ｌ１の値として導出される。つまり、距離Ｌ１の値として、距離Ｌ１を定式化して表した数式の右辺内にある内積の項と根号の項との間にあるプラスの符合を選択した場合と、マイナスの符号を選択した場合の２種類の値が導出される。同様に、上述した式（２）の解を求めることにより、２種類の値が距離Ｌ２の値として導き出される。すなわち、距離Ｌ１を定式化して表した数式の右辺内にある内積の項と根号の項との間にある符合の選択により距離Ｌ１の値が決定付けられる。同様に、距離Ｌ２を定式化して表した数式内の内積の項と根号の項との間にある符合の選択により距離Ｌ２の値が決定付けられる。そこで、実施例１に係る画像処理装置は、距離Ｌ１を定式化して表した数式の右辺内にある根号前の符号の一方と、距離Ｌ２を表す数式の右辺内にある根号前にある符号一方とを組合せた符号パターンを導出する。そして、実施例１に係る画像処理装置は、符号パターンの中から、距離Ｌ１、距離Ｌ２の値として相応しくない値が導き出されるものを棄却する。
【００２４】
例えば、実施例１に係る画像処理装置は、距離Ｌ１およびＬ２が上述した式（２）で表される場合に、（＋、＋）、（−、−）、（＋、−）、（−，＋）の４つの符号パターンを導出できる。次に、実施例１に係る画像処理装置は、導出した符号パターンの中から、少なくとも距離Ｌ１および距離Ｌ２の値の一方が相応しい値となり得えない符号パターンを棄却する。例えば、図１に示すベクトルＶ０とベクトルＶ１とのなす角度「θ」が９０度よりも大きい場合には、距離Ｌ１を表す数式の右辺内にある内積がマイナスの値となる。この場合、式（２）の根号前の符号のうちマイナス符号を選択した場合に計算される距離Ｌ１の値はマイナスとなってしまうので、距離Ｌ１の値として相応しくない。つまり、右辺の第一項は必ず負の値となるため、符号パターンは少なくとも正が採用されないかぎり、右辺は負の値となる。よって、実施例１に係る画像処理装置は、距離Ｌ１を表す数式の右辺内にある根号前の符号のうちマイナスの符号を選択してできる符号パターンを棄却する。また、例えば、図１に示すベクトルＶ０とベクトルＶ２とのなす角度が９０度以下である場合には、距離Ｌ２を表す数式の右辺内にある内積がプラスの値となる。この場合、距離Ｌ２を表す数式に含まれる根号前の各符号をそれぞれ選択した場合に計算される距離Ｌ２の値は、プラスになるかマイナスになるか不明なので、距離Ｌ２の値として相応しいかどうかの判断はできない。よって、実施例１に係る画像処理装置は、距離Ｌ２を表す数式の右辺内にある根号前のプラスの符号およびマイナスの符合を選択してできる各符号パターンを棄却せずに残す。この結果、実施例１に係る画像処理装置は、（−、−）、（−，＋）の２つの符号パターンを棄却し、（＋、−）、（＋，＋）の２つの符号パターンを残すこととなる。
【００２５】
なお、棄却されずに残された符号パターンでは、距離Ｌ１の値の候補は、距離Ｌ１を表す数式の右辺内にある内積の項と根号の項との間にあるプラスの符合を選択した場合に計算される１つの値のみとなる。つまり、図１に示す距離Ｌ１の値の候補は、２種類の符号の選択により与えられる２つの候補のうちの１つの候補に絞り込まれる。一方、距離Ｌ２の値の候補は、距離Ｌ２を表す数式の右辺内にある内積の項と根号の項との間にあるプラスの符合を選択した場合に計算される値、およびマイナスの符号を選択した場合に導出される値の２つとなる。つまり、図１に示す距離Ｌ２の値の候補は、図１に示すＬ２Ａに対応する値と図１に示すＬ２Ｂに対応する値の２つとなり、２種類の符号の選択により与えられる２つの候補がそのまま残される。このように、カメラ座標系の原点からマーカーに対応するカメラ座標系における対応点までの距離を算出する場合に、視線ベクトル同士がなす角度を考慮することにより、２種類の符号の選択により与えられる２つの候補の中から絞り込むことができる場合がある。
【００２６】
次に、実施例１に係る画像処理装置は、棄却されずに残された符号パターンについて、３次元空間上のマーカー間の距離の候補値と３次元空間上のマーカー間の実距離の値との誤差を算出する。例えば、実施例１に係る画像処理装置は、以下に説明するようにして各符号パターンについての誤差を算出する。
【００２７】
まず、実施例１に係る画像処理装置は、距離Ｌ０と、上述した式（２）とに基づいて、例えば、棄却されずに残された符号パターン（＋、＋）について、距離Ｌ１および距離Ｌ２Ａを得られる。また、実施例１に係る画像処理装置は、距離Ｌ０と、上述した式（２）とに基づいて、例えば、棄却されずに残された符号パターン（＋、−）について、距離Ｌ１および距離Ｌ２Ｂを得られる。なお、距離Ｌ０の値は、距離Ｌ０の値が取り得る範囲内で予め設定される。
【００２８】
続いて、実施例１に係る画像処理装置は、符号パターン（＋、＋）について、距離Ｌ１および距離Ｌ２Ａに基づき、３次元空間上のマーカー間の距離の候補値、つまり図１に示す点Ｐ１と点Ｐ２Ａとの距離を算出する。また、実施例１に係る画像処理装置は、符号パターン（＋、−）について、距離Ｌ１および距離Ｌ２Ｂに基づき、３次元空間上のマーカー間の距離の候補値、つまり図１に示す点Ｐ１と点Ｐ２Ｂとの間の距離を算出する。
【００２９】
次に、実施例１に係る画像処理装置は、符号パターン（＋、＋）について算出した点Ｐ１と点Ｐ２Ａとの距離と、３次元空間上のマーカー間の実距離の値との誤差を算出する。同様に、実施例１に係る画像処理装置は、符号パターン（＋、−）について算出した点Ｐ１と点Ｐ２Ｂとの距離と、３次元空間上のマーカー間の実距離の値との誤差を算出する。
【００３０】
続いて、実施例１に係る画像処理装置は、符号パターン（＋、＋）について算出した誤差と、符号パターンである（＋、−）について算出した誤差とを比較する。比較の結果、実施例１に係る画像処理装置は、符号パターン（＋、＋）について算出した誤差の値の方が小さい場合には、距離Ｌ０、距離Ｌ１、距離Ｌ２Ａを、カメラ原点から３次元空間上のマーカーまでの距離に決定する。
【００３１】
実施例１に係る画像処理装置は、上述した符号パターンについての誤差の算出をする処理において、基準点以外に選択した２点間の距離と実距離との誤差が最小となるときの基準点までの距離および基準点以外の点までの距離を検出する処理を行っている。例えば、対応点Ｐ１と対応点Ｐ２との間の距離と実距離との誤差が最小となるときの基準点までの距離および基準点以外の点までの距離を検出する処理は、以下に示す式（３）で表される。なお、以下の式（３）に示すＤ１，２は、上述した図１に示すＤ（１，２）に対応する。以下の式（３）に示すＬ２は、Ｌ２ＡやＬ２Ｂなどに対応する。また、例えば‖Ｖ‖は、ベクトルＶの長さを表わす。また、例えば｜ａ｜は、実数ａの絶対値を表わす。
【００３２】
【数３】
【００３３】
上述してきたように、実施例１に係る画像処理装置は、まず、上述した符号パターンについて高々２種類の符号の選択を行う。そして、実施例１に係る画像処理装置は、選択の結果残された符号パターンについて、基準点以外に選択した２点間の距離と実距離との誤差が最小となるときの基準点までの距離および基準点以外の点までの距離の検出を実行する。このようにして、実施例１に係る画像処理装置は、４個以上のマーカーが映し出された画像平面上の点に対応する３次元空間上の対応点の位置を迅速かつ精度良く算出する。
【００３４】
［画像処理装置の構成（実施例１）］
図２は、実施例１に係る画像処理装置の構成を示す機能ブロック図である。なお、図２には、実施例１に係る画像処理装置の機能を説明する上で必要となる機能ブロックを図示する。以下の実施例中において、単に原点Ｃと表記するものはカメラ座標系の原点を示し、単に対応点と表記するものはカメラ座標系における３次元位置を示すものとする。
【００３５】
図２に戻り、実施例１に係る画像処理装置２００は、例えば、撮影装置１００が接続される。撮影装置１００は、例えば、単眼カメラやステレオカメラなどに該当する。撮影装置１００は、例えば、車両やロボットなどの周囲の実空間上の所定位置に配置された４個のマーカーを撮影する。なお、以下の実施例中、カメラと表記する箇所は撮影装置１００に該当するものとする。
【００３６】
画像処理装置２００は、図２に示すように、取得部２１０、記憶部２２０および制御部２３０を有する。取得部２１０は、撮影装置１００により撮影されたマーカーの画像をそれぞれ取得する。取得部２１０は、撮影装置１００から取得したマーカーの画像を画像記憶部２２１に格納する。
【００３７】
記憶部２２０は、図２に示すように、画像記憶部２２１、カメラパラメータ記憶部２２２およびマーカー距離情報記憶部２２３を有する。なお、記憶部２２０は、例えば、ＲＡＭ（Random Access Memory）やフラッシュメモリ(flash memory)などの半導体メモリ素子である。
【００３８】
画像記憶部２２１は、例えば、取得部２１０により格納されたマーカーの画像をそれぞれ記憶する。カメラパラメータ記憶部２２２は、カメラの焦点距離および光軸位置の情報を含むカメラのパラメータの情報、いわゆるカメラ内部パラメータの情報を記憶する。カメラ内部パラメータの情報は、後述する生成部２３１による視線ベクトルの生成時に用いられる。マーカー距離情報記憶部２２３は、車両やロボットなど周囲に配置されたマーカー間の３次元空間上の実距離の情報をそれぞれ記憶する。なお、記憶部２２０は、さらに距離Ｌｎの値が取り得る範囲を記憶する領域を有する。なお、距離Ｌｎの値が取り得る範囲は予め記憶部に記憶されていても良いし、後述の処理を行う際に、利用者によって入力された数値範囲が記憶部２２０に記憶するとしても良い。また、距離Ｌｎの値が取り得る範囲は、予めカメラの設置位置が予測される環境において、予測値の前後の範囲が設定される。例えば、カメラを備える車体を、マーカーを有する試験場に停車し、詳細なカメラの位置や姿勢を演算する場合においては、試験場における車体を停止する位置や、設計上の車種ごとのカメラ取り付け位置などから推測された数値範囲が設定される。
【００３９】
図２に戻り、制御部２３０は、生成部２３１と、設定部２３２と、作成部２３３と、導出部２３４と、棄却部２３５と、算出部２３６と、決定部２３７と、位置姿勢推定部２３８とを有する。なお、制御部２３０は、例えば、電子回路や集積回路に該当する。電子回路としては、例えば、ＣＰＵ（Central Processing Unit）やＭＰＵ（Micro Processing Unit）がある。また、集積回路としては、例えば、ＡＳＩＣ（Application Specific Integrated Circuit）やＦＰＧＡ（Field Programmable Gate Array)などがある。
【００４０】
生成部２３１は、カメラからマーカーの各々に対応する視線ベクトルを生成する。例えば、生成部２３１は、画像記憶部２２１から４個のマーカーの画像を取得し、カメラの焦点距離および光軸位置の情報を含むカメラ内部パラメータをカメラパラメータ記憶部２２２から取得する。そして、生成部２３１は、４点のマーカーの画像の画像平面上における４個のマーカーそれぞれの位置の情報を取得する。続いて、生成部２３１は、カメラ内部パラメータと、マーカーの画像における４個のマーカーの各々の位置の情報とに基づいて、カメラから４個のマーカーの各々に対応する視線ベクトルを生成する。例えば、視線ベクトルＶｉ（ｉ＝０，１，２，３）は、カメラ内部パラメータを与える正則な３次元正方行列「Ｋ」と、画像の座標「ｘｉ」と、ｘｉの同次座標表現「ｘ´ｉ」を用いて、以下に示す式（４）で表わすことができる。以下に示す式（４）では、視線ベクトルＶｉの大きさが「１」に正規化されている。なお、以下の式（４）に示すＫ−１は、３次元正方行列「Ｋ」の逆行列である。
【００４１】
【数４】
【００４２】
なお、生成部２３１により生成された視線ベクトルを用いて、４個のマーカーが映し出された画像平面上の点に対応するカメラ座標系における仮想的な３次元位置（対応点）は、以下の式（５）で表すことができる。以下の式（５）は、視線ベクトルの延長線上に対応点が位置することを意味している。以下の説明において、単に「対応点」と表記するものは、マーカーが映し出された画像平面上の点に対応するカメラ座標系における仮想的な３次元位置を表すものとする。なお、４個のマーカーが映し出された画像平面上の点に対応する対応点は、以下に説明する設定部２３２〜決定部２３７による処理の結果を用いることにより得ることができる。また、生成部２３１は、マーカーが映し出される画像平面上の各点の位置と、３次元空間上のマーカーの位置との対応関係の情報を予め有するものとする。
【００４３】
【数５】
【００４４】
図３に、生成部２３１が生成する視線ベクトルの一例を示す。図３は、実施例１に係る生成部の説明に用いる図である。図３に示すように、生成部２３１は、カメラ座標系の原点Ｃから対応点の一つである点０に対応するベクトルＶ０、およびカメラ座標系の原点Ｃから対応点の一つである点１に対応するベクトルＶ１をそれぞれ生成する。同様に、図３には示していないが、生成部２３１は、カメラ座標系の原点Ｃから対応点の一つである点２に対応するベクトルＶ２、およびカメラ座標系の原点Ｃから対応点の一つである点３に対応するベクトルＶ３をそれぞれ生成する。
【００４５】
図２に戻り、設定部２３２は、４個のマーカーが映し出された画像平面上の４個に対応する仮想的な４つの対応点の中から基準とするマーカー（以後、基準マーカーと表記する）に対応する点である基準点Ｍ（点０〜点３のいずれか）を選択する。また、設定部２３２は、基準点Ｍまでの距離Ｌ０に所定の値「Ｌ」を設定する。なお、距離Ｌ０に設定される所定の値「Ｌ」は、カメラの設置位置およびマーカーの配置位置から予め推定される。
【００４６】
図２に戻り、作成部２３３は、基準点Ｍ以外の点、つまり基準マーカー以外のマーカーに対応する点１〜点３までの各距離Ｌ１，Ｌ２，Ｌ３を表す数式を作成する。距離Ｌ１，Ｌ２，Ｌ３は、上述した式（２）のように、余弦定理を用いて定式化して表すことができる。図４に、作成部２３３により作成される数式から得られる対応点までの距離の一例を示す。図４は、実施例１に係る作成部の説明に用いる図である。図４に示す点１（＋）は、距離Ｌ１を表す数式の根号前の符号のうちプラスを選択した場合の距離に相当する。図４に示す点１（−）は、距離Ｌ１を表す数式の根号前の符号のうちマイナスを選択した場合の距離に相当する。
【００４７】
図４に示すように、基準点として点０を選択した場合、カメラ座標系の原点Ｃから点１までの距離として、上述した式（２）から、点Ｃから点１（＋）までの距離と点Ｃから点１（−）までの距離の２つの候補が得られる。言い換えれば、カメラ座標系の原点Ｃから点１までの距離は、基準マーカー以外のマーカーに対応する点１〜点３までの各距離Ｌ１，Ｌ２，Ｌ３を表す数式によって、高々２つの候補に限定することができる。
【００４８】
図２に戻り、導出部２３４は、設定部２３２により設定された基準マーカーで採用する符号パターンを全て導出する。図５には、対応点０〜３のうち点０を基準点とした場合に導出される符号パターンを示す。図５は、実施例１に係る導出部の説明に用いる図である。図５に示すように、導出部２３４は、（＋、＋、＋）、（＋、＋、−）、（＋、−、＋）、（＋、−、−）、（−、＋、＋）、（−、＋、−）、（−、−、＋）、（−、−、−）の８つの符号パターンを導出する。
【００４９】
また、導出部２３４は、作成部２３３により作成された各数式の根号内の値を算出し、各数式の根号の値が全て実数であるか否かを判定する。判定の結果、各数式の根号の値の全てが実数である場合には、導出部２３４は、符号パターンを棄却部２３５に送出する。一方、判定の結果、各数式の根号の値の全てが実数ではない場合には、導出部２３４は、後述する決定部２３７に対して処理の終了を送出する。なお、この実施例では、導出部２３４により、各数式の根号の値の全てが実数であるか否かを判定する場合を説明したが、これに限定されるものではなく、例えば、作成部２３３が各数式の根号の値の全てが実数であるか否かを判定してもよい。
【００５０】
棄却部２３５は、導出部２３４により導出された符号パターンの中から、距離Ｌ１、Ｌ２、Ｌ３の少なくとも１つが相応しくない符号パターンを棄却する。まず、棄却部２３５は、基準点Ｍ以外の対応点Ｎを一つ選択する。例えば、点０を基準点Ｍとして選択した場合には、点１〜３のいずれか一つを対応点Ｎとして選択する。次に、棄却部２３５は、原点Ｃから基準点Ｍまでの視線ベクトル、および原点Ｃから対応点Ｎまでの視線ベクトルを生成部２３１から取得する。
【００５１】
続いて、棄却部２３５は、基準点Ｍに対応する視線ベクトルと、選択した対応点Ｎに対応する視線ベクトルとの内積を計算し、視線ベクトルのなす角が９０度よりも大きいか否かを判定する。例えば、棄却部２３５は、計算した内積の値が負の値である場合には、視線ベクトルのなす角が９０度よりも大きいと判定する。判定の結果、棄却部２３５は、視線ベクトルのなす角が９０度よりも大きい場合には、選択した対応点Ｎについてはプラスの符号のみを取り得ることを記録する。ここで、記録するとは、棄却部２３５が、内部的に有する記憶部に、対応点Ｎについてはプラスの符号のみを取り得ることを示すデータを保存することを意味する。
【００５２】
一方、判定の結果、棄却部２３５は、視線ベクトルのなす角が９０度以下である場合には、選択した対応点Ｎについてはプラス、マイナスの両方の符号を取り得ることを記録する。ここで、記録するとは、棄却部２３５が、内部的に有する記憶部に、対応点Ｎについてはプラス、マイナスの両方の符号を取り得ることを示すデータを保存することを意味する。そして、棄却部２３５は、対応点の中から、まだ選択していない他の全ての点についても上述してきたのと同様の処理を行い、全ての対応点が取り得る符号の組合せを列挙することにより符号パターンを決定する。
【００５３】
以下、図６を用いて符号パターンの決定方法の一例を説明する。図６は、実施例１に係る棄却部の処理に関する概念的な説明に用いる図である。棄却部２３５は、図６に示す基準点０以外の点１〜３の中から点１を選択する。次に、棄却部２３５は、基準点０に対応する視線ベクトルＶ０、および点１に対応する視線ベクトルＶ１を生成部２３１から取得する。続いて、棄却部２３５は、視線ベクトルＶ０と視線ベクトルＶ１との内積を計算し、視線ベクトルＶ０と視線ベクトルＶ１とのなす角が９０度よりも大きいか否かを判定する。判定の結果、棄却部２３５は、視線ベクトルＶ０と視線ベクトルＶ１とのなす角が９０度よりも大きい場合には、選択した点１ではプラスの符号のみを取り得ることを記録する。一方、判定の結果、棄却部２３５は、視線ベクトルのなす角が９０度以下である場合には、選択した点１ではプラス、マイナスの両方の符号を取り得ることを記録する。
【００５４】
そして、棄却部２３５は、対応点である点１〜点３の中から、まだ選択していない点２および点３についても上述してきたのと同様の処理を行い、点１〜点３が取り得る符号の組合せを列挙することにより符号パターンを決定する。例えば、棄却部２３５は、点１〜点３が取り得る符号の組合せを列挙する。その結果、棄却部２３５は、図６に示す符号パターンのうち、（−、＋、＋）、（−、＋、−）、（−、−、＋）、（−、−、−）を棄却し、（＋、＋、＋）、（＋、＋、−）、（＋、−、＋）、（＋、−、−）を点１〜点３が取り得る符号の組合せとして残す。よって、棄却部２３５は、棄却されずに残された４つの符号パターン、図６に示す（＋、＋、＋）、（＋、＋、−）、（＋、−、＋）、（＋、−、−）の４つの符号パターンを点１〜点３が取り得る符号パターンとして決定する。
【００５５】
図２に戻り、算出部２３６は、棄却部２３５により決定された各符号パターンについて、基準点以外の各対応点間の距離の候補値と３次元空間上の実距離の値との誤差を演算し、各符号パターンについて最小誤差を算出する。
【００５６】
まず、算出部２３６は、後に用いる最小誤差Ｊの値を初期化する。棄却部２３５により決定された各符号パターンの中から符号パターンを一つ選択し、選択した符号パターンについて距離Ｌ１、Ｌ２およびＬ３の値をそれぞれ算出する。算出部２３６は、設定部２３２により設定された基準点０までの距離Ｌ０の値「Ｌ」と、作成部２３３により作成された数式とを用いて、距離Ｌ１、Ｌ２およびＬ３の値を算出する。
【００５７】
次に、算出部２３６は、距離Ｌ１、Ｌ２およびＬ３の値が正の実数であるか否かを判定する。なお、正の実数とは、実数のうち正の値を意味する。算出部２３６は、判定の結果、距離Ｌ１、Ｌ２およびＬ３の値が正の実数ではない場合には、次の符号パターンを選択する。これとは反対に、算出部２３６は、判定の結果、距離Ｌ１、Ｌ２およびＬ３の値が正の実数である場合には、誤差Ｅの値を、例えば、「０」に初期化する。なお、後に説明するが、誤差Ｅの値は、所定の条件を満足する場合に、基準点０以外の２点の組合せについて算出される誤差の値を加算された値となる。
【００５８】
続いて、算出部２３６は、選択した符号パターン内で、基準点０以外の２点の組合せを一つ取得する。次に、算出部２３６は、取得した２点の組合せについて２点間の距離の候補値を算出する。続いて、算出部２３６は、算出した候補値に対応する３次元空間上のマーカー間の実距離の値をマーカー距離情報記憶部２２３から取得し、この候補値と、この候補値に対応する実距離の値との誤差を演算する。そして、算出部２３６は、算出した誤差の値を誤差Ｅの値に加算して更新し、誤差Ｅの値が最小誤差Ｊの値よりも大きいか否かを判定する。算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値よりも大きい場合には、棄却部２３５により決定された各符号パターンの中から次の符号パターンを選択する。
【００５９】
一方、算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値以下である場合には、選択した符号パターン内で基準点０以外の２点の組合せの全てについて処理を完了したか否かを判定する。算出部２３６は、判定の結果、選択した符号パターン内で基準点０以外の２点の組合せの全てについて処理を完了している場合には、誤差Ｅの値で最小誤差Ｊの値を更新し、棄却部２３５により決定された各符号パターンの中から次の符号パターンを一つ選択する。これとは反対に、算出部２３６は、判定の結果、選択した符号パターン内で基準点０以外の２点の組合せの全てについて処理を完了していない場合には、選択した符号パターン内で、基準点０以外の２点の組合せの中から次の組合せを一つ取得する。以後、算出部２３６は、取得した２点の組合せについて上述したのと同様に処理を行う。
【００６０】
そして、算出部２３６は、全符号パターン内について処理を完了した場合には、「基準点＝点０」および「距離Ｌ０＝Ｌ」に関する処理結果を決定部２３７に送出する。例えば、算出部２３６は、最終的に残った誤差Ｅの値、すなわち該当符号パターン内の最小誤差の値と、最小誤差に対応する符号パターンの距離Ｌ０、Ｌ１、Ｌ２およびＬ３の値を処理結果として送出する。
【００６１】
図７〜図１０を用いて、算出部２３６による処理を概念的に説明する。なお、図７〜図１０では、誤差Ｅの値を初期化した後の算出部２３６の処理について説明する。図７〜図１０は、実施例１に係る算出部の処理に関する概念的な説明に用いる図である。
【００６２】
まず、算出部２３６は、図７に示す各符号パターン（＋、＋、＋）、（＋、＋、−）、（＋、−、＋）、（＋、−、−）の中から、例えば、符号パターン（＋、＋、＋）を選択する。次に、算出部２３６は、選択した符号パターン（＋、＋、＋）内で、基準点０以外の２点の組合せ（点１，点２）を取得する。続いて、算出部２３６は、２点の組合せ（点１，点２）について２点間の距離の候補値を算出する。次に、算出部２３６は、算出した候補値に対応する３次元空間上のマーカー間の実距離の値をマーカー距離情報記憶部２２３から取得し、この候補値と、この候補値に対応する実距離の値との誤差を算出する。続いて、算出部２３６は、算出した誤差の値を誤差Ｅの値に加算して更新し、誤差Ｅの値が最小誤差Ｊの値よりも大きいか否かを判定する。算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値以下である場合には、符号パターン（＋、＋、＋）内で基準点０以外の２点の組合せの全て、（点１，点２）、（点１，点３）および（点２，点３）について処理を完了したか否かを判定する。
【００６３】
算出部２３６は、判定の結果、基準点０以外の２点の組合せの全てについて処理を完了していない場合には、同一の符号パターン（＋、＋、＋）内で基準点０以外の２点の組合せ、例えば（点１，点３）を取得する。そして、算出部２３６は、上述してきた基準点０以外の２点の組合せ（点１，点２）に対して実行した処理と同様の手順で処理を行う。すなわち、算出部２３６は、２点の組合せ（点１，点３）について２点間の距離の候補値を算出し、算出した候補値に対応する３次元空間上の実距離の値を取得し、この候補値と実距離の値との誤差を算出する。続いて、算出部２３６は、算出した誤差の値を誤差Ｅの値に加算して更新し、誤差Ｅの値が最小誤差Ｊの値よりも大きいか否かを判定する。算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値以下である場合には、誤差Ｅの値を最小誤差Ｊの値に加算して更新し、再び、符号パターン（＋、＋、＋）内で基準点０以外の２点の組合せの全てについて処理を完了したか否かを判定する。
【００６４】
なお、算出部２３６により、最初に選択された符号パターン（＋、＋、＋）については、最小誤差Ｊの値が無限大に設定されているので、基準点０以外の２点の組合せの全てについての処理が必ず完了される。よって、図７に示すように、符号パターン（＋、＋、＋）についての誤差Ｅの値「Ｅ０００」が得られる。この誤差Ｅの値「Ｅ０００」は、２点の組合せ（点１，点２）についての誤差の値と、２点の組合せ（点１，点３）についての誤差の値と、２点の組合せ（点２，点３）についての誤差の値が加算されたものに相当する。そして、この誤差Ｅの値「Ｅ０００」は、符号パターン（＋、＋、＋）の後に実行される他の符号パターンに対する処理の中で最小誤差Ｊの値として利用される。このため、上述した手順で処理を行えば、各符号パターンについて算出された誤差の値の中で最も小さい誤差の値を得ることが可能となる。
【００６５】
ここで、符号パターン（＋、＋、＋）内で基準点０以外の２点の組合せの全てについて処理を完了したか否かの判定結果が上述した判定結果と異なり、基準点０以外の２点の組合せの全てについて処理を完了している場合について説明する。算出部２３６は、判定の結果、基準点０以外の２点の組合せの全てについて処理を完了している場合には、誤差Ｅの値で最小誤差Ｊの値を更新する。次に、算出部２３６は、図７に示す各符号パターンの中から次の符号パターン、例えば、符号パターン（＋、＋、−）を選択する。そして、算出部２３６は、上述してきた符号パターン（＋、＋、＋）に対して実行した処理と同様の手順で、符号パターン（＋、＋、−）に対し処理を行う。
【００６６】
ここで、上述した誤差Ｅの値が最小誤差Ｊの値よりも大きいか否かの判定結果が上述した判定結果と異なり、誤差Ｅの値が最小誤差Ｊの値よりも大きい場合について説明する。算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値よりも大きい場合には、現在選択中の符号パターンに対する処理を取りやめ、図７に示す各符号パターンの中から次の符号パターンを選択する。そして、算出部２３６は、上述してきた符号パターン（＋、＋、＋）に対して実行した処理と同様の手順で処理を行う。
【００６７】
例えば、算出部２３６は、符号パターン（＋、＋、−）の処理について、誤差Ｅの値が最小誤差Ｊの値よりも大きいという判定結果が得られた場合には、符号パターン（＋、＋、−）の処理を取りやめ、例えば、未選択の符号パターン（＋、−、−）を選択する。次に、算出部２３６は、符号パターン（＋、−、−）内で基準点０以外の２点の組合せ（点１，点２）を取得する。続いて、算出部２３６は、２点の組合せ（点１，点２）について２点間の距離の候補値を算出し、算出した候補値に対応する３次元空間上の実距離の値を取得し、この候補値と実距離の値との誤差を算出する。次に、算出部２３６は、算出した誤差の値を誤差Ｅの値に加算して更新し、誤差Ｅの値が最小誤差Ｊの値よりも大きいか否かを判定する。算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値以下である場合には、上述してきたのと同様に、符号パターン（＋、−、−）内で基準点０以外の２点の組合せの全てについて処理を完了したか否かを判定する。以後、算出部２３６は、上述してきた同様の手順で処理を行う。一方、算出部２３６は、誤差Ｅの値が最小誤差Ｊの値よりも大きい場合には、誤差Ｅの値を最小誤差Ｊの値に加算して更新し、再び、符号パターン（＋、−、−）内で基準点０以外の２点の組合せの全てについて処理を完了したか否かを判定する。以下、算出部２３６は、上述してきた同様の手順で処理を行う。
【００６８】
上述してきた手順で、算出部２３６は、図７に示す各符号パターンについての演算を行うことにより、図８に示すように、誤差「Ｅ０００」、「Ｅ００１」、「Ｅ００２」、「Ｅ００３」の中で最も値の小さい最小誤差（ｍｉｎＥ０００〜Ｅ００３）の値を得る。そして、算出部２３６は、「基準点＝点０」および「距離Ｌ０＝Ｌ」に関し、最小誤差（ｍｉｎＥ０００〜Ｅ００３）の値と、これに対応するＬ０、Ｌ１、Ｌ２およびＬ３の値を処理結果として後述する決定部２３７に送出する。
【００６９】
また、図９に示すように、算出部２３６は、基準点０までの距離の値として、「Ｌ」とは異なる値「Ｌ＋ΔＬ」が設定部２３２により設定されると、「基準点＝点０」および「距離Ｌ０＝Ｌ＋ΔＬ」に関し、上述してきた手順で処理を実行する。そして、算出部２３６は、「基準点＝点０」および「距離Ｌ０＝Ｌ＋ΔＬ」に関し、最小誤差（ｍｉｎＥ０１０〜Ｅ０１３）の値と、これに対応するＬ０、Ｌ１、Ｌ２およびＬ３の値を処理結果として後述する決定部２３７に送出する。
【００７０】
また、図１０に示すように、算出部２３６は、点０とは異なる点１が新たな基準点として設定部２３２により設定されると、「基準点＝点１」および「距離Ｌ０＝Ｌ´」に関し、上述してきた手順で処理を実行する。そして、算出部２３６は、「基準点＝点１」および「距離Ｌ０＝Ｌ´」に関し、最小誤差ｍｉｎ（Ｅ０２０〜Ｅ０２ｖ：ｖは自然数）の値と、これに対応するＬ０、Ｌ１、Ｌ２およびＬ３の値を処理結果として送出する。また、算出部２３６は、図１０には示していないが、基準点１までの距離の値として、「Ｌ´」とは異なる値「Ｌ´＋ΔＬ´」が設定部２３２により設定されると、「基準点＝点１」および「距離Ｌ０＝Ｌ´＋ΔＬ´」に関し、上述してきた手順で処理を実行する。そして、算出部２３６は、「基準点＝点１」および「距離Ｌ０＝Ｌ´＋ΔＬ´」に関し、最小誤差の値と、これに対応するＬ０、Ｌ１、Ｌ２およびＬ３の値を処理結果として後述する決定部２３７に送出する。
【００７１】
なお、算出部２３６は、基準点までの距離の設定があるたびに、基準点および基準点までの距離に対応する各符号パターンに対応する誤差のうちの最小誤差の値を算出する場合を説明したが、これに限定されるものではない。例えば、算出部２３６は、同一の基準点に対する処理内では、最小誤差Ｊを初期化せずに更新を続け、同一の基準点内の各符号パターンの中で値が最も小さい最小誤差を算出するようにしてもよい。あるいは、算出部２３６は、全ての基準点に対する処理内で最小誤差Ｊを初期化せずに更新を続け、全ての基準点内の各符号パターンに対応する誤差のうちの最小誤差の値を算出するようにしてもよい。
【００７２】
図２に戻り、決定部２３７は、算出部２３６からの処理結果に基づいて、カメラ座標系の原点から、４個以上のマーカーが映し出された画像平面上の点に対応する３次元空間上の対応点の位置までの距離を決定する。
【００７３】
例えば、決定部２３７は、最小誤差の値と、これに対応するＬ０、Ｌ１、Ｌ２およびＬ３の値を処理結果として算出部２３６から取得すると、取得した処理結果を保存する。そして、決定部２３７は、基準点までの距離が取りうる値の全てを設定して処理を完了しているか否かを判定する。例えば、決定部２３７は、現在基準点０が選択されるとすると、基準点までの距離Ｌ０が取りうる値の全てを設定して処理を完了しているか否かを判定することとなる。判定の結果、距離Ｌ０が取りうる値の全てを設定して処理を完了していない場合には、決定部２３７は、設定部２３２に対して、距離Ｌ０の値として「Ｌ」とは異なる別の値を設定するように依頼する。
【００７４】
一方、決定部２３７は、判定の結果、距離Ｌ０が取りうる値の全てを設定して処理を完了している場合には、対応点である点０〜点３の全てを基準点として処理を完了しているか否かを判定する。例えば、決定部２３７は、現在基準点０が選択されるとすると、残りの対応点である点１〜点３の全てを基準点として処理を完了したか否かを判定することとなる。判定の結果、対応点である点０〜点３の全てを基準点として処理を完了していない場合には、決定部２３７は、設定部２３２に対して、新たな基準点および距離の設定を依頼する。
【００７５】
一方、決定部２３７は、判定の結果、対応点である点０〜点３の全てを基準点として処理を完了している場合には、算出部２３６から取得してこれまで保存した処理結果の中で、最小誤差の値が最も小さいものを特定する。例えば、決定部２３７は、図１０に示す最小誤差ｍｉｎ（Ｅ０００〜Ｅ００３）、ｍｉｎ（Ｅ０１０〜Ｅ０１３）、ｍｉｎ（Ｅ０２０〜Ｅ０２ｖ：ｖは自然数）等の中から、値が最も小さい最小誤差を特定する。そして、決定部２３７は、特定された最小誤差に対応付けられているＬ０、Ｌ１、Ｌ２およびＬ３の値を最適解として決定し、位置姿勢推定部２３８に送る。
【００７６】
図２に戻り、位置姿勢推定部２３８は、決定部２３７から最適解として取得したＬ０、Ｌ１、Ｌ２およびＬ３の値を用いて、画像平面上の点に対応するカメラ座標系における３次元位置を算出し、算出した位置を用いてカメラの位置および姿勢を算出する。以下に、図１１を用いて、実施例１に係る位置姿勢推定部２３８について説明する。図１１は、実施例１に係る位置姿勢推定部の説明に用いる図である。図１１に示すαはカメラ座標系を示す。また、図１１に示すβは世界座標系を示す。また、図１１に示すＷ０、Ｗ１、Ｗ２は、世界座標系におけるマーカーの３次元位置を示し、Ｗ０は点０、Ｗ１は点１、Ｗ２は点２にそれぞれ対応するものとする。また、カメラの位置および姿勢を算出する技術自体は公知技術であり、例えば、玉木 徹 他、「姿勢推定と回転行列」（電気情報通信学会研究報告、信学技報,vol.109, no.203, SIS2009-23, pp.59-64, 2009年9月）等を参照されたい。なお、以下では、説明を簡略化するために、マーカーが映し出された画像平面上の点に対応する３つの対応点（点０、点１および点２）を例に挙げて説明するが、対応点の数が４つでも同様に処理できる。
【００７７】
例えば、位置姿勢推定部２３８は、決定部２３７から最適解として取得したＬ０、Ｌ１およびＬ２の値と上述した式（５）とを用いて、画像平面上の点に対応するカメラ座標系における対応点Ｐ０、Ｐ１、Ｐ２を算出する。そして、位置姿勢推定部２３８は、カメラ座標系における３次元位置（Ｐ０、Ｐ１、Ｐ２）と、これらに対応する世界座標系におけるマーカーの位置（Ｗ０、Ｗ１、Ｗ２）とに基づいて、公知技術を利用してカメラの位置および姿勢を算出する。
【００７８】
［画像処理装置による処理（実施例１）］
次に、図１２〜図１４を用いて、実施例１に係る画像処理装置による処理の流れを説明する。図１２は、実施例１に係る最適解算出処理の流れを説明する。図１２は、実施例１に係る最適解算出処理の流れを示す図である。図１２に示す処理は、画像フレームが取得されるたびに実行される。
【００７９】
図１２に示すように、例えば、制御部２３０により取得部２１０による画像取得が検出されると（ステップＳ１０１，ＹＥＳ）、生成部２３１は、視線ベクトルを生成する（ステップＳ１０２）。なお、取得部２１０による画像取得が検出されない場合（ステップＳ１０１，ＮＯ）、制御部２３０は、取得部２１０による画像取得の検出判定を繰り返し実行する。
【００８０】
次に、設定部２３２は、各マーカーに対応する点の中から基準マーカーに対応する基準点Ｍ（点０〜点３のいずれか）を選択する（ステップＳ１０３）。続いて、導出部２３４は、設定部２３２により設定された基準マーカーで採用する符号パターンを全て導出する（ステップＳ１０４）。次に、設定部２３２は、カメラ座標系の原点Ｃから基準点Ｍまでの距離Ｌ０に所定の値を設定する（ステップＳ１０５）。続いて、作成部２３３は、カメラ座標系の原点Ｃから基準点Ｍ以外の各点（点１〜３）までの距離Ｌ１、Ｌ２およびＬ３の値を表す数式を作成する（ステップ１０６）。次に、画像処理装置２００により実行される処理は最適解候補算出処理に移行される（ステップＳ１０７）。最適解候補算出処理は、主に、算出部２３６によりが実行される。なお、最適解候補算出処理については、図１３を用いて後述する。
【００８１】
続いて、決定部２３７は、ステップＳ１０７における最適解候補算出処理の処理結果を取得すると、取得した処理結果を保存する（ステップＳ１０８）。次に、決定部２３７は、基準点Ｍまでの距離Ｌ０が取りうる値の全てを設定して処理を完了しているか否かを判定する（ステップＳ１０９）。
【００８２】
判定の結果、距離Ｌ０が取りうる値の全てを設定して処理を完了していない場合には（ステップＳ１０９，ＮＯ）、決定部２３７は、設定部２３２に対して、距離Ｌ０の値として「Ｌ」とは異なる値を設定するように依頼する。図１２に示す処理は、上述したステップＳ１０５に処理を戻す。
【００８３】
一方、決定部２３７は、判定の結果、距離Ｌ０が取りうる値の全てを設定して処理を完了している場合には（ステップＳ１０９，ＹＥＳ）、点０〜点３の全てを基準点として処理を完了しているか否かを判定する（ステップＳ１１０）。判定の結果、点０〜点３の全てを基準点として処理を完了していない場合には（ステップＳ１１０，ＮＯ）、決定部２３７は、上述したステップＳ１０３に処理を戻す。
【００８４】
一方、決定部２３７は、判定の結果、点０〜点３の全てを基準点として処理を完了している場合には（ステップＳ１１０，ＹＥＳ）、次のように処理する。すなわち、決定部２３７は、ステップＳ１０７にて保存された最小誤差の中から、最小誤差の値が最小のときのＬ０、各Ｌｉ（ｉ＝１〜３）の値を最適解として決定し、位置姿勢推定部２３８に送出する（ステップＳ１１１）。以上で、実施例１に係る最適解算出処理が完了するが、上述してきた図１２に示す最適解算出処理は、処理内容に矛盾を生じさせない範囲でその順序を入れ替えることができる。
【００８５】
なお、上述したステップＳ１０９およびステップＳ１１０の判定処理により、基準点までの距離Ｌの最適化を行われることとなり、図１２に示す処理の内容は以下に示す式（６）で表すことができる。なお、以下の式（６）に示す「ｉ０」は基準点を示し、「Ｌｉ０」は基準点までの距離を示す。
【００８６】
【数６】
【００８７】
続いて、図１３を用いて、実施例１に係る最適解候補算出処理の流れを説明する。図１３は、実施例１に係る最適解候補算出処理の流れを示す図である。図１３に示すように、導出部２３４は、図１２に示すステップＳ１０６にて作成された全数式の根号の値を算出し、全数式の根号の値が実数であるか否かを判定する（ステップＳ２０１）。判定の結果、根号の値が実数ではない場合には（ステップＳ２０１，ＮＯ）、導出部２３４は、図１２に示すステップＳ１０９に処理を戻す。
【００８８】
一方、根号の値が実数であると導出部２３４により判定された場合には（ステップＳ２０１，ＹＥＳ）、画像処理装置２００により実行される処理は符号パターン決定処理に移行される（ステップＳ２０２）。符号パターン決定処理は、棄却部２３５により実行される。符号パターン決定処理については、図１４を用いて後述する。
【００８９】
続いて、算出部２３６は、後に用いる最小誤差Ｊの値を初期化し（ステップＳ２０３）、ステップＳ２０３に示す符号パターン決定処理により得られる各符号パターンについて、以下のステップＳ２０４〜ステップＳ２１５までの処理を実行する。なお、ステップＳ２０４〜ステップＳ２１５は、全符号パターンについてステップＳ２０５〜ステップＳ２１４の処理が完了するまで繰り返し実行することを意味する。算出部２３６は、符号パターンを一つ選択し（ステップＳ２０４）、選択した符号パターンについて距離Ｌ１、Ｌ２およびＬ３の値をそれぞれ算出する（ステップＳ２０５）。次に、算出部２３６は、距離Ｌ１、Ｌ２およびＬ３の値が正の実数であるか否かを判定する（ステップＳ２０６）。判定の結果、算出部２３６は、距離Ｌ１、Ｌ２およびＬ３の値が正の実数ではない場合には（ステップＳ２０６，ＮＯ）、次の符号パターンを選択する。これとは反対に、算出部２３６は、判定の結果、距離Ｌ１、Ｌ２およびＬ３の値が正の実数である場合には（ステップＳ２０６，ＹＥＳ）、誤差Ｅの値を初期化（Ｅ＝０）する（ステップＳ２０７）。
【００９０】
続いて、算出部２３６は、ステップＳ２０４にて選択した符号パターン内で基準点Ｍ以外の２点（点１〜３のうちの２点）の組合せを一つ取得する（ステップＳ２０８）。次に、算出部２３６は、取得した２点の組合せについて２点間の距離の候補値を算出する（ステップＳ２０９）。続いて、算出部２３６は、算出した候補値に対応する３次元空間上のマーカー間の実距離の値をマーカー距離情報記憶部２２３から取得し、この候補値と、この候補値に対応する実距離の値との誤差を演算する（ステップＳ２１０）。そして、算出部２３６は、算出した誤差の値を誤差Ｅの値に加算して更新し（ステップＳ２１１）、誤差Ｅの値が最小誤差Ｊの値よりも大きいか否かを判定する（ステップＳ２１２）。
【００９１】
算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値以下である場合には（ステップＳ２１２，ＮＯ）、選択した符号パターン内で基準点０以外の２点の組合せの全てについて処理を完了したか否かを判定する（ステップＳ２１３）。算出部２３６は、判定の結果、選択した符号パターン内で基準点０以外の２点の組合せの全てについて処理を完了している場合には（ステップＳ２１３，ＹＥＳ）、誤差Ｅの値で最小誤差Ｊの値を更新する（ステップＳ２１４）。続いて、ステップＳ２０２の符号パターン決定処理により得られる全符号パターンについてステップＳ２０５〜ステップＳ２１４の処理が完了するまで（ステップＳ２１５）、上述したステップＳ２０２に処理を戻す。
【００９２】
ここで、ステップＳ２１３の説明に戻る。算出部２３６は、判定の結果、選択した符号パターン内で基準点０以外の２点の組合せの全てについて処理を完了していない場合には（ステップＳ２１３，ＮＯ）、上述したステップＳ２０８に処理を戻す。
【００９３】
ここで、ステップＳ２１２の説明に戻る。算出部２３６は、判定の結果、誤差Ｅの値が最小誤差Ｊの値よりも大きい場合には（ステップＳ２１３，ＹＥＳ）、次のように処理を実行する。すなわち、算出部２３６は、ステップＳ２０２の符号パターン決定処理により得られる全符号パターンについてステップＳ２０５〜ステップＳ２１４の処理が完了するまで（ステップＳ２１５）、上述したステップＳ２０２に処理を戻す。
【００９４】
そして、算出部２３６は、ステップＳ２０４〜ステップＳ２１５までの処理を完了した場合には、次のステップＳ２１６の処理を行う。すなわち、算出部２３６は、「基準点＝Ｍ（点０〜点３のいずれか）」に関し、最終的に残った誤差Ｅの値、すなわち最小誤差の値と、最小誤差に対応する符号パターンの距離Ｌ０、Ｌ１、Ｌ２およびＬ３の値を処理結果として送出する（ステップＳ２１６）。以上で、実施例１に係る最適解候補算出処理が完了するが、上述してきた図１３に示す最適解算出処理は、処理内容に矛盾を生じさせない範囲でその順序を入れ替えることができる。
【００９５】
なお、上述したステップＳ２１０で用いるマーカー間の実距離の値は、以下の式（７）で表すことができる。
【００９６】
【数７】
【００９７】
なお、上述したステップＳ２０５で算出される距離Ｌ１、Ｌ２およびＬ３の値は、以下に示す式（８）の解を求めることにより算出することができる。
【００９８】
【数８】
【００９９】
なお、上述したステップＳ２１０で算出される誤差の値は、以下の式（９）で表すことができる。
【０１００】
【数９】
【０１０１】
なお、上述したステップＳ２１１の処理は、以下の式（１０）で表すことができる。
【０１０２】
【数１０】
【０１０３】
次に、図１４を用いて、実施例１に係る符号パターン決定処理の流れを説明する。図１４は、実施例１に係る符号パターン決定処理の流れを示す図である。図１４に示すように、符号パターン決定処理では、基準点Ｍ以外の全ての対応点Ｎ（点１〜点３）について、ステップＳ３０１〜ステップＳ３０５までの処理が繰り返し実行される。
【０１０４】
まず、棄却部２３５は、基準点Ｍ以外の対応点Ｎ（点１〜点３のいずれか）を一つ取得し（ステップＳ３０１）、∠ＭＣＮが９０度よりも大きいか否かを判定する（ステップＳ３０２）。なお、∠ＭＣＮとは、原点Ｃ及び基準点Ｍを結ぶ線分と、原点Ｃ及び対応点Ｎを結ぶ線分とがなす角度をいう。∠ＭＣＮが９０度よりも大きいか否かの判定は、上述したように、原点Ｃ及び基準点Ｍを結ぶ線分上の視線ベクトルと、原点Ｃ及び対応点Ｎを結ぶ線分上の視線ベクトルとの内積に基づいて行なう。
【０１０５】
棄却部２３５は、判定の結果、∠ＭＣＮが９０度よりも大きい場合には（ステップＳ３０２，ＹＥＳ）、選択した対応点Ｎの一点ではプラスの符号のみを取り得ることを記録する（ステップＳ３０３）。一方、棄却部２３５は、判定の結果、∠ＭＣＮが９０度以下である場合には（ステップＳ３０２，ＮＯ）、選択した対応点Ｎの一点ではプラス、マイナスの両方の符号を取り得ることを記録する（ステップＳ３０４）。
【０１０６】
そして、棄却部２３５は、基準点Ｍ以外の全ての対応点Ｎについて、ステップＳ３０１〜ステップＳ３０５までの処理を終えると、対応点Ｎが取り得る符号の組合せを列挙して符号パターンを決定する（ステップＳ３０６）。以上で、実施例１に係る符号パターン決定処理が完了する。
【０１０７】
上述してきた図１２〜図１４の処理の意味は、以下の数式（１１）で表すことができる。なお、以下に示す式（１１）の左辺のＪは、上述した式（６）のＪ、つまり、Ｌｉを符号選択によって求めることを含んだ誤差評価関数である。そして、式（１１）から得られる解は、基準点「ｉ０」までの距離を「Ｌｉ０」とした時に、基準点「ｉ０」以外の点ｉまでの距離「Ｌｉ」を符号選択により最適に選んだ場合に残っている誤差となる。よって、式（１１）は、Ｊを最小化する解を探索することにより、基準点「ｉ０」までの最適な距離「Ｌｉ０」と、基準点「ｉ０」以外の点ｉまでの最適な距離「Ｌｉ」を同時に得ることができることを表している。
【０１０８】
【数１１】
【０１０９】
［実施例１による効果］
上述してきたように、実施例１に係る画像処理装置２００は、導出された符号パターンの中から、距離Ｌ１、Ｌ２、Ｌ３の値の少なくとも１つが相応しい値となり得えない符号パターンを棄却する。よって、実施例１に係る画像処理装置２００は、４個以上のマーカーが映し出された画像平面上の点に対応する３次元空間上の対応点の位置を迅速かつ精度良く算出できる。この結果、実施例１によれば、カメラの取り付け位置や取り付け姿勢の情報を迅速かつ精度良く得ることができる。なお、３点の場合は、最少誤差を与える解の組合せである最適解が１つに定まらない。このため、上述してきた実施例１は、４点以上のマーカーを用いて、画像処理装置が最適解算出処理を行う。この結果、実施例１では、最適解を得ることができる。
【０１１０】
また、実施例１に係る画像処理装置は、例えば、図１３に示すように、符号パターンの誤差を算出するときに、従前に算出された符号パターンの誤差にうちの最小誤差よりも誤差が大きくなる符号パターンの処理は取りやめて、次の符号パターンの処理を行う。このため、実施例１によれば、４個以上のマーカーが映し出された画像平面上の点に対応する３次元空間上の対応点の位置をより迅速に算出できる。この結果、実施例１によれば、カメラの取り付け位置や取り付け姿勢の情報をより迅速に得ることができる。
【０１１１】
また、実施例１に係る画像処理装置は、基準点Ｍ以外の点、つまり基準マーカー以外のマーカーに対応する点１〜点３までの各距離Ｌ１，Ｌ２，Ｌ３を表す数式の中の根号内が虚数となる場合には、基準点までの距離や基準点の設定をやり直す。このため、実施例１によれば、４個以上のマーカーが映し出された画像平面上の点に対応する３次元空間上の対応点の位置をより迅速に算出できる。この結果、実施例１によれば、カメラの取り付け位置や取り付け姿勢の情報をより迅速に得ることができる。
【実施例２】
【０１１２】
以下、本願の開示する画像処理プログラムおよび画像処理装置の他の実施形態を説明する。
【０１１３】
（１）装置構成等
例えば、図２に示した画像処理装置２００の機能ブロックの構成は概念的なものであり、必ずしも物理的に図示の如く構成されていることを要しない。例えば、図２に示す制御部２３０を、図１５に示すように、カメラ座標系制御ユニット２３０Ａと位置姿勢推定ユニット２３０Ｂとに機能的または物理的に分散してもよい。このように、画像処理装置２００の機能ブロックの全部または一部を、各種の負荷や使用状況などに応じて、任意の単位で機能的または物理的に分散・統合して構成することができる。図１５は、実施例２に係る画像処理装置の構成を示す機能ブロック図である。
【０１１４】
（２）画像処理プログラム
また、上述の実施例１にて説明した画像処理装置２００により実行される各種の処理は、例えば、車両に搭載されるＥＣＵ（Electronic Control Unit）に実装されたマイコンなどの電子機器で所定のプログラムを実行することによって実現することもできる。あるいは、上述の実施例１にて説明した画像処理装置２００により実行される各種の処理は、ロボットなどに実装されるマイクロプロセッサなどの電子機器で所定のプログラムを実行することによって実現することもできる。そこで、以下では、図１６を用いて、実施例１にて説明した画像処理装置２００により実行される処理と同様の機能を実現する画像処理プログラムを実行する電子機器の一例を説明する。図１６は、画像処理プログラムを実行する電子機器の一例を示す図である。
【０１１５】
図１６に示すように、画像処理装置２００により実行される各種処理を実現する電子機器３００は、各種演算処理を実行するＣＰＵ（Central Processing Unit）３１０を有する。また、図１６に示すように、電子機器３００は、カメラ画像を取得するためのカメラインターフェース３２０、ディスプレイとの間で各種データのやり取りを行うためのディスプレイインターフェース３３０を有する。また、図１６に示すように、電子機器３００は、ハードウェアアクセラレータとして機能するグラフィックエンジン３４０を有する。
【０１１６】
また、図１６に示すように、電子機器３００は、ＣＰＵ３１０により各種処理を実現するためのプログラムやデータ等を記憶するハードディスク装置３５０と、各種情報を一時記憶するＲＡＭ（Random Access Memory）などのメモリ３６０とを有する。そして、各装置３１０〜３６０は、バス３７０に接続される。
【０１１７】
なお、ＣＰＵ３１０の代わりに、例えば、ＭＰＵ（Micro Processing Unit）などの電子回路、ＡＳＩＣ（Application Specific Integrated Circuit）やＦＰＧＡ(Field Programmable Gate Array)などの集積回路を用いることもできる。また、メモリ３６０の代わりに、フラッシュメモリ(flash memory)などの半導体メモリ素子を用いることもできる。また、ハードディスク装置３５０の代わりに、フラッシュメモリなどの不揮発性の半導体メモリ素子用いることもできる。
【０１１８】
ハードディスク装置３５０には、画像処理装置２００の機能と同様の機能を発揮する画像処理プログラム３５１および画像処理用データ３５２が記憶されている。なお、この画像処理プログラム３５１を適宜分散させて、ネットワークを介して通信可能に接続された他のコンピュータの記憶部に記憶させておくこともできる。
【０１１９】
そして、ＣＰＵ３１０が、画像処理プログラム３５１をハードディスク装置３５０から読み出してメモリ３６０に展開することにより、図１６に示すように、画像処理プログラム３５１は画像処理プロセス３６１として機能する。画像処理プロセス３６１は、ハードディスク装置３５０から読み出した画像処理用データ３５２等の各種データを適宜メモリ３６０上の自身に割当てられた領域に展開し、この展開した各種データに基づいて各種処理を実行する。なお、図１６に示すように、画像処理プロセス３６１は、プロセス実行時にメモリ３６０に補助的に格納されるが、画像処理プロセス３６１は、あくまでもＣＰＵ３１０上で動作しているものとする。
【０１２０】
なお、画像処理プロセス３６１は、例えば、実施例１で説明した画像処理装置２００の制御部２３０にて実行される処理、例えば、図１２〜１４に示す処理を含む。
【０１２１】
なお、画像処理プログラム３５１については、必ずしも最初からハードディスク装置３５０に記憶させておく必要はない。例えば、電子機器３００が実装されたＥＣＵへ対応ドライブを接続可能なフレキシブルディスク（ＦＤ）、ＣＤ−ＲＯＭ、ＤＶＤディスク、光磁気ディスク、ＩＣカードなどの「可搬用の物理媒体」に各プログラムを記憶させておく。そして、電子機器３００がこれらから各プログラムを読み出して実行するようにしてもよい。
【０１２２】
さらには、公衆回線、インターネット、ＬＡＮ、ＷＡＮなどを介して、電子機器３００が実装されたＥＣＵに接続される「他のコンピュータ（またはサーバ）」などに各プログラムを記憶させておく。そして、電子機器３００がこれらから各プログラムを読み出して実行するようにしてもよい。
【符号の説明】
【０１２３】
１００ 撮影装置
２００ 画像処理装置
２１０ 取得部
２２０ 記憶部
２２１ 画像記憶部
２２２ カメラパラメータ記憶部
２２３ マーカー距離情報記憶部
２３０ 制御部
２３１ 生成部
２３２ 設定部
２３３ 作成部
２３４ 導出部
２３５ 棄却部
２３６ 算出部
２３７ 決定部
２３８ 位置姿勢推定部
３００ 電子機器
３１０ ＣＰＵ
３２０ カメラインターフェース
３３０ ディスプレイインターフェース
３４０ グラフィックエンジン
３５０ ハードディスク装置
３５１ 画像処理プログラム
３５２ 画像処理用データ
３６０ メモリ
３６１ 画像処理プロセス
【特許請求の範囲】
【請求項１】
コンピュータに、
実空間に設けられた４個以上のマーカーをカメラにより撮影した画像を取得し、
前記カメラの焦点距離および光軸位置の情報を含む内部パラメータおよび前記画像における前記４個以上のマーカーの各々の位置に基づいて、前記カメラから該４個以上のマーカーの各々に対応するベクトルを生成し、
前記４個以上のマーカーの中から基準マーカーを一つ選択し、
前記基準マーカー以外の残りのマーカーのうちのそれぞれに対するベクトルと、該基準マーカーのベクトルとの内積値を算出し、
前記内積値の符号が負となる前記基準マーカー以外のマーカーについて、該基準マーカーを用いて該基準マーカー以外のマーカーと前記カメラとの距離を求める二次方程式の解を表す数式に含まれる符号のうち、負の符号の採用を棄却し、
棄却した結果に基づいて、前記基準マーカー以外のマーカーのそれぞれに対する前記数式に含まれる符号を、全ての該基準マーカー以外のマーカーについて組み合わせた符号パターンを生成し、
前記基準マーカーと前記カメラとの距離として第１の値を設定し、
前記第１の値と前記二次方程式と棄却した前記結果とを用いて、該第１の値における、前記基準マーカー以外のマーカーの各々と前記カメラとの距離の候補値を、１つまたは２つ算出し、
前記符号パターンごとに、該符号パターン及び前記１つまたは２つの候補値に基づいて算出されるマーカー間距離と、前記実空間におけるマーカー間の距離との誤差を算出し
前記第１の値とは異なる第２の値を設定した場合に、前記符号パターンごとに他の誤差を算出し、
前記第１の値を選択した場合に算出された前記誤差および前記第２の値を選択した場合に算出された前記他の誤差に基づいて、前記４個以上のマーカーの各々と前記カメラとの距離を決定し、
決定した前記４個以上のマーカーの各々と前記カメラとの距離に基づいて、該カメラの世界座標系における位置および姿勢を演算する
処理を実行させることを特徴とする画像処理プログラム。
【請求項２】
誤差を算出する処理において、従前に算出された第一の符号パターンのうちの最小誤差の値よりも、第二の符号パターンにおける前記基準マーカー以外のマーカーのうちの第一のマーカーと該第一のマーカとは異なる第二のマーカーとのマーカー間距離に対する誤差が小さい場合に、該第二の符号パターンにおける第三のマーカと該第三のマーカとは異なる第四のマーカーとの誤差を算出し、該第一のマーカーと該第二のマーカーとのマーカー間距離の誤差、及び該第三のマーカーと該第四のマーカーとのマーカー間距離の誤差を加算することを特徴とする請求項１に記載の画像処理プログラム。
【請求項３】
前記コンピュータに、
前記数式を作成する処理により作成された数式で与えられる解が実数となるか否かを判定し、前記数式の解が実数とならないとの判定結果を得た場合に、前記第１の値とは異なる第２の値を設定する処理をさらに実行させることを特徴とする請求項１または２に記載の画像処理プログラム。
【請求項４】
前記第一の値および前記第二の値は、前記カメラを有する車体の設計における取り付け位置に基づいて予め設定された数値範囲に含まれる値であることを特徴とする請求項１乃至請求項３のいずれか一項に記載の画像処理プログラム。
【請求項５】
前記コンピュータに、
演算した前記カメラの世界座標系における位置および姿勢を、前記車体が有する記憶領域に格納する処理を更に実行させることを特徴とする請求項４に記載の画像処理プログラム。
【請求項６】
実空間に設けられた４個以上のマーカーをカメラにより撮影した画像を取得する取得部と、
前記カメラの焦点距離および光軸位置の情報を含む内部パラメータおよび前記画像における前記４個以上のマーカーの各々の位置に基づいて、前記カメラから該４個以上のマーカーの各々に対応するベクトルを生成する生成部と、
前記４個以上のマーカーの中から基準マーカーを一つ選択し、前記基準マーカーと前記カメラとの距離として第１の値、または前記第１の値とは異なる第２の値を設定する設定部と、
前記基準マーカー以外の残りのマーカーのうちのそれぞれに対するベクトルと、該基準マーカーのベクトルとの内積値を算出し、前記内積値の符号が負となる前記基準マーカー以外のマーカーについて、該基準マーカーを用いて該基準マーカー以外のマーカーと前記カメラとの距離を求める二次方程式の解を表す数式に含まれる符号のうち、負の符号の採用を棄却し、棄却した結果に基づいて、前記基準マーカー以外のマーカーのそれぞれに対する前記数式に含まれる符号を、全ての該基準マーカー以外のマーカーについて組み合わせた符号パターンを生成する棄却部と、
前記第１の値と、前記二次方程式と、棄却した前記結果とを用いて、該第１の値における、前記基準マーカー以外のマーカーの各々と前記カメラとの距離の候補値を、１つまたは２つ算出し、前記符号パターンごとに、該符号パターン及び前記１つまたは２つの候補値に基づいて算出されるマーカー間距離と、前記実空間におけるマーカー間の距離との誤差を算出する算出部と、
前記第１の値とは異なる第２の値が前記設定部により設定された場合に、前記符号パターンごとに他の誤差を算出し、
前記第１の値を設定した場合に算出された前記誤差および前記第２の値を設定した場合に算出された前記他の誤差に基づいて、前記４個以上のマーカーの各々と前記カメラとの距離を決定する決定部と、
前記決定部により決定された前記４個以上のマーカーの各々と前記カメラとの距離に基づいて、該カメラの世界座標系における位置および姿勢を演算する演算部と
を有することを特徴とする画像処理装置。
【請求項１】
コンピュータに、
実空間に設けられた４個以上のマーカーをカメラにより撮影した画像を取得し、
前記カメラの焦点距離および光軸位置の情報を含む内部パラメータおよび前記画像における前記４個以上のマーカーの各々の位置に基づいて、前記カメラから該４個以上のマーカーの各々に対応するベクトルを生成し、
前記４個以上のマーカーの中から基準マーカーを一つ選択し、
前記基準マーカー以外の残りのマーカーのうちのそれぞれに対するベクトルと、該基準マーカーのベクトルとの内積値を算出し、
前記内積値の符号が負となる前記基準マーカー以外のマーカーについて、該基準マーカーを用いて該基準マーカー以外のマーカーと前記カメラとの距離を求める二次方程式の解を表す数式に含まれる符号のうち、負の符号の採用を棄却し、
棄却した結果に基づいて、前記基準マーカー以外のマーカーのそれぞれに対する前記数式に含まれる符号を、全ての該基準マーカー以外のマーカーについて組み合わせた符号パターンを生成し、
前記基準マーカーと前記カメラとの距離として第１の値を設定し、
前記第１の値と前記二次方程式と棄却した前記結果とを用いて、該第１の値における、前記基準マーカー以外のマーカーの各々と前記カメラとの距離の候補値を、１つまたは２つ算出し、
前記符号パターンごとに、該符号パターン及び前記１つまたは２つの候補値に基づいて算出されるマーカー間距離と、前記実空間におけるマーカー間の距離との誤差を算出し
前記第１の値とは異なる第２の値を設定した場合に、前記符号パターンごとに他の誤差を算出し、
前記第１の値を選択した場合に算出された前記誤差および前記第２の値を選択した場合に算出された前記他の誤差に基づいて、前記４個以上のマーカーの各々と前記カメラとの距離を決定し、
決定した前記４個以上のマーカーの各々と前記カメラとの距離に基づいて、該カメラの世界座標系における位置および姿勢を演算する
処理を実行させることを特徴とする画像処理プログラム。
【請求項２】
誤差を算出する処理において、従前に算出された第一の符号パターンのうちの最小誤差の値よりも、第二の符号パターンにおける前記基準マーカー以外のマーカーのうちの第一のマーカーと該第一のマーカとは異なる第二のマーカーとのマーカー間距離に対する誤差が小さい場合に、該第二の符号パターンにおける第三のマーカと該第三のマーカとは異なる第四のマーカーとの誤差を算出し、該第一のマーカーと該第二のマーカーとのマーカー間距離の誤差、及び該第三のマーカーと該第四のマーカーとのマーカー間距離の誤差を加算することを特徴とする請求項１に記載の画像処理プログラム。
【請求項３】
前記コンピュータに、
前記数式を作成する処理により作成された数式で与えられる解が実数となるか否かを判定し、前記数式の解が実数とならないとの判定結果を得た場合に、前記第１の値とは異なる第２の値を設定する処理をさらに実行させることを特徴とする請求項１または２に記載の画像処理プログラム。
【請求項４】
前記第一の値および前記第二の値は、前記カメラを有する車体の設計における取り付け位置に基づいて予め設定された数値範囲に含まれる値であることを特徴とする請求項１乃至請求項３のいずれか一項に記載の画像処理プログラム。
【請求項５】
前記コンピュータに、
演算した前記カメラの世界座標系における位置および姿勢を、前記車体が有する記憶領域に格納する処理を更に実行させることを特徴とする請求項４に記載の画像処理プログラム。
【請求項６】
実空間に設けられた４個以上のマーカーをカメラにより撮影した画像を取得する取得部と、
前記カメラの焦点距離および光軸位置の情報を含む内部パラメータおよび前記画像における前記４個以上のマーカーの各々の位置に基づいて、前記カメラから該４個以上のマーカーの各々に対応するベクトルを生成する生成部と、
前記４個以上のマーカーの中から基準マーカーを一つ選択し、前記基準マーカーと前記カメラとの距離として第１の値、または前記第１の値とは異なる第２の値を設定する設定部と、
前記基準マーカー以外の残りのマーカーのうちのそれぞれに対するベクトルと、該基準マーカーのベクトルとの内積値を算出し、前記内積値の符号が負となる前記基準マーカー以外のマーカーについて、該基準マーカーを用いて該基準マーカー以外のマーカーと前記カメラとの距離を求める二次方程式の解を表す数式に含まれる符号のうち、負の符号の採用を棄却し、棄却した結果に基づいて、前記基準マーカー以外のマーカーのそれぞれに対する前記数式に含まれる符号を、全ての該基準マーカー以外のマーカーについて組み合わせた符号パターンを生成する棄却部と、
前記第１の値と、前記二次方程式と、棄却した前記結果とを用いて、該第１の値における、前記基準マーカー以外のマーカーの各々と前記カメラとの距離の候補値を、１つまたは２つ算出し、前記符号パターンごとに、該符号パターン及び前記１つまたは２つの候補値に基づいて算出されるマーカー間距離と、前記実空間におけるマーカー間の距離との誤差を算出する算出部と、
前記第１の値とは異なる第２の値が前記設定部により設定された場合に、前記符号パターンごとに他の誤差を算出し、
前記第１の値を設定した場合に算出された前記誤差および前記第２の値を設定した場合に算出された前記他の誤差に基づいて、前記４個以上のマーカーの各々と前記カメラとの距離を決定する決定部と、
前記決定部により決定された前記４個以上のマーカーの各々と前記カメラとの距離に基づいて、該カメラの世界座標系における位置および姿勢を演算する演算部と
を有することを特徴とする画像処理装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【公開番号】特開２０１２−１０３２１３（Ｐ２０１２−１０３２１３Ａ）
【公開日】平成２４年５月３１日（２０１２．５．３１）
【国際特許分類】
【出願番号】特願２０１０−２５４１９９（Ｐ２０１０−２５４１９９）
【出願日】平成２２年１１月１２日（２０１０．１１．１２）
【出願人】（０００００５２２３）富士通株式会社 (25,993)
【Ｆターム（参考）】
【公開日】平成２４年５月３１日（２０１２．５．３１）
【国際特許分類】
【出願日】平成２２年１１月１２日（２０１０．１１．１２）
【出願人】（０００００５２２３）富士通株式会社 (25,993)
【Ｆターム（参考）】
[ Back to top ]