JP5439277B2 - 位置姿勢計測装置及び位置姿勢計測プログラム - Google Patents

Description

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

位置姿勢計測装置は、カメラ等の撮像装置によって入力された少なくとも１枚以上の撮像画像列から、カメラの位置及び向きを示すカメラポーズと、撮像された３次元シーンの３次元座標情報を計測し、計測したカメラの位置及び向きを用いて、撮像画像中にコンピュータグラフィクス等を合成することや、計測した３次元座標情報を用いて３次元シーンを理解することなどを可能としている。

従来の位置姿勢計測装置は、撮像装置から入力される初期時刻においてカメラポーズと、対象の３次元座標情報が与えられていることを前提に、（１）撮像画像中での特徴点の追跡、（２）カメラポーズの推定、（３）特徴点の３次元座標の推定・更新、を繰り返すことによって、時々刻々と変化するカメラポーズ、及び撮像された３次元シーンの３次元座標情報の計測を実現している。

初期時刻におけるカメラポーズと、対象の３次元座標情報は、例えば、非特許文献１に記載の方法では、予め対象の３次元シーンを２地点で撮像した画像から特徴点を抽出し、当該特徴点の３次元座標情報をステレオ計測原理により算出することによって、カメラポーズの初期状態を一方の地点で撮像したカメラポーズ、対象の３次元座標情報の初期状態を算出した３次元座標情報としている。

また、例えば非特許文献２に記載の方法では、ユーザが本やワインボトル等の平面に近い形状である撮影対象に対しおおむね正対してカメラを持つと仮定し、初期状態のカメラポーズである位置及び姿勢をそれぞれ、平行移動成分を０ベクトル、回転成分を単位行列としている。また撮像画像中での特徴点ｐの３次元座標の初期状態を（Ｘ_ｐ，Ｙ_ｐ，Ｚ_ｐ）＝（ｘ_ｐ，ｙ_ｐ，１）としている。

"Parallel Tracking and Mapping for Small AR Workspaces"（Georg Klein、 David Murray: Proc. ISMAR, pp. 225-234、 2007) 「動画像からの三次元復元による湾曲した紙面のビデオモザイキング」（佐藤智和、池谷彰彦、池田聖、神原誠之、中島昇、横矢直和：電子情報通信学会論文誌Ｄ、巻：Vol.J90-D,No.8、pp.1900-1911,2007）

しかしながら、非特許文献１では、初期設定にユーザの手間が掛かり煩わしいとともに時間が掛かるといった問題があった。また非特許文献２では、対象が非平面と大きく異なる３次元座標を有する場合、つまり初期値が真値から大きく離れる場合には、初期時刻以降のカメラポーズの推定、特徴点の３次元座標の推定・更新時の最適化処理において局所解に陥ることで正しい解が得られないことがあるといった問題があった。

本発明は、上述したような従来技術が有する問題点に鑑みてなされたものであり、初期設定でユーザに手間を掛けることなく正確にカメラポーズ及び対象の３次元形状を計測できる位置姿勢計測装置及び位置姿勢計測プログラムを提供することを目的とする。

上述の課題を解決するために、本発明に係る位置姿勢計測装置は、撮像装置から入力される少なくとも１枚の入力画像列から、該撮像装置の位置及び姿勢を示すカメラポーズと、該撮像装置により撮像される撮像物体の３次元座標を逐次計測する位置姿勢計測装置であって、予め設定された前記撮像装置の内部パラメータと、初期時刻におけるカメラポーズである初期カメラポーズと、前記撮像装置に対する前記撮像物体の位置関係と、前記撮像物体の大きさを表す値とから、初期時刻における前記撮像物体の３次元座標である初期３次元座標を算出する初期値算出手段と、前記入力画像列のうち初期時刻における画像である初期時刻画像から前記撮像物体の特徴点の画像における座標である特徴点座標を抽出し、前記初期カメラポーズと初期３次元座標とから前記特徴点座標に対応する初期３次元座標を算出する初期値決定手段と、前記入力画像列のうち初期時刻画像から順に、現時刻における画像である現時刻画像と、前時刻における画像である前時刻画像とそれぞれの前記撮像物体の特徴点座標を対応付ける特徴点追跡手段と、前記現時刻画像内の特徴点座標と、前記特徴点追跡手段が対応付けた現時刻画像と前時刻画像とそれぞれの特徴点座標と、前時刻画像内の特徴点の３次元座標とから、現時刻におけるカメラポーズを算出するカメラポーズ推定手段と、初期時刻から現時刻に亘るカメラポーズと特徴点座標とから、現時刻の撮像画像内の特徴点各々の３次元座標を算出して更新する３次元座標算出手段と、現時刻画像から予め定められた一定時刻前までの撮像画像列に亘り、前記カメラポーズ推定手段が算出したカメラポーズと、前記３次元座標算出手段が算出した特徴点の３次元座標と、前記特徴点追跡手段により対応付けられた特徴点座標とから予め定められた誤差関数を最小化する特徴点の３次元座標及びカメラポーズを算出する最適化処理手段と、を有することを特徴とする。

また、上記に記載の本発明に係る位置姿勢計測装置において、前記初期値算出手段は、撮像物体として予め定められた半径を有する球体を平面上に配置された近似撮像対象を、前記撮像装置が予め定められた位置関係から観測するものとして、初期時刻における初期３次元座標を算出することを特徴とする。

また、上記に記載の本発明に係る位置姿勢計測装置において、前記初期値算出手段は、撮像物体として予め定められた複数の半径を有する球体のうち１つを平面上に配置された近似撮像対象を、前記撮像装置が予め定められた位置関係から観測するものとして、初期時刻における初期３次元座標を算出する処理を、前記複数の半径を有する球体に亘って行って初期３次元座標候補を算出することを特徴とする。

また、上記に記載の本発明に係る位置姿勢計測装置において、前記初期値算出手段は、予め定められた複数の基本形状のうち１つを平面上に配置された近似撮像対象を、前記撮像装置が予め定められた位置関係から観測するものとして、初期時刻における初期３次元座標を算出する処理を、前記複数の基本形状に亘って行って初期３次元座標候補を算出することを特徴とする。

また、上記に記載の本発明に係る位置姿勢計測装置において、前記初期値算出手段は、平面上に配置する近似撮像対象を、前記撮像装置が予め入力される複数の位置関係から観測するものとして、初期時刻における初期３次元座標を算出する処理を、前記複数の位置関係に亘って行って初期３次元座標候補を算出することを特徴とする。

