JP5439343B2 - ３次元情報取得装置および３次元情報取得プログラム - Google Patents

Description

本発明は、カメラによって入力された少なくとも１枚以上のカメラ画像列から、カメラの位置及び姿勢を示すカメラポーズ情報と、撮影された３次元実空間の３次元情報を計測する３次元情報取得装置および３次元情報取得プログラムに関する。

カメラによって入力された少なくとも１枚以上のカメラ画像列から、カメラの位置及び姿勢を示すカメラポーズ情報と、撮影された３次元実空間における３次元座標を示す３次元情報を計測する従来の３次元情報取得装置では、計測したカメラの位置及び姿勢を用いて、カメラ画像中にコンピュータグラフィクス等を合成することや、計測した３次元座標情報を用いて３次元実空間の状況を理解することなどを可能としている。
従来の３次元情報取得装置では、カメラから入力される初期時刻におけるカメラポーズ情報と、対象の３次元座標情報が与えられていることを前提に、（１）カメラ画像中での特徴点の追跡、（２）カメラポーズ情報の推定、（３）特徴点の３次元座標の推定・更新を繰り返すことによって、時々刻々と変化するカメラポーズ情報、及び撮影された３次元シーンの３次元座標情報の計測を実現している。

初期時刻におけるカメラポーズ情報と、対象の３次元座標情報は、例えば下記非特許文献１に記載の方法では、ユーザが本やワインボトル等の平面に近い形状である撮影対象に対しおおむね正対してカメラを持つと仮定する。この初期状態のカメラポーズ情報に含まれるカメラの位置は、カメラ画像内における２次元の座標と平行移動成分を０ベクトルを用いて表わすことができる。また、カメラポーズ情報に含まれるカメラの姿勢は、カメラの方位角と仰角で表わすことができるが、この方位角と仰角の回転成分を単位行列として表わすことができる。
またカメラ画像中での特徴点pの３次元座標の初期状態を(Ｘｐ，Ｙｐ，Ｚｐ)＝(ｘｐ， ｙｐ，１)としている。

「動画像からの三次元復元による湾曲した紙面のビデオモザイキング」（佐藤智和，池谷彰彦，池田聖，神原誠之，中島昇，横矢直和：電子情報通信学会論文誌D，巻:Vol.J90-D，No.8，pp.1900-1911，2007）

しかしながら、非特許文献１では、対象が非平面と大きく異なる３次元座標を有する場合がある。初期値が真値と大きく異なる場合、つまり、対象と初期状態が大きく異なる場合には、最適化処理時に局所解に陥り正しい解が得られないことがあるといった問題があった。
本発明は、上述したような従来技術が有する問題点に鑑みてなされたものであり、３次元情報取得装置において、対象と初期状態が異なる場合、この対象と初期状態との違いに基づく最適化処理の解の誤差を軽減することができる３次元情報取得装置および３次元情報取得プログラムを提供することを目的とする。

上述の課題を鑑み、本発明に係る３次元情報取得装置は、所定の被写体を撮影し連続する複数のフレーム毎のカメラ画像データをカメラから入力する入力部と、前記カメラ画像データに基づきカメラ画像内における特徴点を抽出し、前記カメラ画像内における前記特徴点の位置を示す特徴点座標と、前記被写体に対する前記カメラの相対的な位置および姿勢を示すカメラポーズ情報とを、実空間中における観測点とこれに対応するカメラ画像内における投影点の誤差を最小化することにより、複数フレームにわたって求める位置姿勢逐次計測手段と、仮想３次元空間における前記特徴点の位置を示す３次元座標、前記特徴点座標、および前記カメラポーズ情報に対して、前記カメラポーズ情報と前記３次元座標の信頼度に応じて予め決められているフレーム単位の加重値に基づく補正をフレーム単位で行い、前記カメラポーズ情報と前記３次元座標の最適化を行う最適化処理手段と、を備える。

上述の３次元情報取得装置において、前記最適化処理手段は、前記３次元座標と前記カメラポーズ情報の信頼度が低いとして予め決められているフレームに対応して与えられる前記加重値を決定するフレーム単位加重値決定手段と、前記カメラポーズ情報と前記３次元座標の最適化を行う際、複数のフレームに対応する前記カメラポーズ情報と前記３次元座標と前記特徴点座標とに基づき表わされる投影誤差に、前記フレーム単位加重値決定手段が決定した前記加重値を乗算して、フレーム単位の誤差を算出し、非線形最小二乗により前記フレーム単位の誤差が最小化となるように前記カメラポーズ情報と前記３次元座標の最適化を行う非線形最小二乗手段とを含むことを特徴とする。

上述の３次元情報取得装置において、前記加重値は、初期フレームに対して１．０よりも大きい第１の加重値、前記初期フレーム以降のフレームに対して１．０の加重値を、それぞれ用いることが予め決められていることを特徴とする。

上述の３次元情報取得装置において、前記加重値は、初期フレームに対して１．０よりも大きい第１の加重値、前記初期フレーム以降の予め決められた数の中期フレームに対して前記第１の加重値から１．０まで線形に減少する第２の加重値、前記中期フレーム以降のフレームに対して１．０の第３の加重値を、それぞれ用いることが予め決められていることを特徴とする。

上述の３次元情報取得装置において、前記加重値は、初期フレームに対して１．０よりも大きい第１の加重値、前記初期フレーム以降の予め決められた数の中期フレームに対して前記第１の加重値から１．０までがガウス関数に基づき減少する第２の加重値、前記中期フレーム以降のフレームに対して１．０の第３の加重値を、それぞれ用いることが予め決められていることを特徴とする。

