(57)【特許請求の範囲】
【請求項１】
カメラの位置姿勢の推定に成功した時点でのカメラの撮影画像を示すキーフレームに含まれる複数の第１特徴点と、前記カメラによって撮影された撮影画像に含まれる複数の第２特徴点とを対応付ける対応付け部と、
前記対応付け部によって対応付けられた第１特徴点と第２特徴点との組それぞれについて、前記第１特徴点の特徴量および前記第２特徴点の特徴量に基づく画像対応ベクトルを算出する画像対応ベクトル算出部と、
前記画像対応ベクトル算出部によって算出された複数の画像対応ベクトルの分布に基づいて、前記第１特徴点と前記第２特徴点との対応付けが適切に行われているか否かを判定する判定部と、
前記判定部によって、前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定された場合に、前記カメラの位置姿勢を推定する処理を行わないように制御する制御部と
を有することを特徴とするカメラ位置姿勢推定装置。
【請求項２】
前記判定部は、各画像対応ベクトルの長さの平均が基準平均以上で、かつ、各画像対応ベクトルの長さの分散が基準分散以上である場合に、前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定することを特徴とする請求項１に記載のカメラ位置姿勢推定装置。
【請求項３】
前記判定部は、画像対応ベクトル毎に、各画像対応ベクトルの中央値と画像対応ベクトルとの距離を算出し、距離が閾値以上となる画像対応ベクトルの数が、画像対応ベクトル全体のうち、所定の割合以上である場合に、前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定することを特徴とする請求項１に記載のカメラ位置姿勢推定装置。
【請求項４】
前記制御部は、前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定した場合に、カメラの位置姿勢の推定を行わない旨の情報を表示装置に表示させることを特徴とする請求項１、２または３に記載のカメラ位置姿勢推定装置。
【請求項５】
前記制御部は、対応付けられた第１特徴点と第２特徴点との組のうち、前記中央値と画像対応ベクトルとの距離が閾値未満となる第１特徴点と第２特徴点との組を用いて、前記カメラの位置姿勢を推定することを特徴とする請求項３に記載のカメラ位置姿勢推定装置。
【請求項６】
コンピュータが実行するカメラ位置姿勢推定方法であって、
カメラの位置姿勢の推定に成功した時点でのカメラの撮影画像を示すキーフレームに含まれる複数の第１特徴点と、前記カメラによって撮影された撮影画像に含まれる複数の第２特徴点とを対応付け、
対応付けた第１特徴点と第２特徴点との組それぞれについて、前記第１特徴点の特徴量および前記第２特徴点の特徴量に基づく画像対応ベクトルを算出し、
複数の画像対応ベクトルの分布に基づいて、前記第１特徴点と前記第２特徴点との対応付けが適切に行われているか否かを判定し、
前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定した場合に、前記カメラの位置姿勢を推定する処理を行わないように制御する
処理を実行することを特徴とするカメラ位置姿勢推定方法。
【請求項７】
コンピュータに、
カメラの位置姿勢の推定に成功した時点でのカメラの撮影画像を示すキーフレームに含まれる複数の第１特徴点と、前記カメラによって撮影された撮影画像に含まれる複数の第２特徴点とを対応付け、
対応付けた第１特徴点と第２特徴点との組それぞれについて、前記第１特徴点の特徴量および前記第２特徴点の特徴量に基づく画像対応ベクトルを算出し、
複数の画像対応ベクトルの分布に基づいて、前記第１特徴点と前記第２特徴点との対応付けが適切に行われているか否かを判定し、
前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定した場合に、前記カメラの位置姿勢を推定する処理を行わないように制御する
処理を実行させることを特徴とするカメラ位置姿勢推定プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、カメラ位置姿勢推定装置等に関する。
【背景技術】
【０００２】
近年、ＰＣ（Personal  Computer）、携帯端末、ウェアラブル端末等に装着されるカメラの位置姿勢を推定し、推定結果を用いて、画面に表示されたカメラ画像に付加情報を重畳表示させ、ユーザの作業を支援する技術がある。
【０００３】
ユーザが作業中の場合には、カメラの位置姿勢が頻繁に変化するため、カメラの位置姿勢の推定が一時的に失敗する状態になりやすい。このため、推定が失敗した状態から再びカメラの位置姿勢を推定する処理を再開することが求められている。以下の説明では、適宜、推定失敗により再びカメラの位置姿勢を推定する処理を復帰処理と表記する。
【０００４】
復帰処理を行う従来技術の一例について説明する。従来技術では、カメラの位置姿勢の推定に成功したタイミングで断続的に、成功した時点の画像とカメラの位置姿勢とを対応付けたキーフレームを生成する。そして、従来技術では、復帰処理を行う場合に、現在のカメラの画像に近いキーフレームを検出し、検出したキーフレームの特徴点と、現在のカメラの画像の特徴点とを対応付けることで、現在のカメラの位置姿勢を推定する。
【０００５】
また、キーフレームと現在の画像との間で対応付けた複数の特徴点の組のうち、誤差の少ない特徴点の組を選び出し、選び出した特徴点の組を用いて、復帰処理を実行する従来技術もある。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】米国特許第８８３６７９９Ｂ２号明細書
【特許文献２】特開２０１２−２２１０４２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、上述した従来技術では、キーフレームを用いた特徴点の対応付けが誤った状態で復帰処理を実行してしまうという問題がある。
【０００８】
例えば、特徴点の対応付けが誤っていると、カメラの位置姿勢の推定精度が悪化するため、カメラの画像に付加情報を重畳する位置が適切な位置からずれてしまう場合がある。また、複数の特徴点の組のうち、誤差の少ない特徴点の組を選び出す従来技術では、特徴点の対応付けに誤りがあると、誤差の少ない特徴点の組を選び出す処理の試行回数が増加し、処理量が増加するという問題も発生する。
【０００９】
１つの側面では、本発明は、適切な特徴点の対応関係に基づいた復帰処理を実行することができるカメラ位置姿勢推定装置、カメラ位置姿勢推定方法およびカメラ位置姿勢推定プログラムを提供することを目的とする。
【課題を解決するための手段】
【００１０】
第１の案では、カメラ位置姿勢推定装置は、対応付け部と、画像対応ベクトル算出部と、判定部と、制御部とを有する。対応付け部は、カメラの位置姿勢の推定に成功した時点でのカメラの撮影画像を示すキーフレームに含まれる複数の第１特徴点と、カメラによって撮影された撮影画像に含まれる複数の第２特徴点とを対応付ける。画像対応ベクトル算出部は、対応付け部によって対応付けられた第１特徴点と第２特徴点との組それぞれについて、第１特徴点の特徴量および第２特徴点の特徴量に基づく画像対応ベクトルを算出する。判定部は、画像対応ベクトル算出部によって算出された複数の画像対応ベクトルの分布に基づいて、第１特徴点と第２特徴点との対応付けが適切に行われているか否かを判定する。制御部は、判定部によって、第１特徴点と第２特徴点との対応付けが適切に行われていないと判定された場合に、カメラの位置姿勢を推定する処理を行わないように制御する。
【発明の効果】
【００１１】
適切な特徴点の対応関係に基づいた復帰処理を実行することができる。
【図面の簡単な説明】
【００１２】
【図１】図１は、本実施例に係るカメラ位置姿勢推定装置の構成を示す機能ブロック図である。
【図２】図２は、３次元マップのデータ構造の一例を示す図である。
【図３】図３は、カメラ位置姿勢推定部の処理を説明するための図である。
【図４】図４は、キーフレームテーブルのデータ構造の一例を示す図である。
【図５】図５は、ベクトルの分布の一例を示す図である。
【図６】図６は、撮影画像上の特徴点と対応付けた３次元マップのデータ構造の一例を示す図である。
【図７】図７は、本実施例に係るカメラ位置姿勢推定装置の処理手順を示すフローチャートである。
【図８】図８は、判定部のその他の処理を説明するための図（１）である。
【図９】図９は、判定部のその他の処理を説明するための図（２）である。
【図１０】図１０は、復帰処理部のその他の処理を説明するための図である。
【図１１】図１１は、カメラ位置姿勢推定プログラムを実行するコンピュータの一例を示す図である。
【発明を実施するための形態】
【００１３】
以下に、本願の開示するカメラ位置姿勢推定装置、カメラ位置姿勢推定方法およびカメラ位置姿勢推定プログラムの実施例を図面に基づいて詳細に説明する。なお、この実施例によりこの発明が限定されるものではない。
【実施例１】
【００１４】
図１は、本実施例に係るカメラ位置姿勢推定装置の構成を示す機能ブロック図である。図１に示すように、カメラ位置姿勢推定装置１００は、カメラ５０に接続される。カメラ位置姿勢推定装置１００は、画像取得部１１０と、特徴点抽出部１２０と、カメラ位置姿勢推定部１３０とを有する。カメラ位置姿勢推定装置１００は、品質判定部１４０と、キーフレーム検出部１５０と、対応付け部１６０と、ベクトル算出部１７０と、判定部１８０と、制御部１９０と、復帰処理部２００とを有する。ベクトル算出部１７０は、画像対応ベクトル算出部の一例である。
【００１５】
カメラ５０は、ＰＣ、携帯端末、ウェアラブル端末等に装着された単眼ＲＧＢ（Red  Green  Blue）カメラである。カメラ５０は、任意の視点の画像を撮影し、撮影画像を画像取得部１１０に出力する。
【００１６】
画像取得部１１０は、カメラ５０に接続され、カメラ５０から撮影画像を取得する処理部である。画像取得部１１０は、撮影画像を特徴点抽出部１２０に出力する。
【００１７】
特徴点抽出部１２０は、撮影画像から特徴点を抽出する処理部である。例えば、特徴点抽出部１２０は、SIFT、SURF、ORB等の処理を実行することで、特徴点を抽出する。例えば、特徴点は、撮影画像のエッジ等に対応する点となる。特徴点抽出部１２０は、特徴点の情報を、カメラ位置姿勢推定部１３０に出力する。特徴点の情報は、例えば、撮影画像から抽出した各特徴点の２次元座標、特徴量が含まれる。特徴量は、特徴点周辺の画素値やエッジの傾き等の情報である。
【００１８】
カメラ位置姿勢推定部１３０は、特徴点抽出部１２０から取得した特徴点の情報と、３次元マップとを基にして、特徴点とマップ点とのマッチングを行い、マッチング結果を基にして、カメラ５０の位置姿勢を推定する処理部である。ここで、３次元マップは、対象物に関して予め定義された３次元座標のマップ点を示すものである。
【００１９】
図２は、３次元マップのデータ構造の一例を示す図である。図２に示すように、３次元マップは、マップ点ＩＤと、Ｘ、Ｙ、Ｚと、特徴量とを対応付ける。図２において、マップ点ＩＤは、マップ点を一意に特定する番号である。図２に示す例では、３次元マップに６８個のマップ点が含まれる。Ｘ、Ｙ、Ｚは、マップ点の３次元座標を示す。特徴量は、マップ点をカメラ５０の画像上に投影した場合における、画像上の投影点の特徴量である。例えば、特徴量は、投影点周辺の画素値やエッジの傾き等の情報である。
【００２０】
カメラ位置姿勢推定部１３０が、マップ点と特徴点とをマッチングする処理の一例について説明する。カメラ位置姿勢推定部１３０は、前回推定したカメラ５０の位置姿勢を用いて、特徴点が、３次元マップのどのマップ点に対応するのかを判定する。カメラ位置姿勢推定部１３０は、直前の時刻において、マップ点が撮影画像上に投影される座標を、式（１）により求める。以下の説明において、撮影画像上に投影されたマップ点を適宜、投影点と表記する。
【００２１】
【数１】
0006575325.tif 000002
【００２２】
式（１）において、Ａは、３行３列の行列であり、カメラ５０の内部パラメータに対応する。利用者は、非特許文献「Z.Zhang,  A  flexible  new  technique  for  camera  calibration,  IEEE  Transactions  on  Pattern  Analysis  and  Machine  Intelligence,  Vol.22,  No.11,  pp.1330-1334  (2000)」に基づいて、事前にカメラ５０のキャリブレーションを行っておく。
【００２３】
式（１）において、（Ｒ｜ｔ）は、前回推定したカメラ５０の位置姿勢を、式（２）に示すロドリゲス変換の公式を用いて３行３列の回転行列Ｒと並進ベクトルｔに変換した値を融合した３行４列の行列である。（ｕ，ｖ）は、マップ点を撮影画像上に投影した場合の投影点の２次元座標である。（Ｘ，Ｙ，Ｚ）は、各マップ点の３次元座標である。
【００２４】
【数２】
0006575325.tif 000003
【００２５】
式（２）において、Ｉは単位行列を示す。θは、式（３）によって定義される。ｒは、式（４）によって定義される。
【００２６】
【数３】
0006575325.tif 000004
【数４】
0006575325.tif 000005
【００２７】
図３は、カメラ位置姿勢推定部の処理を説明するための図である。図３において、マップ点Ｓ１〜Ｓ６が存在するものとする。また、撮影画像２０には、特徴点ｘ１〜ｘ６が存在するものとする。カメラ位置姿勢推定部１３０は、式（１）に基づき、マップ点Ｓ１〜Ｓ６を撮影画像２０上に投影すると、投影点ｘ１’〜ｘ６’が求められる。
【００２８】
カメラ位置姿勢推定部１３０は、各投影点に対してある閾値の範囲内に存在する現時刻の撮影画像の特徴点との距離をそれぞれ計算する。カメラ位置姿勢推定部１３０は、距離が最小となる投影点と特徴点とを特定し、特定した投影点に対応するマップ点と、特徴点とをペアとする。カメラ位置姿勢推定部１３０は、全ての投影点について上記処理を繰り返し実行し、投影点に対応するマップ点と、現時刻の撮影画像の特徴点とのマッチングを行う。閾値を例えば、２０pixelとする。
【００２９】
例えば、図３に示す例では、特徴点ｘ１と、投影点ｘ１’との距離が最小となるため、特徴点ｘ１と、投影点ｘ１’に対応するマップ点Ｓ１とがペアとなる。同様にして、特徴点ｘ２〜ｘ６は、マップ点Ｓ２〜Ｓ６とそれぞれペアとなる。
【００３０】
カメラ位置姿勢推定部１３０は、マッチングを行った後に、カメラ５０の位置姿勢を推定する処理を実行する。カメラ位置姿勢推定部１３０は、３点以上の特徴点とマップ点とのペアが存在する場合、PnPアルゴリズムを用いてカメラの位置姿勢を推定する。例えば、カメラ位置姿勢推定部１３０は、非特許文献「V.Lepetit  et  al.,  EPnP:  An  Accurate  O(n)  Solution  to  the  PnP  Problem,  International  Journal  of  Computer  Vision,  Vol.81,  Issue  2,  pp.155-166  (2008)」に記載されたPnPアルゴリムを用いて、位置姿勢を推定しても良い。
【００３１】
カメラ位置姿勢推定部１３０は、推定したカメラの位置姿勢の情報を品質判定部１４０に出力する。また、カメラ位置姿勢推定部１３０は、３次元マップに含まれるマップ点の総数と、マッチングしたペアの数とを、品質判定部１４０に出力する。
【００３２】
品質判定部１４０は、カメラ位置姿勢推定部１３０によって推定されたカメラ５０の位置姿勢の品質を判定する処理部である。例えば、品質判定部１４０は、３次元マップに含まれるマップ点の総数に対するマッチングしたペアの数の割合を算出する。品質判定部１４０は、算出した割合が所定の割合以上である場合には、カメラ５０の位置姿勢の推定値が高品質であると判定する。一方、品質判定部１４０は、算出した割合が所定の割合未満である場合には、カメラ５０の位置姿勢の推定値が低品質であると判定する。例えば、所定の割合を３割とする。
【００３３】
品質判定部１４０は、カメラ５０の位置姿勢の推定値が高品質であると判定した場合には、現時刻のカメラ５０の位置姿勢を、カメラ位置姿勢推定部１３０によって推定された位置姿勢であると確定し、所定の記憶部に記憶する。図示しないＨＭＤの表示制御部が、記憶部に記憶されたカメラの位置姿勢を参照し、位置姿勢を利用して、カメラの撮影画像に付加情報を重畳させた画像を、ＨＭＤのディスプレイに表示させる。
【００３４】
品質判定部１４０が、カメラ５０の位置姿勢の推定値が低品質であると判定した場合には、復帰処理要求を、キーフレーム検出部１５０に出力する。
【００３５】
キーフレーム検出部１５０は、品質判定部１４０から復帰処理要求を取得した場合に、現時刻の撮影画像と、キーフレームテーブルとを比較して、基準キーフレームを検出する処理部である。キーフレーム検出部１５０は、基準キーフレームの情報を、対応付け部１６０に出力する。
【００３６】
図４は、キーフレームテーブルのデータ構造の一例を示す図である。図４に示すように、このキーフレームテーブルは、番号と、位置姿勢と、撮影画像と、特徴点群と、マップ点ＩＤ群とを対応付ける。図４において、番号は、キーフレームを一意に識別する番号である。位置姿勢は、カメラの位置姿勢を示すものである。撮影画像は、カメラの位置姿勢の推定に成功した時点において、カメラによって撮影された画像データである。特徴点群は、キーフレームに含まれる各特徴点の２次元座標を示すものである。マップ点ＩＤ群は、各特徴点に対応付けられた各マップ点を一意に識別する情報である。
【００３７】
例えば、特徴点とマップ点との対応関係は、図４の特徴点群の各座標の並び順と、マップ点ＩＤ群の並び順によって示される。例えば、マップ点ＩＤ「３」に対応する特徴点の座標は「１１，４２」となる。図４に示す例では、キーフレームテーブルに２５個のキーフレームが含まれる。
【００３８】
位置姿勢は、６次元（ｒ１，ｒ２，ｒ３，ｔ１，ｔ２，ｔ３）で表される。このうち、（ｒ１，ｒ２，ｒ３）は世界座標におけるカメラの姿勢を示す。（ｔ１，ｔ２，ｔ３）は、世界座標におけるカメラの位置を示す。
【００３９】
ここで、キーフレーム検出部１５０が、現時刻の撮影画像と最も類似した撮影画像を有するキーフレームを検出する処理の一例について説明する。キーフレーム検出部１５０は、現時刻の撮影画像と各キーフレームの撮影画像を所定のサイズに縮小し、ガウシアンフィルタによりぼかした状態で、各画素についてＳＳＤ（Sum  of  Squared  Distance）を計算し、最もＳＳＤの値が小さいキーフレームを基準キーフレームとして特定する。キーフレーム検出部１５０は、現時刻の撮影画像の情報と、基準キーフレームの情報とを、対応付け部１６０に出力する。
【００４０】
対応付け部１６０は、基準キーフレームに含まれる複数の特徴点と、現時刻の撮影画像に含まれる複数の特徴点とを対応付ける処理部である。以下の説明では適宜、基準キーフレームに含まれる特徴点を第１特徴点と表記し、現時刻の撮影画像の特徴点を第２特徴点と表記する。
【００４１】
対応付け部１６０は、第１特徴点の特徴量と、第２特徴点の特徴量とを比較し、特徴量の類似度が最も高くなる第１特徴点と第２特徴点とのペアを対応付ける。例えば、第１特徴点の特徴量をｆｂとし、第２特徴点の特徴量をｆｃとすると、対応付け部１６０は、式（５）に基づいて比較値を算出し、比較値が小さいほど、類似度が大きいと判定する。
【００４２】
比較値＝｜ｆｂ-ｆｃ｜・・・（５）
【００４３】
対応付け部１６０は、未対応の第１特徴点と類似度が最も高くなる未対応の第２特徴点を特定してペアにする処理を、繰り返し実行することで、第１特徴点と第２特徴点との複数の組を生成する。対応付け部１６０は、対応付けた第１特徴点および第２特徴点の組の情報を、ベクトル算出部１７０に出力する。
【００４４】
ベクトル算出部１７０は、対応付け部１６０によって対応付けられた第１特徴点と第２特徴点との組それぞれについて、第１特徴点の特徴量および第２特徴点の特徴量に基づく画像対応ベクトルを算出する処理部である。ベクトル算出部１７０は、算出した画像対応ベクトルの情報を、判定部１８０に出力する。以下の説明において、画像対応ベクトルを単に、ベクトルと表記する。
【００４５】
例えば、第１特徴点の画像中の座標を（ｕｂ，ｖｂ）とし、第２特徴点の画像中の座標を（ｕｃ，ｖｃ）とすると、ベクトル算出部１７０は、式（６）に基づき、対応付けられた第１特徴点と第２特徴点とのベクトルｖを算出する。ベクトル算出部１７０は、対応付けられた第１特徴点と第２特徴点との組毎に、ベクトルｖを算出する。
【００４６】
ベクトルｖ＝（ｕｃ-ｕｂ，ｖｃ-ｖｂ）・・・（６）
【００４７】
判定部１８０は、ベクトル算出部１７０によって算出された複数のベクトルの分布に基づいて、第１特徴点と第２特徴点との対応付けが適切に行われているか否かを判定する処理部である。判定部１８０は、判定結果を制御部１９０に出力する。
【００４８】
判定部１８０は、各ベクトルの長さの平均値ａと、各ベクトルの長さの標準偏差σの値を算出し、下記の条件１、条件２を満たすか否かを判定する。基準値Ａおよび基準値Σの値は、利用者が予め設定しておくものとする。
【００４９】
条件１：平均値ａ＜基準値Ａ
条件２：標準偏差σ＜基準値Σ
【００５０】
例えば、判定部１８０は、条件１を満たし、かつ、条件２を満たす場合に、第１特徴点と第２特徴点との対応付けが適切に行われていると判定する。一方、判定部１８０は、条件１を満たさない、または、条件２を満たさない場合には、第１特徴点と第２特徴点との対応付けが適切に行われていないと判定する。
【００５１】
図５は、ベクトルの分布の一例を示す図である。図５において、横軸は、ベクトルのｘ軸方向の成分（１番目の成分ｕｃ-ｕｂ）に対応する軸であり、縦軸は、ベクトルのｙ軸方向の成分（２番目の成分ｖｃ-ｖｂ）に対応する軸である。図５において、丸印は、第１特徴点と第２特徴点との対応付けが適切に行われていると判定される、各ベクトルに対応する位置を示す。ダイヤ印は、第１特徴点と第２特徴点との対応付けが適切に行われていないと判定される各ベクトルに対応する位置を示す。
【００５２】
図５に示すように、第１特徴点と第２特徴点との対応付けが適切に行われている場合には、各ベクトルは、原点（０，０）付近に分布する。これに対して、第１特徴点と第２特徴点との対応付けが適切に行われていない場合には、各ベクトルは、原点（０，０）付近に分布せず、全体的に広がる。
【００５３】
図１の説明に戻る。制御部１９０は、判定部１８０によって、第１特徴点と第２特徴点との対応付けが適切に行われていないと判定された場合に、カメラの位置姿勢を推定する処理を行わないように制御する。一方、制御部１９０は、判定部１８０によって、第１特徴点と第２特徴点との対応付けが適切に行われていると判定された場合に、復帰処理部２００に、復帰処理要求を行う。
【００５４】
また、制御部１９０は、判定部１８０によって、第１特徴点と第２特徴点との対応付けが適切に行われていないと判定された場合に、カメラの位置姿勢の推定を行うことができない旨の情報を図示しない表示装置に表示させてもよい。
【００５５】
復帰処理部２００は、判定部１８０から復帰処理要求を受け付けた場合に、復帰処理を実行する処理部である。例えば、復帰処理部２００は、image-to-map手法に基づき、カメラ５０の位置姿勢を推定する。image-to-map手法は、上述したカメラ位置姿勢推定部１３０と同様にして、カメラの位置姿勢を推定するものである。復帰処理部２００は、カメラの位置姿勢の推定結果を、所定の記憶部に格納する。
【００５６】
復帰処理部２００は、対応付け部１６０によって対応付けられた基準キーフレームの特徴点と、現時刻の撮影画像の特徴点との対応関係を利用しても良い。基準キーフレームの特徴点は、図４で説明したように、マップ点ＩＤと対応付けられている。このため、基準キーフレームの特徴点と、現時刻の撮影画像の特徴点との対応関係により、現時刻の撮影画像の特徴点と、マップ点ＩＤとの対応関係がわかる。
【００５７】
図６は、撮影画像上の特徴点と対応付けた３次元マップのデータ構造の一例を示す図である。図６において、マップ点ＩＤ、Ｘ、Ｙ、Ｚ、特徴量に関する説明は、図２で説明したものと同様である。ｘ、ｙは、マップ点に対応付けられた撮影画像上の特徴点のｘ座標、ｙ座標を示す。
【００５８】
復帰処理部２００は、３点以上の特徴点とマップ点とのペアが存在する場合、PnPアルゴリズムを用いてカメラの位置姿勢を推定する。
【００５９】
次に、本実施例に係るカメラ位置姿勢推定装置１００の処理手順について説明する。図７は、本実施例に係るカメラ位置姿勢推定装置の処理手順を示すフローチャートである。図７に示すように、カメラ位置姿勢推定装置１００の画像取得部１１０は、撮影画像を取得する（ステップＳ１０１）。カメラ位置姿勢推定装置１００の特徴点抽出部１２０は、撮影画像から特徴点を抽出する（ステップＳ１０２）。
【００６０】
カメラ位置姿勢推定装置１００のカメラ位置姿勢推定部１３０は、マップ点と特徴点とをマッチングし（ステップＳ１０３）、カメラの位置姿勢を推定する（ステップＳ１０４）。カメラ位置姿勢推定装置１００の品質判定部１４０は、位置姿勢の推定値の品質を判定する（ステップＳ１０５）。
【００６１】
品質判定部１４０は、推定値が高品質であるか否かを判定する（ステップＳ１０６）。品質判定部１４０は、推定値が高品質である場合には（ステップＳ１０６，Ｙｅｓ）、処理を終了する。
【００６２】
一方、品質判定部１４０により、推定値が高品質でないと判定された場合には（ステップＳ１０６，Ｎｏ）、カメラ位置姿勢推定装置１００のキーフレーム検出部１５０は、基準キーフレームを検出する（ステップＳ１０７）。
【００６３】
カメラ位置姿勢推定装置１００の対応付け部１６０は、基準キーフレームの第１特徴点と、現時刻の撮影画像の第２特徴点とを対応付ける（ステップＳ１０８）。カメラ位置姿勢推定装置１００のベクトル算出部１７０は、各対応点間のベクトルを計算する（ステップＳ１０９）。
【００６４】
カメラ位置姿勢推定装置１００の判定部１８０は、複数のベクトルの分布に基づいて、第１特徴点と第２特徴点との対応付けが適切に行われているか否かを判定する（ステップＳ１１０）。
【００６５】
判定部１８０は、第１特徴点と第２特徴点との対応付けが適切に行われていない場合に、復帰処理が実行可能でないと判定し（ステップＳ１１１，Ｎｏ）、復帰処理を行うことなく、処理を終了する。すなわち、カメラ位置姿勢推定装置１００の制御部１９０は、第１特徴点と第２特徴点との対応付けが適切に行われていないと判定された場合に、復帰処理部２００によって、復帰処理が行われないように制御する。
【００６６】
一方、判定部１８０は、第１特徴点と第２特徴点との対応付けが適切に行われている場合に、復帰処理が実行可能であると判定する（ステップＳ１１１，Ｙｅｓ）。カメラ位置姿勢推定装置１００の復帰処理部２００は、復帰処理を実行する（ステップＳ１１２）。
【００６７】
次に、本実施例に係るカメラ位置姿勢推定装置１００の効果について説明する。カメラ位置姿勢推定装置１００は、カメラの位置姿勢の品質が低品質となり、復帰処理を行う場合に、基準キーフレームの第１特徴点と、撮影画像の第２特徴点とを対応付け、各特徴点の分布に基づき、対応付けが適切であるか否かを判定する。カメラ位置姿勢推定装置１００は、対応付けた適切でないと判定した場合には、復帰処理が行われないように制御する。このため、誤った特徴点の対応付けのまま、復帰処理が実行されることを抑止でき、適切な特徴点の対応関係に基づいた復帰処理を実行することができる。
【００６８】
また、適切な特徴点の対応関係に基づいた復帰処理を実行することができるので、復帰処理によるカメラの位置姿勢の推定精度を向上させることができる。また、適切な特徴点の対応関係に基づいた復帰処理を実行することができるので、誤差の少ない特徴点の組を選び出す処理の試行回数が抑えられ、処理量を軽減させることもできる。
【００６９】
カメラ位置姿勢推定装置１００は、各ベクトルの長さの平均が基準平均以上で、かつ、各ベクトルの長さの分散が基準分散以上である場合に、第１特徴点と第２特徴点との対応付けが適切に行われていないと判定する。図５で説明したように、第１特徴点と第２特徴点との対応付けが適切に行われている場合には、各ベクトルは、原点（０，０）付近に分布する。これに対して、第１特徴点と第２特徴点との対応付けが適切に行われていない場合には、各ベクトルは、原点（０，０）付近に分布せず、全体的に広がるという特徴がある。このため、上記のような判定をすることで、第１特徴点と第２特徴点との対応付けが適切に行われているか否かを精度良く判定できる。
【００７０】
ところで、上述したカメラ位置姿勢推定装置１００の処理は一例であり、カメラ位置姿勢推定装置１００は、その他の処理を実行しても良い。以下では、カメラ位置姿勢推定装置１００のその他の処理（１）、（２）について説明する。
【００７１】
カメラ位置姿勢推定装置１００のその他の処理（１）について説明する。カメラ位置姿勢推定装置１００の判定部１８０は、ベクトル算出部１７０によって算出された複数のベクトルｖの中央値ｖｍを算出する。判定部１８０は、ベクトルｖ毎に、ベクトルｖと中央値ｖｍとの距離を式（７）に基づいて算出する。
【００７２】
距離＝｜ｖ-ｖｍ｜・・・（７）
【００７３】
判定部１８０は、全体のベクトルｖのうち、距離が閾値Ｄｔ未満となるベクトルの割合が、ｒ％以上である場合に、第１特徴点と第２特徴点との対応付けが適切に行われていると判定する。一方、判定部１８０は、全体のベクトルｖのうち、距離が閾値Ｄｔ未満となるベクトルの割合が、ｒ％未満である場合に、第１特徴点と第２特徴点との対応付けが適切に行われていないと判定する。
【００７４】
図８および図９は、判定部のその他の処理を説明するための図である。図８および図９に示すヒストグラムの横軸は距離を示し、縦軸は第１特徴点と第２特徴点とのペア数を示す。ペア数は、ベクトルｖの数と同じである。ここでは一例として、閾値Ｄｔを４０とする。
【００７５】
図８は、全体のペア数のうち、距離が閾値Ｄｔ未満となるペア数の割合が、ｒ％以上である場合のヒストグラムである。このようなヒストグラムとなれば、第１特徴点と第２特徴点との対応付けが適切に行われているといえる。距離が閾値Ｄｔ未満となるペア数の割合が、ｒ％以上である場合には、図５の丸印で説明したように、各ベクトルは、原点（０，０）付近に分布する。
【００７６】
図９は、全体のペア数のうち、距離が閾値Ｄｔ未満となるペア数の割合が、ｒ％未満である場合のヒストグラムである。このようなヒストグラムとなれば、第１特徴点と第２特徴点との対応付けが適切に行われていないといえる。距離が閾値Ｄｔ未満となるペア数の割合が、ｒ％未満である場合には、図５のダイヤ印で説明したように、各ベクトルは、原点（０，０）付近に分布せず、全体的に広がる。
【００７７】
上記のように、判定部１８０が、ベクトルｖ毎にベクトルｖと中央値ｖｍとの距離を算出し、算出した距離が閾値以上となるベクトルの数が、ベクトル全体のうち、所定の割合以上の場合に、対応付けが適切に行われていないと判定する。これにより、誤った対応付けのまま復帰処理を実行することを抑止することができる。
【００７８】
カメラ位置姿勢推定装置１００のその他の処理（２）について説明する。カメラ位置姿勢推定装置１００の復帰処理部２００は、制御部１９０から復帰処理要求を受け付けた場合に、image-to-map手法に基づき、カメラ５０の位置姿勢を推定する。
【００７９】
ここで、復帰処理部２００は、image-to-map手法に基づき、カメラ５０の位置姿勢を推定する場合に、対応付け部１６０に対応付けられた第１特徴点と第２特徴点との組のうち、距離が閾値未満となる組を用いて、復帰処理を実行する。距離は、上述した式（７）により算出される値である。
【００８０】
図１０は、復帰処理部のその他の処理を説明するための図である。図１０に示すヒストグラムの横軸は距離を示し、縦軸は第１特徴点と第２特徴点とのペア数を示す。ペア数は、ベクトルｖの数と同じである。復帰処理部２００は、距離が閾値Ｄｔ未満となるペアを利用して、image-to-map手法に基づき、カメラ５０の位置姿勢を推定する。すなわち、復帰処理部２００は、ベクトルと中央値との距離の関係が、領域４０に含まれるペアを用いないで、復帰処理を実行する。このため、誤った対応付けのまま復帰処理を実行することを抑止することができる。
【００８１】
なお、本実施例に係るカメラ位置姿勢推定部１３０、復帰処理部２００は、一例として、image-to-map手法に基づき、カメラ５０の位置姿勢を推定する場合について説明したがこれに限定されるものではない。例えば、カメラ位置姿勢推定部１３０、復帰処理部２００は、image-to-image手法等その他の周知技術を用いて、カメラの位置姿勢の推定、復帰処理を実行しても良い。
【００８２】
次に、上記実施例に示したカメラ位置姿勢推定装置１００と同様の機能を実現するカメラ位置姿勢推定プログラムを実行するコンピュータの一例について説明する。図１１は、カメラ位置姿勢推定プログラムを実行するコンピュータの一例を示す図である。
【００８３】
図１１に示すように、コンピュータ３００は、各種演算処理を実行するＣＰＵ３０１と、ユーザからのデータの入力を受け付ける入力装置３０２と、ディスプレイ３０３とを有する。また、コンピュータ３００は、記憶媒体からプログラム等を読み取る読み取り装置３０４と、ネットワークを介して他のコンピュータとの間でデータの授受を行うインターフェース装置３０５と、カメラ３０６とを有する。また、コンピュータ３００は、各種情報を一時記憶するＲＡＭ３０７と、ハードディスク装置３０８とを有する。そして、各装置３０１〜３０８は、バス３０９に接続される。
【００８４】
ハードディスク装置３０８は、対応付けプログラム３０８ａ、ベクトル算出プログラム３０８ｂ、判定プログラム３０８ｃ、制御プログラム３０８ｄを有する。ＣＰＵ３０１は、対応付けプログラム３０８ａ、ベクトル算出プログラム３０８ｂ、判定プログラム３０８ｃ、制御プログラム３０８ｄを読み出してＲＡＭ３０７に展開する。
【００８５】
対応付けプログラム３０８ａは、対応付けプロセス３０７ａとして機能する。ベクトル算出プログラム３０８ｂは、ベクトル算出プロセス３０７ｂとして機能する。判定プログラム３０８ｃは、判定プロセス３０７ｃとして機能する。制御プログラム３０８ｄは、制御プロセス３０７ｄとして機能する。
【００８６】
対応付けプロセス３０７ａの処理は、対応付け部１６０の処理に対応する。ベクトル算出プロセス３０７ｂの処理は、ベクトル算出部１７０の処理に対応する。判定プロセス３０７ｃの処理は、判定部１８０の処理に対応する。制御プロセス３０７ｄの処理は、制御部１９０の処理に対応する。
【００８７】
なお、対応付けプログラム３０８ａ、ベクトル算出プログラム３０８ｂ、判定プログラム３０８ｃ、制御プログラム３０８ｄについては、必ずしも最初からハードディスク装置３０８に記憶させておかなくても良い。例えば、コンピュータ３００に挿入されるフレキシブルディスク（ＦＤ）、ＣＤ−ＲＯＭ、ＤＶＤディスク、光磁気ディスク、ＩＣカードなどの「可搬用の物理媒体」に各プログラムを記憶させておく。そして、コンピュータ３００が各プログラム３０８ａ〜３０８ｄを読み出して実行するようにしてもよい。
【００８８】
以上の各実施例を含む実施形態に関し、さらに以下の付記を開示する。
【００８９】
（付記１）カメラの位置姿勢の推定に成功した時点でのカメラの撮影画像を示すキーフレームに含まれる複数の第１特徴点と、前記カメラによって撮影された撮影画像に含まれる複数の第２特徴点とを対応付ける対応付け部と、
前記対応付け部によって対応付けられた第１特徴点と第２特徴点との組それぞれについて、前記第１特徴点の特徴量および前記第２特徴点の特徴量に基づく画像対応ベクトルを算出する画像対応ベクトル算出部と、
前記画像対応ベクトル算出部によって算出された複数の画像対応ベクトルの分布に基づいて、前記第１特徴点と前記第２特徴点との対応付けが適切に行われているか否かを判定する判定部と、
前記判定部によって、前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定された場合に、前記カメラの位置姿勢を推定する処理を行わないように制御する制御部と
を有することを特徴とするカメラ位置姿勢推定装置。
【００９０】
（付記２）前記判定部は、各画像対応ベクトルの長さの平均が基準平均以上で、かつ、各画像対応ベクトルの長さの分散が基準分散以上である場合に、前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定することを特徴とする付記１に記載のカメラ位置姿勢推定装置。
【００９１】
（付記３）前記判定部は、画像対応ベクトル毎に、各画像対応ベクトルの中央値と画像対応ベクトルとの距離を算出し、距離が閾値以上となる画像対応ベクトルの数が、画像対応ベクトル全体のうち、所定の割合以上である場合に、前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定することを特徴とする付記１に記載のカメラ位置姿勢推定装置。
【００９２】
（付記４）前記制御部は、前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定した場合に、カメラの位置姿勢の推定を行わない旨の情報を表示装置に表示させることを特徴とする付記１、２または３に記載のカメラ位置姿勢推定装置。
【００９３】
（付記５）前記制御部は、対応付けられた第１特徴点と第２特徴点との組のうち、前記中央値と画像対応ベクトルとの距離が閾値未満となる第１特徴点と第２特徴点との組を用いて、前記カメラの位置姿勢を推定することを特徴とする付記３または４に記載のカメラ位置姿勢推定装置。
【００９４】
（付記６）コンピュータが実行するカメラ位置姿勢推定方法であって、
カメラの位置姿勢の推定に成功した時点でのカメラの撮影画像を示すキーフレームに含まれる複数の第１特徴点と、前記カメラによって撮影された撮影画像に含まれる複数の第２特徴点とを対応付け、
対応付けた第１特徴点と第２特徴点との組それぞれについて、前記第１特徴点の特徴量および前記第２特徴点の特徴量に基づく画像対応ベクトルを算出し、
複数の画像対応ベクトルの分布に基づいて、前記第１特徴点と前記第２特徴点との対応付けが適切に行われているか否かを判定し、
前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定した場合に、前記カメラの位置姿勢を推定する処理を行わないように制御する
処理を実行することを特徴とするカメラ位置姿勢推定方法。
【００９５】
（付記７）前記判定する処理は、各画像対応ベクトルの長さの平均が基準平均以上で、かつ、各画像対応ベクトルの長さの分散が基準分散以上である場合に、前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定することを特徴とする付記６に記載のカメラ位置姿勢推定方法。
【００９６】
（付記８）前記判定する処理は、画像対応ベクトル毎に、各画像対応ベクトルの中央値と画像対応ベクトルとの距離を算出し、距離が閾値以上となる画像対応ベクトルの数が、画像対応ベクトル全体のうち、所定の割合以上である場合に、前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定することを特徴とする付記６に記載のカメラ位置姿勢推定方法。
【００９７】
（付記９）前記コンピュータは、前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定した場合に、カメラの位置姿勢の推定を行わない旨の情報を表示装置に表示させる処理を更に実行することを特徴とする付記６、７または８に記載のカメラ位置姿勢推定方法。
【００９８】
（付記１０）前記コンピュータは、対応付けられた第１特徴点と第２特徴点との組のうち、前記中央値と画像対応ベクトルとの距離が閾値未満となる第１特徴点と第２特徴点との組を用いて、前記カメラの位置姿勢を推定する処理を更に実行することを特徴とする付記８または９に記載のカメラ位置姿勢推定方法。
【００９９】
（付記１１）コンピュータに、
カメラの位置姿勢の推定に成功した時点でのカメラの撮影画像を示すキーフレームに含まれる複数の第１特徴点と、前記カメラによって撮影された撮影画像に含まれる複数の第２特徴点とを対応付け、
対応付けた第１特徴点と第２特徴点との組それぞれについて、前記第１特徴点の特徴量および前記第２特徴点の特徴量に基づく画像対応ベクトルを算出し、
複数の画像対応ベクトルの分布に基づいて、前記第１特徴点と前記第２特徴点との対応付けが適切に行われているか否かを判定し、
前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定した場合に、前記カメラの位置姿勢を推定する処理を行わないように制御する
処理を実行させることを特徴とするカメラ位置姿勢推定プログラム。
【０１００】
（付記１２）前記判定する処理は、各画像対応ベクトルの長さの平均が基準平均以上で、かつ、各画像対応ベクトルの長さの分散が基準分散以上である場合に、前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定することを特徴とする付記１１に記載のカメラ位置姿勢推定プログラム。
【０１０１】
（付記１３）前記判定する処理は、画像対応ベクトル毎に、各画像対応ベクトルの中央値と画像対応ベクトルとの距離を算出し、距離が閾値以上となる画像対応ベクトルの数が、画像対応ベクトル全体のうち、所定の割合以上である場合に、前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定することを特徴とする付記１１に記載のカメラ位置姿勢推定プログラム。
【０１０２】
（付記１４）前記コンピュータに、前記第１特徴点と前記第２特徴点との対応付けが適切に行われていないと判定した場合に、カメラの位置姿勢の推定を行わない旨の情報を表示装置に表示させる処理を更に実行させることを特徴とする付記１１、１２または１３に記載のカメラ位置姿勢推定プログラム。
【０１０３】
（付記１５）前記コンピュータに、対応付けられた第１特徴点と第２特徴点との組のうち、前記中央値と画像対応ベクトルとの距離が閾値未満となる第１特徴点と第２特徴点との組を用いて、前記カメラの位置姿勢を推定する処理を更に実行させることを特徴とする付記１３または１４に記載のカメラ位置姿勢推定プログラム。
【符号の説明】
【０１０４】
５０  カメラ
１００  カメラ位置姿勢推定装置
１１０  画像取得部
１２０  特徴点抽出部
１３０  カメラ位置姿勢推定部
１４０  品質判定部
１５０  キーフレーム検出部
１６０  対応付け部
１７０  ベクトル算出部
１８０  判定部
１９０  制御部
２００  復帰処理部
【図１】
0006575325.tif 000006
【図２】
0006575325.tif 000007
【図３】
0006575325.tif 000008
【図４】
0006575325.tif 000009
【図５】
0006575325.tif 000010
【図６】
0006575325.tif 000011
【図７】
0006575325.tif 000012
【図８】
0006575325.tif 000013
【図９】
0006575325.tif 000014
【図１０】
0006575325.tif 000015
【図１１】
0006575325.tif 000016