また、上記に記載の本発明に係る位置姿勢計測装置において、前記初期値決定手段は、算出された前記初期３次元座標候補のうちいずれか１つを初期３次元座標に選択する際に、前記初期３次元座標候補から順に１つを選択し、選択した初期３次元座標候補に基づいて、初期時刻の撮像画像の特徴点に対応する３次元座標を初期３次元座標に設定し、初期時刻から時刻を１つ進めた現時刻の撮像画像を前記撮像画像列から取り出し、現時刻と初期時刻のそれぞれの画像の特徴点座標を対応付け、前記現時刻の特徴点座標と、対応付けた現時刻画像と前時刻画像とそれぞれの特徴点座標と、前時刻画像内の特徴点の３次元座標とから現時刻におけるカメラポーズを算出し、初期時刻から現時刻に亘るカメラポーズ及び特徴点座標から、前記現時刻の特徴点座標各々の３次元座標を算出して更新する処理を、予め定められた期間に亘って繰り返した後に、該期間の撮像画像に亘って最適化処理を実施して誤差値を算出する処理を、全ての初期３次元座標候補に亘って行い、算出した誤差値のうち最小の誤差値に対応する初期３次元座標候補を初期３次元座標に選択することを特徴とする。

また、上記に記載の本発明に係る位置姿勢計測装置において、前記初期値決定手段は、前記初期３次元座標候補から初期３次元座標を選択する際に、前記初期３次元座標候補に対応する近似撮像対象と、初期時刻画像とに基づいて初期３次元座標候補を選択することを特徴とする。

また、上記に記載の本発明に係る位置姿勢計測装置において、前記初期値決定手段は、前記初期３次元座標候補から初期３次元座標を選択する際に、初期時刻の撮像画像を撮像対象の特徴に基づいて複数の領域に分割し、前記撮像画像の中心を含む前記領域の輪郭線を抽出し、前記初期３次元座標候補に対応する距離画像を領域分割し、前記距離画像の中心を含む領域の輪郭線を抽出し、前記撮像画像から抽出した輪郭線の長さと、前記距離画像から抽出した輪郭線の長さとの差が予め定められた閾値以下の前記初期３次元座標候補を初期３次元座標に選択することを特徴とする。

また、本発明に係る位置姿勢計測プログラムは、撮像装置から入力される少なくとも１枚の入力画像列から、該撮像装置の位置及び姿勢を示すカメラポーズと、該撮像装置により撮像される撮像物体の３次元座標を逐次計測する位置姿勢計測装置が有するコンピュータを、予め設定された前記撮像装置の内部パラメータと、初期時刻におけるカメラポーズである初期カメラポーズと、前記撮像装置に対する前記撮像物体の位置関係と、前記撮像物体の大きさを表す値とから、初期時刻における前記撮像物体の３次元座標である初期３次元座標を算出する初期値算出手段、前記入力画像列のうち初期時刻における画像である初期時刻画像から前記撮像物体の特徴点の画像における座標である特徴点座標を抽出し、前記初期カメラポーズと初期３次元座標とから前記特徴点座標に対応する初期３次元座標を算出する初期値決定手段、前記入力画像列のうち初期時刻画像から順に、現時刻における画像である現時刻画像と、前時刻における画像である前時刻画像とそれぞれの前記撮像物体の特徴点座標を対応付ける特徴点追跡手段、前記現時刻画像内の特徴点座標と、前記特徴点追跡手段が対応付けた現時刻画像と前時刻画像とそれぞれの特徴点座標と、前時刻画像内の特徴点の３次元座標とから、現時刻におけるカメラポーズを算出するカメラポーズ推定手段、初期時刻から現時刻に亘るカメラポーズと特徴点座標とから、現時刻の撮像画像内の特徴点各々の３次元座標を算出して更新する３次元座標算出手段、現時刻画像から予め定められた一定時刻前までの撮像画像列に亘り、前記カメラポーズ推定手段が算出したカメラポーズと、前記３次元座標算出手段が算出した特徴点の３次元座標と、前記特徴点追跡手段により対応付けられた特徴点座標とから予め定められた誤差関数を最小化する特徴点の３次元座標及びカメラポーズを算出する最適化処理手段として機能させることを特徴とする。

本発明によれば、ユーザによる事前の作業なく、初期値として実際の３次元シーン形状に近い形状を指定できるため、ユーザの手間を掛けることなくカメラポーズ及び対象の３次元座標を計測することが可能となる。また、本発明によれば、初期値の候補から実際の３次元シーンの真値に近い値を選択し初期値として設定できるため、逐次のカメラポーズ及び対象の３次元座標推定が局所解に陥ることなく実行でき、正確に各々を計測することが可能となる。

本発明の第１の実施形態のシステムの構成を示すブロック図である。 本発明の第１の実施形態における初期値算出処理の第１の説明図である。 本発明の第１の実施形態における初期値算出処理の第２の説明図である。 本発明の第１の実施形態における初期値算出処理の第３の説明図である。 本発明の第１の実施形態における位置姿勢計測処理を説明するためのフローチャートである。 本発明の第１の実施形態における初期値決定処理の第１の説明図である。 本発明の第１の実施形態における初期値決定処理の第２の説明図である。 本発明の第２の実施形態における初期値算出処理の第１の説明図である。 本発明の第２の実施形態における初期値算出処理の第２の説明図である。 本発明の第２の実施形態における初期値算出処理の第３の説明図である。 本発明の第２の実施形態における位置姿勢計測処理を説明するためのフローチャートである。 本発明の第２の実施形態における初期値決定処理の第１の説明図である。 本発明の第２の実施形態における初期値決定処理の第２の説明図である。

＜第１の実施形態＞
以下、本発明の実施の形態について図面を参照しながら説明する。図１は、本発明の第１の実施形態のシステム構成を示すブロック図である。

図１において、撮像装置１は、対象となる３次元シーンを撮像する。撮像装置１は、例えば、ＣＣＤ（Charge-Coupled Device）カメラ、ＣＭＯＳカメラ（Complementary Mental-Oxide Semiconductor）等を含んでいる。なお、本発明の実施形態において、焦点距離などを示す撮像装置１の内部パラメータは、例えば、焦点距離などであり、予め設定されているものとする。

位置姿勢計測装置２は、コンピュータシステムにより構成されており、撮像装置１から入力された撮像画像列から、カメラの位置及び向きを示すカメラポーズと、撮像された３次元シーンの３次元座標情報を計測する。位置姿勢計測装置２に対して、メモリ４及びハードディスクドライブ装置５が設けられる。なお、メモリ４及びハードディスクドライブ装置５は、位置姿勢計測装置２の本体に内蔵されても良いし、位置姿勢計測装置２の本体の外部に備えられても良い。