上述の３次元情報取得装置において、前記位置姿勢逐次計測手段は、初期フレームに対応する前記カメラ画像データを前記入力部から入力した場合、当該初期フレームに対応するカメラ画像データに基づきカメラ画像内における特徴点を抽出し、当該カメラ画像内における前記特徴点の位置を示す特徴点座標を求めて、前記被写体に対する前記カメラの相対的な位置および姿勢を示すカメラポーズ情報の初期値として予め決められている初期カメラポーズ情報と、仮想３次元空間における前記特徴点の位置を示す３次元座標として予め決められている初期３次元座標とを対応付けて記憶部に書き込み、前記初期フレームの以降に撮影された非初期フレームに対応するカメラ画像データを前記入力部から入力した場合、当該入力したカメラ画像データに基づきカメラ画像内における前記特徴点の前記特徴点座標を求めるとともに、当該カメラ画像データもより前の複数のフレームに対応するカメラ画像データから求めた前記特徴点座標および前記３次元座標に基づき、カメラ画像内における観測点と投影点の誤差を最小化することにより、前記非初期フレームに対応する前記カメラポーズ情報を求め、前記特徴点座標と前記カメラポーズ情報とを各フレームに対応付けて前記記憶部に書き込む。

上述の課題を鑑み、本発明に係る３次元情報取得プログラムは、コンピュータを、所定の被写体を撮影し連続する複数のフレーム毎のカメラ画像データをカメラから入力する入力手段、前記カメラ画像データに基づきカメラ画像内における特徴点を抽出し、前記カメラ画像内における前記特徴点の位置を示す特徴点座標と、前記被写体に対する前記カメラの相対的な位置および姿勢を示すカメラポーズ情報とを、実空間中における観測点とこれに対応するカメラ画像内における投影点の誤差を最小化することにより、複数フレームにわたって求める位置姿勢逐次計測手段、仮想３次元空間における前記特徴点の位置を示す３次元座標と前記特徴点座標と前記カメラポーズ情報とに対して、前記カメラポーズ情報と前記３次元座標の信頼度に応じて予め決められているフレーム単位の加重値に基づく補正をフレーム単位で行い、前記カメラポーズ情報と前記３次元座標の最適化を行う最適化処理手段、として機能させるための３次元情報取得プログラムである。

本発明によれば、対象と初期状態が異なる可能性が高い初期値及び当該初期値により逐次算出された初期フレーム以降のカメラポーズ情報及び３次元座標に対して、重み付けをもって最適化処理を行うため、カメラポーズ情報及び対象の３次元座標を局所解に陥ることを回避し、対象と初期状態との違いに基づく最適化処理の解の誤差を軽減することができる。

本発明の実施形態に係る３次元情報取得装置の構成の一例を示すブロック図である。 本発明の実施形態に係る３次元情報取得装置による３次元情報所得処理フローの一例を示すフローチャートを示す図である 本発明の実施形態に係るフレーム対応テーブルの一例を示す図である。 本発明の実施形態に係る特徴点対応テーブルの一例を示す図である。 本発明の実施形態に係る３次元情報取得装置による最適化処理フローの一例を示すフローチャートを示す図である。 本発明の実施形態に係る重み関数の一例を示す図である。 本発明の実施形態に係る重み関数の他の例を示す図である。 本発明の実施形態に係る重み関数の他の例を示す図である。

以下、本発明の一実施形態による３次元情報取得装置１００について図１を参照して説明する。
図１は、本実施形態による３次元情報取得装置１００の構成を示す概略ブロック図である。
３次元情報取得装置１００は、カメラ２００と、モニタ装置３００と、ＨＤＤ４０１と、メモリ４０２と、有線あるいは無線、若しくはネットワークを介して接続されている。

カメラ２００は、対象となる３次元の被写体を撮影する装置で、例えば、ＣＣＤ（Ｃｈａｒｇｅ−Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ）カメラ、ＣＭＯＳカメラ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｍｅｎｔａｌ−Ｏｘｉｄｅ Ｓｅｍｉｃｏｎｄｕｃｔｏｒ）などを含む装置である。本発明においては当該カメラ１００の焦点距離などを示す内部パラメータは、予め設定されており、メモリ４０２に記憶されているものとする。
モニタ装置３００は、３次元情報取得装置１００から出力される情報に基づき画像を表示する。

メモリ４０２は、３次元情報取得装置１００における様々なデータ、例えば、フレーム毎のカメラポーズ情報、フレーム対応テーブル、特徴点対応テーブル、初期カメラポーズ情報等を格納する。このメモリ４０２は、３次元情報取得装置１００の本体に内蔵されても良いし、図示の通り３次元情報取得装置１００本体の外部に備えられても良い。
この初期カメラポーズ情報と初期３次元座標群は適当な値で良い。例えば、非特許文献１と同様に、初期状態のカメラポーズ情報である位置及び姿勢を、それぞれ、平行移動成分を０ベクトル、回転成分を単位行列と、３次元座標点群のｚ座標を全て１としても良い。

このメモリ４０２に記憶されているカメラの位置および姿勢が示すカメラポーズ情報とは、カメラ２００が撮影する被写体とこのカメラ２００との位置や向きの関係を示す情報であり、例えば、被写体に対するカメラ２００の相対的な位置および姿勢を示す情報である。なお、このカメラポーズ情報は、被写体に対するカメラ２００の位置や姿勢を示す情報であって、例えば、実空間におけるカメラ２００のカメラ位置と、カメラ２００の撮像面（例えば、ＣＣＤの光電変換面）の方位角や仰角で表わされるカメラ姿勢を示す情報であってもよい。

