(57)【要約】【課題】高精度かつ安定して自己位置推定の精度を向上することができる撮像装置を提供する。【解決手段】撮像装置１００は、第１の被写体を撮像する第１撮像部１０１と、前記第１撮像部で撮像した画像データから第１自己位置姿勢の推定を行う第１位置姿勢推定手段１０５と、第１撮像部１０１と異なる第２の被写体を撮像する第２撮像部１０３と、第２撮像部１０２で撮像した画像データから第２自己位置姿勢の推定を行う第２位置姿勢推定手段１０６と、第１自己位置姿勢の推定と第２自己位置姿勢の推定に基づいて、撮像装置１００の自己位置姿勢を決定する第３位置姿勢推定手段１０７とを備える。【選択図】図１
2019041223.tif 000002
【特許請求の範囲】
【請求項１】
第１の被写体を撮像する第１撮像部と、
前記第１撮像部で撮像した画像データから第１自己位置姿勢の推定を行う第１位置姿勢推定手段と、
前記第１撮像部と異なる第２の被写体を撮像する第２撮像部と、
前記第２撮像部で撮像した画像データから第２自己位置姿勢の推定を行う第２位置姿勢推定手段と、
前記第１自己位置姿勢の推定と前記第２自己位置姿勢の推定に基づいて、撮像装置の自己位置姿勢を決定する第３位置姿勢推定手段と
を備える
ことを特徴とする撮像装置。
【請求項２】
前記第２撮像部は、前記第２の被写体として静止している撮影者を撮像する
ことを特徴とする請求項に記載の撮像装置。
【請求項３】
前記第２撮像部の撮影方向と前記第１撮像部の撮影方向は反対である
ことを特徴とする請求項に記載の撮像装置。
【請求項４】
前記第１撮像部によって撮影された画像データを表示する第１表示手段
をさらに備え、
前記第１表示手段が表示する方向と前記第２撮像部の撮影方向は同一である
ことを特徴とする請求項２に記載の撮像装置。
【請求項５】
前記第２撮像部は、
前記第２の被写体である前記撮影者の顔認識を行う顔認識処理手段と、
前記顔認識処理手段により顔認識ができない場合、前記撮影者に通知する通知部と
を備える
ことを特徴とする請求項２乃至４のいずれか１項に記載の撮像装置。
【請求項６】
前記通知部は、前記第１表示手段に通知を行う
ことを特徴とする請求項５に記載の撮像装置。
【請求項７】
前記第１撮像部は、
撮影の倍率変化を判定する倍率変化手段と、
予め決定された第１撮像部の撮影倍率の閾値を記憶する記憶手段と
をさらに備え、
前記倍率変化手段により前記記憶手段が記憶している閾値よりも前記第１撮像部の撮影倍率が大きくなったと判定された場合、前記第１位置姿勢推定手段は、前記第１自己位置姿勢の推定を行わず、第２位置姿勢推定手段が行う第２自己位置姿勢の推定に基づいて前記第３位置姿勢推定手段が前記己位置姿勢を決定する
ことを特徴とする請求項１に記載の撮像装置。
【請求項８】
前記第２撮像部は、前記第２の被写体として地面を撮像する
ことを特徴とする請求項１に記載の撮像装置。
【請求項９】
前記第１撮像部と前記第２撮像部の撮影方向のなす角が前記第１撮像部の撮影方向から地面の方向に向かう角度で１３５°以上であり、前記第１撮像部と前記第２撮像部の撮影方向が張る面内の前記第２撮像部の視野角が９０°以上である
ことを特徴とする請求項８に記載の撮像装置。
【請求項１０】
前記第１撮像部と前記第２撮像部の撮影方向のなす角が前記第１撮像部の撮影方向から地面の方向に向かう角度で９０°以上であり、前記第１撮像部と前記第２撮像部の撮影方向が張る面内の前記第２撮像部の視野角が１８０°以上である
ことを特徴とする請求項８に記載の撮像装置。
【請求項１１】
前記第２撮像部の位置、撮影方向を変える可動部と、
前記第２撮像部が撮影する前記第２の被写体のうち、前記第２位置姿勢推定手段が前記第２自己位置姿勢の推定に用いる静的被写体を選択する選択手段と
をさらに備える
ことを特徴とする請求項１に記載の撮像装置。
【請求項１２】
前記第２撮像部によって出力された画像データを表示する第２表示手段
をさらに備え、
前記選択手段は、前記第２表示手段が表示する画像データに写った被写体から選択する
ことを特徴とする請求項１１に記載の撮像装置。
【請求項１３】
前記第２位置姿勢推定手段は、前記第２撮像部によって出力された前記画像中から特徴点を抽出する抽出手段と、
前記第２表示手段は、前記画像データに、前記抽出手段により抽出された特徴点を重畳表示する
ことを特徴とする請求項１２に記載の撮像装置。
【請求項１４】
前記第１位置姿勢推定手段は、前記第１自己位置姿勢の推定の第１誤差を算出し、前記第２位置姿勢推定手段は、前記第２自己位置姿勢の推定の第２誤差を算出し、
前記第３位置姿勢推定手段は、前記第１誤差と前記第２誤差のうち小さい方に対応する自己位置姿勢の推定に基づいて前記撮像装置の自己位置姿勢を決定する
ことを特徴とする請求項１乃至１３のいずれか１項に記載の撮像装置。
【請求項１５】
前記第１位置姿勢推定手段による第１自己位置姿勢の推定の第１目的関数を算出する第１算出手段と、
前記第２位置姿勢推定手段による第２自己位置姿勢の推定の第２目的関数を算出する第２算出手段と、
前記第１目的関数と前記第２目的関数に基づいて第３目的関数を算出する第３算出手段と
をさらに備え、
前記第３位置姿勢推定手段は、前記第１目的関数と前記第２目的関数を統合した前記第３目的関数が最小になるように前記撮像装置の自己位置姿勢を決定する
ことを特徴とする請求項１乃至１３のいずれか１項に記載の撮像装置。
【請求項１６】
前記第１表示手段に表示するオブジェクトを記憶する記憶装置と、
前記第３位置姿勢推定手段により決定された前記撮像装置の自己位置姿勢に基づいて前記オブジェクトを変形する演算部と
をさらに備え、
前記第１表示手段は、前記演算部により変形したオブジェクトを重畳表示する
ことを特徴とする請求項４乃至１５のいずれか１項に記載の撮像装置。
【請求項１７】
前記第１撮像部によって撮像された画像データと撮影した時刻を記憶する第１記憶装置と、
前記第３位置姿勢推定手段により決定された前記撮像装置の自己位置姿勢を対応する前記画像データの撮影した時刻とともに記憶する第２記憶装置と、
前記画像データのうち、前記第１表示手段に表示させる画像データを選択する選択手段と
をさらに備え、
前記演算部は、前記選択手段で選択された画像データの撮影した時刻に対応する前記自己位置姿勢に基づいて前記オブジェクトを変形する
ことを特徴とする請求項１６に記載の撮像装置。
【請求項１８】
前記第１撮像部によって撮影された画像データを表示する第１表示手段と、
過去の前記第３位置姿勢推定手段による前記撮像装置の自己位置姿勢の決定で使用された被写体の３次元座標と、前記３次元座標に対する画像上の画素位置のうち少なくとも一方を記憶する記憶装置と、
前記撮像装置の自己位置姿勢を指定する指定手段と、
前記被写体の３次元座標の前記指定手段により指定された自己位置姿勢で撮影したときの
画素位置を計算する演算部と
をさらに備え、
前記演算部は、前記記憶装置に前記３次元座標が記憶されていない場合に、前記第１表示手段の前記画素位置の画素値を前記画素値として画像を生成する
ことを特徴とする請求項１に記載の撮像装置。
【請求項１９】
第１の被写体を撮像する第１撮像部と、前記第１撮像部と異なる第２の被写体を撮像する第２撮像部とを備える撮像装置の制御方法であって、
前記第１撮像部で撮像した画像データから第１自己位置姿勢の推定を行う第１位置姿勢推定工程と、
前記第２撮像部で撮像した画像データから第２自己位置姿勢の推定を行う第２位置姿勢推定工程と、
前記第１自己位置姿勢の推定と前記第２自己位置姿勢の推定に基づいて、撮像装置の自己位置姿勢を決定する第３位置姿勢推定工程と
を有する
ことを特徴とする撮像装置の制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、撮像装置およびその制御方法に関し、特に、自己位置姿勢推定可能な撮像装置に関する。
【背景技術】
【０００２】
カメラで撮影した画像から自己位置姿勢を推定する技術の応用分野は多岐にわたる。ロボットの自己位置認識、車の走行経路や実写映像にアニメを重畳させるＡＲなどが挙げられる。自己位置姿勢推定では、異なる時刻に撮影した画像に写った同じ被写体の画像内における位置の変化からカメラの位置と姿勢を算出している。このため、異なる画像間で同一被写体の対応が精度よく取れていることが、自己位置姿勢推定を精度よく行うために必要となる。自己位置姿勢推定の精度が下がってしまう例として、被写体のテクスチャが低い場合には対応点が適切に取れなかったり、動的被写体が写っている場合には被写体とカメラのどちらが動いているのかが判別できないといったものがある。また、自己位置推定そのものができない例として、カメラの向きが急に変わった場合に被写体がフレームアウトして、対応点自体がなくなってしまうといったものがある。
【０００３】
特許文献１では、車の前方に前向き、後方に後ろ向きの２台のカメラを設定して、それぞれのカメラが自己位置推定を行い画像を蓄積しておく。これによって、車が急に進行方向を変えて、前方カメラから注目被写体がフレームアウトし、前方カメラが新たに捉えた被写体が、後方のカメラによって蓄積された画像に写っていれば車の位置を見失わない。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１２−１７３８３４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１が開示している、２台のカメラを用いて、自己位置推定に用いる被写体を見失わないようにする方法では、車が急に向きを変えたときに、前方カメラで新たに捉えた被写体が、後方カメラが蓄積した画像にも写っている必要がある。しかし、前方カメラと後方カメラの向きと画角によってはその限りではない。複数のカメラで自己位置姿勢推定を行う場合、少なくとも１つのカメラが被写体を見失うことなくいつも捉えるためには、超広角のカメラを用いたり、カメラを複数取り付ければよい。しかし、超広角カメラでは歪曲収差によって被写体の形がひずんだり、解像力が下がったりして画像間の対応づけが難しくなったり、カメラを多数取りつけると撮影する画像の枚数が大量になって処理に時間がかかってしまう恐れがある。
【０００６】
本発明は、上記課題を鑑みて、高精度かつ安定して自己位置推定の精度を向上することができる撮像装置を提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記の課題を解決するために、本発明の撮像装置は、第１の被写体を撮像する第１撮像部と、前記第１撮像部で撮像した画像データから第１自己位置姿勢の推定を行う第１位置姿勢推定手段と、前記第１撮像部と異なる第２の被写体を撮像する第２撮像部と、前記第２撮像部で撮像した画像データから第２自己位置姿勢の推定を行う第２位置姿勢推定手段と、前記第１自己位置姿勢の推定と前記第２自己位置姿勢の推定に基づいて、撮像装置の自己位置姿勢を決定する第３位置姿勢推定手段とを備えることを特徴とする。
【発明の効果】
【０００８】
本発明によれば、高精度かつ安定して自己位置推定の精度を向上することができる撮像装置を提供することができる。
【図面の簡単な説明】
【０００９】
【図１】撮像装置の側面外観図と機能的構成を示す図である。
【図２】自己位置姿勢推定の原理と、画像の対応づけを示す図である。
【図３】撮像装置の処理を示すフローチャートである。
【図４】第２実施形態に係る撮像装置の側面外観を示す外観図である。
【図５】第３実施形態に係る撮像装置の側面外観を示す外観図である。
【図６】第３実施形態に係る撮像装置の背面外観図と機能的構成を示す図である。
【図７】第４実施形態に係る撮像装置の背面外観図と機能的構成を示す図である。
【図８】第５実施形態に係る撮像装置の背面外観図と機能的構成を示す図である。
【図９】第６実施形態に係る撮像装置の側面外観を示す外観図である。
【図１０】第６実施形態に係る撮像装置の自己位置姿勢推定の原理を示す図である。
【図１１】第７実施形態に係る撮像装置の側面外観図である。
【図１２】第７実施形態に係る撮像装置の自己位置姿勢推定の原理を示す図である。
【図１３】第８実施形態に係る撮像装置の側面外観図である。
【図１４】第８実施形態に係る撮像装置の背面外観図と機能的構成を示す図である。
【図１５】第９実施形態に係る撮像装置の背面外観図と機能的構成を示す図である。
【図１６】第１０実施形態に係る撮像装置の機能的構成を示す図である。
【図１７】第１１実施形態に係る撮像装置の背面外観図と機能的構成を示す図である。
【発明を実施するための形態】
【００１０】
以下、本発明を実施するための最良の形態について図面などを参照して説明する。
【００１１】
（第１実施形態）
図１（Ａ）は、本実施形態に係る撮像装置１００の側面外観を示す図である。撮像装置１００は、第１撮像部１０１、第２撮像部１０２を備えている。第１撮像部１０１と第２撮像部１０２は、互いに他方が撮影していない被写体を含むように撮影できるような光学系の位置、向きや画角が設定されている。
【００１２】
図１（Ｂ）は、本実施形態に係る撮像装置１００の構成例を表すブロック図である。第１撮像部１０１は、レンズと撮像素子からなる。レンズは被写体から放たれた光を集光し、撮像素子上に光学像を作る。撮像素子は、光学像を電子像に変換して画像データを出力する。記憶装置１０３は、第１撮像部１０１が出力した画像データを記憶する。第１自己位置姿勢推定演算部１０５は、記憶装置１０３が記憶した撮影位置の異なる２枚の画像に対して、第１自己位置姿勢推定を算出する。２枚の画像は、連続フレームとして撮影されたものでもよく、２フレーム以上離れた画像でもよい。ただし、２枚の画像中に同じ被写体が写っている必要がある。第２撮像部１０２に対しても第１撮像部１０１と同様な構成を有し、第２自己位置姿勢推定演算部１０６は、第２撮像部１０２から出力された画像に対して、第２自己位置姿勢推定を演算する。なお、不図示であるが、撮像装置１００は、各構成部品を制御する制御部を有する。
【００１３】
次に、第１自己位置姿勢推定の算出方法を説明する。図２（Ａ）は、撮像装置１００が異なる時刻で撮影するときの位置姿勢を示す。位置姿勢３０１はある時刻での撮像装置１００の位置姿勢を表し、位置姿勢３０２は、それとは異なる時刻での撮像装置１００の位置姿勢を表す。位置姿勢３０１と位置姿勢３０２は、第１撮像部１０１が撮影する連続フレームに対応する撮像装置の位置姿勢であってもよく、離れたフレームに対するものであってもよい。ただし、位置姿勢３０１での撮影と位置姿勢３０２での第１撮像部１０１の撮影において、同一の被写体が写っている必要がある。同一被写体が写っている２フレームは、第１撮像部１０１の設置位置、撮影範囲Ｗｆ、撮影フレームレートと撮像装置１００の動く速度から決まる。ここで、位置姿勢３０１を基準位置姿勢として、位置姿勢３０２での基準位置に対する撮像装置１００の位置ベクトルを
【数１】
2019041223.tif 000003
基準姿勢に対する姿勢の回転行列を
【数２】
2019041223.tif 000004
と表す。ただし、Ｒは３次元の回転行列であり、例えばｚ軸の周りの回転角がθ２のときには
【数３】
2019041223.tif 000005
と表せる。基準位置姿勢は、空間上のどの位置やどの向きに取ってもよいが、その状態での基準位置ベクトルを
【数４】
2019041223.tif 000006
とし、基準姿勢を表す回転行列を
【数５】
2019041223.tif 000007
と決める。
【００１４】
図２（Ｂ）は、位置姿勢３０１と位置姿勢３０２のときに撮影した画像を示す。位置姿勢３０１で撮影した画像が画像４０１、位置姿勢３０２で撮影した画像が画像４０２であり、特徴点４０３と特徴点４０４は被写体中の対応点である。特徴点４０３と特徴点４０４の世界座標を
【数６】
2019041223.tif 000008
とし、この点の位置姿勢３０１と位置姿勢３０２において撮影した画像上での２次元座標をそれぞれ
【数７】
2019041223.tif 000009
とすると、
【数８】
2019041223.tif 000010
が成り立つ。ここで、
【数９】
2019041223.tif 000011
はカメラ行列、ｆｘ、ｆｙはそれぞれ本撮像装置のレンズの焦点距離のｘ、ｙ成分であり、ｃｘ、ｃｙはそれぞれ本撮像装置のレンズの光軸位置のｘ、ｙ成分であるが、これらは事前にキャリブレーションを行うことで既知とすることができる。ｔとＲの算出方法の一つとして、線形計算がある。（式１）から
【数１０】
2019041223.tif 000012
を消し去ると、
【数１１】
2019041223.tif 000013
が得られる。ここで、Ｃ−Ｔは行列Ｃの逆行列の転置行列を表す。特徴点４０３と特徴点４０４の組み合わせを８組以上とって（式２）を８個作り、
【数１２】
2019041223.tif 000014
を計算した後、特異値分解などによってｔとＲを分解すればよい。また、特徴点４０３と特徴点４０４の組み合わせを７組以上とって非線形解法により算出することも可能である。目的関数は
【数１３】
2019041223.tif 000015
であり、（式３）を最小にするようなｔとＲを反復計算により求めればよい。
ここで、
【数１４】
2019041223.tif 000016
はそれぞれ位置姿勢３０１と位置姿勢３０２において撮影した画像上での対応点の２次元座標である。添え字のｊは特徴点４０３と特徴点４０４のような対応点を複数取ったときにｊ番目の対応点であることを示し、Ｎはその組み合わせ数である。特徴点４０３と特徴点４０４のような対応点は、画像４０１からＦＡＳＴ、ＳＩＦＴ、ＳＵＲＦなどの手法によって画像中から特徴点を見つけて（抽出して）、その特徴点が画像４０２内のどこに位置するかを探索すればよい。例えば、ＳＳＤ（Ｓｕｍ  ｏｆ  Ｓｑｕａｒｅｄ  Ｄｉｆｆｅｒｅｎｃｅ）、ＺＮＣＣ（Ｚｅｒｏ−ｍｅａｎ  Ｎｏｒｍａｌｉｚｅｄ  Ｃｒｏｓｓ−Ｃｏｒｒｅｌａｔｉｏｎ）などの手法によって探索すればよい。特に、非線形解法では特徴点に限らずＮ＝画素数として全画素に対して（式３）を最小にするｔとＲを算出してもよい。
【００１５】
次に、第２自己位置姿勢推定の算出方法を説明する。第１撮像部１０１での撮影と同様に、位置姿勢３０１での撮影と位置姿勢３０２での撮影において同一の被写体が写っている必要がある。同一被写体が写っている２フレームは、第２撮像部１０２の設置位置、撮影範囲Ｗｂ、撮影フレームレートと撮像装置１００の動く速度から決まる。また、第１撮像部１０１の撮影と第２撮影部１０２の撮影とで、異なる被写体を含んでいる必要がある。これは、第１撮像部１０１の設置位置と第２撮像部１０２の設置位置、第１撮像部の撮影範囲Ｗｆ、第２撮像部の撮影範囲Ｗｂなどから決まる。その他の第２自己位置姿勢推定の算出方法は第１自己位置姿勢推定の算出方法と同じである。
【００１６】
次に、第３自己位置姿勢推定の算出方法を説明する。第３自己位置姿勢推定演算部１０７は第１自己位置姿勢推定ｔ１とＲ１と第２自己位置姿勢推定ｔ２とＲ２から撮像装置１００の自己位置姿勢推定ｔとＲを唯一に決定する。第３自己位置姿勢推定演算部１０７が自己位置姿勢推定ｔとＲを決定するには、第１自己位置姿勢推定ｔ１とＲ１と第２自己位置姿勢推定ｔ２とＲ２に対して、それぞれ（式４）、（式５）によりエラー（第１誤差、第２誤差）を計算する。
【数１５】
2019041223.tif 000017
【数１６】
2019041223.tif 000018
ここで、
【数１７】
2019041223.tif 000019
であり、
【数１８】
2019041223.tif 000020
はそれぞれ第１撮像部１０１と第２撮像部１０２のカメラ行列である。
【数１９】
2019041223.tif 000021
はそれぞれ位置姿勢３０１と位置姿勢３０２において第１撮像部１０１が撮影した画像上でのｊ番目の対応点の２次元座標であり、
【数２０】
2019041223.tif 000022
はそれぞれ位置姿勢３０１と位置姿勢３０２において第２撮像部１０２が撮影した画像上でのｊ番目の対応点の２次元座標である。ここで、第１撮像部１０１が撮影した画像上での特徴点の個数をＮ１、第２撮像部１０２が撮影した画像上での特徴点の個数をＮ２とした。
【００１７】
こうして、算出したエラーＥ１（Ｒ１，ｔ１）、Ｅ２（Ｒ２，ｔ２）で大小を比較して、
【数２１】
2019041223.tif 000023
であるならｔ＝ｔ１、Ｒ＝Ｒ１として撮像装置１００の自己位置姿勢推定ｔおよびＲを唯一に決定する。一方、Ｅ１（Ｒ１，ｔ１）＞Ｅ２（Ｒ２，ｔ２）であるならｔ＝ｔ２、Ｒ＝Ｒ２として撮像装置１００の自己位置姿勢推定ｔおよびＲを唯一に決定する。エラーが小さいほうの自己位置姿勢推定値を採用することによって、より高精度な自己位置姿勢推定値に決定することができる。
【００１８】
また、次のように第３自己位置姿勢推定演算部１０７が自己位置姿勢推定ｔおよびＲを決定することもできる。つまり、目的関数として(式３）の代わりに
【数２２】
2019041223.tif 000024
を用いて、第１目的関数と第２目的関数を統合した第３目的関数である（式６）を最小にするようなｔとＲを反復計算により求める。
ここで、
【数２３】
2019041223.tif 000025
は上述の通りであり、α１、α２は正の数である。この場合には第１自己位置姿勢推定演算部１０５と第２自己位置姿勢推定演算部１０６は、自己位置姿勢推定を行わずに、それぞれ第１撮像部１０１と第２撮像部１０２で撮影した画像から特徴点４０３と特徴点４０４のような対応点を見つけて、
【数２４】
2019041223.tif 000026
の設定のみを行う。
α１、α２の決め方は例えば、α１＝α２＝１とすれば、第１撮像部１０１と第２撮像部１０２から得られる画像中の特徴点をすべて同等に扱って自己位置姿勢推定を行うことができる。この場合には、両撮像部を合わせた広い視野角で自己位置姿勢推定を行うことに相当し、単一の光学系を広角にした場合よりも歪曲収差や解像力の低下を抑えることができる。また、
【数２５】
2019041223.tif 000027
とすれば、第１撮像部１０１と第２撮像部１０２でそれぞれ撮影した画像中の対応点の個数によらずに、両者を対等に扱って自己位置姿勢推定を行うことができる。この場合には、例えば視野角が狭く対応点の個数が少ないが高精度に自己位置姿勢推定ができているの撮像部に対しても、他方の撮像部と同等に自己位置姿勢推定演算に寄与させることができる。
【００１９】
その他、次式で表わされる目的関数を用いて第３自己位置姿勢推定演算部１０７が自己位置姿勢推定ｔおよびＲを決定することもできる。
【数２６】
2019041223.tif 000028
このときには、位置姿勢３０１と位置姿勢３０２において撮影した画像の位置合わせ精度によって特徴点ごとにα１、α２を決める。例えば、上述の位置合わせ手法ＺＮＣＣを用いて特徴点４０３と特徴点４０４の対応付けの精度からα１、α２を決める方法について説明する。まず、画像４０１内に特徴点４０３を含むようにウィンドウ４０５を設定する。ウィンドウ４０５は特徴点４０３の周辺のテクスチャを表現できるサイズに設定する。画像４０２内にウィンドウ４０５と同じサイズのウィンドウ４０６を設定する。ウィンドウ４０６の初期位置は画像内のどこにとってもよいが、あらかじめ特徴点４０４のおおよその位置が分かっているならばその付近の設定すると計算を高速化することができる。以下の式によってウィンドウ４０５とウィンドウ４０６のＺＮＣＣを計算する。
【数２７】
2019041223.tif 000029
【００２０】
ここで、Ａ（ｘ，ｙ）はウィンドウ４０５内の水平方向ｘ番目、垂直方向ｙ番目の画素値であり、Ｎ、Ｍはそれぞれ水平方向と垂直方向の画素値である。Ｂ（ｘ，ｙ）は画像４０２内の水平方向ｘ番目、垂直方向ｙ番目の画素値であり、ウィンドウ４０６の開始画素位置は水平方向ｘ０番目、垂直方向ｙ０番目である。
【数２８】
2019041223.tif 000030
はそれぞれウィンドウ４０５とウィンドウ４０６内の画素値の平均値であり、以下の式で表わされる。
【数２９】
2019041223.tif 000031
（式８）のＣ（ｘ０，ｙ０）は
【数３０】
2019041223.tif 000032
の範囲の値を取り、ウィンドウ４０５内のテクスチャとウィンドウ４０６内のテクスチャが似ているほど大きい値をとる。ｘ０、ｙ０を変化させて、ウィンドウ４０６を図２（Ｂ）に示した矢印のごとく水平方向と垂直方向にずらしていき、（式８）のＣ（ｘ０，ｙ０）の値が最大となるときの（ｘ０，ｙ０）がウィンドウ４０５に対するウィンドウ４０６の位置ずれ量である。このときのＣ（ｘ０，ｙ０）をＣｍａｘ＝Ｃ（ｘ０，ｙ０）と表す。ここから特徴点４０４も特徴点４０３に対して（ｘ０，ｙ０）だけずれたところにあると対応付けすることができる。ここで、第１撮像部１０１が撮影した画像内のｊ番目の特徴点のＣｍａｘをＣｍａｘ，１，ｊ、第２撮像部１０２に対する特徴点をＣｍａｘ，2，ｊと表す。この時に、（式７）でα１，ｊ＝Ｃｍａｘ，１，ｊ、α２，ｊ＝Ｃｍａｘ，2，ｊとすることで、特徴点の対応付けの精度を反映した目的関数を作ることができる。この場合には、対応付けの精度が高い特徴点ほど大きく自己位置姿勢推定演算に寄与するため、高精度に自己位置姿勢推定が行える。
【００２１】
図３は、第３自己位置姿勢推定演算部１０７が、エラーＥ１（Ｒ１，ｔ１）、Ｅ２（Ｒ２，ｔ２）の大小によって本撮像装置の自己位置姿勢推定を決定する場合の処理のフローを示す。ステップＳ５０１で、第１撮像部１０１と第２撮像部１０２の撮影フレームのループを開始する。次に、ステップＳ５０２で、第１撮像部１０１で撮影をする。次に、ステップＳ５０３で、現在のフレームが第２フレーム以降である場合に、第１撮像部１０１が出力した現在のフレームと過去のフレームの２枚の画像を用いて、第１自己位置姿勢推定演算部１０５が自己位置姿勢算出を行う。そして、自己位置姿勢推定値（Ｒ１，ｔ１）を得る。過去のフレームは直前のフレームでもよいし、２フレーム以上前のものであってもよい。ただし、２枚の画像内に同じ被写体が写っている必要がある。
【００２２】
次に、ステップＳ５０４で、第１自己位置姿勢推定演算部１０５が処理５０３で算出した自己位置姿勢推定値（Ｒ１，ｔ１）に対して（式４）によりＥ１（Ｒ１，ｔ１）を算出する。次に、ステップＳ５０５で、第２撮影部１０２で撮影をする。第１撮像部１０１と第２撮像部１０２の撮影時刻は揃っていることが望ましいが、必ずしも揃っていることが必要ではない。両者の撮影時刻が揃っていない場合、例えば、第１撮影部１０１の撮影時刻に最も近い撮影時刻で撮影した第２撮像部１０２の撮影画像を用いる方法採用してもよい。また、第２撮像部１０２の複数の撮影画像から補完処理などで同一時刻の撮影画像を仮想的に作り出す方法を採用してもよい。本実施形態では、第１撮像部１０１と第２撮像部１０２の撮影時刻は揃っている場合を扱う。
【００２３】
次に、ステップＳ５０６で、現在のフレームが第２フレーム以降である場合に、第２撮像部１０２が出力した現在のフレームと過去のフレームの２枚の画像を用いて、第２自己位置姿勢推定演算部１０６が自己位置姿勢算出を行う。そして、自己位置姿勢推定値（Ｒ２，ｔ２）を得る。次に、ステップＳ５０７で、第２自己位置姿勢推定演算部１０６が処理５０６で算出した自己位置姿勢推定値（Ｒ２，ｔ２）に対して（式４）によりＥ２（Ｒ２，ｔ２）を算出する。次に、ステップＳ５０８で、Ｅ１（Ｒ１，ｔ１）とＥ２（Ｒ２，ｔ２）の大小を判定する。そして、Ｅ２（Ｒ２，ｔ２）の方が大きい場合、ステップＳ５０９に進み、Ｅ１（Ｒ１，ｔ１）の方が大きい場合、ステップＳ５１０に進む。
【００２４】
次に、ステップＳ５０９で、第３自己位置姿勢推定演算部１０７がステップＳ５０３で算出した自己位置姿勢推定値（Ｒ１，ｔ１）を本撮像装置の自己位置姿勢推定値（Ｒ，ｔ）に決定する。一方、ステップＳ５１０で、第３自己位置姿勢推定演算部が処理５０６で算出した自己位置姿勢推定値（Ｒ２，ｔ２）を本撮像装置の自己位置姿勢推定値（Ｒ，ｔ）に決定する。そして、ステップＳ５１１でフレームのループを終了する。
【００２５】
（第２実施形態）
図４は、第２の実施形態の撮像装置１００の側面外観と、撮影者が撮像装置１００を手で持って撮影している様子を示す。撮像装置１００は、第１撮像部１０１、第２撮像部１０２を備えている。第１撮像部１０１が撮影する方向１と第２撮像部１０２が撮影する方向２を図中に矢印で示す。お互いが正反対の方向を撮影するように第１撮影部１０１と第２撮影部１０２が設置されている。これによって、撮影者が撮像装置１００を手で持って第１撮像部１０１によって撮影した場合には、第２撮像部１０２は撮影者自身を撮影することができる。
【００２６】
第１撮像部で撮影する被写体が遠方にあったり、霧などの環境下では特徴点を精度よく抽出できなかったり、特徴点の対応点の検出を精度よくできなかったりしてしまう。また、被写体に動的被写体を含んでいるときには被写体が動いているのか、撮像装置１００が動いているのかの判別ができず、自己位置姿勢推定が正しく行えない。しかし、第１撮像部１０１による撮影がこのような場合でも、第２撮像部１０２が撮影者を撮影していれば自己位置姿勢推定を精度よく行える。例えば、第１撮像部１０１が遠方を撮影していて霧などの環境化で解像力の高い画像が得られない場合でも、第２撮像部１０２と撮影者の距離は２０ｃｍ〜７０ｃｍ程度であるため霧の影響が少なく解像力の高い画像が得られる。本実施形態では、方向１と方向２が正反対である例を挙げたが、第１撮像部１０１を用いて撮影中に、第２撮像部１０２が撮影者を撮影できる位置、撮影方向、視野角であれば第２撮像部１０２はどのように設定されていてもよい。また、本実施形態の構成によって、撮影に不向きな環境下でも良好に自己位置姿勢推定を行える。人は特徴点抽出のための十分なテクスチャを持っているため、特徴点の抽出や特徴点の対応点の検出が精度よく行える。なお、本実施形態においても、その他の構成や処理については第１実施形態と同様である。
【００２７】
（第３実施形態）
図５は、第３の実施形態の撮像装置１００の側面外観と、撮影者が撮像装置１００を手で持って撮影している様子を示す。図６（Ａ）は、本実施形態に係る撮像装置１００の背面外観を示す。撮像装置１００は、第１撮像部１０１、第２撮像部１０２と第１撮像部１０１が撮影する画像の表示部７０１を備えている。第２撮像部１０２が撮影する方向２と表示部６０１が表示する方向３を図中に矢印で示す。お互いが同じ方向となるように第２撮影部１０２と表示部７０１が設置されている。
【００２８】
これによって、撮影者が撮像装置１００を手で持って表示部７０１を見ながら、第１撮像部１０１によって撮影する場合には、第２撮像部１０２は撮影者自身を撮影することができる。撮影者を撮影することによる自己位置姿勢推定の効果は第２実施形態に記載の通りである。図６（Ｂ）は、本実施形態に係る撮像装置１００の構成例を表すブロック図である。第１撮像部１０１で出力する画像データを表示部７０１で表示する。本実施形態においても、その他の構成や処理については第１実施形態と同様である。
【００２９】
（第４実施形態）
図７（Ａ）は、第４の実施形態の撮像装置１００の背面外観を示す。撮像装置１００は、第１撮像部１０１、第２撮像部１０２と通知部９０１を備えている。通知部９０１は、第２撮像部１０２に撮影者が写っていないときに警告９０２を撮影者に知らせる。通知部９０１は、第３実施形態の表示部７０１であって、警告９０２を第１撮像部１０１が撮影する画像に重畳表示してもよい。また、通知部９０１は、音源であって警告音を出すことにより警告９０２を撮影者に知らせてもよく、通知部９０１は、光源であって点灯または点滅することにより警告９０２を撮影者に知らせてもよい。図７（Ｂ）は、本実施形態に係る撮像装置１００の構成例を表すブロック図を示す。演算装置９０３は、第２撮像部１０２が撮影した画像から顔認識を行い、顔が検出されたか否かを通知部９０１に出力する。この顔認識処理の結果、顔認識ができない撮影者が写っていないことを撮影者に伝える。これによって、撮影者は、本撮像装置１００の位置や向きを変えるなどの手段によって撮影者自身を撮影することができる。本実施形態においても、その他の構成や処理については第１実施形態と同様である。
【００３０】
（第５実施形態）
図８（Ａ）は、第５実施形態の撮像装置１００の背面を示す。撮像装置１００は、撮影倍率操作部１１０１を備える。そして、撮影者がこの撮影倍率操作部１１０１を操作することで第１撮像部１０１のレンズを光学ズームさせたり、もしくは記憶装置１０３に記憶されている第１撮像部１０１が撮影した画像に対してデジタルズームさせる等の処理をすることができる。図８（Ｂ）は、本実施形態に係る撮像装置１００の構成例を表すブロック図を示す。撮影者が撮影倍率操作部１１０１を操作して、光学ズームまたはデジタルズームさせて倍率変化させた第１撮像部１０１の撮影倍率は、記憶装置１２０１に記憶される。
【００３１】
記憶装置１２０１は、事前に決定してある第１撮像部１０１の撮影倍率の閾値も記憶している。撮影倍率の閾値は、例えば、第１撮像部１０１のシャッタースピードから手ぶれによるボケを無視できる撮影倍率とする等の方法で決定しておく。また、記憶装置１２０１は、第１撮像部１０１の撮影倍率と撮影倍率の閾値を第３自己位置姿勢推定演算部１０７に出力する。第３自己位置姿勢推定演算部１０７は、第１撮像部１０１の撮影倍率が撮影倍率の閾値よりも大きいならば、第２自己位置姿勢推定演算部１０６で算出した自己位置姿勢推定値（Ｒ２，ｔ２）を本撮像装置の自己位置姿勢推定値（Ｒ，ｔ）に決定する。逆に、第１撮像部１０１の撮影倍率が撮影倍率の閾値よりも大きくない場合、第１実施形態に示した通りの方法で自己位置姿勢推定値（Ｒ，ｔ）を決定する。
【００３２】
第１撮像部１０１を光学ズームまたはデジタルズームによって高倍率で撮影する場合には、本撮像装置１００のわずかな動きでも自己位置姿勢推定演算に用いていた被写体がフレームアウトしてしまう。このときに、第２撮像部１０２を用いて自己位置姿勢推定を行うように切り替えることで安定して自己位置姿勢推定を行うことができる。本実施形態においても、その他の構成や処理については第１実施形態と同様である。
【００３３】
（第６実施形態）
図９は、第６実施形態の撮像装置１００の側面外観を示す。撮像装置１００は、第１撮像部１０１、第２撮像部１０２を備えている。第１撮像部１０１が撮影する方向１と第２撮像部１０２が撮影する方向２と地面の方向０を図中に矢印で示す。方向１から方向０へ向かう方向に角度を定義したときに、方向１と方向２のなす角θを１３５°以上にする。また、方向１と方向２が張る面内での第２撮像部１０２の視野角Ｗｂを９０°以上にする。
【００３４】
図１０（Ａ）は、方向１と方向０が垂直になるように本実施形態の撮像装置の姿勢を設定して撮影した場合を示し、図１０（Ｂ）は、方向１と方向０が正反対になるように本実施形態の撮像装置の姿勢を設定して撮影した場合を示す。どちらの場合にも第２撮像装置１０２は、視野の範囲に地面が含まれるため、方向１が方向０に対して９０°以上の向きに対して第２撮像装置１０２はいつも地面を撮影することができる。
【００３５】
第１撮像部で撮影する被写体が遠方にあったり、霧などの環境下では特徴点を精度よく抽出できなかったり、特徴点の対応点の検出を精度よくできなかったりしてしまう。また、被写体に動的被写体を含んでいるときには被写体が動いているのか、撮像装置１００が動いているのかの判別ができず、自己位置姿勢推定が正しく行えない。しかし、このような状況下でも、本撮像装置１００を撮影者が手で持って撮影し、第２撮像部１０２が地面を撮影していれば自己位置姿勢推定を精度よく行える。
【００３６】
例えば、第１撮像部１０１が遠方を撮影していて霧などの環境化で解像力の高い画像が得られない場合でも、第２撮像部１０２と撮影者の距離は５０ｃｍ〜１６０ｃｍ程度であるため霧の影響が少なく解像力の高い画像が得られる。本実施例ではθが１３５°以上、Ｗｂを９０°以上である例を挙げたが、第１撮像部１０１を用いて撮影中に、第２撮像部１０２が地面を撮影できる位置、撮影方向、視野角であれば第２撮像部１０２はどのように設定されていても構わない。本実施形態においても、その他の構成や処理については第１実施形態と同様である。
【００３７】
（第７実施形態）
図１１は、第７実施形態の撮像装置１００の側面外観を示す。撮像装置１００は、第１撮像部１０１、第２撮像部１０２を備えている。方向１、方向２、方向３やθ、Ｗｂの定義は第６実施形態と同じであるため、その説明を省略する。なお、θを９０°以上、Ｗｂを１８０°以上にする。図１２（Ａ）は、方向１と方向０が同じ向きになるように本実施形態の撮像装置の姿勢を設定して撮影した場合を示し、図１２（Ｂ）は、方向１と方向０が正反対になるように本実施形態の撮像装置の姿勢を設定して撮影した場合を示す。どちらの場合にも第２撮像装置１０２は、視野の範囲に地面が含まれるため、方向１がどの方向を向いていても第２撮像装置１０２は、いつも地面を撮影することができる。第２撮像部１０２が地面を撮影することによる効果は第６実施形態と同じである。本実施形態においても、その他の構成や処理については第１実施形態と同様である。
【００３８】
（第８実施形態）
図１３は、第８実施形態の撮像装置１００の側面外観を示す。撮像装置１００は、第１撮像部１０１、第２撮像部１０２を備えている。第２撮像部１０２には可動部１７０１が備えてあり、第１撮像部１０１の撮影方向に対する第２撮像部１０２の撮影方向を変化させることができる。変化させる方法は、撮影者が可動部１７０１を手動で動かしてもよく、撮像装置１００に可動部１７０１を動かすための駆動が備えてあって、撮影者がそれを操作してもよい。以上の方法によって、撮影者が第２撮像部１０２で十分なテクスチャを持つ静的被写体を撮影するように第２撮像部１０２の撮影方向を設定することができる。これによって、精度の高い自己位置姿勢推定を行うことができる。さらに、第２撮像部１０２の撮影方向を設定後に、第２撮像部１０２が撮影する被写体の中で自己位置姿勢推定に用いる被写体を撮影者が選択できる構成にすることも可能である。
【００３９】
図１４（Ａ）は、第８実施形態の撮像装置１００の背面外観を示し、図１４（Ｂ）は第２撮像部１０２の撮影方向を変化させた後に、自己位置姿勢推定に用いる被写体を撮影者が選択できる構成例を示す。撮像装置１００には、第２自己位置姿勢推定演算部１０６が自己位置姿勢推定を行う際に使用する被写体を撮影者が選択する入力部１９０１が備えてある。例えば、表示部８０１が設けてあり、表示部８０１中に第２撮像部が撮影する画像を表示する表示領域１８０２を持っており、撮影者が表示領域１８０２内の被写体を選択することができる。選択の方法としては、表示部８０１がタッチパネルになっていて、撮影者が被写体をタッチしてもよく、表示領域１８０２内にカーソルが表示されていて、撮影者がボタン操作などによってカーソルで被写体を選択してもよい。
【００４０】
表示領域１８０２として第１撮像で撮影する画像を表示する表示部８０１を利用する場合には、表示部８０１内が表示領域１８０２と第１撮像部で撮影する画像を表示する表示領域１８０１に分割されていてもよい。また、第２自己位置姿勢推定演算部１０６が抽出した画像内の特徴点の指示１８０３を表示領域１８０２に重畳表示させることで撮影者が被写体を選択しやすくすることができる。例えば、対応付けのしやすさによって指示１８０３の色や大きさを変えるとさらに選択しやすくなる。第２撮像部が撮影する被写体が撮影者である場合、自己位置姿勢推定を行うためには撮影者は静止していなければならない。被写体が地面である場合には、特徴点を抽出しにくいこともある。十分なテクスチャを持っている静的被写体を撮影者が選択して自己位置姿勢推定を行うことで、精度の高い自己位置姿勢推定を行うことができる。本実施形態においても、その他の構成や処理については第１実施形態と同様である。
【００４１】
（第９実施形態）
図１５（Ａ）は、第９実施形態の撮像装置１００の背面外観を示す。撮像装置１００は、第１撮像部１０１、第２撮像部１０２を備えている。また、第１撮像部１０１で撮影した画像を表示する表示部９０１を備えており、その画像にオブジェクト２００１を合成して表示することができる。図１５（Ｂ）は、第９実施形態による撮像装置１００の構成例を表すブロック図を示す。記憶装置２１０１は、合成させるオブジェクト２００１の３次元データを記憶している。演算装置２１０２は、第３自己位置推定演算部１０７が決定した自己位置姿勢推定値によって、オブジェクト２００１をその位置姿勢から見た状態に変化（変形）させて、表示部８０１に第１撮像部が撮影している画像にリアルタイムで重ねて表示させる。
【００４２】
例えば、第１撮像部１０１によって標準マクロ撮影を行う場合には、ピントが合いにくく、解像力の高い画像が得られないことがある。しかし、第２撮像部１０２が撮影者、地面、静的被写体などを撮影していれば、解像力の高い画像が得られ、高精度で安定した自己位置姿勢推定が行える。これによって、リアリティのあるオブジェクトの合成が可能になる。本実施形態においても、その他の構成や処理については第１実施形態と同様である。
【００４３】
（第１０実施形態）
図１６は、第１０実施形態による撮像装置１００の構成例を表すブロック図を示す。記憶装置１０３は、第１撮像部１０１が撮影した画像とそれを撮影した時刻を記憶している。記憶装置２１０３は、第３自己位置推定演算部１０７が決定した自己位置姿勢推定値とそのときの時刻を記憶している。ここで、時刻とは、画像と自己位置姿勢推定値の対応付けができる情報であるなら、絶対的な時刻でもあってもよく、撮影開始時からの撮影枚数でもよい。
【００４４】
また、撮像装置１００には、第１撮像部１０１が撮影した複数の画像の中から、オブジェクトを重ねて表示したい画像を選択する入力部２１０４を備える。選択の方法としては表示部８０１がタッチパネルになっていて、サムネイル表示された複数の画像の中から操作者が選択したい画像をタッチしてもよく、表示部８０１内にカーソルが表示されていて、操作者がボタン操作などによってカーソルで画像を選択してもよい。演算装置２１０２は、操作者によって選出された画像が撮影された時刻における、第３自己位置推定演算部１０７が決定した自己位置姿勢推定値によって、オブジェクト２００１をその位置姿勢から見た状態に変化させる。そして、それを操作者によって選出された画像に重ねて表示部８０１に表示させる。本実施形態においても、その他の構成や処理については第１実施形態と同様である。
【００４５】
（第１１実施形態）
図１７（Ａ）は、第１１実施形態の撮像装置１００の背面外観を示す。撮像装置１００は、第１撮像部１０１、第２撮像部１０２を備えている。また、第１撮像部１０１で撮影した画像を表示する表示部９０１を備えており、第１撮像部が過去に撮影した被写体をユーザーが指定した方向から見た像２２０１に変換して表示することができる。ユーザーが方向を指定する方法は、例えば、見たい位置と方向に本撮像装置を移動または傾けるなどがある。このときにも、移動量や傾き角の算出にも本発明の自己位置姿勢推定を用いることができる。また、表示部９０１がタッチパネルになっている場合には、それを指でなぞるなどして見たい方向に変えることもできる。
【００４６】
図１７（Ｂ）は、第１１実施形態による撮像装置１００の構成例を表すブロック図を示す。記憶装置２３０１は、第３自己位置姿勢推定演算部１０７で使用した被写体の画素位置
【数３１】
2019041223.tif 000033
と被写体の３次元データである世界座標（３次元座標）
【数３２】
2019041223.tif 000034
のうち少なくとも一方を記憶している。
【００４７】
演算装置２３０２では、ユーザーが指定した位置ｔと姿勢Ｒに本発明の撮像装置があるとした場合に、被写体の世界座標
【数３３】
2019041223.tif 000035
の画像上の画素位置
【数３４】
2019041223.tif 000036
を次式により算出する。
【数３５】
2019041223.tif 000037
ここで、記憶装置２３０１に世界座標
【数３６】
2019041223.tif 000038
が記憶されていない場合には、第３自己位置推定演算部１０７が決定した自己位置姿勢推定値（Ｒ，ｔ）と画素位置
【数３７】
2019041223.tif 000039
から、逆投影によって世界座標
【数３８】
2019041223.tif 000040
を（式１０）の計算をする前に算出する。
【００４８】
以上の処理によって（式１０）で計算された画素位置
【数３９】
2019041223.tif 000041
の画素値を
【数４０】
2019041223.tif 000042
として画像を生成して記憶装置２３０３に記憶する。ここで、画素値
【数４１】
2019041223.tif 000043
の３成分はそれぞれ画素位置
【数４２】
2019041223.tif 000044
における赤、緑、青成分である。生成した画像は、本実施形態に係る撮像装置１００に設置された表示部に即座に表示してもよく、後に外部の表示装置に表示させてもよい。例えば、第１撮像部１０１によって標準マクロ撮影を行う場合には、ピントが合いにくいことがあり、解像力の高い画像が得られない場合がある。しかし、第２撮像部１０２が撮影者、地面、静的被写体などを撮影していれば、解像力の高い画像が得られ、高精度で安定した自己位置姿勢推定が行える。これによって、リアリティのある自由視点画像の表示が可能になる。本実施形態においても、その他の構成や処理については第１実施形態と同様である。
【００４９】
また、本発明の好ましい実施形態について説明したが、本発明は、これらの実施形態に限定されず、その要旨の範囲内で種々の変形および変更が可能である。
【符号の説明】
【００５０】
１００  撮像装置
１０１  第１撮像部
１０２  第２撮像部
１０５  第１自己位置姿勢推定演算部
１０６  第２自己位置姿勢推定演算部
１０７  第３自己位置姿勢推定演算部
【図１】
2019041223.tif 000045
【図２】
2019041223.tif 000046
【図３】
2019041223.tif 000047
【図４】
2019041223.tif 000048
【図５】
2019041223.tif 000049
【図６】
2019041223.tif 000050
【図７】
2019041223.tif 000051
【図８】
2019041223.tif 000052
【図９】
2019041223.tif 000053
【図１０】
2019041223.tif 000054
【図１１】
2019041223.tif 000055
【図１２】
2019041223.tif 000056
【図１３】
2019041223.tif 000057
【図１４】
2019041223.tif 000058
【図１５】
2019041223.tif 000059
【図１６】
2019041223.tif 000060
【図１７】
2019041223.tif 000061