モニタ装置３は、撮像装置１で撮像された画像や位置姿勢計測装置２での結果をモニタ画像として映し出す。モニタ装置３としては、液晶ディスプレイやプロジェクタが用いられる。

位置姿勢計測装置２は、初期値算出部１１と、初期値決定部１２と、特徴点追跡部１３と、カメラポーズ推定部１４と、３次元座標算出部１５と、最適化処理部１６との各機能部から構成される。

初期値算出部１１は、予め設定されている焦点距離等の撮像装置１の内部パラメータと、撮像装置１に対する撮像物体の位置関係（並進成分Ｔ_０及び回転成分Ｒ_０）と、撮像物体の大きさを表す値から、位置姿勢計測を開始する初期時刻におけるカメラポーズである初期カメラポーズを設定し、位置姿勢計測を開始する初期時刻における対象物体の３次元座標である初期３次元座標の算出を行い、その値をハードディスクドライブ装置５に記録する。

初期値決定部１２は、撮像装置１から出力される初期時刻の画像と、初期値算出部１１が決定した初期カメラポーズ及び初期３次元座標とから、初期時刻画像における特徴点の座標値を抽出し、抽出した特徴点に対応する初期３次元座標値を決定し、メモリ４に記録する。ここでいう決定とは、初期カメラポーズ及び初期３次元座標とから初期３次元座標値を算出する処理のことである。

特徴点追跡部１３は、入力される現時刻の画像と、前時刻の画像内の特徴点の座標値とから、前時刻画像内の全特徴点について現時刻画像内において対応する特徴点の座標値を決定し、メモリ４に記録する。ここで、前時刻の画像は、現在、処理対象にしている現時刻の画像である現時刻画像の直前に入力された画像であり、以下、前時刻画像という。

カメラポーズ推定部１４は、現時刻画像内における特徴点の座標値と、現時刻画像内における特徴点に対応する前時刻画像における特徴点の３次元座標値との組から、現時刻のカメラポーズを算出し、メモリ４に記録する。ここで前時刻における３次元座標値は、メモリ４に一時的に格納されてあって、そのメモリ４から読み出しても良い。

３次元座標算出部１５は、初期時刻から現時刻に亘っての撮像画像におけるカメラポーズと特徴点の座標値から、現時刻の特徴点の各々について３次元座標を算出し、メモリ４上に記録されている同一点の３次元座標値を更新する。

最適化処理部１６は、現時刻画像から予め定められた一定時刻前までの撮像画像列に亘る、カメラポーズ推定部１４で算出したカメラポーズと３次元座標算出部１５で算出した特徴点の３次元座標値から、予め定めた誤差関数を最小化する特徴点の３次元座標及びカメラポーズを決定し出力する。

次に、本発明の第１の実施形態の位置姿勢計測装置２で実行される処理について詳細に説明する。本発明の第１の実施形態における位置姿勢計測装置２では、オフライン処理で、予め初期値算出部１１が起動される。初期値算出部１１が起動されると、初期値算出部１１は焦点距離等の撮像装置１の内部パラメータを受信する。

ここで、ユーザが撮像する３次元空間内では、例えば図２に示すように、撮像対象５２が机等の平面５１上に配置されており、ユーザはこの撮像対象５２のほぼ中心を斜め下向きに撮影するものと仮定する。以下、この仮定のもとで、初期値算出部１１での初期３次元座標の算出方法について説明する。

まず、初期値算出部１１は、初期カメラポーズを示す撮像装置１の並進成分Ｔ_０（Ｘ，Ｙ，Ｚ）を０ベクトル、回転成分Ｒ_０を単位行列に初期化する。ここで、並進成分Ｔ_０は、Ｘ軸、Ｙ軸、Ｚ軸の各座標系に沿った直線上の運動成分を表し、回転成分Ｒ_０は、Ｘ軸、Ｙ軸、Ｚ軸の各座標系に対して回転する運動成分を表す。また、初期時刻においては、図３に示すように、世界座標系とカメラ座標系は合致しているものとする。

図２、３は、本実施形態において、撮像装置と、撮像対象との位置関係の一例を示す図である。
初期値算出部１１は、図２の平面５１上に撮像対象５２が配置されている状態を、図３に示すように、平面６１上に半径ｒの球体の近似撮像対象６２が配置されている状態であると近似し、前述の仮定のもとで、カメラ座標系原点から並進成分Ｔ_０、回転成分Ｒ_０を設置して、初期カメラポーズを決定する。

ここで、近似撮像対象６２の半径ｒ、並びに、撮像装置１に対する近似撮像対象６２の並進成分Ｔ_０及び回転成分Ｒ_０は、予めユーザにより与えられているものとする。初期値算出部１１は、この近似撮像対象６２と、撮像装置１の内部パラメータ及びカメラポーズ（即ち、並進成分Ｔ_０は０ベクトル、回転成分Ｒ_０は単位行列）を用いて、初期３次元座標を算出する。すなわち、初期値算出部１１は、予め定められた半径ｒを有し、平面上に配置された球体を近似撮像対象として、予め定められた初期カメラポーズから近似撮像対象を撮像した場合における初期３次元座標を算出する。

図４は、このようにして算出された初期３次元座標を距離画像表現で示したものである。図４においては、初期３次元座標を近い方が明るく、遠い方が暗くなる距離画像で表現している。

以上のようにして、初期値算出部１１は、初期カメラポーズの設定と、初期３次元座標の算出を行い、この初期３次元座標と、初期カメラポーズをハードディスクドライブ装置５等の記録媒体に記録して、処理を終了する。その後、オンライン処理で、位置姿勢計測処理が行われる。次に、このオンライン処理における位置姿勢計測処理について説明する。

図５は、位置姿勢計測装置２で実行されるオンライン処理の流れを示すフローチャートである。位置姿勢計測装置２におけるオンライン処理では、まず初期値決定部１２が起動される。

初期値決定部１２は、はじめに初期値算出部１１においてハードディスクドライブ装置５に記録された初期３次元座標と初期カメラポーズを読み出す。続いて、初期値決定部１２は、撮像装置１から出力される初期時刻画像を受信する。
図６は、撮像対象における特徴点の一例を示す図である。初期値決定部１２は、受信した撮像画像において、図６に示すように、画像上の角や交点となる特徴点Ａを抽出する。