この３次元情報取得装置１００は、位置姿勢逐次計測手段１０１と、最適化処理手段１０２を備える。
最適化処理手段１０２は、パラメータ読出手段１２１と、フレーム単位加重値決定手段１２２と、非線形最小二乗手段１２３とを備える。

位置姿勢逐次計測手段１０１は、現フレームに対応するカメラポーズ情報と特徴点座標列を算出し、その値をメモリ４０２に記録する。初期フレームの場合、位置姿勢逐次計測手段１０１は、予め与えられた初期カメラポーズ情報と初期３次元座標群に基づき、現フレームに対応するカメラポーズ情報と特徴点座標列を算出する。初期フレーム以外の場合、位置姿勢逐次計測手段１０１は、メモリから読出した前フレームの特徴点座標列、カメラポーズ情報、及び最新の３次元座標群に基づき、現フレームに対応するカメラポーズ情報と特徴点座標列を算出し、メモリ４０２に記録する。

最適化処理手段１０２は、３次元座標群と、予め定められた対象フレーム数に亘ったカメラポーズ情報および３次元座標群の最適化処理を実施して各々の値を算出し、既存の値を修正出力する。

以下、最適化処理手段１０２を構成するパラメータ読出手段１２１、フレーム単位加重値決定手段１２２、及び非線形最小二乗手段１２３の概要をそれぞれ説明する。
パラメータ読出手段１２１は、メモリ４０２からフレーム毎のカメラポーズ情報と特徴点座標列を、予め与えられた対象フレーム数分に亘って読み出す。このパラメータ読出手段１２１は、併せて、メモリ４０２から最新の３次元座標群を読み出す。
フレーム単位加重値決定手段１２２は、予め与えられた重み付け関数を用いて、フレーム毎に対応する加重値を前記対象フレーム数分に亘って決定し、非線形最小二乗手段１２３に出力する。
非線形最小二乗手段１２３は、処理対象フレームのカメラポーズ情報と３次元座標群及び前記特徴点座標列を用いて算出した値に、フレーム単位加重値決定手段１２２が算出した加重値を乗算してフレーム単位の誤差を算出する。この非線形最小二乗手段１２３は、フレーム単位の誤差を対象フレーム数分に亘って合算することにより誤差総和値を算出し、当該誤差総和値を用いて非線形最小二乗により３次元座標群と前記対象フレーム数に亘ったカメラポーズ情報の真値（推定値）を算出し、メモリ４０２に新たな情報を書き込むことで情報を更新する。
なお、本実施形態において、真値とは、初期状態におけるカメラの位置および姿勢を示す情報として概ね正確であると推定される推定値である。

本発明の実施形態に係る３次元情報取得装置１００で実行される詳細な処理を以下に説明する。
図２は、３次元情報取得装置１００での全体の処理の流れを示すフローチャートである。
（ステップＳＴ１）
まず、本実施形態における３次元情報取得装置１００は、例えば、ユーザにより終了が指示されて操作部（図示せず）から入力する終了指示信号の受信有無を確認する。
終了指示信号を受信した場合（ステップＳＴ１−ＹＥＳ）、３次元情報取得装置１００は、処理を終える。
一方、終了指示信号を受信した場合（ステップＳＴ１−ＮＯ）、３次元情報取得装置１００は、以下の処理を行なう。

まず、カメラ１００が、撮影を開始すると、撮影を開始したことを示す信号を３次元情報取得装置１００に出力する。そして、カメラ２００は、現時刻（ｔ＝１）において撮影された被写体の画像を示すカメラ画像データＤ１を自身のメモリに保存する。なお、このカメラ画像データＤ１は、第１フレーム目のデータである。
（ステップＳＴ２）
３次元情報取得装置１００は、撮影を開始したことを示す信号をカメラ２００から受信すると、カメラ２００に対して、このカメラ画像データの転送を要求する信号を送信する。
（ステップＳＴ３）
続いて、３次元情報取得装置１００は、位置姿勢逐次計測手段１０１を起動する。そして、カメラ２００は、カメラ画像データの転送を要求する信号を受信すると、３次元情報取得装置１００に対して、カメラ画像データＤ１を送信する。

（ステップＳＴ４）
起動されると、位置姿勢逐次計測手段１０１は、カメラ２００からカメラ画像データＤ１を受信する。そして、位置姿勢逐次計測手段１０１は、この起動が、初回起動であるかどうかを判定する。例えば、３次元情報取得装置１００の電源がＯＮされてから一度も起動されていない状態において、起動した場合、位置姿勢逐次計測手段１０１は、初回起動であると判定する。

（ステップＳＴ５）
初回起動の場合（ステップＳＴ４−ＹＥＳ）、位置姿勢逐次計測手段１０１は、予め与えられた初期カメラポーズ情報ＣＰ_１をメモリ４０２から読み出す。
続いて、位置姿勢逐次計測手段１０１は、カメラ画像データＤ１に基づき、カメラ画像上において角部分や交点部分となる特徴点を少なくとも１つ抽出する。この位置姿勢逐次計測手段１０１は、例えば、カメラ画像データＤ１に対して、矩形図面を検出するパターン検出を行うことで、この矩形の角部分を検出することができる。また、位置姿勢逐次計測手段１０１は、カメラ画像データＤ１に対して画像処理をすることにより、同一色と判定できる領域を同一色領域として検出し、３つ以上の同一色領域が接触する点を交点部分として検出する。本実施形態において、位置姿勢逐次計測手段１０１は、Ｎ個（Ｎ＞１）の特徴点ｐ_１〜ｐ_ｎを検出する。

