以下に、図面を参照しつつ、本願の開示する画像処理プログラムおよび画像処理装置の一実施形態について詳細に説明する。なお、以下の実施例では、実空間である世界座標系の既知の3次元位置に位置する4個のマーカーを車両やロボットなどに搭載されたカメラにより撮影して、撮影された画像を用いてカメラの位置および姿勢を推定する技術に関する一実施形態について説明する。なお、マーカーは、4個以上であれば何個であっても構わない。以下の実施例では、実空間のことを3次元空間と表記するものとする。本願の開示する画像処理プログラムおよび画像処理装置の一実施形態として後述する実施例により、本願が開示する技術が限定されるものではなく、処理内容に矛盾を生じさせない範囲で各実施例を適宜組み合わせることが可能である。
実施例1に係る画像処理装置の構成の説明に入る前に、図1を用いて、実施例1に係る画像処理装置を説明する。図1は、実施例1に係る画像処理装置の説明に用いる図である。なお、以下に説明する実施例では、世界座標系の既知の3次元位置に4個のマーカーを配置する場合の一実施形態を説明するが、図1では、説明の便宜上、配置される4個のマーカーのうち3個のマーカーに焦点をあてて説明する。
図1に示すCはカメラ座標系の原点を示す。なお、図1の説明において、図1に示すCを「原点C」と表記するものとする。図1に示すP0,P1,P2A,P2Bは、4個のマーカーが映し出された画像平面上の点のうち、3個のマーカーに対応するカメラ座標系における仮想的な3次元位置(対応点)を示す。また、P2A,P2Bは、画像平面上のある点に対応するカメラ座標系における3次元位置となり得る候補に相当する。なお、図1の説明において、図1に示すP0,P1,P2A,P2Bを、「対応点P0」,「対応点P1」,「対応点P2A」,「対応点P2B」とそれぞれ表記するものとする。図1に示すV0,V1,V2は、原点Cから対応点P0,対応点P1,対応点P2A,対応点P2Bへ向かう視線ベクトルを示し、例えば、長さが1の単位ベクトルである。なお、図1の説明において、図1に示すV0,V1,V2を、「ベクトルV0」,「ベクトルV1」,「ベクトルV2」とそれぞれ表記するものとする。
また、図1に示すL0は原点Cと対応点P0との距離を示し、図1に示すL1は原点Cと対応点P1との距離の値を示し、図1に示すL2Aは原点Cと対応点P2Aとの距離の値を示し、図1に示すL2Bは原点Cと対応点P2Bとの距離の値を示す。なお、図1の説明において、図1に示すL0を「距離L0」と表記するものとする。同様に、図1の説明において、図1に示すL1,L2A,L2Bを、「距離L1」,「距離L2A」,「距離L2B」とそれぞれ表記するものとする。
また、図1に示すD(0,1)は、対応点P0と対応点P1との3次元空間上の実距離を示し、図1に示すD(0,2)は、対応点P0と対応点P2との3次元空間上の実距離を示し、図1に示すD(1,2)は、対応点P1と対応点P2との3次元空間上の実距離を示す。なお、図1の説明において、図1に示すD(0,1)を「実距離D(0,1)」と表記するものとする。同様に、図1の説明において、図1に示すD(0,2)を「実距離D(0,2)」と記し、図1に示すD(1,2)を「実距離D(1,2)」と表記するものとする。
実施例1に係る画像処理装置は、まず、基準点として対応点P0を選択した場合に、距離L0を用いて、距離L1および距離L2を余弦定理により定式化して表した各数式からなる連立方程式を立てる。この連立方程式は、例えば、以下の式(2)で示される。なお、以下の式(2)に示すD0,1、D0,2、は、上述した図1に示すD(0,1)、D(0,2)にそれぞれ対応する。
ここで、距離L0の値が与えられれば、上述した式(2)の解を求めることにより、2種類の値が距離L1の値として導出される。つまり、距離L1の値として、距離L1を定式化して表した数式の右辺内にある内積の項と根号の項との間にあるプラスの符合を選択した場合と、マイナスの符号を選択した場合の2種類の値が導出される。同様に、上述した式(2)の解を求めることにより、2種類の値が距離L2の値として導き出される。すなわち、距離L1を定式化して表した数式の右辺内にある内積の項と根号の項との間にある符合の選択により距離L1の値が決定付けられる。同様に、距離L2を定式化して表した数式内の内積の項と根号の項との間にある符合の選択により距離L2の値が決定付けられる。そこで、実施例1に係る画像処理装置は、距離L1を定式化して表した数式の右辺内にある根号前の符号の一方と、距離L2を表す数式の右辺内にある根号前にある符号一方とを組合せた符号パターンを導出する。そして、実施例1に係る画像処理装置は、符号パターンの中から、距離L1、距離L2の値として相応しくない値が導き出されるものを棄却する。
例えば、実施例1に係る画像処理装置は、距離L1およびL2が上述した式(2)で表される場合に、(+、+)、(−、−)、(+、−)、(−,+)の4つの符号パターンを導出できる。次に、実施例1に係る画像処理装置は、導出した符号パターンの中から、少なくとも距離L1および距離L2の値の一方が相応しい値となり得えない符号パターンを棄却する。例えば、図1に示すベクトルV0とベクトルV1とのなす角度「θ」が90度よりも大きい場合には、距離L1を表す数式の右辺内にある内積がマイナスの値となる。この場合、式(2)の根号前の符号のうちマイナス符号を選択した場合に計算される距離L1の値はマイナスとなってしまうので、距離L1の値として相応しくない。つまり、右辺の第一項は必ず負の値となるため、符号パターンは少なくとも正が採用されないかぎり、右辺は負の値となる。よって、実施例1に係る画像処理装置は、距離L1を表す数式の右辺内にある根号前の符号のうちマイナスの符号を選択してできる符号パターンを棄却する。また、例えば、図1に示すベクトルV0とベクトルV2とのなす角度が90度以下である場合には、距離L2を表す数式の右辺内にある内積がプラスの値となる。この場合、距離L2を表す数式に含まれる根号前の各符号をそれぞれ選択した場合に計算される距離L2の値は、プラスになるかマイナスになるか不明なので、距離L2の値として相応しいかどうかの判断はできない。よって、実施例1に係る画像処理装置は、距離L2を表す数式の右辺内にある根号前のプラスの符号およびマイナスの符合を選択してできる各符号パターンを棄却せずに残す。この結果、実施例1に係る画像処理装置は、(−、−)、(−,+)の2つの符号パターンを棄却し、(+、−)、(+,+)の2つの符号パターンを残すこととなる。
なお、棄却されずに残された符号パターンでは、距離L1の値の候補は、距離L1を表す数式の右辺内にある内積の項と根号の項との間にあるプラスの符合を選択した場合に計算される1つの値のみとなる。つまり、図1に示す距離L1の値の候補は、2種類の符号の選択により与えられる2つの候補のうちの1つの候補に絞り込まれる。一方、距離L2の値の候補は、距離L2を表す数式の右辺内にある内積の項と根号の項との間にあるプラスの符合を選択した場合に計算される値、およびマイナスの符号を選択した場合に導出される値の2つとなる。つまり、図1に示す距離L2の値の候補は、図1に示すL2Aに対応する値と図1に示すL2Bに対応する値の2つとなり、2種類の符号の選択により与えられる2つの候補がそのまま残される。このように、カメラ座標系の原点からマーカーに対応するカメラ座標系における対応点までの距離を算出する場合に、視線ベクトル同士がなす角度を考慮することにより、2種類の符号の選択により与えられる2つの候補の中から絞り込むことができる場合がある。
次に、実施例1に係る画像処理装置は、棄却されずに残された符号パターンについて、3次元空間上のマーカー間の距離の候補値と3次元空間上のマーカー間の実距離の値との誤差を算出する。例えば、実施例1に係る画像処理装置は、以下に説明するようにして各符号パターンについての誤差を算出する。
まず、実施例1に係る画像処理装置は、距離L0と、上述した式(2)とに基づいて、例えば、棄却されずに残された符号パターン(+、+)について、距離L1および距離L2Aを得られる。また、実施例1に係る画像処理装置は、距離L0と、上述した式(2)とに基づいて、例えば、棄却されずに残された符号パターン(+、−)について、距離L1および距離L2Bを得られる。なお、距離L0の値は、距離L0の値が取り得る範囲内で予め設定される。
続いて、実施例1に係る画像処理装置は、符号パターン(+、+)について、距離L1および距離L2Aに基づき、3次元空間上のマーカー間の距離の候補値、つまり図1に示す点P1と点P2Aとの距離を算出する。また、実施例1に係る画像処理装置は、符号パターン(+、−)について、距離L1および距離L2Bに基づき、3次元空間上のマーカー間の距離の候補値、つまり図1に示す点P1と点P2Bとの間の距離を算出する。
次に、実施例1に係る画像処理装置は、符号パターン(+、+)について算出した点P1と点P2Aとの距離と、3次元空間上のマーカー間の実距離の値との誤差を算出する。同様に、実施例1に係る画像処理装置は、符号パターン(+、−)について算出した点P1と点P2Bとの距離と、3次元空間上のマーカー間の実距離の値との誤差を算出する。
続いて、実施例1に係る画像処理装置は、符号パターン(+、+)について算出した誤差と、符号パターンである(+、−)について算出した誤差とを比較する。比較の結果、実施例1に係る画像処理装置は、符号パターン(+、+)について算出した誤差の値の方が小さい場合には、距離L0、距離L1、距離L2Aを、カメラ原点から3次元空間上のマーカーまでの距離に決定する。
実施例1に係る画像処理装置は、上述した符号パターンについての誤差の算出をする処理において、基準点以外に選択した2点間の距離と実距離との誤差が最小となるときの基準点までの距離および基準点以外の点までの距離を検出する処理を行っている。例えば、対応点P1と対応点P2との間の距離と実距離との誤差が最小となるときの基準点までの距離および基準点以外の点までの距離を検出する処理は、以下に示す式(3)で表される。なお、以下の式(3)に示すD1,2は、上述した図1に示すD(1,2)に対応する。以下の式(3)に示すL2は、L2AやL2Bなどに対応する。また、例えば‖V‖は、ベクトルVの長さを表わす。また、例えば|a|は、実数aの絶対値を表わす。
上述してきたように、実施例1に係る画像処理装置は、まず、上述した符号パターンについて高々2種類の符号の選択を行う。そして、実施例1に係る画像処理装置は、選択の結果残された符号パターンについて、基準点以外に選択した2点間の距離と実距離との誤差が最小となるときの基準点までの距離および基準点以外の点までの距離の検出を実行する。このようにして、実施例1に係る画像処理装置は、4個以上のマーカーが映し出された画像平面上の点に対応する3次元空間上の対応点の位置を迅速かつ精度良く算出する。
[画像処理装置の構成(実施例1)]
図2は、実施例1に係る画像処理装置の構成を示す機能ブロック図である。なお、図2には、実施例1に係る画像処理装置の機能を説明する上で必要となる機能ブロックを図示する。以下の実施例中において、単に原点Cと表記するものはカメラ座標系の原点を示し、単に対応点と表記するものはカメラ座標系における3次元位置を示すものとする。
図2に戻り、実施例1に係る画像処理装置200は、例えば、撮影装置100が接続される。撮影装置100は、例えば、単眼カメラやステレオカメラなどに該当する。撮影装置100は、例えば、車両やロボットなどの周囲の実空間上の所定位置に配置された4個のマーカーを撮影する。なお、以下の実施例中、カメラと表記する箇所は撮影装置100に該当するものとする。
画像処理装置200は、図2に示すように、取得部210、記憶部220および制御部230を有する。取得部210は、撮影装置100により撮影されたマーカーの画像をそれぞれ取得する。取得部210は、撮影装置100から取得したマーカーの画像を画像記憶部221に格納する。
記憶部220は、図2に示すように、画像記憶部221、カメラパラメータ記憶部222およびマーカー距離情報記憶部223を有する。なお、記憶部220は、例えば、RAM(Random Access Memory)やフラッシュメモリ(flash memory)などの半導体メモリ素子である。
画像記憶部221は、例えば、取得部210により格納されたマーカーの画像をそれぞれ記憶する。カメラパラメータ記憶部222は、カメラの焦点距離および光軸位置の情報を含むカメラのパラメータの情報、いわゆるカメラ内部パラメータの情報を記憶する。カメラ内部パラメータの情報は、後述する生成部231による視線ベクトルの生成時に用いられる。マーカー距離情報記憶部223は、車両やロボットなど周囲に配置されたマーカー間の3次元空間上の実距離の情報をそれぞれ記憶する。なお、記憶部220は、さらに距離Lnの値が取り得る範囲を記憶する領域を有する。なお、距離Lnの値が取り得る範囲は予め記憶部に記憶されていても良いし、後述の処理を行う際に、利用者によって入力された数値範囲が記憶部220に記憶するとしても良い。また、距離Lnの値が取り得る範囲は、予めカメラの設置位置が予測される環境において、予測値の前後の範囲が設定される。例えば、カメラを備える車体を、マーカーを有する試験場に停車し、詳細なカメラの位置や姿勢を演算する場合においては、試験場における車体を停止する位置や、設計上の車種ごとのカメラ取り付け位置などから推測された数値範囲が設定される。
図2に戻り、制御部230は、生成部231と、設定部232と、作成部233と、導出部234と、棄却部235と、算出部236と、決定部237と、位置姿勢推定部238とを有する。なお、制御部230は、例えば、電子回路や集積回路に該当する。電子回路としては、例えば、CPU(Central Processing Unit)やMPU(Micro Processing Unit)がある。また、集積回路としては、例えば、ASIC(Application Specific Integrated Circuit)やFPGA(Field Programmable Gate Array)などがある。
生成部231は、カメラからマーカーの各々に対応する視線ベクトルを生成する。例えば、生成部231は、画像記憶部221から4個のマーカーの画像を取得し、カメラの焦点距離および光軸位置の情報を含むカメラ内部パラメータをカメラパラメータ記憶部222から取得する。そして、生成部231は、4点のマーカーの画像の画像平面上における4個のマーカーそれぞれの位置の情報を取得する。続いて、生成部231は、カメラ内部パラメータと、マーカーの画像における4個のマーカーの各々の位置の情報とに基づいて、カメラから4個のマーカーの各々に対応する視線ベクトルを生成する。例えば、視線ベクトルVi(i=0,1,2,3)は、カメラ内部パラメータを与える正則な3次元正方行列「K」と、画像の座標「xi」と、xiの同次座標表現「x´i」を用いて、以下に示す式(4)で表わすことができる。以下に示す式(4)では、視線ベクトルViの大きさが「1」に正規化されている。なお、以下の式(4)に示すK−1は、3次元正方行列「K」の逆行列である。
なお、生成部231により生成された視線ベクトルを用いて、4個のマーカーが映し出された画像平面上の点に対応するカメラ座標系における仮想的な3次元位置(対応点)は、以下の式(5)で表すことができる。以下の式(5)は、視線ベクトルの延長線上に対応点が位置することを意味している。以下の説明において、単に「対応点」と表記するものは、マーカーが映し出された画像平面上の点に対応するカメラ座標系における仮想的な3次元位置を表すものとする。なお、4個のマーカーが映し出された画像平面上の点に対応する対応点は、以下に説明する設定部232〜決定部237による処理の結果を用いることにより得ることができる。また、生成部231は、マーカーが映し出される画像平面上の各点の位置と、3次元空間上のマーカーの位置との対応関係の情報を予め有するものとする。
図3に、生成部231が生成する視線ベクトルの一例を示す。図3は、実施例1に係る生成部の説明に用いる図である。図3に示すように、生成部231は、カメラ座標系の原点Cから対応点の一つである点0に対応するベクトルV0、およびカメラ座標系の原点Cから対応点の一つである点1に対応するベクトルV1をそれぞれ生成する。同様に、図3には示していないが、生成部231は、カメラ座標系の原点Cから対応点の一つである点2に対応するベクトルV2、およびカメラ座標系の原点Cから対応点の一つである点3に対応するベクトルV3をそれぞれ生成する。
図2に戻り、設定部232は、4個のマーカーが映し出された画像平面上の4個に対応する仮想的な4つの対応点の中から基準とするマーカー(以後、基準マーカーと表記する)に対応する点である基準点M(点0〜点3のいずれか)を選択する。また、設定部232は、基準点Mまでの距離L0に所定の値「L」を設定する。なお、距離L0に設定される所定の値「L」は、カメラの設置位置およびマーカーの配置位置から予め推定される。
図2に戻り、作成部233は、基準点M以外の点、つまり基準マーカー以外のマーカーに対応する点1〜点3までの各距離L1,L2,L3を表す数式を作成する。距離L1,L2,L3は、上述した式(2)のように、余弦定理を用いて定式化して表すことができる。図4に、作成部233により作成される数式から得られる対応点までの距離の一例を示す。図4は、実施例1に係る作成部の説明に用いる図である。図4に示す点1(+)は、距離L1を表す数式の根号前の符号のうちプラスを選択した場合の距離に相当する。図4に示す点1(−)は、距離L1を表す数式の根号前の符号のうちマイナスを選択した場合の距離に相当する。
図4に示すように、基準点として点0を選択した場合、カメラ座標系の原点Cから点1までの距離として、上述した式(2)から、点Cから点1(+)までの距離と点Cから点1(−)までの距離の2つの候補が得られる。言い換えれば、カメラ座標系の原点Cから点1までの距離は、基準マーカー以外のマーカーに対応する点1〜点3までの各距離L1,L2,L3を表す数式によって、高々2つの候補に限定することができる。
図2に戻り、導出部234は、設定部232により設定された基準マーカーで採用する符号パターンを全て導出する。図5には、対応点0〜3のうち点0を基準点とした場合に導出される符号パターンを示す。図5は、実施例1に係る導出部の説明に用いる図である。図5に示すように、導出部234は、(+、+、+)、(+、+、−)、(+、−、+)、(+、−、−)、(−、+、+)、(−、+、−)、(−、−、+)、(−、−、−)の8つの符号パターンを導出する。
また、導出部234は、作成部233により作成された各数式の根号内の値を算出し、各数式の根号の値が全て実数であるか否かを判定する。判定の結果、各数式の根号の値の全てが実数である場合には、導出部234は、符号パターンを棄却部235に送出する。一方、判定の結果、各数式の根号の値の全てが実数ではない場合には、導出部234は、後述する決定部237に対して処理の終了を送出する。なお、この実施例では、導出部234により、各数式の根号の値の全てが実数であるか否かを判定する場合を説明したが、これに限定されるものではなく、例えば、作成部233が各数式の根号の値の全てが実数であるか否かを判定してもよい。
棄却部235は、導出部234により導出された符号パターンの中から、距離L1、L2、L3の少なくとも1つが相応しくない符号パターンを棄却する。まず、棄却部235は、基準点M以外の対応点Nを一つ選択する。例えば、点0を基準点Mとして選択した場合には、点1〜3のいずれか一つを対応点Nとして選択する。次に、棄却部235は、原点Cから基準点Mまでの視線ベクトル、および原点Cから対応点Nまでの視線ベクトルを生成部231から取得する。
続いて、棄却部235は、基準点Mに対応する視線ベクトルと、選択した対応点Nに対応する視線ベクトルとの内積を計算し、視線ベクトルのなす角が90度よりも大きいか否かを判定する。例えば、棄却部235は、計算した内積の値が負の値である場合には、視線ベクトルのなす角が90度よりも大きいと判定する。判定の結果、棄却部235は、視線ベクトルのなす角が90度よりも大きい場合には、選択した対応点Nについてはプラスの符号のみを取り得ることを記録する。ここで、記録するとは、棄却部235が、内部的に有する記憶部に、対応点Nについてはプラスの符号のみを取り得ることを示すデータを保存することを意味する。
一方、判定の結果、棄却部235は、視線ベクトルのなす角が90度以下である場合には、選択した対応点Nについてはプラス、マイナスの両方の符号を取り得ることを記録する。ここで、記録するとは、棄却部235が、内部的に有する記憶部に、対応点Nについてはプラス、マイナスの両方の符号を取り得ることを示すデータを保存することを意味する。そして、棄却部235は、対応点の中から、まだ選択していない他の全ての点についても上述してきたのと同様の処理を行い、全ての対応点が取り得る符号の組合せを列挙することにより符号パターンを決定する。
以下、図6を用いて符号パターンの決定方法の一例を説明する。図6は、実施例1に係る棄却部の処理に関する概念的な説明に用いる図である。棄却部235は、図6に示す基準点0以外の点1〜3の中から点1を選択する。次に、棄却部235は、基準点0に対応する視線ベクトルV0、および点1に対応する視線ベクトルV1を生成部231から取得する。続いて、棄却部235は、視線ベクトルV0と視線ベクトルV1との内積を計算し、視線ベクトルV0と視線ベクトルV1とのなす角が90度よりも大きいか否かを判定する。判定の結果、棄却部235は、視線ベクトルV0と視線ベクトルV1とのなす角が90度よりも大きい場合には、選択した点1ではプラスの符号のみを取り得ることを記録する。一方、判定の結果、棄却部235は、視線ベクトルのなす角が90度以下である場合には、選択した点1ではプラス、マイナスの両方の符号を取り得ることを記録する。
そして、棄却部235は、対応点である点1〜点3の中から、まだ選択していない点2および点3についても上述してきたのと同様の処理を行い、点1〜点3が取り得る符号の組合せを列挙することにより符号パターンを決定する。例えば、棄却部235は、点1〜点3が取り得る符号の組合せを列挙する。その結果、棄却部235は、図6に示す符号パターンのうち、(−、+、+)、(−、+、−)、(−、−、+)、(−、−、−)を棄却し、(+、+、+)、(+、+、−)、(+、−、+)、(+、−、−)を点1〜点3が取り得る符号の組合せとして残す。よって、棄却部235は、棄却されずに残された4つの符号パターン、図6に示す(+、+、+)、(+、+、−)、(+、−、+)、(+、−、−)の4つの符号パターンを点1〜点3が取り得る符号パターンとして決定する。
図2に戻り、算出部236は、棄却部235により決定された各符号パターンについて、基準点以外の各対応点間の距離の候補値と3次元空間上の実距離の値との誤差を演算し、各符号パターンについて最小誤差を算出する。
まず、算出部236は、後に用いる最小誤差Jの値を初期化する。棄却部235により決定された各符号パターンの中から符号パターンを一つ選択し、選択した符号パターンについて距離L1、L2およびL3の値をそれぞれ算出する。算出部236は、設定部232により設定された基準点0までの距離L0の値「L」と、作成部233により作成された数式とを用いて、距離L1、L2およびL3の値を算出する。
次に、算出部236は、距離L1、L2およびL3の値が正の実数であるか否かを判定する。なお、正の実数とは、実数のうち正の値を意味する。算出部236は、判定の結果、距離L1、L2およびL3の値が正の実数ではない場合には、次の符号パターンを選択する。これとは反対に、算出部236は、判定の結果、距離L1、L2およびL3の値が正の実数である場合には、誤差Eの値を、例えば、「0」に初期化する。なお、後に説明するが、誤差Eの値は、所定の条件を満足する場合に、基準点0以外の2点の組合せについて算出される誤差の値を加算された値となる。
続いて、算出部236は、選択した符号パターン内で、基準点0以外の2点の組合せを一つ取得する。次に、算出部236は、取得した2点の組合せについて2点間の距離の候補値を算出する。続いて、算出部236は、算出した候補値に対応する3次元空間上のマーカー間の実距離の値をマーカー距離情報記憶部223から取得し、この候補値と、この候補値に対応する実距離の値との誤差を演算する。そして、算出部236は、算出した誤差の値を誤差Eの値に加算して更新し、誤差Eの値が最小誤差Jの値よりも大きいか否かを判定する。算出部236は、判定の結果、誤差Eの値が最小誤差Jの値よりも大きい場合には、棄却部235により決定された各符号パターンの中から次の符号パターンを選択する。
一方、算出部236は、判定の結果、誤差Eの値が最小誤差Jの値以下である場合には、選択した符号パターン内で基準点0以外の2点の組合せの全てについて処理を完了したか否かを判定する。算出部236は、判定の結果、選択した符号パターン内で基準点0以外の2点の組合せの全てについて処理を完了している場合には、誤差Eの値で最小誤差Jの値を更新し、棄却部235により決定された各符号パターンの中から次の符号パターンを一つ選択する。これとは反対に、算出部236は、判定の結果、選択した符号パターン内で基準点0以外の2点の組合せの全てについて処理を完了していない場合には、選択した符号パターン内で、基準点0以外の2点の組合せの中から次の組合せを一つ取得する。以後、算出部236は、取得した2点の組合せについて上述したのと同様に処理を行う。
そして、算出部236は、全符号パターン内について処理を完了した場合には、「基準点=点0」および「距離L0=L」に関する処理結果を決定部237に送出する。例えば、算出部236は、最終的に残った誤差Eの値、すなわち該当符号パターン内の最小誤差の値と、最小誤差に対応する符号パターンの距離L0、L1、L2およびL3の値を処理結果として送出する。
図7〜図10を用いて、算出部236による処理を概念的に説明する。なお、図7〜図10では、誤差Eの値を初期化した後の算出部236の処理について説明する。図7〜図10は、実施例1に係る算出部の処理に関する概念的な説明に用いる図である。
まず、算出部236は、図7に示す各符号パターン(+、+、+)、(+、+、−)、(+、−、+)、(+、−、−)の中から、例えば、符号パターン(+、+、+)を選択する。次に、算出部236は、選択した符号パターン(+、+、+)内で、基準点0以外の2点の組合せ(点1,点2)を取得する。続いて、算出部236は、2点の組合せ(点1,点2)について2点間の距離の候補値を算出する。次に、算出部236は、算出した候補値に対応する3次元空間上のマーカー間の実距離の値をマーカー距離情報記憶部223から取得し、この候補値と、この候補値に対応する実距離の値との誤差を算出する。続いて、算出部236は、算出した誤差の値を誤差Eの値に加算して更新し、誤差Eの値が最小誤差Jの値よりも大きいか否かを判定する。算出部236は、判定の結果、誤差Eの値が最小誤差Jの値以下である場合には、符号パターン(+、+、+)内で基準点0以外の2点の組合せの全て、(点1,点2)、(点1,点3)および(点2,点3)について処理を完了したか否かを判定する。
算出部236は、判定の結果、基準点0以外の2点の組合せの全てについて処理を完了していない場合には、同一の符号パターン(+、+、+)内で基準点0以外の2点の組合せ、例えば(点1,点3)を取得する。そして、算出部236は、上述してきた基準点0以外の2点の組合せ(点1,点2)に対して実行した処理と同様の手順で処理を行う。すなわち、算出部236は、2点の組合せ(点1,点3)について2点間の距離の候補値を算出し、算出した候補値に対応する3次元空間上の実距離の値を取得し、この候補値と実距離の値との誤差を算出する。続いて、算出部236は、算出した誤差の値を誤差Eの値に加算して更新し、誤差Eの値が最小誤差Jの値よりも大きいか否かを判定する。算出部236は、判定の結果、誤差Eの値が最小誤差Jの値以下である場合には、誤差Eの値を最小誤差Jの値に加算して更新し、再び、符号パターン(+、+、+)内で基準点0以外の2点の組合せの全てについて処理を完了したか否かを判定する。
なお、算出部236により、最初に選択された符号パターン(+、+、+)については、最小誤差Jの値が無限大に設定されているので、基準点0以外の2点の組合せの全てについての処理が必ず完了される。よって、図7に示すように、符号パターン(+、+、+)についての誤差Eの値「E000」が得られる。この誤差Eの値「E000」は、2点の組合せ(点1,点2)についての誤差の値と、2点の組合せ(点1,点3)についての誤差の値と、2点の組合せ(点2,点3)についての誤差の値が加算されたものに相当する。そして、この誤差Eの値「E000」は、符号パターン(+、+、+)の後に実行される他の符号パターンに対する処理の中で最小誤差Jの値として利用される。このため、上述した手順で処理を行えば、各符号パターンについて算出された誤差の値の中で最も小さい誤差の値を得ることが可能となる。
ここで、符号パターン(+、+、+)内で基準点0以外の2点の組合せの全てについて処理を完了したか否かの判定結果が上述した判定結果と異なり、基準点0以外の2点の組合せの全てについて処理を完了している場合について説明する。算出部236は、判定の結果、基準点0以外の2点の組合せの全てについて処理を完了している場合には、誤差Eの値で最小誤差Jの値を更新する。次に、算出部236は、図7に示す各符号パターンの中から次の符号パターン、例えば、符号パターン(+、+、−)を選択する。そして、算出部236は、上述してきた符号パターン(+、+、+)に対して実行した処理と同様の手順で、符号パターン(+、+、−)に対し処理を行う。
ここで、上述した誤差Eの値が最小誤差Jの値よりも大きいか否かの判定結果が上述した判定結果と異なり、誤差Eの値が最小誤差Jの値よりも大きい場合について説明する。算出部236は、判定の結果、誤差Eの値が最小誤差Jの値よりも大きい場合には、現在選択中の符号パターンに対する処理を取りやめ、図7に示す各符号パターンの中から次の符号パターンを選択する。そして、算出部236は、上述してきた符号パターン(+、+、+)に対して実行した処理と同様の手順で処理を行う。
例えば、算出部236は、符号パターン(+、+、−)の処理について、誤差Eの値が最小誤差Jの値よりも大きいという判定結果が得られた場合には、符号パターン(+、+、−)の処理を取りやめ、例えば、未選択の符号パターン(+、−、−)を選択する。次に、算出部236は、符号パターン(+、−、−)内で基準点0以外の2点の組合せ(点1,点2)を取得する。続いて、算出部236は、2点の組合せ(点1,点2)について2点間の距離の候補値を算出し、算出した候補値に対応する3次元空間上の実距離の値を取得し、この候補値と実距離の値との誤差を算出する。次に、算出部236は、算出した誤差の値を誤差Eの値に加算して更新し、誤差Eの値が最小誤差Jの値よりも大きいか否かを判定する。算出部236は、判定の結果、誤差Eの値が最小誤差Jの値以下である場合には、上述してきたのと同様に、符号パターン(+、−、−)内で基準点0以外の2点の組合せの全てについて処理を完了したか否かを判定する。以後、算出部236は、上述してきた同様の手順で処理を行う。一方、算出部236は、誤差Eの値が最小誤差Jの値よりも大きい場合には、誤差Eの値を最小誤差Jの値に加算して更新し、再び、符号パターン(+、−、−)内で基準点0以外の2点の組合せの全てについて処理を完了したか否かを判定する。以下、算出部236は、上述してきた同様の手順で処理を行う。
上述してきた手順で、算出部236は、図7に示す各符号パターンについての演算を行うことにより、図8に示すように、誤差「E000」、「E001」、「E002」、「E003」の中で最も値の小さい最小誤差(minE000〜E003)の値を得る。そして、算出部236は、「基準点=点0」および「距離L0=L」に関し、最小誤差(minE000〜E003)の値と、これに対応するL0、L1、L2およびL3の値を処理結果として後述する決定部237に送出する。
また、図9に示すように、算出部236は、基準点0までの距離の値として、「L」とは異なる値「L+ΔL」が設定部232により設定されると、「基準点=点0」および「距離L0=L+ΔL」に関し、上述してきた手順で処理を実行する。そして、算出部236は、「基準点=点0」および「距離L0=L+ΔL」に関し、最小誤差(minE010〜E013)の値と、これに対応するL0、L1、L2およびL3の値を処理結果として後述する決定部237に送出する。
また、図10に示すように、算出部236は、点0とは異なる点1が新たな基準点として設定部232により設定されると、「基準点=点1」および「距離L0=L´」に関し、上述してきた手順で処理を実行する。そして、算出部236は、「基準点=点1」および「距離L0=L´」に関し、最小誤差min(E020〜E02v:vは自然数)の値と、これに対応するL0、L1、L2およびL3の値を処理結果として送出する。また、算出部236は、図10には示していないが、基準点1までの距離の値として、「L´」とは異なる値「L´+ΔL´」が設定部232により設定されると、「基準点=点1」および「距離L0=L´+ΔL´」に関し、上述してきた手順で処理を実行する。そして、算出部236は、「基準点=点1」および「距離L0=L´+ΔL´」に関し、最小誤差の値と、これに対応するL0、L1、L2およびL3の値を処理結果として後述する決定部237に送出する。
なお、算出部236は、基準点までの距離の設定があるたびに、基準点および基準点までの距離に対応する各符号パターンに対応する誤差のうちの最小誤差の値を算出する場合を説明したが、これに限定されるものではない。例えば、算出部236は、同一の基準点に対する処理内では、最小誤差Jを初期化せずに更新を続け、同一の基準点内の各符号パターンの中で値が最も小さい最小誤差を算出するようにしてもよい。あるいは、算出部236は、全ての基準点に対する処理内で最小誤差Jを初期化せずに更新を続け、全ての基準点内の各符号パターンに対応する誤差のうちの最小誤差の値を算出するようにしてもよい。
図2に戻り、決定部237は、算出部236からの処理結果に基づいて、カメラ座標系の原点から、4個以上のマーカーが映し出された画像平面上の点に対応する3次元空間上の対応点の位置までの距離を決定する。
例えば、決定部237は、最小誤差の値と、これに対応するL0、L1、L2およびL3の値を処理結果として算出部236から取得すると、取得した処理結果を保存する。そして、決定部237は、基準点までの距離が取りうる値の全てを設定して処理を完了しているか否かを判定する。例えば、決定部237は、現在基準点0が選択されるとすると、基準点までの距離L0が取りうる値の全てを設定して処理を完了しているか否かを判定することとなる。判定の結果、距離L0が取りうる値の全てを設定して処理を完了していない場合には、決定部237は、設定部232に対して、距離L0の値として「L」とは異なる別の値を設定するように依頼する。
一方、決定部237は、判定の結果、距離L0が取りうる値の全てを設定して処理を完了している場合には、対応点である点0〜点3の全てを基準点として処理を完了しているか否かを判定する。例えば、決定部237は、現在基準点0が選択されるとすると、残りの対応点である点1〜点3の全てを基準点として処理を完了したか否かを判定することとなる。判定の結果、対応点である点0〜点3の全てを基準点として処理を完了していない場合には、決定部237は、設定部232に対して、新たな基準点および距離の設定を依頼する。
一方、決定部237は、判定の結果、対応点である点0〜点3の全てを基準点として処理を完了している場合には、算出部236から取得してこれまで保存した処理結果の中で、最小誤差の値が最も小さいものを特定する。例えば、決定部237は、図10に示す最小誤差min(E000〜E003)、min(E010〜E013)、min(E020〜E02v:vは自然数)等の中から、値が最も小さい最小誤差を特定する。そして、決定部237は、特定された最小誤差に対応付けられているL0、L1、L2およびL3の値を最適解として決定し、位置姿勢推定部238に送る。
図2に戻り、位置姿勢推定部238は、決定部237から最適解として取得したL0、L1、L2およびL3の値を用いて、画像平面上の点に対応するカメラ座標系における3次元位置を算出し、算出した位置を用いてカメラの位置および姿勢を算出する。以下に、図11を用いて、実施例1に係る位置姿勢推定部238について説明する。図11は、実施例1に係る位置姿勢推定部の説明に用いる図である。図11に示すαはカメラ座標系を示す。また、図11に示すβは世界座標系を示す。また、図11に示すW0、W1、W2は、世界座標系におけるマーカーの3次元位置を示し、W0は点0、W1は点1、W2は点2にそれぞれ対応するものとする。また、カメラの位置および姿勢を算出する技術自体は公知技術であり、例えば、玉木 徹 他、「姿勢推定と回転行列」(電気情報通信学会研究報告、信学技報,vol.109, no.203, SIS2009-23, pp.59-64, 2009年9月)等を参照されたい。なお、以下では、説明を簡略化するために、マーカーが映し出された画像平面上の点に対応する3つの対応点(点0、点1および点2)を例に挙げて説明するが、対応点の数が4つでも同様に処理できる。
例えば、位置姿勢推定部238は、決定部237から最適解として取得したL0、L1およびL2の値と上述した式(5)とを用いて、画像平面上の点に対応するカメラ座標系における対応点P0、P1、P2を算出する。そして、位置姿勢推定部238は、カメラ座標系における3次元位置(P0、P1、P2)と、これらに対応する世界座標系におけるマーカーの位置(W0、W1、W2)とに基づいて、公知技術を利用してカメラの位置および姿勢を算出する。
[画像処理装置による処理(実施例1)]
次に、図12〜図14を用いて、実施例1に係る画像処理装置による処理の流れを説明する。図12は、実施例1に係る最適解算出処理の流れを説明する。図12は、実施例1に係る最適解算出処理の流れを示す図である。図12に示す処理は、画像フレームが取得されるたびに実行される。
図12に示すように、例えば、制御部230により取得部210による画像取得が検出されると(ステップS101,YES)、生成部231は、視線ベクトルを生成する(ステップS102)。なお、取得部210による画像取得が検出されない場合(ステップS101,NO)、制御部230は、取得部210による画像取得の検出判定を繰り返し実行する。
次に、設定部232は、各マーカーに対応する点の中から基準マーカーに対応する基準点M(点0〜点3のいずれか)を選択する(ステップS103)。続いて、導出部234は、設定部232により設定された基準マーカーで採用する符号パターンを全て導出する(ステップS104)。次に、設定部232は、カメラ座標系の原点Cから基準点Mまでの距離L0に所定の値を設定する(ステップS105)。続いて、作成部233は、カメラ座標系の原点Cから基準点M以外の各点(点1〜3)までの距離L1、L2およびL3の値を表す数式を作成する(ステップ106)。次に、画像処理装置200により実行される処理は最適解候補算出処理に移行される(ステップS107)。最適解候補算出処理は、主に、算出部236によりが実行される。なお、最適解候補算出処理については、図13を用いて後述する。
続いて、決定部237は、ステップS107における最適解候補算出処理の処理結果を取得すると、取得した処理結果を保存する(ステップS108)。次に、決定部237は、基準点Mまでの距離L0が取りうる値の全てを設定して処理を完了しているか否かを判定する(ステップS109)。
判定の結果、距離L0が取りうる値の全てを設定して処理を完了していない場合には(ステップS109,NO)、決定部237は、設定部232に対して、距離L0の値として「L」とは異なる値を設定するように依頼する。図12に示す処理は、上述したステップS105に処理を戻す。
一方、決定部237は、判定の結果、距離L0が取りうる値の全てを設定して処理を完了している場合には(ステップS109,YES)、点0〜点3の全てを基準点として処理を完了しているか否かを判定する(ステップS110)。判定の結果、点0〜点3の全てを基準点として処理を完了していない場合には(ステップS110,NO)、決定部237は、上述したステップS103に処理を戻す。
一方、決定部237は、判定の結果、点0〜点3の全てを基準点として処理を完了している場合には(ステップS110,YES)、次のように処理する。すなわち、決定部237は、ステップS107にて保存された最小誤差の中から、最小誤差の値が最小のときのL0、各Li(i=1〜3)の値を最適解として決定し、位置姿勢推定部238に送出する(ステップS111)。以上で、実施例1に係る最適解算出処理が完了するが、上述してきた図12に示す最適解算出処理は、処理内容に矛盾を生じさせない範囲でその順序を入れ替えることができる。
なお、上述したステップS109およびステップS110の判定処理により、基準点までの距離Lの最適化を行われることとなり、図12に示す処理の内容は以下に示す式(6)で表すことができる。なお、以下の式(6)に示す「i0」は基準点を示し、「Li0」は基準点までの距離を示す。
続いて、図13を用いて、実施例1に係る最適解候補算出処理の流れを説明する。図13は、実施例1に係る最適解候補算出処理の流れを示す図である。図13に示すように、導出部234は、図12に示すステップS106にて作成された全数式の根号の値を算出し、全数式の根号の値が実数であるか否かを判定する(ステップS201)。判定の結果、根号の値が実数ではない場合には(ステップS201,NO)、導出部234は、図12に示すステップS109に処理を戻す。
一方、根号の値が実数であると導出部234により判定された場合には(ステップS201,YES)、画像処理装置200により実行される処理は符号パターン決定処理に移行される(ステップS202)。符号パターン決定処理は、棄却部235により実行される。符号パターン決定処理については、図14を用いて後述する。
続いて、算出部236は、後に用いる最小誤差Jの値を初期化し(ステップS203)、ステップS203に示す符号パターン決定処理により得られる各符号パターンについて、以下のステップS204〜ステップS215までの処理を実行する。なお、ステップS204〜ステップS215は、全符号パターンについてステップS205〜ステップS214の処理が完了するまで繰り返し実行することを意味する。算出部236は、符号パターンを一つ選択し(ステップS204)、選択した符号パターンについて距離L1、L2およびL3の値をそれぞれ算出する(ステップS205)。次に、算出部236は、距離L1、L2およびL3の値が正の実数であるか否かを判定する(ステップS206)。判定の結果、算出部236は、距離L1、L2およびL3の値が正の実数ではない場合には(ステップS206,NO)、次の符号パターンを選択する。これとは反対に、算出部236は、判定の結果、距離L1、L2およびL3の値が正の実数である場合には(ステップS206,YES)、誤差Eの値を初期化(E=0)する(ステップS207)。
続いて、算出部236は、ステップS204にて選択した符号パターン内で基準点M以外の2点(点1〜3のうちの2点)の組合せを一つ取得する(ステップS208)。次に、算出部236は、取得した2点の組合せについて2点間の距離の候補値を算出する(ステップS209)。続いて、算出部236は、算出した候補値に対応する3次元空間上のマーカー間の実距離の値をマーカー距離情報記憶部223から取得し、この候補値と、この候補値に対応する実距離の値との誤差を演算する(ステップS210)。そして、算出部236は、算出した誤差の値を誤差Eの値に加算して更新し(ステップS211)、誤差Eの値が最小誤差Jの値よりも大きいか否かを判定する(ステップS212)。
算出部236は、判定の結果、誤差Eの値が最小誤差Jの値以下である場合には(ステップS212,NO)、選択した符号パターン内で基準点0以外の2点の組合せの全てについて処理を完了したか否かを判定する(ステップS213)。算出部236は、判定の結果、選択した符号パターン内で基準点0以外の2点の組合せの全てについて処理を完了している場合には(ステップS213,YES)、誤差Eの値で最小誤差Jの値を更新する(ステップS214)。続いて、ステップS202の符号パターン決定処理により得られる全符号パターンについてステップS205〜ステップS214の処理が完了するまで(ステップS215)、上述したステップS202に処理を戻す。
ここで、ステップS213の説明に戻る。算出部236は、判定の結果、選択した符号パターン内で基準点0以外の2点の組合せの全てについて処理を完了していない場合には(ステップS213,NO)、上述したステップS208に処理を戻す。
ここで、ステップS212の説明に戻る。算出部236は、判定の結果、誤差Eの値が最小誤差Jの値よりも大きい場合には(ステップS213,YES)、次のように処理を実行する。すなわち、算出部236は、ステップS202の符号パターン決定処理により得られる全符号パターンについてステップS205〜ステップS214の処理が完了するまで(ステップS215)、上述したステップS202に処理を戻す。
そして、算出部236は、ステップS204〜ステップS215までの処理を完了した場合には、次のステップS216の処理を行う。すなわち、算出部236は、「基準点=M(点0〜点3のいずれか)」に関し、最終的に残った誤差Eの値、すなわち最小誤差の値と、最小誤差に対応する符号パターンの距離L0、L1、L2およびL3の値を処理結果として送出する(ステップS216)。以上で、実施例1に係る最適解候補算出処理が完了するが、上述してきた図13に示す最適解算出処理は、処理内容に矛盾を生じさせない範囲でその順序を入れ替えることができる。
なお、上述したステップS210で用いるマーカー間の実距離の値は、以下の式(7)で表すことができる。
なお、上述したステップS205で算出される距離L1、L2およびL3の値は、以下に示す式(8)の解を求めることにより算出することができる。
なお、上述したステップS210で算出される誤差の値は、以下の式(9)で表すことができる。
なお、上述したステップS211の処理は、以下の式(10)で表すことができる。
次に、図14を用いて、実施例1に係る符号パターン決定処理の流れを説明する。図14は、実施例1に係る符号パターン決定処理の流れを示す図である。図14に示すように、符号パターン決定処理では、基準点M以外の全ての対応点N(点1〜点3)について、ステップS301〜ステップS305までの処理が繰り返し実行される。
まず、棄却部235は、基準点M以外の対応点N(点1〜点3のいずれか)を一つ取得し(ステップS301)、∠MCNが90度よりも大きいか否かを判定する(ステップS302)。なお、∠MCNとは、原点C及び基準点Mを結ぶ線分と、原点C及び対応点Nを結ぶ線分とがなす角度をいう。∠MCNが90度よりも大きいか否かの判定は、上述したように、原点C及び基準点Mを結ぶ線分上の視線ベクトルと、原点C及び対応点Nを結ぶ線分上の視線ベクトルとの内積に基づいて行なう。
棄却部235は、判定の結果、∠MCNが90度よりも大きい場合には(ステップS302,YES)、選択した対応点Nの一点ではプラスの符号のみを取り得ることを記録する(ステップS303)。一方、棄却部235は、判定の結果、∠MCNが90度以下である場合には(ステップS302,NO)、選択した対応点Nの一点ではプラス、マイナスの両方の符号を取り得ることを記録する(ステップS304)。
そして、棄却部235は、基準点M以外の全ての対応点Nについて、ステップS301〜ステップS305までの処理を終えると、対応点Nが取り得る符号の組合せを列挙して符号パターンを決定する(ステップS306)。以上で、実施例1に係る符号パターン決定処理が完了する。
上述してきた図12〜図14の処理の意味は、以下の数式(11)で表すことができる。なお、以下に示す式(11)の左辺のJは、上述した式(6)のJ、つまり、Liを符号選択によって求めることを含んだ誤差評価関数である。そして、式(11)から得られる解は、基準点「i0」までの距離を「Li0」とした時に、基準点「i0」以外の点iまでの距離「Li」を符号選択により最適に選んだ場合に残っている誤差となる。よって、式(11)は、Jを最小化する解を探索することにより、基準点「i0」までの最適な距離「Li0」と、基準点「i0」以外の点iまでの最適な距離「Li」を同時に得ることができることを表している。
[実施例1による効果]
上述してきたように、実施例1に係る画像処理装置200は、導出された符号パターンの中から、距離L1、L2、L3の値の少なくとも1つが相応しい値となり得えない符号パターンを棄却する。よって、実施例1に係る画像処理装置200は、4個以上のマーカーが映し出された画像平面上の点に対応する3次元空間上の対応点の位置を迅速かつ精度良く算出できる。この結果、実施例1によれば、カメラの取り付け位置や取り付け姿勢の情報を迅速かつ精度良く得ることができる。なお、3点の場合は、最少誤差を与える解の組合せである最適解が1つに定まらない。このため、上述してきた実施例1は、4点以上のマーカーを用いて、画像処理装置が最適解算出処理を行う。この結果、実施例1では、最適解を得ることができる。
また、実施例1に係る画像処理装置は、例えば、図13に示すように、符号パターンの誤差を算出するときに、従前に算出された符号パターンの誤差にうちの最小誤差よりも誤差が大きくなる符号パターンの処理は取りやめて、次の符号パターンの処理を行う。このため、実施例1によれば、4個以上のマーカーが映し出された画像平面上の点に対応する3次元空間上の対応点の位置をより迅速に算出できる。この結果、実施例1によれば、カメラの取り付け位置や取り付け姿勢の情報をより迅速に得ることができる。
また、実施例1に係る画像処理装置は、基準点M以外の点、つまり基準マーカー以外のマーカーに対応する点1〜点3までの各距離L1,L2,L3を表す数式の中の根号内が虚数となる場合には、基準点までの距離や基準点の設定をやり直す。このため、実施例1によれば、4個以上のマーカーが映し出された画像平面上の点に対応する3次元空間上の対応点の位置をより迅速に算出できる。この結果、実施例1によれば、カメラの取り付け位置や取り付け姿勢の情報をより迅速に得ることができる。