続いて、初期値決定部１２は、この特徴点Ａに対応する初期３次元座標値を、初期値算出部１１で算出された初期３次元座標（図４）から選択することによって決定する。
図７は、抽出した特徴点Ａと、初期３次元座標値とが対応付けられた組の一例を示す図である。初期値決定部１２は、図７に示すように、特徴点Ａの座標値と初期３次元座標値との組を対応付けて、初期カメラポーズと併せてメモリ４に記録し、初期値決定部１２における処理を終了する（ステップＳ１０１）。

特徴点Ａの座標値と初期３次元座標との対応付けは、例えば、初期３次元座標値として、世界座標系原点（図３の世界座標系原点０）に撮像装置１と同じ内部パラメータを有する仮想カメラ４１を配置し、この仮想カメラ４１に初期３次元座標を射影することによって距離画像表現したもの（図４）を予め作成しておく。続いて、初期値決定部１２は、撮像画像から出力される初期画像上での特徴点Ａの画像座標（ｘ，ｙ）と同様の画像座標に対応する点を距離画像（図４）から抽出し、その距離値Ｚを読み出し、この距離値Ｚから特徴点Ａの初期３次元座標を演算し決定する。この初期３次元座標は、撮像装置１の内部パラメータが焦点距離ｆのみによって規定される場合は、Ｘ＝Ｚ_ｘ／ｆ、Ｙ＝Ｚ_ｙ／ｆ、Ｚ＝Ｚと演算し決定できる。

初期値決定部１２での処理が終了したら、次に、位置姿勢計測装置２はユーザからの終了指示があるかを判定し（ステップＳ１０６）、ユーザからの終了指示がなければ、続いて特徴点追跡部１３が起動される。特徴点追跡部１３は、撮像装置１から入力される次時刻画像を現時刻画像に設定するとともに現時刻画像を前時刻画像に設定する。そして、特徴点追跡部１３は、メモリ４に記録されている前時刻画像内の各特徴点Ａについて、現時刻画像内における対応点を決定する（ステップＳ１０２）。

この対応点の決定は、特徴点近傍画素値の類似度を評価するテンプレートマッチング等により実現される。この対応点決定処理を前時刻画像内の全特徴点Ａに亘って処理を行い、現時刻画像内における特徴点Ａの座標を決定する。すなわち、特徴点追跡部１３は、前時刻画像の特徴点Ａと、現時刻画像の特徴点Ａとを対応付けることにより、各特徴点Ａの移動を追跡する。

続いてカメラポーズ推定部１４が起動される。カメラポーズ推定部１４は、まず、特徴点追跡部１３が決定した現時刻画像内の特徴点Ａの画像座標を受信し、メモリ４から前時刻の３次元座標値を読み出す。続いて、現時刻画像内の特徴点Ａの画像座標と、現時刻画像内の特徴点Ａに対応する前時刻画像内の特徴点Ａの３次元座標値とから、現時刻のカメラポーズを算出する（ステップＳ１０３）。このカメラポーズの算出は、以下のようにして行われる。

まず、３次元空間中の点（Ｘ，Ｙ，Ｚ，１）と撮像画像上の投影点（ｕ，ｖ，１）との間には、次式（１）の関係が成り立つ。

ここで、Ｐは、３×４の行列であり、撮像装置１の外部パラメータであるカメラポーズを示す。また、Ｔは転置を表す。続いて、画像上の観測点（ｕ’，ｖ’）^Ｔと投影点の誤差を次式（２）で表現することができる。

現時刻画像内の特徴点の座標（ｕ_ｐ’，ｖ_ｐ’）^Ｔと前時刻の３次元座標値（Ｘ_ｐ，Ｙ_ｐ，Ｚ_ｐ）を用い非線形最適化法により現時刻のカメラポーズＰを算出する。例えば式（２）で定義した誤差関数の和を示す次式（３）の値をマーカート法により最小化することでカメラポーズＰを得ることができる。

続いて、３次元座標算出部１５が起動される。３次元座標算出部１５は、まず、メモリ４から初期時刻の次の時刻から現時刻に亘っての撮像画像におけるカメラポーズと、特徴点Ａの画像座標とを読み出す。続いて、３次元座標算出部１５は、現時刻の特徴点Ａの各々について３次元座標を算出し、メモリ４上に記録されている同一点の３次元座標値を更新する（ステップＳ１０４）。この３次元座標の算出は、現時刻ｔに存在する全ての特徴点Ａについて、特徴点Ａごとに式（２）で定義した誤差関数の和を示す次式（４）の値を最小化することにより３次元座標を算出する。

続いて、最適化処理部１６が起動される。最適化処理部１６は、まず、正確なカメラポーズを推定するべく誤追跡の発生しにくい特徴点Ａのみを選択するため、特徴点Ａの追加と削除を行う。この特徴点Ａの追加と削除は、例えば、一定時刻以上追跡された特徴点Ａにおいて、各画像間の対応付け誤差値が予め定めた閾値以下であれば追加、予め別に定めた閾値以上であれば削除することによって実現できる。

最後に、最適化処理部１６は、バンドルアジャスメントの枠組み（参考文献１：”Bundle Adjustment-A Modern Synthesis",（B.Triggs, P.McLauchlan, R.Hartley and A.Fitzgibbon : LNCS, Springer Verlag, pp.298-375, 2000）に記載されている技術）を用い、現時刻画像から予め定められた一定時刻前までの撮像画像列に対して、再投影誤差の総和を最小化することで特徴点Ａの３次元座標とカメラポーズとを同時に最適化する。これは、最適化の誤差関数に、例えば、次式（５）を設定し、特徴点Ａの３次元座標Ｓ_ｐとカメラポーズＭ_ｆをパラメータとしてＥを最小化する３次元座標Ｓ_ｐ、カメラポーズＭ_ｆとを決定する処理により実現される。なお、３次元座標Ｓ_ｐと、カメラポーズＭ_ｆの初期値には、逐次処理によって得られた推定結果を用いる（ステップＳ１０５）。

続いて、位置姿勢計測装置２は、終了指示が与えられたかどうかを判定し（ステップＳ１０６）、終了指示が与えられていなければ、以上の述べた特徴点追跡処理（ステップＳ１０２）、カメラポーズ推定処理（ステップＳ１０３）、３次元座標算出処理（ステップＳ１０４）、最適化処理（ステップＳ１０５）を繰り返すことによって各時刻におけるカメラポーズと３次元座標とを逐次的に算出することにより、カメラポーズと３次元座標とを推定することができる。