そして、位置姿勢逐次計測手段１０１は、カメラ画像データＤ１に基づき、抽出した特徴点ｐ_１〜ｐ_ｎ毎に、カメラ画像内における２次元の特徴点座標２ＤＰ_１（ｘｐ_１，ｙｐ_１），・・・，２ＤＰ_ｎ（ｘｐ_ｎ，ｙｐ_ｎ）を求める。この位置姿勢逐次計測手段１０１は、全ての特徴点座標２ＤＰ_１〜２ＤＰ_ｎをまとめた特徴点座標列ＰＧ_１を、第１フレームおよび初期カメラポーズ情報Ｐ_１と対応付けて、メモリ４０２内のフレーム対応テーブルに書き込む。なお、このフレーム対応テーブルは、フレームごとにカメラポーズ情報と特徴点座標列を対応付けるテーブルであって、一例を図３に示す。
また、位置姿勢逐次計測手段１０１は、特徴点座標２ＤＰ_１（ｘｐ_１，ｙｐ_１），・・・，２ＤＰ_ｎ（ｘｐ_ｎ，ｙｐ_ｎ）を、特徴点ｐ_１，・・・，ｐ_ｎと対応付けて、メモリ４０２内の特徴点対応テーブルに書き込む。なお、この特徴点対応テーブルは、２次元のカメラ画像内における特徴点ｐ_１〜ｐ_ｎの位置を示す特徴点座標と、仮想実空間上における特徴点ｐ_１〜ｐ_ｎの位置を示す３次元座標位置を、特徴点ｐ_１〜ｐ_ｎごとに対応付けるテーブルであって、一例を図４に示す。

続いて、位置姿勢逐次計測手段１０１は、抽出した特徴点ｐ_１〜ｐ_ｎ毎に、初期３次元座標群の値を対応付けて、メモリ４０２の特徴点対応テーブルに書き込む。
ここで、２次元のカメラ画像内における特徴点pの特徴点座標は、（ｘｐ，ｙｐ）である。３次元座標点群のｚ座標を全て１とした場合、特徴点pの初期３次元座標３ＤＰ（Ｘｐ，Ｙｐ，Ｚｐ）は、（Ｘｐ，Ｙｐ，Ｚｐ）＝（ｘｐ，ｙｐ，１）とすることによって実現できる。

本実施形態の場合、カメラ画像データＤ１からＮ点の特徴点ｐ_１，・・・，ｐ_ｎが抽出されており、位置姿勢逐次計測手段１０１は、すべての特徴点ｐ_１，・・・，ｐ_ｎに亘って、同じ処理を繰り返す。つまり、位置姿勢逐次計測手段１０１は、全ての特徴点ｐ_１，・・・，ｐ_ｎに対応する初期３次元座標３ＤＰ_１〜３ＤＰ_ｎを求める。なお、この複数の特徴点ｐ_１，・・・，ｐ_ｎに対応する初期３次元座標３ＤＰ_１〜３ＤＰ_ｎの集合を、３次元座標群という。
すなわち、位置姿勢逐次計測手段１０１は、特徴点ｐ_１，・・・，ｐ_ｎの特徴点座標（ｘｐ_１，ｙｐ_１），・・・，（ｘｐ_ｎ，ｙｐ_ｎ）に基づき、各特徴点ｐ_１，・・・，ｐ_ｎに基づく初期３次元座標点群｛３ＤＰ_１（Ｘｐ_１，Ｙｐ_１，Ｚｐ_１），・・・，３ＤＰ_ｎ（Ｘｐ_ｎ，Ｙｐ_ｎ，Ｚｐ_ｎ）｝を求める。位置姿勢逐次計測手段１０１は、各特徴点ｐ_１，・・・，ｐ_ｎに対して、初期３次元座標点群｛３ＤＰ_１（Ｘｐ_１，Ｙｐ_１，Ｚｐ_１），・・・，３ＤＰ_ｎ（Ｘｐ_ｎ，Ｙｐ_ｎ，Ｚｐ_ｎ）｝を対応付けてメモリ４０２の特徴点対応テーブルに保存する。なお、３次元座標点３ＤＰ_１（Ｘｐ_１，Ｙｐ_１，Ｚｐ_１）＝（ｘｐ_１，ｙｐ_１，１）であり、初期３次元座標点３ＤＰ_ｎ（Ｘｐ_ｎ，Ｙｐ_ｎ，Ｚｐ_ｎ）＝（ｘｐ_ｎ，ｙｐ_ｎ，１）である。

そして、位置姿勢逐次計測手段１０１は、求めた初期３次元座標群｛（Ｘｐ_１，Ｙｐ_１，Ｚｐ_１），・・・，（Ｘｐ_ｎ，Ｙｐ_ｎ，Ｚｐ_ｎ）｝を、最新の３次元座標群としてメモリ４０２の特徴点対応テーブルに書き出す。併せて、位置姿勢逐次計測手段１０１は、初期カメラポーズ情報ＣＰ_１と特徴点座標列ＰＧ_１を、現フレームのカメラ画像データＤ１と対応付けて、メモリ４０２のフレーム対応テーブルに書出す。そして、位置姿勢逐次計測手段１０１は、ステップＳＴ１へと戻る。