以上、説明したように、本発明の第１の実施形態では、初期値算出部１１は、図３に示したように、平面６１上に半径ｒの球体の近似撮像対象６２が配置されている状態であると近似し、カメラ座標系原点から並進成分Ｔ_０、回転成分Ｒ_０を設置して、初期カメラポーズを決定している。このため、ユーザによる事前の作業なく、初期値として実際の３次元シーン形状に近い形状を指定で、ユーザの手間を掛けることなくカメラポーズ及び対象の３次元座標を計測することが可能となる。

＜第２の実施形態＞
次に本発明における第２実施の形態を具体的に説明する。
前述の第１の実施形態では、位置姿勢計測装置が、図２に示したように、ユーザが撮像装置１により、平面５１上に配置された撮像対象５２のほぼ中心を斜め下向きに撮影するものと仮定する。そして、位置姿勢計測装置は、図３に示したように、撮像対象の初期３次元座標値を平面６１上に半径ｒの球体が近似撮像対象６２として据えられているものと近似して、カメラポーズを設定し、撮像対象の初期３次元座標を算出している。
しかしながら、撮像対象５２の大きさ、撮像対象５２の形状、撮像時の撮像装置１の向きによっては、このような所定の半径の球体を近似撮影対象として初期３次元座標を求めたのでは、初期値が真値から離れることがある。

そこで、本発明の第２の実施形態では、初期値算出部１１において、複数の初期３次元座標候補を予め算出して準備しておき、初期値決定部１２において、これら複数の初期３次元座標候補から確からしい１つを初期値に選択して設定するようにしている。なお、本発明の第２の実施形態は、初期値算出部１１と初期値決定部１２以外の構成及び動作は、第１の実施形態と同様であるので、ここでは説明を省略する。以下では、第１の実施形態と異なる動作となる点についてのみ説明する。

図８は、本実施形態において用いる近似撮像対象の一例を示す図である。
本発明の第２の実施形態では、図８（Ａ）〜図８（Ｄ）に示すように、半径の異なる複数の近似撮像対象６２ａ〜６２ｄが用いられる。つまり、撮像対象の大きさ、或いは、画像上での撮像対象の大きさ（即ち撮像対象と撮像装置１との間の距離）が不明な場合に対処するために、半径（ｉ＝０〜Ｎ−１）を有するＮ個の球体が予め準備され、これらの球体と平面とが撮像対象として設定される。このような処理を行うために、図８（Ａ）〜図８（Ｄ）に示すような大きさの異なる複数の近似撮像対象６２ａ〜６２ｄが生成され、これらが初期３次元座標候補としてハードディスクドライブ装置５等の記録媒体に記録される。

図９は、本実施形態において用いる近似撮像対象の一例を示す図である。
また、本発明の第２の実施形態では、図９（Ａ）〜図９（Ｄ）に示すように、形状の異なる複数の近似撮像対象６２ｅ〜６２ｈが用いられる。つまり、例えば、撮像対象の形状が不明の時に対処するために、例えば、球形、立方体、円柱、角錐、円錐などの複数の基本形状が予め準備され、これらの基本形状と平面が撮像対象として設定される。このような処理を行うために、図９（Ａ）〜図９（Ｄ）に示すような形状の異なる複数の近似撮像対象６２ｅ〜６２ｈが予め生成され、これらが初期３次元座標候補としてハードディスクドライブ装置５等の記録媒体に記録される。

図１０は、本実施形態における近似撮像対象の一例を示す図である。
また、本発明の第２の実施形態では、図１０（Ａ）〜図１０（Ｈ）に示すように、撮像装置１と異なる位置関係を有する複数の近似撮像対象６２ｉ〜６２ｐが用いられる。つまり、例えば、撮像時の撮像装置１の向き（即ち初期カメラポーズの回転成分Ｒ_０）が不明の場合に対処するために、例えば、撮像装置１の光軸周りに４５度ずつ回転させる行列が、回転成分Ｒ_０として設定される。このような処理を行うために、図１０に示すような位置関係の異なる近似撮像対象６２ｉ〜６２ｐが複数生成され、これらが初期３次元座標候補としてハードディスクドライブ装置５等の記録媒体に記録する。

続いて、本発明の第２の実施形態におけるオンライン処理での初期値決定部１２の動作を図１１に基づいて説明する。
図１１は、本実施形態における位置姿勢計測装置の位置姿勢計測処理を示すフローチャートである。本実施形態の初期値決定部１２において、第１の実施形態と第２の実施形態の異なる点は、複数の初期３次元座標候補のうちいずれか１つを初期３次元座標に決定する動作が加わったところである。以下、本発明の第２の実施形態における初期値決定部１２の処理の詳細を説明する。

まず、初期値決定部１２は、初期値算出部１１において記録された初期３次元座標候補と初期カメラポーズを読み出す。続いて、撮像画像から出力される初期時刻画像を受信し、該撮像画像において第１の実施形態同様に、画像上の角や交点となる特徴点Ａを抽出する（ステップＳ２０１）。

続いて、図８〜図１０に示した初期３次元座標候補から順に１つを選択し、選択した初期３次元座標候補から抽出された特徴点Ａの３次元座標算出して、初期時刻における撮像画像の初期３次元座標に設定する（ステップＳ２０２）。

続いて、位置姿勢計測装置２は一定時刻の撮像画像に対して処理済みかどうかを判定し（ステップＳ２０６）、処理済みでなければ、特徴点追跡部１３での処理が起動され（ステップＳ２０３）、カメラポーズ推定部１４での処理が起動され（ステップＳ２０４）、３次元座標算出部１５での処理が起動される（ステップＳ２０５）。これらの処理は、第１の実施形態での処理と同じであるので説明を省略する。そして、位置姿勢計測装置２は一定時刻の撮像画像に対して処理済みかどうかを判定し（ステップＳ２０６）、処理済みでなければ、処理はステップＳ２０３にリターンし、特徴点追跡から３次元座標算出までの処理（ステップＳ２０３〜Ｓ２０５）を、予め定めた一定時刻の撮像画像に亘って繰り返して行う。

繰り返し処理が終わったら、第１の実施形態と同様に、最適化処理部１６が起動される。最適化処理部１６は、初期時刻から予め定められた時刻までの一定の期間、撮像画像列に対して、再投影誤差の総和を最小化することで特徴点の３次元座標とカメラポーズを最適化し、その誤差値を、選択した初期３次元座標候補と併せて記録する（ステップＳ２０７）。続いて、位置姿勢計測装置２は、全ての初期３次元座標候補に対して処理済みかどうかを判定し（ステップＳ２０５）、処理済みでなければ、処理をステップＳ２０２にリターンし、初期３次元座標候補から、先に処理した初期３次元座標と異なる初期３次元座標をひとつ選択し、特徴点Ａに対応する初期３次元座標を初期３次元座標として設定する処理を行う。続けて、位置姿勢計測装置２は、ステップＳ２０２からステップＳ２０８の処理を全ての初期３次元座標候補に亘って行う。

最後に、位置姿勢計測装置２は、初期３次元座標候補と併せて記録されている誤差値を比較し、その最小値を決定し、当該最小誤差値に対応する初期３次元座標候補を、初期３次元座標として、初期カメラポーズと併せて出力し（ステップＳ２０９）、初期値決定部１２における処理を終了する。

なお、以上で説明した初期値決定部１２における処理は、ステップＳ２０２における前処理として、複数存在する初期３次元座標候補から、初期時刻画像と比較することによって確からしい候補を選択し、当該候補のみを初期３次元座標候補として以降の処理を行うことで、実現しても良い。確からしい候補の選択方法は、例えば初期時刻画像を色若しくは輝度情報等を利用して領域分割し、画像中心に存在する領域の輪郭線を領域線追跡等により抽出する。初期値決定部１２は、初期３次元座標候補に対応する近似撮像物体を距離画像表現した際の輪郭線と、抽出した輪郭線とのそれぞれの長さを画像座標において算出し、算出した長さの差が予め定められた閾値以下であれば確からしい候補として選択することによって実現できる。すなわち、初期値決定部１２は、前記初期３次元座標候補に対応する近似撮像対象と、初期時刻画像とに基づいて初期３次元座標候補を選択して決定するようにしてもよい。

図１２は、初期時刻画像の輪郭線追跡を行い、初期３次元座標の候補を選択する処理を説明するものである。また、図１３は、図９において示した基本形状の輪郭線を示す図である。例えば、図１２（Ａ）に示すような初期時刻画像７１がある場合には、位置姿勢計測装置２は、ステップＳ２０２における前処理として、初期時刻画像７１を色若しくは輝度により領域分割し、図１２（Ｂ）のような領域分割画像７２を取得する。そして、位置姿勢計測装置２は、この領域分割画像７２から、図１２（Ｃ）に示すように、画像中心に存在する領域の輪郭線７３を抽出する。

また、位置姿勢計測装置２は、図１３（Ａ）〜図１３（Ｄ）に示すように、基本形状の初期３次元座標の候補の輪郭線７４ａ〜７４ｄを用意しておき、ハードディスクドライブ装置５に記録しておく。この基本形状の輪郭線７４ａ〜７４ｄは、例えば、図９（Ａ）〜図９（Ｄ）に示したような、基本形状の輪郭線を抽出したものである。

図１２（Ｃ）に示すように、初期時刻画像７１の輪郭線７３が抽出されたら、位置姿勢計測装置２は、基本形状の初期３次元座標の候補の輪郭線７４ａ〜７４ｄを読み出し、これら初期３次元座標の候補の輪郭線７４ａ〜７４ｄと、初期時刻画像７１から抽出された輪郭線７３とを比較し、最も確からしい候補を選択する。例えば、位置姿勢計測装置２は、初期３次元座標の候補の輪郭線７４ａ〜７４ｄの長さと、初期時刻画像７１から抽出された輪郭線７３の長さとそれぞれを算出し、算出した長さを比較し、算出した長さの差が最小の基本形状を、最も確からしい候補として選択する。図１２（Ｃ）に示すような輪郭線７３が抽出された場合には、図１３（Ａ）の球体の輪郭線７４ａ又は図１３（Ｂ）の円柱の輪郭線７４ｂが候補として選択されることになる。このようにして候補となる形状の輪郭線が選択された場合には、位置姿勢計測装置２は、選択された候補のみについて、初期３次元座標候補として、ステップＳ２０２以降の処理を行う。また、輪郭線の形状に対してパターンマッチングを行い、最も確からしい候補を選択するようにしてもよい。

以上説明したように、本発明の第２の実施形態では、初期値算出部１１において複数の初期３次元座標候補を算出、準備しておき、初期値決定部１２において、これら複数の初期３次元座標候補から確からしい１つの初期値を決定し、これを初期値として設定するようにしている。このため、初期値の候補から実際の３次元シーンの真値に近い値を自動で選択し初期値として設定でき、逐次のカメラポーズ及び対象の３次元座標推定を局所解に陥ることなく実行でき、正確に各々を計測することが可能となる。

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

１…撮像装置
２…位置姿勢計測装置
３…モニタ装置
４…メモリ
５…ハードディスクドライブ装置
１１…初期値算出部
１２…初期値決定部
１３…特徴点追跡部
１４…カメラポーズ推定部
１５…３次元座標算出部
１６…最適化処理部

Claims (9)

1. 撮像装置から入力される少なくとも１枚の入力画像列から、該撮像装置の位置及び姿勢を示すカメラポーズと、該撮像装置により撮像される撮像物体の３次元座標を逐次計測する位置姿勢計測装置であって、
   予め設定された前記撮像装置の内部パラメータと、初期時刻におけるカメラポーズである初期カメラポーズと、前記撮像装置に対する前記撮像物体の位置関係と、前記撮像物体の大きさを表す値とから、初期時刻における前記撮像物体の３次元座標である初期３次元座標を算出する初期値算出手段と、
   前記入力画像列のうち初期時刻における画像である初期時刻画像から前記撮像物体の特徴点の画像における座標である特徴点座標を抽出し、前記初期カメラポーズと初期３次元座標とから前記特徴点座標に対応する初期３次元座標を算出する初期値決定手段と、
   前記入力画像列のうち初期時刻画像から順に、現時刻における画像である現時刻画像と、前時刻における画像である前時刻画像とそれぞれの前記撮像物体の特徴点座標を対応付ける特徴点追跡手段と、
   前記現時刻画像内の特徴点座標と、前記特徴点追跡手段が対応付けた現時刻画像と前時刻画像とそれぞれの特徴点座標と、前時刻画像内の特徴点の３次元座標とから、現時刻におけるカメラポーズを算出するカメラポーズ推定手段と、
   初期時刻から現時刻に亘るカメラポーズと特徴点座標とから、現時刻の撮像画像内の特徴点各々の３次元座標を算出して更新する３次元座標算出手段と、
   現時刻画像から予め定められた一定時刻前までの撮像画像列に亘り、前記カメラポーズ推定手段が算出したカメラポーズと、前記３次元座標算出手段が算出した特徴点の３次元座標と、前記特徴点追跡手段により対応付けられた特徴点座標とから予め定められた誤差関数を最小化する特徴点の３次元座標及びカメラポーズを算出する最適化処理手段と、
   を有し、
   前記初期値算出手段は、
   撮像物体として予め定められた半径を有する球体が平面上に配置された近似撮像対象を、前記撮像装置が予め定められた位置関係から観測するものとして、初期時刻における初期３次元座標を算出し、
   前記初期値決定手段は、
   前記初期３次元座標に基づいた距離画像において特徴点に対応する点を抽出し、抽出した点の距離を用いて特徴点の初期値３次元座標を算出する
   ことを特徴とする位置姿勢計測装置。