（ステップＳＴ６）
一方、ステップＳＴ４において、初回起動でない場合、位置姿勢逐次計測手段１０１は、まずメモリ４０２のフレーム対応テーブルから、直前のフレームのカメラ画像データと対応付けられているカメラポーズ情報と特徴点座標列を読み出す。位置姿勢逐次計測手段１０１は、併せて、メモリ４０２の特徴点対応テーブルから、最新の３次元座標群も読み出す。

次いで、カメラ２００から第ｆフレーム目のカメラ画像データＤｆ（ｆ＞１）を入力した場合、位置姿勢逐次計測手段１０１は、第ｆ−１フレーム目のカメラ画像データＤｆ−１と対応付けられているカメラポーズ情報ＣＰ_ｆ−１と特徴点座標列ＰＧ_ｆ−１｛２ＤＰ_１（ｘｐ_１，ｙｐ_１），・・・，２ＤＰ_ｎ（ｘｐ_ｎ，ｙｐ_ｎ）｝を読出す。
例えば、カメラ２００から第２フレーム目のカメラ画像データＤ２を入力した場合、位置姿勢逐次計測手段１０１は、メモリ４０２のフレーム対応テーブルにおいて、第１フレーム目のカメラ画像データＤ１と対応付けられているカメラポーズ情報ＣＰ_１と特徴点座標列ＰＧ_１を読み出すとともに、メモリ４０２の特徴点対応テーブルにおいて、特徴点座標列ＰＧ_１と対応付けられている特徴点座標｛２ＤＰ_１（ｘｐ_１，ｙｐ_１），・・・，２ＤＰ_ｎ（ｘｐ_ｎ，ｙｐ_ｎ）｝を読み出す。

続けて、位置姿勢逐次計測手段１０１は、このメモリ４０２から読み出した前フレームの特徴点座標列｛２ＤＰ_１（ｘｐ_１，ｙｐ_１），・・・，２ＤＰ_ｎ（ｘｐ_ｎ，ｙｐ_ｎ）｝について、現フレーム内における対応点を特徴点追跡により決定し当該特徴点の座標を求める。
この特徴点追跡処理は、特徴点近傍画素値の類似度を評価するテンプレートマッチング等により実現される。この特徴点追跡処理をメモリから読出した前フレームの特徴点に亘って処理を行い、現フレームにおける特徴点座標列を決定する。
例えば、位置姿勢逐次計測手段１０１は、特徴点追跡処理により、第２フレームのカメラ画像内における特徴点ｐ_１，・・・，ｐ_ｎを抽出する。そして、位置姿勢逐次計測手段１０１は、抽出した第２フレームのカメラ画像内における特徴点ｐ_１，・・・，ｐ_ｎの特徴点座標（ｘｐ_１，ｙｐ_１），・・・，（ｘｐ_ｎ，ｙｐ_ｎ）を求める。この特徴点座標（ｘｐ_１，ｙｐ_１），・・・，（ｘｐ_ｎ，ｙｐ_ｎ）の集合が、特徴点座標列ＰＧ_２である。

続いて、位置姿勢逐次計測手段１０１は、決定した現フレームの特徴点座標列と、メモリ４０２から読出した最新の３次元座標群を用いて現フレームのカメラポーズ情報を算出する。
例えば、位置姿勢逐次計測手段１０１は、現フレームの特徴点座標列として、第２フレームの特徴点座標ＰＧ_２｛（ｘｐ_１，ｙｐ_１），・・・，（ｘｐ_ｎ，ｙｐ_ｎ）｝を決定した場合、メモリ４０２の特徴点対応テーブルに書き込まれている最新の３次元座標列として、第１フレームに基づき求められた初期３次元座標列ＰＧ_１｛３ＤＰ_１（Ｘｐ_１，Ｙｐ_１，Ｚｐ_１），・・・，３ＤＰ_ｎ（Ｘｐ_ｎ，Ｙｐ_ｎ，Ｚｐ_ｎ）｝を読み出し、第２フレームに対応するカメラポーズ情報ＣＰ_２を求める。そして、位置姿勢逐次計測手段１０１は、求めたカメラポーズ情報ＣＰ_２を、第２フレームのカメラ画像データＤ２と対応付けて、メモリ４０２のフレーム対応テーブルに書き込む。

この位置姿勢逐次計測手段１０１によるカメラポーズ情報の算出は、従来の技術と特に変わるところが無いので、以下では簡単な説明に留める。
まず、仮想実空間中の３次元空間中の点（Ｘ，Ｙ，Ｚ，１）と、カメラ画像上の投影点（ｕ，ｖ，１）には、式（１）の関係が成り立つ。

式（１）に示すＰは、３×４の行列であり、カメラ２００の外部パラメータであるカメラポーズ情報Ｐを示す。またＴは、転値を表す。
続いて画像上の観測点を、以下に示す。

この観測点と、投影点の誤差を、式（２）で表現する。

現フレームのカメラポーズ情報Ｐは、これをパラメータとして、現フレーム内の特徴点pの座標（ｕ´_ｐ，ｖ´_ｐ）^Ｔと、これに対応する最新の３次元座標値（Ｘｐ，Ｙｐ，Ｚｐ）を用いることで決定される式（２）で定義した誤差関数の和を、以下に示す。

位置姿勢逐次計測手段１０１は、この式（３）に示す誤差関数の和を、非線形最小二乗法によって最小化することにより算出できる。
ここで、ｐは、フレーム内の特徴点に唯一に付与されたＩＤを示す。
非線形最小二乗法の処理は、例えばニュートン法や、マーカート法を用いることで実現可能である。