2. 撮像装置から入力される少なくとも１枚の入力画像列から、該撮像装置の位置及び姿勢を示すカメラポーズと、該撮像装置により撮像される撮像物体の３次元座標を逐次計測する位置姿勢計測装置であって、
   予め設定された前記撮像装置の内部パラメータと、初期時刻におけるカメラポーズである初期カメラポーズと、前記撮像装置に対する前記撮像物体の位置関係と、前記撮像物体の大きさを表す値とから、初期時刻における前記撮像物体の３次元座標である初期３次元座標を算出する初期値算出手段と、
   前記入力画像列のうち初期時刻における画像である初期時刻画像から前記撮像物体の特徴点の画像における座標である特徴点座標を抽出し、前記初期カメラポーズと初期３次元座標とから前記特徴点座標に対応する初期３次元座標を算出する初期値決定手段と、
   前記入力画像列のうち初期時刻画像から順に、現時刻における画像である現時刻画像と、前時刻における画像である前時刻画像とそれぞれの前記撮像物体の特徴点座標を対応付ける特徴点追跡手段と、
   前記現時刻画像内の特徴点座標と、前記特徴点追跡手段が対応付けた現時刻画像と前時刻画像とそれぞれの特徴点座標と、前時刻画像内の特徴点の３次元座標とから、現時刻におけるカメラポーズを算出するカメラポーズ推定手段と、
   初期時刻から現時刻に亘るカメラポーズと特徴点座標とから、現時刻の撮像画像内の特徴点各々の３次元座標を算出して更新する３次元座標算出手段と、
   現時刻画像から予め定められた一定時刻前までの撮像画像列に亘り、前記カメラポーズ推定手段が算出したカメラポーズと、前記３次元座標算出手段が算出した特徴点の３次元座標と、前記特徴点追跡手段により対応付けられた特徴点座標とから予め定められた誤差関数を最小化する特徴点の３次元座標及びカメラポーズを算出する最適化処理手段と、
   を有し、
   前記初期値算出手段は、
   撮像物体として予め定められた複数の半径を有する球体のうち１つが平面上に配置された近似撮像対象を、前記撮像装置が予め定められた位置関係から観測するものとして、初期時刻における初期３次元座標を算出する処理を、前記複数の半径を有する球体に亘って行って初期３次元座標候補を算出する
   ことを特徴とする位置姿勢計測装置。
3. 撮像装置から入力される少なくとも１枚の入力画像列から、該撮像装置の位置及び姿勢を示すカメラポーズと、該撮像装置により撮像される撮像物体の３次元座標を逐次計測する位置姿勢計測装置であって、
   予め設定された前記撮像装置の内部パラメータと、初期時刻におけるカメラポーズである初期カメラポーズと、前記撮像装置に対する前記撮像物体の位置関係と、前記撮像物体の大きさを表す値とから、初期時刻における前記撮像物体の３次元座標である初期３次元座標を算出する初期値算出手段と、
   前記入力画像列のうち初期時刻における画像である初期時刻画像から前記撮像物体の特徴点の画像における座標である特徴点座標を抽出し、前記初期カメラポーズと初期３次元座標とから前記特徴点座標に対応する初期３次元座標を算出する初期値決定手段と、
   前記入力画像列のうち初期時刻画像から順に、現時刻における画像である現時刻画像と、前時刻における画像である前時刻画像とそれぞれの前記撮像物体の特徴点座標を対応付ける特徴点追跡手段と、
   前記現時刻画像内の特徴点座標と、前記特徴点追跡手段が対応付けた現時刻画像と前時刻画像とそれぞれの特徴点座標と、前時刻画像内の特徴点の３次元座標とから、現時刻におけるカメラポーズを算出するカメラポーズ推定手段と、
   初期時刻から現時刻に亘るカメラポーズと特徴点座標とから、現時刻の撮像画像内の特徴点各々の３次元座標を算出して更新する３次元座標算出手段と、
   現時刻画像から予め定められた一定時刻前までの撮像画像列に亘り、前記カメラポーズ推定手段が算出したカメラポーズと、前記３次元座標算出手段が算出した特徴点の３次元座標と、前記特徴点追跡手段により対応付けられた特徴点座標とから予め定められた誤差関数を最小化する特徴点の３次元座標及びカメラポーズを算出する最適化処理手段と、
   を有し、
   前記初期値算出手段は、
   予め定められた複数の基本形状のうち１つが平面上に配置された近似撮像対象を、前記撮像装置が予め定められた位置関係から観測するものとして、初期時刻における初期３次元座標を算出する処理を、前記複数の基本形状に亘って行って初期３次元座標候補を算出する
   ことを特徴とする位置姿勢計測装置。
4. 前記初期値算出手段は、
   平面上に配置する近似撮像対象を、前記撮像装置が予め入力される複数の位置関係から観測するものとして、初期時刻における初期３次元座標を算出する処理を、前記複数の位置関係に亘って行って初期３次元座標候補を算出する
   ことを特徴とする請求項２又は請求項３のいずれかに記載の位置姿勢計測装置。
5. 前記初期値決定手段は、
   算出された前記初期３次元座標候補のうちいずれか１つを初期３次元座標に選択する際に、
   前記初期３次元座標候補から順に１つを選択し、選択した初期３次元座標候補に基づいて、初期時刻の撮像画像の特徴点に対応する３次元座標を初期３次元座標に設定し、
   初期時刻から時刻を１つ進めた現時刻の撮像画像を前記撮像画像列から取り出し、
   現時刻と初期時刻のそれぞれの画像の特徴点座標を対応付け、
   前記現時刻の特徴点座標と、対応付けた現時刻画像と前時刻画像とそれぞれの特徴点座標と、前時刻画像内の特徴点の３次元座標とから現時刻におけるカメラポーズを算出し、
   初期時刻から現時刻に亘るカメラポーズ及び特徴点座標から、前記現時刻の特徴点座標各々の３次元座標を算出して更新する処理を、予め定められた期間に亘って繰り返した後に、該期間の撮像画像に亘って最適化処理を実施して誤差値を算出する処理を、全ての初期３次元座標候補に亘って行い、
   算出した誤差値のうち最小の誤差値に対応する初期３次元座標候補を初期３次元座標に選択する
   ことを特徴とする請求項２から請求項４のいずれか１項に記載の位置姿勢計測装置。
6. 前記初期値決定手段は、
   前記初期３次元座標候補から初期３次元座標を選択する際に、
   前記初期３次元座標候補に対応する近似撮像対象と、初期時刻画像とに基づいて初期３次元座標候補を選択する
   ことを特徴とする請求項２から請求項５のいずれか１項に記載の位置姿勢計測装置。
7. 前記初期値決定手段は、
   前記初期３次元座標候補から初期３次元座標を選択する際に、
   初期時刻の撮像画像を撮像対象の特徴に基づいて複数の領域に分割し、前記撮像画像の中心を含む前記領域の輪郭線を抽出し、
   前記初期３次元座標候補に対応する距離画像を領域分割し、前記距離画像の中心を含む領域の輪郭線を抽出し、
   前記撮像画像から抽出した輪郭線の長さと、前記距離画像から抽出した輪郭線の長さとの差が予め定められた閾値以下の前記初期３次元座標候補を初期３次元座標に選択する
   ことを特徴とする請求項６に記載の位置姿勢計測装置。
8. 撮像装置から入力される少なくとも１枚の入力画像列から、該撮像装置の位置及び姿勢を示すカメラポーズと、該撮像装置により撮像される撮像物体の３次元座標を逐次計測する位置姿勢計測装置が有するコンピュータを、
   予め設定された前記撮像装置の内部パラメータと、初期時刻におけるカメラポーズである初期カメラポーズと、前記撮像装置に対する前記撮像物体の位置関係と、前記撮像物体の大きさを表す値とから、初期時刻における前記撮像物体の３次元座標である初期３次元座標を算出する初期値算出手段、
   前記入力画像列のうち初期時刻における画像である初期時刻画像から前記撮像物体の特徴点の画像における座標である特徴点座標を抽出し、前記初期カメラポーズと初期３次元座標とから前記特徴点座標に対応する初期３次元座標を算出する初期値決定手段、
   前記入力画像列のうち初期時刻画像から順に、現時刻における画像である現時刻画像と、前時刻における画像である前時刻画像とそれぞれの前記撮像物体の特徴点座標を対応付ける特徴点追跡手段、
   前記現時刻画像内の特徴点座標と、前記特徴点追跡手段が対応付けた現時刻画像と前時刻画像とそれぞれの特徴点座標と、前時刻画像内の特徴点の３次元座標とから、現時刻におけるカメラポーズを算出するカメラポーズ推定手段、
   初期時刻から現時刻に亘るカメラポーズと特徴点座標とから、現時刻の撮像画像内の特徴点各々の３次元座標を算出して更新する３次元座標算出手段、
   現時刻画像から予め定められた一定時刻前までの撮像画像列に亘り、前記カメラポーズ推定手段が算出したカメラポーズと、前記３次元座標算出手段が算出した特徴点の３次元座標と、前記特徴点追跡手段により対応付けられた特徴点座標とから予め定められた誤差関数を最小化する特徴点の３次元座標及びカメラポーズを算出する最適化処理手段
   として機能させ、
   前記初期値算出手段は、
   撮像物体として予め定められた半径を有する球体が平面上に配置された近似撮像対象を、前記撮像装置が予め定められた位置関係から観測するものとして、初期時刻における初期３次元座標を算出し、
   前記初期値決定手段は、
   前記初期３次元座標に基づいた距離画像において特徴点に対応する点を抽出し、抽出した点の距離を用いて特徴点の初期値３次元座標を算出する
   ことを特徴とする位置姿勢計測プログラム。
9. 撮像装置から入力される少なくとも１枚の入力画像列から、該撮像装置の位置及び姿勢を示すカメラポーズと、該撮像装置により撮像される撮像物体の３次元座標を逐次計測する位置姿勢計測装置が有するコンピュータを、
   予め設定された前記撮像装置の内部パラメータと、初期時刻におけるカメラポーズである初期カメラポーズと、前記撮像装置に対する前記撮像物体の位置関係と、前記撮像物体の大きさを表す値とから、初期時刻における前記撮像物体の３次元座標である初期３次元座標を算出する初期値算出手段、
   前記入力画像列のうち初期時刻における画像である初期時刻画像から前記撮像物体の特徴点の画像における座標である特徴点座標を抽出し、前記初期カメラポーズと初期３次元座標とから前記特徴点座標に対応する初期３次元座標を算出する初期値決定手段、
   前記入力画像列のうち初期時刻画像から順に、現時刻における画像である現時刻画像と、前時刻における画像である前時刻画像とそれぞれの前記撮像物体の特徴点座標を対応付ける特徴点追跡手段、
   前記現時刻画像内の特徴点座標と、前記特徴点追跡手段が対応付けた現時刻画像と前時刻画像とそれぞれの特徴点座標と、前時刻画像内の特徴点の３次元座標とから、現時刻におけるカメラポーズを算出するカメラポーズ推定手段、
   初期時刻から現時刻に亘るカメラポーズと特徴点座標とから、現時刻の撮像画像内の特徴点各々の３次元座標を算出して更新する３次元座標算出手段、
   現時刻画像から予め定められた一定時刻前までの撮像画像列に亘り、前記カメラポーズ
   推定手段が算出したカメラポーズと、前記３次元座標算出手段が算出した特徴点の３次元座標と、前記特徴点追跡手段により対応付けられた特徴点座標とから予め定められた誤差関数を最小化する特徴点の３次元座標及びカメラポーズを算出する最適化処理手段
   として機能させ、
   前記初期値算出手段は、
   撮像物体として予め定められた複数の半径を有する球体のうち１つが平面上に配置された近似撮像対象を、前記撮像装置が予め定められた位置関係から観測するものとして、初期時刻における初期３次元座標を算出する処理を、前記複数の半径を有する球体に亘って行って初期３次元座標候補を算出する
   ことを特徴とする位置姿勢計測プログラム。