例えば、位置姿勢逐次計測手段１０１は、式（１）における（Ｘ，Ｙ，Ｚ，１）を、特徴点ｐ_１，・・・，ｐ_ｎに対応する最新の３次元座標点群｛３ＤＰ_１（Ｘｐ_１，Ｙｐ_１，Ｚｐ_１），・・・，３ＤＰ_ｎ（Ｘｐ_ｎ，Ｙｐ_ｎ，Ｚｐ_ｎ）｝として、カメラ画像上の投影点｛（ｕ_１，ｖ_１，１），・・・，（ｕ_ｎ，ｖ_ｎ，１）｝を求める。また、位置姿勢逐次計測手段１０１は、式（２）における（ｕ´，ｖ´）^Ｔを、カメラ画像上の観測点である特徴点座標｛（ｘｐ_１，ｙｐ_１），・・・，（ｘｐ_ｎ，ｙｐ_ｎ）｝として、カメラ画像上の投影点｛（ｕ_１，ｖ_１，１），・・・，（ｕ_ｎ，ｖ_ｎ，１）｝のそれぞれについて、カメラ画像上の観測点である特徴点座標｛（ｘｐ_１，ｙｐ_１），・・・，（ｘｐ_ｎ，ｙｐ_ｎ）｝との誤差Ｅ_１，・・・，Ｅ_ｎを、特徴点ｐ_１，・・・，ｐ_ｎごとに求める。
そして、位置姿勢逐次計測手段１０１は、式（３）に示すこの誤差Ｅ_１，・・・，Ｅ_ｎの総和を、非線形最小二乗法によって最小化することにより、第２フレームのカメラ画像データＤ２に対応するカメラポーズ情報ＣＰ_２を求めることができる。

続いて、位置姿勢逐次計測手段１０１は、算出したカメラポーズ情報と現フレームの特徴点座標列を、現フレームと対応付けてメモリ４０２に書出す。
つまり、位置姿勢逐次計測手段１０１は、算出したカメラポーズ情報ＣＰ_２と現フレームの第２フレームの特徴点座標列ＰＧ_２を、第２フレームのカメラ画像データＤ２と対応付けて、メモリ４０２のフレーム対応テーブルに書き出す。

（ステップＳＴ７）
続いて、位置姿勢逐次計測手段１０１は、予め与えられた閾値により最適化処理要否判定を行なう。
この判定は、例えば、閾値として、最適化処理が可能となる処理フレーム数として決められたフレーム数と、最新の３次元座標群を求めるために処理対象としたフレーム数とを、位置姿勢逐次計測手段１０１が比較することにより行う。位置姿勢逐次計測手段１０１は、処理対象としたフレーム数が、閾値以上であった場合（ステップＳＴ７−要）、最適化処理を行う。一方、処理対象としたフレーム数が閾値未満であった場合（ステップＳＴ７‐不要）、位置姿勢逐次計測手段１０１は、最適化処理が不要と判定し、行わない。なお、この処理対象としたフレーム数とは、最適化処理を行うために必要なフレーム数であって、例えば３フレームである。
勿論、位置姿勢逐次計測手段１０１は、最小化された結果の誤差関数Ｅやその複数フレームでの和等に対する閾値処理によっても実現できる。

上記判定で最適化処理が不要となれば、位置姿勢逐次計測手段１０１は、処理を終えてステップＳＴ１へと戻る。
（ステップＳＴ８）
上記判定で最適化処理が必要となれば、位置姿勢逐次計測手段１０１は、処理を終えて、最適化処理手段１０２を起動する。

次いで、図５を参照して、最適化処理手段１０２による処理フローについて説明する。図５は、最適化処理手段による処理フローの一例を説明するためのフローチャートである。
（ステップＳＴ１１）
最適化処理手段１０２は、起動されると、まずパラメータ読出手段１２１を起動する。
パラメータ読出手段１２１は起動されると、メモリ４０２に記憶されているフレーム毎のカメラポーズ情報と特徴点座標列を、予め与えられた対象フレーム数分に亘って読出す。
また、パラメータ読出手段１２１は、メモリ４０２に記憶されている最新の３次元座標群も読出す。そして、パラメータ読出手段１２１は、読み出した情報を夫々、フレーム単位加重値前決定手段１２２へと転送する。これにより、フレーム単位加重値決定手段１２２は、起動する。

（ステップＳＴ１２）
フレーム単位加重値前決定手段１２１は起動すると、予め与えられた重み付け関数Ｗ（ｆ）を用いて、フレーム毎に対応する加重値Ｗ_ｆを、前記対象フレーム数分に亘って決定する。
ここで重み付け関数Ｗ（ｆ）としては、例えば、以下、式（４）、（５）、（６）に示すような関数のいずれか１つを用いることができる。
式（４）〜（６）において、ｃは自然数で予め与えられた非信頼区間フレーム数を示す。この非信頼区間フレーム数とは、実験的あるいは経験的に、位置姿勢逐次計測手段１０１によって求められた３次元座標とカメラポーズ情報の信頼性が低いと判断されるフレーム数である。このフレーム数は予め決められている。ｂは、１．０より大きな実数で予めユーザにより与えられた値を示す。ｆはフレーム番号を示す。

式（４）、（５）、（６）をグラフ化したものを、それぞれ、図６，７，８に示す。図６，７，８は、それぞれ、横軸にフレーム数を、縦軸に加重値Ｗ_ｆを、それぞれとるグラフであって、式（４）、（５）、（６）に示す重み付け関数Ｗ（ｆ）を表わすものである。

図６に示す通り、式（４）では、初期フレームに対する加重値のみ大きく、次フレーム以降は１．０となる。つまり、真値と異なる初期フレームのみ重み付けを行い、最適化処理に要する時間を短縮するとともに、局所解とならないようにすることができる。

図７に示す通り、式（５）では、初期フレームは加重値が大きく次フレーム以降線形に加重値が減少し、対象フレーム数を超えた時点で１．０となる。これは、真値と異なる初期フレームから徐々にフレーム数が増えるに従って、３次元座標列と真値とのズレを収束することができていると考えられる。よって、初期フレームに対して最も重い重み付けを行い、線形に徐々に重みを減らしていくことにより、最適化処理に要する時間を短縮するとともに、局所解とならないようにすることができる。

図８に示す通り、式（６）では、初期フレーム近辺では加重値が大きく初期フレームから離れるに従ってガウス関数に基づき加重値が減少し、対象フレーム数を超えた時点で約１．０となる。ここでも、初期フレームに対して最も重い重み付けを行い、線形に徐々に重みを減らしていくことにより、最適化処理に要する時間を短縮するとともに、局所解とならないようにすることができる。

例えば、このフレーム単位加重値決定手段１２２による加重値の算出について、例えば式（４）を利用し、非信頼区間フレーム数cが３、対象フレーム数が５であった場合、フレームＩＤであるｆの順で加重値Ｗ_ｆを並べるとＷ_ｆ＝｛ｂ，１，１，１，１，１｝となる。

（ステップＳＴ１３）
続けて、非線形最小二乗手段１２３が起動する。
非線形最小二乗手段１２３は、始めに、フレーム単位の誤差Ｅ´を、処理対象フレームのカメラポーズ情報と３次元座標群及び特徴点座標列を用いて算出した値に加重値Ｗ_ｆを乗算して算出する。

式（２）で定義した誤差関数を用いると、Ｅ´は下式（７）で表される。

続いて、非線形最小二乗手段１２３は、前記フレーム単位の誤差Ｅ´を対象フレーム数分に亘って合算することにより誤差総和値Ｅ´´を算出する。
式（２）で定義した誤差関数を用いると、Ｅ´´は、下式（８）で表される。

続いて、非線形最小二乗手段１２３は、フレーム毎のカメラポーズ情報と最新の３次元座標群をパラメータとして、当該誤差Ｅ´´を非線形最小二乗により最小化することによって、真値と推定されるパラメータ（カメラポーズ情報および３次元座標群）を算出し、更新することによって処理を終了する。つまり、非線形最小二乗手段１２３は、誤差Ｅ´´が非線形最小二乗により最小となるときのカメラポーズ情報および３次元座標群を、真値と推定されるパラメータとして決定する。

ここで、フレーム毎のカメラポーズ情報と最新の３次元座標群をパラメータとした非線形最小二乗は、例えば非特許文献２記載のバンドルアジャスメントの枠組みを用いることによって、非線形最小二乗手段１２３が実現できる。
＜非特許文献２＞「バンドルアジャスメント」(岡谷貴之:情報処理学会研究報告 ２００９年６月 Ｖｏｌ．２００９‐ＣＶＩＭ‐１６７ Ｎｏ．３７， ｐｐ． １‐１６， ２００９)

以上、本発明を実施形態例に基づき具体的に説明したが、上記実施の形態の説明は、本発明を説明するためのものであって、特許請求の範囲に記載の発明を限定し、或は範囲を減縮する様に解すべきではない。
また、本発明の各手段構成は上記実施の形態に限らず、特許請求の範囲に記載の技術的範囲内で種々の変形が可能であることは勿論である。

上述の通り、本発明によれば、対象と初期状態が異なる可能性が高い初期値及び当該初期値により逐次算出された初期フレーム以降のカメラポーズ及び３次元座標に対して、重み付けをもって最適化処理を行うため、カメラポーズ及び対象の３次元座標を局所解に陥ることを回避し、対象と初期状態との違いに基づく最適化処理の解の誤差を軽減することができる。
また、初期値が真値と異なる場合、つまり、対象と初期状態が異なる場合、３次元座標の推定・更新を行う最適化処理に時間を要するといった問題があった。本発明によれば、最適化処理において、対象と初期状態との違いに応じて重み付けを行うことにより、その対象と初期状態の違いに基づく最適化処理に用いられるパラメータのズレを補正することができる。よって、対象と初期状態との違いを補正しないで最適化処理した場合に要する時間に比べて、時間を短縮することができる。

なお、上述の３次元情報取得装置１００は、内部にコンピュータシステムを有している。そして、３次元情報取得装置１００の動作の過程は、プログラムの形式でコンピュータ読み取り可能な記録媒体に記憶されており、このプログラムをコンピュータシステムが読み出して実行することによって、上記処理が行われる。ここでいう「コンピュータシステム」とは、ＣＰＵ及び各種メモリやＯＳ、周辺機器等のハードウェアを含むものである。
また、「コンピュータシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。

また、図２，５に示す各ステップを実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、また、図１に示す３次元情報取得装置１００の機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより、各処理を行ってもよい。
また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、フラッシュメモリ等の書き込み可能な不揮発性メモリ、ＣＤ−ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。

さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバやクライアントとなるコンピュータシステム内部の揮発性メモリ（例えばＤＲＡＭ（Dynamic Random Access Memory））のように、一定時間プログラムを保持しているものも含むものとする。
また、上記プログラムは、このプログラムを記憶装置等に格納したコンピュータシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピュータシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。
また、上記プログラムは、前述した機能の一部を実現するためのものであっても良い。さらに、前述した機能をコンピュータシステムに既に記録されているプログラムとの組合せで実現できるもの、いわゆる差分ファイル（差分プログラム）であっても良い。

１００・・・３次元情報取得装置、１０１・・・位置姿勢逐次計算手段、１０２・・・最適化処理手段、１２１・・・パラメータ読出手段、１２２・・・フレーム単位加重値決定手段、１２３・・・非線形最小二乗手段、２００・・・カメラ、３００・・・モニタ装置、４０１・・・ＨＤＤ、４０２・・・メモリ

Claims (7)

1. 所定の被写体を撮影し連続する複数のフレーム毎のカメラ画像データをカメラから入力する入力部と、
   前記カメラ画像データに基づきカメラ画像内における特徴点を抽出し、前記カメラ画像内における前記特徴点の位置を示す特徴点座標と、前記被写体に対する前記カメラの相対的な位置および姿勢を示すカメラポーズ情報とを、実空間中における観測点とこれに対応するカメラ画像内における投影点の誤差を最小化することにより、複数フレームにわたって求める位置姿勢逐次計測手段と、
   仮想３次元空間における前記特徴点の位置を示す３次元座標、前記特徴点座標、および前記カメラポーズ情報に対して、前記カメラポーズ情報と前記３次元座標の信頼度に応じて予め決められているフレーム単位の加重値に基づく補正をフレーム単位で行い、前記カメラポーズ情報と前記３次元座標の最適化を行う最適化処理手段と、
   を備えることを特徴とする３次元情報取得装置。
2. 前記最適化処理手段は、
   前記３次元座標と前記カメラポーズ情報の信頼度が低いとして予め決められているフレームに対応して与えられる前記加重値を決定するフレーム単位加重値決定手段と、
   前記カメラポーズ情報と前記３次元座標の最適化を行う際、複数のフレームに対応する前記カメラポーズ情報と前記３次元座標と前記特徴点座標とに基づき表わされる投影誤差に、前記フレーム単位加重値決定手段が決定した前記加重値を乗算して、フレーム単位の誤差を算出し、非線形最小二乗により前記フレーム単位の誤差が最小化となるように前記カメラポーズ情報と前記３次元座標の最適化を行う非線形最小二乗手段と
   を含むことを特徴とする請求項１に記載の３次元情報取得装置。
3. 前記加重値は、初期フレームに対して１．０よりも大きい第１の加重値、前記初期フレーム以降のフレームに対して１．０の加重値を、それぞれ用いることが予め決められていることを特徴とする請求項１あるいは２に記載の３次元情報取得装置。
4. 前記加重値は、初期フレームに対して１．０よりも大きい第１の加重値、前記初期フレーム以降の予め決められた数の中期フレームに対して前記第１の加重値から１．０まで線形に減少する第２の加重値、前記中期フレーム以降のフレームに対して１．０の第３の加重値を、それぞれ用いることが予め決められていることを特徴とする請求項１あるいは２に記載の３次元情報取得装置。
5. 前記加重値は、初期フレームに対して１．０よりも大きい第１の加重値、前記初期フレーム以降の予め決められた数の中期フレームに対して前記第１の加重値から１．０までがガウス関数に基づき減少する第２の加重値、前記中期フレーム以降のフレームに対して１．０の第３の加重値を、それぞれ用いることが予め決められていることを特徴とする請求項１あるいは２に記載の３次元情報取得装置。
6. 前記位置姿勢逐次計測手段は、
   初期フレームに対応する前記カメラ画像データを前記入力部から入力した場合、当該初期フレームに対応するカメラ画像データに基づきカメラ画像内における特徴点を抽出し、当該カメラ画像内における前記特徴点の位置を示す特徴点座標を求めて、前記被写体に対する前記カメラの相対的な位置および姿勢を示すカメラポーズ情報の初期値として予め決められている初期カメラポーズ情報と、仮想３次元空間における前記特徴点の位置を示す３次元座標として予め決められている初期３次元座標とを対応付けて記憶部に書き込み、
   前記初期フレームの以降に撮影された非初期フレームに対応するカメラ画像データを前記入力部から入力した場合、当該入力したカメラ画像データに基づきカメラ画像内における前記特徴点の前記特徴点座標を求めるとともに、当該カメラ画像データもより前の複数のフレームに対応するカメラ画像データから求めた前記特徴点座標および前記３次元座標に基づき、カメラ画像内における観測点と投影点の誤差を最小化することにより、前記非初期フレームに対応する前記カメラポーズ情報を求め、前記特徴点座標と前記カメラポーズ情報とを各フレームに対応付けて前記記憶部に書き込むことを特徴とする請求項１から５のうちいずれか１つに記載の３次元情報取得装置。
7. コンピュータを、
   所定の被写体を撮影し連続する複数のフレーム毎のカメラ画像データをカメラから入力する入力手段、
   前記カメラ画像データに基づきカメラ画像内における特徴点を抽出し、前記カメラ画像内における前記特徴点の位置を示す特徴点座標と、前記被写体に対する前記カメラの相対的な位置および姿勢を示すカメラポーズ情報とを、実空間中における観測点とこれに対応するカメラ画像内における投影点の誤差を最小化することにより、複数フレームにわたって求める位置姿勢逐次計測手段、
   仮想３次元空間における前記特徴点の位置を示す３次元座標と前記特徴点座標と前記カメラポーズ情報とに対して、前記カメラポーズ情報と前記３次元座標の信頼度に応じて予め決められているフレーム単位の加重値に基づく補正をフレーム単位で行い、前記カメラポーズ情報と前記３次元座標の最適化を行う最適化処理手段、
   として機能させるための３次元情報取得プログラム。

Images