位置計測装置及び位置計測方法
【課題】 動画の複数フレーム画像の特徴点の対応点に対して推定処理を適用して三次元計測を行う装置・方法について、高速化、高精度化を実現する。
【解決手段】 動画の各フレーム画像から抽出された特徴点の対応点を用い、ステレオペアの選定、相互標定、バンドル調整の各過程を経て位置情報を算出する装置・方法において、各過程に二段階の処理を設け、第１の処理部（５Ａ、６Ａ）にロバスト推定を適用して演算パラメータの推定値を求め、第２の処理部（５Ｂ，６Ｂ）で大きな残差の対応点やステレオペアを除外し、再び推定演算を行って三次元計測を行う。
【解決手段】 動画の各フレーム画像から抽出された特徴点の対応点を用い、ステレオペアの選定、相互標定、バンドル調整の各過程を経て位置情報を算出する装置・方法において、各過程に二段階の処理を設け、第１の処理部（５Ａ、６Ａ）にロバスト推定を適用して演算パラメータの推定値を求め、第２の処理部（５Ｂ，６Ｂ）で大きな残差の対応点やステレオペアを除外し、再び推定演算を行って三次元計測を行う。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は位置計測装置及び位置計測方法に関する。詳しくは、対象物に対して撮影装置が相対的に移動して得られた対象物の動的画像から、撮影装置又は対象物の三次元座標を得る、位置計測装置及び位置計測方法に関する。
【背景技術】
【０００２】
対象物に対して撮像装置が相対的な移動をしながら対象物を連続的に撮影し、得られた画像を用い写真測量の手法により自己位置や対象物の位置を計測する技術がある。これは、ある距離を隔てて撮影した複数の撮影画像間の対応点を追跡（トラッキング）し、その中の２枚の撮影画像からステレオペアを組み、モデル画像を作成し、これらの複数のモデル画像をもとにバンドル調整をおこない最終的な三次元計測結果を得るものである。このような技術として、例えば、特許文献１のような技術が開示されている。
【０００３】
【特許文献１】特願２００５−３５５４７０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
上記従来の技術においては、得られた撮影画像から特徴点を抽出し、撮影画像前後に取得された複数の撮影画像についてリアルタイムに前記特徴点に対応する候補対応点をトラッキングし、その結果から対応点を確定し、確定された対応点を用いて撮影位置又は対象物の座標を演算する構成となっている。この過程において、後述するように、ステレオペアを組んだ２枚の撮影画像の各画像が満足すべき条件（共面条件、共線条件、縦視差除去法等）を満たすようにモデル座標系を求める相互標定、さらには、撮影装置の移動工程を求めるため複数のモデル座標を調整するバンドル調整等の操作を行うが、各処理・計算工程は、何れも、誤差を含む多数の観測データから最尤の座標値や座標変換パラメータを求める演算を伴う。従来、これらの演算では、最小二乗法が用いられることが多かった。
【０００５】
しかしながら、最終的に撮影装置の自己位置や撮影対象物の三次元座標を得るときに、特徴点に誤対応点があると、三次元座標が正確に求まらないだけでなく、場合によっては計算過程で解が収束せず、その結果、トラッキングができなくなる場合もある。
トラッキング部でのエラーは、例えば、誤対応点や計算に適切でない対応点（例えば、微小に動いている点（木の葉、その他）、擬似対応点：見かけ上一致しているような点）が含まれていた場合、その後段での相互標定演算等において、計算精度が悪化し、最悪の場合、解が収束しなくなる。さらに、それらが多く含まれているステレオペアを選択した場合も同様である。解の収束性が悪くなると演算速度が低下する。仮にモデル座標が計算でき、その後のバンドル調整に進んだ場合にそれらに誤差がまだ含まれていた場合、大誤差除去をしきれずに精度が不確かになる場合がある。また誤差が多く残っていたり、あるいは誤差の大きい点がいくつか残っていると、結果的に最小二乗法等の推定計算を行うので、収束するのに時間がかかりリアルタイムで処理できない、などの問題があった。
【０００６】
そこで、本願発明は、高速でかつ高精度に自己位置または対象物の三次元計測ができる装置および方法を提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記課題を解決するために、請求項１に記載の位置計測装置は、例えば、図１に記載のように、撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段（２）から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出部（３）と、複数の前記撮像画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡部（４Ａ）と、前記特徴点追跡部（４Ａ）の追跡結果から、一つの撮影画像フレームと他の一つの撮影画像フレームを選択し、当該２つの撮影画像フレーム間の各々の前記対応点の任意の複数の点を元に相互標定を行い、縦視差を求め、求められた縦視差に対してロバスト推定を行い、大きな誤差の影響を少なくして相互標定のパラメータの推定値を求める第１の相互標定処理部（５Ａ）と、前記第１の相互標定処理部（５Ａ）で推定された相互標定のパラメータに基づき、各対応点の前記２つの画像フレーム間で相互標定を行い、前記特徴点の縦視差を算出し、その縦視差に対して閾値処理を施し、誤対応点の除外又は撮影画像フレームの適否の判断を行う第２の相互標定処理部（５Ｂ）と、前記複数の撮影画像フレームを用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段（２）の撮影位置、又は、撮影姿勢を計測する位置計測部（７）とを備える。
【０００８】
ここにおいて、ロバスト推定とは、推定値の間に大きな誤差を含むものが観測データに含まれる場合にも比較的容易に推定が行えるもので、例えば、ＬＭｅｄＳ法、メディアン推定（Ｍ推定）法，ランザック（RANSAC）法である。閾値処理とは、予め設定した閾値と比較して、その大小によりデータを取捨選択する処理をいう。このように構成すると、各推定演算処理の前に大きな誤差を除去してあるので、演算の収束が速く、高精度な結果が得られる。
【０００９】
請求項２に記載の発明は、請求項１において、例えば図１のように、ＧＰＳデータから撮像画像取得手段の位置のデータ得るＧＰＳ位置データ取得部（１２）を備え、取得した位置データから十分な基線長を有するものから前記ステレオペアを選択するものである。このように構成すると、単にランダムにステレオペアを選択する場合に比べて、適切なステレオペアを的確に選定することができ、計測結果の収束時間を短くできる。
【００１０】
請求項３に記載の位置計測装置は、例えば、図１に記載のように、撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段（２）から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出部（３）と、複数の前記撮影画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡部（４Ａ）と、前記特徴点追跡部（４Ａ）の追跡結果から、複数の撮影画像フレームを選択し、当該複数の画像フレーム間の各々の前記特徴点の座標位置、または各フレームの撮影位置にバンドル調整処理を施し、得られた特徴点の座標位置データまたは、各フレームの撮影位置の残差に対してロバスト推定を行い、それらの推定データを求める第１のバンドル調整処理部（６Ａ）と、前記、第１のバンドル調整処理部（６Ａ）で推定された推定データに基づき、それらの閾値を決定し、決定された閾値により、誤対応点の除去又は撮影画像フレーム選択を行う選択・除去処理部（６Ｂ）と前記適切と判断された特徴点およびフレームを用いた前記バンドル調整結果を用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段の撮影位置、又は、撮影姿勢を計測する位置計測部（７）とを備える。
【００１１】
このように構成すると、複数のフレーム画像に基づいて特徴点、フレームの評価を行うので不適な特徴点やフレームを最終的なバンドル調整の前に除去することができる。また、各推定演算処理の前に大きな誤差を除去してあるので、演算の収束が速く、高精度な結果が得られる。
【００１２】
請求項４に記載の発明は、請求項３において、例えば図１のように、ＧＰＳデータから撮影画像取得手段の位置のデータを得るＧＰＳ位置データ取得部（１２）を備え、取得した位置データから前記撮影画像フレームを選択する。このように構成すると、単にランダムにステレオペアを選択する場合に比べて、撮影位置を的確に選定することができ、初期条件を推定値に近い値にすることができ計測結果の収束時間を短くできる。
【００１３】
請求項５に記載の発明は、請求項１乃至請求項４のロバスト推定が、ＬＭｅｄＳ法、メディアン推定（Ｍ推定）法，ランザック（RANSAC）法のいずれか一つであることを明示したものである。このように構成すると、各推定演算処理の前に大きな誤差を簡単な処理で除去できるので、演算の収束が速く、高精度な結果が得られる。
【００１４】
請求項６記載の発明は、請求項１に対応する方法の発明であり、例えば、図１、図４（主に、図４）に記載のように、撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段（２）から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出工程（３）と、複数の前記撮影画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡工程（４Ａ）と、前記特徴点追跡工程（４Ａ）の追跡結果から、一つの撮影画像フレームと他の一つの撮影画像フレームからなるステレオペアを選択し（Ｓ４２１）、当該２つの撮影画像フレーム間の各々の前記対応点の任意の複数の点を元に相互標定を行い（Ｓ４２３）、縦視差を求め、求められた縦視差に対してロバスト推定を行い、大きな誤差の影響を少なくして相互標定のパラメータの推定値を求める（Ｓ４２４，Ｓ４２５）、第１の相互標定処理工程（Ｓ４２）と、第１の相互標定処理工程（Ｓ４２）で推定された相互標定のパラメータに基づき、２つの画像フレーム間で相互標定を行い（Ｓ４３０、Ｓ４４１）、特徴点の縦視差を算出し（Ｓ４３１、Ｓ４４２）、その縦視差に対して閾値処理を施し（Ｓ４３２、Ｓ４４３）、誤対応点のを除外又は撮影画像の適否の判断を行う第２の相互標定処理工程（Ｓ４３、Ｓ４４）と、前記複数の撮影画像フレームを用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段の撮影位置、又は、撮影姿勢を計測する位置計測工程（Ｓ４５）とを；備える。
【００１５】
このように構成すると、各推定演算処理の前に大きな誤差を除去してあるので、演算の収束が速く、高精度な結果が得られる。
【００１６】
請求項７記載の発明は、請求項３に対応する方法の発明であり、例えば、図１、図６（主に、図６）に記載のように、撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段（２）から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出工程（３）と、複数の前記撮影画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡工程（４Ａ）と、前記特徴点追跡工程の追跡結果から、複数の撮影画像フレームを選択し（Ｓ６７１）、当該複数の画像フレーム間の各々の前記特徴点の座標位置または、各フレームの撮影位置にバンドル調整処理を施し（Ｓ６７４）、得られた特徴点の座標位置データまたは、各フレームの撮影位置の残差に対してロバスト推定を行い（Ｓ６７５，Ｓ６７６）、それらの推定データを求める第１のバンドル調整処理工程（Ｓ６７）と、前記、第１のバンドル調整工程（Ｓ６７）で推定された推定データに基づき、それらの閾値を決定し、決定された閾値により、誤対応点を除去又は撮影画像フレーム選択を行う選択・除去処理工程と（Ｓ６８）、適切と判断された特徴点およびフレームを用いたバンドル調整結果を用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段（２）の撮影位置、又は、撮影姿勢を計測する位置計測工程（Ｓ６９）とを備える。
【００１７】
このように構成すると、複数のフレーム画像に基づいて特徴点、フレームの評価を行うので不適な特徴点やフレームを最終的なバンドル調整の前に除去することができる。また、各推定演算処理の前に大きな誤差を除去してあるので、演算の収束が速く、高精度な結果が得られる。
【発明を実施するための最良の形態】
【００１８】
以下に図面に基づき本発明の実施の形態について説明する。
【００１９】
［第１の実施の形態］
図９は本実施の形態における概念を説明するための図である。自動車にカメラ２を装着し、自動車の位置を少しずつ変えて対象物である市街地を撮影し、これら複数の撮影画像における特徴ある目印（特徴点１０１ａ、１０１ｂ：例えば、特徴あるビルの特定の点等）のトラッキング結果から、カメラ２の位置座標すなわち自動車の軌跡を求める例である。これによりカーナビゲーションに自動車の位置を連続的に表示可能になるが、ＧＰＳ電波を受信できない区間で補完的に利用される意義も大きい。図のような自動車にカメラ２（以下、撮影画像取得部２ということがある）を搭載した場合、撮像画面が左右／上下、或いはある方向に回転するなど揺動する。また、その撮像画像には他の自動車、人など動くものが含まれることも多い。これらは全て誤差の要因となる。
【００２０】
[ハードウェアおよびソフトウェアモジュール構成]
図１に本実施の形態における位置計測装置１００の構成例を示す。図１は、ハードウェアと、それに搭載されるソフトウェアモジュールを模式的に示したものである。図において、操作部１は位置計測装置１００の各部を制御して、位置計測装置として機能せしめる部分であり、具体的には、撮影画像取得部２への撮影開始・停止の指示、特徴抽出部３の設定変更、トラッキング部４の設定変更、開始、停止の指示、相互標定部５、バンドル調整部６への処理開始指示、標定実行指示等を行う。
撮影画像取得部２は動画像等の撮影画像を順次取得する部分であり、典型的にはビデオ撮影カメラである。撮影画像の取得の他に特徴抽出部３への出力、動画像メモリ１０への撮影画像の保存等を行う。なお、保存データは撮影した全フレームであってもよいし、間引きしたデータであってもよい。また、位置計測装置１００は撮影画像取得部２を持たず、外部の撮影装置からケーブルや無線等の通信手段により撮影画像を取得しても良い。
【００２１】
特徴抽出部３は順次取得した少しずつ異なる撮影画像から画像内の特徴点を抽出するもので、撮影画像取得部２より入力された撮影画像から特徴点の抽出を行い、得られた各特徴点の周囲の画像の濃淡等の情報をトラッキング部４、及び相互標定部５へ出力する。
トラッキング部４は特徴抽出部３より入力された特徴点の濃淡情報等から別のフレーム画像をサーチして、その点の周囲がほぼ同じ濃淡情報を持つ点（対応点）を捜索し、特徴点の追跡を行う。又、このトラッキングの他に、トラッキング結果の相互標定部５への出力、実行開始指示、候補対応点の配置の判断と特徴抽出部３への特徴点の新設指示等を行う。
尚、以降の説明で、「特徴点」とは、特徴点抽出部から得られた点をいい、「対応点」とは、特徴点のうち、二つの以上のフレーム画像間で撮影対象が同一であると確認された特徴点どうしの互いに対応する点を言う。
【００２２】
図１において、撮影画像取得部２、特徴抽出部３、トラッキング部４における処理は、典型的には、画像データが入力された時にリアルタイムに処理を行う部分であり、次に述べる、相互標定部５、およびバンドル調整部６は、典型的には、前記特徴抽出部３、トラッキング部４で得られた複数画像にわたるデータをメモリに蓄積の上処理するもので、非リアルタイム処理である。但し、必要に応じて、前記トラッキングの処理を非リアルタイム処理としてもよいし、相互標定部５、およびバンドル調整部６の処理をリアルタイムで行うことも勿論可能である。
【００２３】
これらの処理の基準となる特徴点の情報は特徴点情報メモリ９に記憶されている。このメモリには、各フレームの特徴点の座標値、他のフレームの特徴点との対応関係（対応点）等の情報が処理の過程で随時書込み／消去できるようになっている。１０は撮影画像を記憶する動画メモリである。位置計測部７は、処理された特徴点（又は対応点）の座標値や外部標定要素等により最終的な三次元データを出力するものである。この結果は、表示部８や外部出力部１１に出力される。
表示部８は、例えば、三次元計測された対象物の画像を例えば立体的に表示する。
【００２４】
[全体のフロー]
図２に第１の実施の形態における画像処理方法の全体フロー例を示す。尚。各部分処理の詳細については後述する。まず撮影した動画データを順次取り込む（Ｓ１０）。これらは一連のフレーム画像である。次に、トラッキング処理を行う（Ｓ２０）。即ち、取り込んだ動画の一つのフレームの画像内で特徴点を抽出し、その特徴点の近傍を含む濃淡等のパターンを保存する。ここで、このフレーム画像内の特徴点のパターンをテンプレートとして、別のフレームの画像で同様のパターンの部分を求め対応点を求める。次に、このようにして求めた複数の対応点について適正な対応点か否かの評価を行い、誤対応点を特徴点情報メモリ９から除外かするか、又はその旨の情報を書き込む（以下単に「除外等」と言う）。Ｓ２０の処理は、典型的には、リアルタイム処理であり、入力されるフレームについて順次処理を行い、その都度対応点を保存し、誤対応点か否かの評価の結果で特徴点情報メモリ９の書き換えを行う。尚、入力されるフレームについては適宜間引いて処理することも出来ることは勿論である。以上により計測の基準点となる対応点（仮対応点）を決定する（Ｓ３０）。ここで、「仮」としたのは、以降の処理（相互標定、バンドル調整等）において対応点の適正性の再評価の結果、除外されることがあるからである。
【００２５】
次に、Ｓ４０において、相互標定処理を行い、２つの画像データの対応点の間で各座標軸を合わせる。即ち、相互標定のための適切なステレオペアを選択し、Ｓ３０で決定された対応点を元にそれらの間で例えば共面条件を満足するように画像の座標軸を調整して立体像が得られるようにする。この相互標定過程で判明する不適切な対応点があれば特徴点情報メモリを書き換えそのデータを除去する。この結果、充分な基線長を有する等、三次元座標決定に適切なステレオペアを決定する（Ｓ５０）。
【００２６】
次に、連続したモデル画像を得るため接続標定を行なう（Ｓ５５）。これは、各モデル間の傾き、縮尺を統一して同一座標系とする処理である。そして、画像上に含まれる、タイポイントやパスポイントを用いて、各画像の外部標定要素を最小二乗法により同時に決定する連続したモデル画像を得るためバンドル調整処理を行う（Ｓ６０）。ここでも、この処理の過程で判明する大きな誤差を与える対応点の除外等を行い、その後最終的なバンドル調整を行い、撮影画像取得部２の位置座標、姿勢、等の外部標定要素、および三次元座標を求める（Ｓ７０）。
以下、各処理について、更に詳細に説明する。
【００２７】
［トラッキング］
図３Ａにトラッキング処理部４の処理内容を示す。処理内容を図１を併用して説明する。図１に示すように、まず、撮影画像取得部２において撮影画像を取得する。自己の撮影装置で画像を撮影して取得しても良く、他の撮影装置で撮影した画像を通信回線やメモリ、テープ等を介して取得しても良い。次に、少しずつ変化する撮影画像を順次特徴抽出部３に供給する。本実施の形態では自動車にカメラ２を装着し、移動しながら撮影するので、少しずつ変化する撮影画像とは、時間的、従って、空間的に少しずつ変化する撮影画像であり、隣接するフレーム（或いは、ある枚数だけ間引いたフレーム）の画像の大部分で対象物が共通に含まれる。但し、カメラ２は自動車だけでなく、人等が持って歩いたり、自転車に載せて撮影したりしてもよい。
【００２８】
[特徴点抽出]
特徴抽出（図３ＡのＳ２１）は特徴抽出部３において行なわれる。典型的には初期フレームでは全画面から抽出を行い、次のフレームからは、初期フレームと重複しない新たな画面領域から抽出が行われる。あるいは、あるタイミングで、例えばテンプレートの点数が３〜５割減ったら、特徴抽出を行い、新たな特徴点を発生させる等の処理を行い、テンプレートを更新しながらトラッキングしてもよい。すなわち毎回特徴点を発生させる必要は必ずしもない。特徴点の抽出には、例えばＭＯＲＡＶＥＣオペレータ（Ｈ． Ｐ． Ｍｏｒａｖｅｃ． Ｔｏｗａｒｄｓ Ａｕｔｏｍａｔｉｃ Ｖｉｓｕａｌ Ｏｂｓｔａｃｌｅ Ａｖｏｉｄａｎｃｅ． Ｐｒｏｃ． ５ｔｈ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｉｎｔ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ａｒｔｉｆｉｃｉａｌ Ｉｎｔｅｌｌｉｇｅｎｃｅ， ｐｐ． ５８４， １９７７．）や、Ｈａｒｉｓｓ，Ｐｒｅｓｓｙ，Ｓｕｓａｎなどのオペレーターを適宜採用できる。
【００２９】
特徴抽出オペレータは、どのようなものを利用しても、画像上のわずかなノイズ（エッジ上のノイズなど）に反応しやすいという問題がある。この性質の改善のために、特徴抽出オペレータを使用する前に画像のノイズ除去処理を平均値フィルターなどのノイズフィルターを用いて行う。
【００３０】
また、撮影対象によっては、画像上のある部分（木や芝など）に特徴点が集中することがある。これは、画面の一部の情報から全体を評価することになり、後述のテンプレートマッチングなどに悪影響を及ぼすことがある。これを避けるために選点処理を行なう。更に、特徴点の最大数が予め指定される場合には、画像全体に均等に配置されるようにし、以降の処理の相互標定等を確実にできるようにする。このように、特徴点抽出に関しては、種々の処理を経て実行されるものであるが、本願発明とは直接関係がないのでこれ以上の説明は省略する。
【００３１】
[対応点追跡]
図３Ａを参照して説明を続ける。追跡処理（Ｓ２２）は、特徴点追跡部４で行われる処理で、特徴抽出処理により選点された各特徴点に対応する点をフレーム間に渡って追跡処理する。すなわち、特徴点に対応する別の画像の中の候補対応点を求める処理である。具体的には、隣接撮影画像について周囲の画像パターンが前記特徴点の周囲の濃淡パターンと近似している部分のサーチ（テンプレートマッチング）を行い、１つの特徴点に対応する候補対応点を求める。テンプレートマッチングとして、例えばＳＳＤＡ法（逐次残差法）などが使用できる。ＳＳＤＡ法（逐次残差法）とは、対応する点の濃淡等の差を用いてパターンの類似度を決定するもので、比較範囲（テンプレートサイズ）内の濃淡の差が最小となるような位置を候補対応点として求めるものである。また、テンプレートマッチングとして正規化相関法などの他の方式も採用することもできる（例えば、秋山実著、「写真測量」、株式会社山海堂発行、ＰＰ２３４−２３５、参照）。テンプレートマッチングには、テンプレートサイズと探索範囲を最適に選択することが重要であり、探索範囲については、ビデオカメラ２のフレームレート、移動速度などを基に最適な設定とする。
【００３２】
テンプレートマッチングより、各特徴点の候補対応点が求められるが、ミスマッチングも多く発生する。例えば、ＳＳＤＡテンプレートマッチング自体のエラー、撮影画面中に、走行中の自動車、飛鳥、落葉などの動くものに特徴点が付与された場合、またカメラの揺動が激しい場合などミスマッチングによる誤対応点が生じ得る。このような誤対応点は以降の相互標定、バンドル調整等で誤差の要因となり、処理時間の増加の原因となるので極力除外しておく必要がある。
【００３３】
Ｓ２３〜Ｓ２５は、求められた対応点から上記誤対応点を抽出し、対応特徴点から除外するもので、Ｓ２３の第１の追跡判断工程、Ｓ２４の第２の追跡判断工程と、Ｓ２５の再テンプレートマッチングで構成される。第１の追跡判断部（Ｓ２３）はトラッキング結果からの画面全体の動きから画面間の座標変換射影パラメータ（以下、単に「射影パラメータ」と言う）を推定するものである。具体的には、図３Ｂを参照して説明する。第２の追跡判断工程（Ｓ２４）は、Ｓ２３の結果を受けて処理するもので、第１の追跡判断工程で推定した射影パラメータを基に、各対応点の評価を行い誤対応点を特徴点情報メモリ９から除外等するもので、具体的には図３Ｃを参照して説明する。再テンプレートマッチング（Ｓ２５）は、誤対応点を除いた対応点について再度テンプレートマッチングを行い、その結果より再度対応点を評価し、不適当な対応点を更に除外するものである（Ｓ２６）。具体的な内容については図３Ｄを参照して説明する。
【００３４】
Ｓ２７は、これらの処理を繰り返して、ある一定以上のフレーム数の対応点修正が終了したら、次のＳ３０へ、仮の対応点として出力する。ここでのフレーム数は、システムの処理能力、速度によって適宜決める。
【００３５】
図３Ｂに前記第１の追跡判断工程の処理内容を示す。まず典型的な処理内容を説明し、その後、その他の具体例を説明する。
本処理は、一つの撮影画像フレーム（フレームＡと言う）と他の一つの撮像フレーム（フレームＢと言う）の２つのフレームに関する処理である。先ず、Ｓ２２でのテンプレートマッチングで求められた候補対応点のうちから複数点の組をランダムに選定する。組の数は次の処理（Ｓ２３２）の射影パラメータの数以上とする。尚、選定する候補対応点は誤差を少なくするため、画面内の一部に局在せずフレーム全体に渡って選択するのが望ましい。次に、フレームＡ、Ｂ間の射影変換行列の要素（射影パラメータ）を求める。射影変換として種々のものがあるが、通常、アフィン変換やヘルマート変換が用いられる。ここでは、並進変換と回転変換の双方が比較的取り扱いやすいアフィン変換の例で説明する。
【００３６】
対応するフレームの座標値を（ｘ’，ｙ’），（ｘ”，ｙ”）とすると、アフィン変換は以下の６個の射影パラメータを使って表される。
ｘ’=ａｘ”+ｂｙ”+ｃ （１ａ）
ｙ’=ｄｘ”+ｅｙ”+ｆ （１ｂ）
上記、ａ〜ｆの６個の射影パラメータは選定した６組以上の対応点の組の座標から求める。このパラメータと式（１）を用い、上記選定された対応点について一方を固定して他方の座標値（予想座標）を求める。次に、各対応点について、求められた予想座標と実際の対応点の座標との残差を求める（Ｓ２３３）。この残差は例えば、ｘ軸、ｙ軸の二つの座標点の距離の差としても良い。対応点が全て正しく認識されていればこの残差は非常に小さくなるはずである。上記選定した６組以上の対応点の中には誤対応点や誤対応点とまでは言えないが大きな誤差を含むものがある。このような対応点を選定すると正しい射影パラメータが求まらない。そこで、上記残差を基に選定対応点の評価を行う。本実施の形態として、以下の残差のメディアン値（中央値）を用いるものを説明する。メディアン値は周知の通り、複数のデータを大きさの順にソートし、その中心にあるもののデータの大きさであり、例えば、５個のデータがあるときは３番目のデータの値である。メディアン値で母集団の全体的な傾向を把握することができる。メディアン値は演算としてデータのソートを行えばよく、また、分散値の大きな観測データを無視するので、大きな残差を持つデータの影響を受けにくいという利点がある。本実施の形態では、上記各対応点の残差からメディアン値の大きさを求め（Ｓ２３４）、この値が所定の値（ε１）であるか判断する。尚、メディアン値の大きさとして、その２乗を用いてもよい。所定の閾値（ε１）より大きいときは、選定した対応点が不適切であるのでＳ２３１に戻り、新たな対応点を選定する。新たな対応点に関して同様の処理を行い、メディアン値の大きさが所定の閾値（ε１）以下になればその時の射影パラメータが求められる（Ｓ２３６）。この際の閾値は、例えば、１pixel（画素）に設定する。
【００３７】
また、本実施の形態では、メディアン値の大きさ、またはその２乗が最小となるようなアルゴリズム（ＬＭｅｄＳ法）について説明したが、これに限定されるものではなく、誤差の影響が小さい所謂ロバスト推定法（例えば、ランザック法、メディアン推定法（Ｍ推定法））を使用しても同じ効果が得られる。これらの推定法は周知であるのでここでは説明を省略する。
【００３８】
図３Ｃは、第２の追跡判断部の処理内容を説明したものである。前述したＳ２３の処理（図３Ｂ参照）で求められた射影パラメータから、フレームＡの各特徴点に対応するフレームＢにおける対応点を計算して予測値とし、全対応点について、各々の対応点のフレームＢにおける座標値との残差を求める。次に、各々の対応点について残差値の大きさが予め定めた閾値より大きなものと比較する閾値処理（Ｓ２４３）を行い、閾値より大きなものは誤対応点として除外等する（Ｓ２４４）。
【００３９】
図３Ｄは、Ｓ２５の再テンプレートマッチングの内容を示したものである。第１の追跡判断部で求めた射影パラメータを用い、誤対応点と判断された点について各対応点の推定座標を求め（Ｓ２３７）、この座標点付近について再度テンプレートマッチングを行う（Ｓ２３８）。この結果、推定座標から所定の距離以内の対応点が発見された場合はこの点を対応点として特徴点情報メモリ９に追加し、所定の距離より離れた点でマッチングした場合、または、マッチング点が発見できない場合は不適切な対応点として特徴点情報メモリ９から除外等する。但し、当該対応点情報が特徴点情報メモリ９にない場合はそのままとする。以上のように、第２の追跡判断部で除外されなかった対応点と、再テンプレートマッチング（Ｓ２５）で新しく求めた対応点をあわせて新しく対応点とする。
【００４０】
以上のトラッキング処理については、撮影した画像をリアルタイムに処理する例について説明したが、撮影した画像をメモリ等に蓄積しておき、撮影後その画像データを処理する非リアルタイム処理とすることも出来ることは勿論である。また、撮影した全フレームについて処理することなく、適当に間引くことも出来る。間引く画面については撮影装置の移動速度、他の手段からの位置データから適応的に決定することもできる。
【００４１】
[ロバスト推定を用いた相互標定による誤対応点除去およびステレオペアの選択ステレオペアの選定]
図４に具体的な処理な処理内容を示す。本処理は、ロバスト推定を用いた相互標定処理を行うことにより、誤対応点の除去を行い、さらに三次元計測に使用できる適切な視差の異なるフレームのペア（ステレオペア）を求めるもので、前記トラッキング処理で求めた対応点を入力データとして行う。先ず、ある程度の長さの基線長が得られるステレオペアの候補を複数選定する（Ｓ４１）。例えば、あるフレーム上で観測された特徴点の、以降のフレーム上で配置を調べ、十分な移動があった場合、あるいは配置が画面の片側に偏っているフレームをステレオペア候補として複数組選択する。これらは、フレーム数やスピード等から、あるいはトラッキングしている点の軌跡等を利用する。
次に第１の相互標定処理部（Ｓ４２）で前記ステレオペアの候補からランダムにステレオペアを選択し、２つの撮影画像フレーム間の各々の対応点の任意の複数の点を元に相互標定を行い、縦視差を求め、求められた縦視差に対してロバスト推定を行い、大きな誤差の影響を少なくして相互標定のパラメータの推定値を求める。
誤対応点の除去を行う場合は、第２の相互標定処理部（Ｓ４３）で、第１の相互標定処理部で推定された推定相互標定のパラメータ（外部標定要素）に基づき、再度相互標定を行い、特徴点の縦視差を算出しなおし、その縦視差に対して閾値処理を施して立体視が十分得られるステレオペアを選定し、第２の相互標定処理部で選定したステレオペア中の対応点の評価を行い、対応点として相応しくない点を誤対応点として除外等する（Ｓ４３）。次に、Ｓ４４で前記誤対応点を以外の対応点を使って相互標定（具体的内容は後述する）を実施し、選ばれたステレオペアの再評価を行い、相応しい場合はステレオペアとして採用し、相応しくない場合は再び第１の相互標定処理部（Ｓ４２）に戻り、別のステレオペアを選定し、同様の処理を行う。このように、第１の相互標定処理部で適切な相互標定パラメータ値を推定し、不適なステレオペアの排除、第２の相互標定処理部で誤対応点を除外しておき、その後の演算（Ｓ４４）で不適なステレオペアの排除を行う。本処理（Ｓ４１〜Ｓ４５）により適正なステレオペアの選択を行い、誤対応除去を行っておくことでこの段階での最適な相互標定パラメータ（外部標定要素）と対応点が得られる。
【００４２】
相互標定はステレオペア間の座標軸の相対的位置と傾きを撮影時と相似の関係にセットする処理であり、相互標定が完了すると、立体視可能なステレオ画像が作成できるようになる。この時、二つの画像間で２つの画像に写っている同一点の光路はすべて１直線上に乗るようになる（２点からできる空間直線が同一平面に含まれる：共面条件）。実際には不完全性のため直線上からずれる。このずれを縦視差といい、相互標定の完全性の評価尺度となる。逆に言えば、縦視差がなくなるように二つの画像間で座標軸および座標値を変換すれば、共面条件を満足するようになり、適切なステレオペアが得られる。
【００４３】
具体的な相互標定の手順について簡単に説明する。図７は相互標定の説明図である。相互標定では、以下の共面条件式により各パラメータを求める。
【数１】
図７に示すように、モデル座標系（撮影対象物の座標系）の原点を左側の投影中心にとり、右側の投影中心を結ぶ線をＸ軸にとるようにする。縮尺は、基線長を単位長さにとる。このとき求めるパラメータは、左側のカメラＯ１のＺ軸の回転角κ１、Ｙ軸の回転角φ１、右側のカメラＯ２のＺ軸の回転角κ２、Ｙ軸の回転角φ２、Ｘ軸の回転角ω２の５つの回転角となる。この場合左側のカメラＯ１のＸ軸の回転角ω１は０なので、考慮する必要がない。このような条件にすると、式２の共面条件式は式３のようになり、この式を解けば各パラメータが求まる。
【数２】
【００４４】
ここで、モデル座標系ＸＹＺとカメラ座標系ｘｙｚの間には、次に示すような座標変換の関係式が成り立つ。
【数３】
【００４５】
これらの式を用いて、例えば、次の手順により、未知パラメータを求める。
(1)初期近似値は通常０とする。
(2)共面条件式（２）を近似値のまわりにテーラー展開し、線形化したときの微分係数の値を式（４）、式（５）により求め、観測方程式をたてる。
(3)推定法をあてはめ、近似値に対する補正量を求める。
(4)近似値を補正する。
(5)補正された近似値を用いて、(2)〜(5)までの操作を収束するまで繰り返す。
【００４６】
次に、第１の相互標定処理部（Ｓ４２）の内容について図４を参照して詳しく説明する。この処理は、選定したステレオペアである画像フレーム間で相互標定を実施することにより適切な相互標定パラメータを推定するもので、その推定パラメータより、次の第２の相互標定処理部にて相互標定を行い（Ｓ４３０）、の誤対応点の検出／除外を行う。そしてその結果から再度相互標定を行い（Ｓ４４１）、最適なステレオペアかどうか評価選定する。誤対応点は前述のトラッキング処理（Ｓ２０）において既に除外されているが、あくまでも二次元的な処理であり、不適な対応点が除外されていないものがあるので相互標定処理を行い、縦視差を評価することで、擬似的に三次元的な評価を行い、さらに適切な対応点だけを残すようにする。
【００４７】
先ず、ステレオペアの候補（Ｓ４１）から１組のステレオペアをランダムに選択する（Ｓ４１ａ）。次に、その中から対応点を複数ランダムに選定する。対応点の数は５個以上である（Ｓ４２２）。次に、この５点以上の対応点を使って相互標定を行う（Ｓ４２３）。相互標定の結果、それら対応点を使った相互標定パラメータが求められ、さらに各対応点について相互標定の誤差の指標である縦視差（相互標定後の各対応点の基線と直交するｙ座標の差）が算出できる。縦視差の大きさは対応点が適切なものかの評価尺度となり、できるだけその大きさは小さい方がよい。ここでは、この評価を行うため、各対応点について縦視差のメディアン値を求め（Ｓ４２４）、その値が予め設定した閾値ε２（第２の所定値）と比較する（Ｓ４２５）。所定値より大きい場合はＳ４１ａに戻り、新たな対応点を設定する。この際の閾値は、例えば、１pixel（１画素）程度に設定する。所定値以下の時は、この段階での最適な相互標定パラメータが推定されたとして第２の相互評定処理部に移る。
【００４８】
ここでは、メディアン値を求め、それを最小値とするアルゴリズムＬＭｅｄＳ法を用いた場合について説明したが、これに限定されるものではなく、誤差のばらつきの影響が少ないいわゆるロバスト推定法が広く使用できることは明らかである。
【００４９】
次に、第２の相互標定処理部の処理内容について説明する。ここでは、Ｓ４２の結果を受け、推定された相互標定パラメータから相互標定を行い（Ｓ４３０）、当該ステレオペアのフレーム内の各対応点の縦視差を算出する（Ｓ４３１）。対応点はすべての点であってもよいし、一部であってもよいが、画面内の大部分が望ましい。これらの縦視差に閾値処理を行い（Ｓ４３２）、閾値（ε３）以上の対応点を誤対応点として特徴点情報メモリ９から除外する。この時の閾値（ε３）は、例えば、縦視差が０．５〜１pixel程度の値に設定する。尚、この閾値は所望精度や対象物により変更してもよい。これで、ステレオペア上の不適切な誤対応点は除去されたことになる。
【００５０】
次に、適切なステレオペアであったかどうか評価を行うために、誤対応点を除いた対応点のみで再度相互標定を実施し（Ｓ４４１）、相互標定パラメータを求め、各対応点の縦視差を求める（Ｓ４４２）。この縦視差のメディアン値と所定値ε４（第４の所定値）との比較を行い（Ｓ４４３）、所定値以上の時は不適切なステレオペアであるのでＳ４１ａに戻り、新たなステレオペアを選定する。所定値より小さい時は適切なステレオペアの候補とする。尚、ここで「候補」としたのは、次のバンドルによる大誤差除去処理で不適なステレオペアとされる場合があるからである。
【００５１】
ロバスト推定法として、ＲＡＮＳＡＣ推定やＭ推定法、あるいはそれらを変形したものも利用できる。これらは、評価方法やその評価関数がＬＭｅｄＳと異なるだけで、大きな観測誤差の影響を受けにくい点で同じである。
【００５２】
尚、ステレオペアの選択を行わず、誤対応点の除去だけを行ってもよい。その場合は、Ｓ４１ａ、Ｓ４４は行う必要がない。あるいは、適切なフレーム選択として、Ｓ４３の処理だけを行わなくともよい。この場合、４１ａとＳ４２の処理が、第１の相互標定処理部として、Ｓ４４の処理のみが第２の相互標定処理部として行われる。すなわち、Ｓ４２で相互標定パラメータを推定し、Ｓ４４で最適フレームの適否を判定する。これらは、システム全体の性能として、例えば速度やトラッキング部の誤対応除去能力、対象とする景観、計測対象などにより適切な構成とする。
【００５３】
[バンドル調整１]
次に、複数のフレーム間に渡る対応点の動きに注目し、異常な動きをする対応点及びフレームを除外する。具体的には、バンドル調整により複数のフレーム画像に対し、対応点、フレーム画像の評価を行う。
【００５４】
図５にその処理内容を示す。Ｓ４０／Ｓ５０は、図２、図４で説明した相互標定処理による誤対応点の除去と最適ステレオペアの選定処理である。本処理は、これら処理を本処理の前段階の処理とし、この処理を受けて行うものである。選定されたフレームについて複数のフレームを選択（Ｓ６１）する。次に、これらのフレームについて接続標定を行う。接続標定とは、ステレオペア毎に生成される立体モデル固有の座標系（モデル座標系）を、隣り合うモデル同士と関係付け、モデル間の傾き、縮尺を統一して、統一した座標系（コース座標系）で記述する処理を言う。具体的には、隣接する画像座標において、例えば右のモデル内の点を（Ｘｊｒ，Ｙｊｒ，Ｚｊｒ）、左のモデル内の対応する点を（Ｘｊｌ，Ｙｊｌ，Ｚｊｌ）、カメラ２の座標を（Ｘ０，Ｙ０，Ｚ０）とした時、以下のΔＸｊ，ΔＹｊ，ΔＺｊ，ΔＤｊを計算し、ΔＺｊおよびΔＤｊが十分小さければ（例えば、１／２０００以下）ならば、接続標定が正常に行われたと判断する。
ΔＸｊ＝（Ｘｊｒ−Ｘｊｌ）／（Ｚ０−Ｚｊｌ） （式６ａ）
ΔＹｊ＝（Ｙｊｒ−Ｙｊｌ）／（Ｚ０−Ｚｊｌ） （式６ｂ）
ΔＺｊ＝（Ｚｊｒ−Ｚｊｌ）／（Ｚ０−Ｚｊｌ） （式６ｃ）
ΔＤｊ＝（ΔＸｊ２＋ΔＹｊ２）1/2 （式６ｄ）
【００５５】
次に、選択された各フレーム画像を元にバンドル調整を行う（Ｓ６３）。バンドル調整とは複数（３個以上）のフレーム画像と三次元座標を関係付ける方程式を立てて各フレーム画像の外部標定要素を一括して得るものである。
具体的には、投影中心、写真像および地上の対象物が一直線上にあるというバンドル調整の基本式である以下の共線条件（式７）を用いる。式７に含まれる６つの外部標定要素Ｘ０，Ｙ０，Ｚ０、ω、φ、κを求める。即ち、３点以上の基準点の対象物座標と対応する画像座標から、逐次近似解法によってこれら６つの外部標定要素を算出する。
【数４】
ここで、
c：画面距離（焦点距離）、x,y：画像座標
X,Y,Z：対象空間座標（基準点、未知点）
X0,Y0,Z0：カメラ２の撮影位置
ａｉｊ：カメラ２の傾き（３×３回転行列の要素）
Δx,Δy：カメラの内部標定補正項。
具体的には、未知変量の近似値を与え、式７を近似値のまわりにテーラー展開して線形化し、最小二乗法により補正量を求めて近似値を補正し、同様の操作をくりかえし収束解を求める逐次近似解法を用いる。これによってこれら６つの外部標定要素を求める。更に各対応点の画像座標の残差を求め、残差が所定の閾値ε５（第５の所定値）と比較し、閾値ε５以上のものについてはその対応点を除外し（Ｓ６４）、再びＳ４０〜Ｓ５０の処理に戻り最適ステレオペアの選定（具体的には、相互標定処理による誤対応点除去と最適フレームの選択）を行う。すべての対応点の残差が所定値より小さい場合は処理を打ち切り（Ｓ６４）、三次元座標と、外部標定要素を出力し（Ｓ６５）処理を終了する。
【００５６】
[バンドル調整２]
図６に、他の方法によるバンドル調整による評価方法を示す。この方法により、各特徴点の座標位置データの誤差や各フレーム撮影位置の誤差を取り除くことにより、最終的な三次元座標や外部標定要素を得る。
Ｓ２０／Ｓ３０〜Ｓ６２’までは図５の場合と同様である。Ｓ６２’の接続標定は、図５の接続標定Ｓ６２と同様の処理である。
【００５７】
次に第１のバンドル調整処理部（Ｓ６７）で、得られた特徴点の座標位置データまたは、各フレームの撮影位置の残差に対してロバスト推定を行い、それらの推定データを求め、選択・除去処理部（Ｓ６８）で、第１のバンドル調整処理部で推定された推定データに基づき、それらの閾値を決定し、決定された閾値により、誤対応点を除去又は撮影画像フレーム選択を行う。そして、最終のバンドル調整を行い（Ｓ６８２）、その三次元座標と各フレームの外部標定要素を得る（Ｓ６９）。
【００５８】
第１のバンドル調整処理部（Ｓ６７）について、説明する。Ｓ６７１で使用するフレームの選択候補を選び、その中から複数（３個以上）のフレームをランダムに選択する（Ｓ６７２）。通常ビデオもしくは動画像の枚数はかなりの量になるので、全部を利用しても良いが、利用する候補を選択しておく方が速度やハードウェアの負荷を考えた場合良い。例えば、一定のフレーム間隔でサンプリングしたり、相互標定処理で選択したものやその周辺を選択したりする。次に対応点（特徴点）をランダムに選択する（Ｓ６７３）。前述したバンドル調整計算を行い（Ｓ６７４）、対応点座標推定位置データならびに各フレームの撮影推定位置（外部標定要素）を算出する（これらは同時に求まる）。次に、ロバスト推定基準を計算する。即ち、選択された点以外の座標残差を算出し（Ｓ６７５）、そのメディアン値を求めロバスト推定基準にする。この場合の評価値は推定された点の残差である。例えば、１pixel（画素）程度に設定する（Ｓ６７６）。本方法はいわゆるロバスト推定法で、具体的には前述したＬＭｅｄＳ法である。即ち、各対応点の画像座標の残差を求め、そのメディアン値の残差の大きさが所定の閾値ε６（第６の所定値）より以上大きいものがある場合はＳ６７３に戻り、新しい対応点を選択し、同様の処理を繰り返す。メディアン値の大きさとして閾値ε６以下のものが得られたら、選択・除去処理部（工程）（Ｓ６８）に移る。
【００５９】
選択・除去処理部（工程）（Ｓ６８）では、第１のバンドル調整処理部(工程)（Ｓ６７）で求められた推定座標位置データあるいは、撮影位置の残差の大きさを閾値ε７と比較し、それより大きい大誤差をもった座標位置データは、誤対応点として除外等する（Ｓ６８）。また、大きい大誤差をもった撮影位置に対応するフレームは、不適切な画像フレームとして除外する。閾値ε７としては、精度をよくしたければ、例えば０．５pixelとし、それほどの精度が必要でなければ３pixel程度とする。
次に、これらの誤対応点を除いてもう一度最終のバンドル調整を実施する（Ｓ６８２）、そしてＳ６８２の最終バンドル調整で算出された値を三次元座標と各フレームの外部標定要素として出力する（Ｓ６９）。
【００６０】
上記説明では、フレーム（撮影位置）の選択と大誤差点（座標位置データ）の除去をロバスト推定基準で行っているが、どちらか一方を行っても、あるいは両方同時に行ってもよい。例えば、処理が重ければ、フレームのロバスト推定を除外してもよい。
【００６１】
[第２の実施の形態]
近年ＧＰＳの利用が容易になっており、また、民生開放システムでもＤＧＰＳ等の相対測位方式、ＲＴＫ−ＧＰＳ等の干渉測位方式の開発により精度が向上しており、その計測精度は後者の場合、数ｃｍのオーダとなっている。本実施の形態はＧＰＳによる位置計測データの補完的に使用し、全体の精度の向上および計測時間の短縮を実現するものである。
【００６２】
具体的には、トラッキングにＧＰＳデータを適用するものがある。これは、各フレームに対応するカメラ２の位置のＧＰＳでの計測データとフレームを対応付けるものであり、具体的には、ＧＰＳで計測出力値と撮影画像フレームの同期がとれていれば、ＧＰＳの計測値が出力したときの撮影フレームが対応付けされ、同期が取れていない場合、もしくは同期がとれない場合は、ＧＰＳと撮影カメラのタイムスタンプを利用してお互いに一番近い時間のものを対応付ける。あるいはハード的に他にタイマーを用意させたりしてもよい。きちんと同期が取れているものが理想であるが、本発明では概略の値でもよい。
【００６３】
ＧＰＳデータは、ステレオペアの選択にも適用することが出来る。即ち、各フレームとカメラ２の位置が対応付けられているので適切な基線長のフレームを選択することが出来る。また、撮影カメラとＧＰＳ計測出力値に同期がきちんと取れていなくとも、本発明のフレームのロバスト推定選択を適用し処理することができる。図８に具体的な適用例を示す。図８は、図４のＳ４１ａの部分のランダムに選定するのに代えて、図８のＳ４１ａ’のようにＧＰＳによるカメラ２の位置データから十分な基線長を有するステレオペアを選定するものである。ここでは、先に説明したように、きちんと同期をとり対応付けがされてなくとも、基線長は概略値でもよい。このように構成すると、無駄なフレームを選択することがない。また、同期をとるための余計なハードウェアが無くとも適切なフレーム選択が可能となる。
【００６４】
更に、バンドル調整においては、ＧＰＳによるカメラ位置データをバンドル調整の初期値に使用する。このようにすると実際に近い値から計算を開始できるので収束の時間が飛躍的に短くできるという利点がある。
【００６５】
以上説明したように、本発明の位置計測装置および位置計測方法によれば、複数の写真のデジタルデータ処理による測量法において使用される推定演算に、第１段階として大きな観測誤差の影響を受けにくいロバスト推定法を適用し、その推定結果を基準に大きな誤差要因となる誤対応点や適切でないフレームを除去し、誤差要因を除いた上で第２段階である最終推定演算を行っているので、全体として収束時間が短く、高精度な計測結果が得られる。
【図面の簡単な説明】
【００６６】
【図１】図１は本発明のハードウェアおよびソフトウェアモジュール構成を示す図である。
【図２】図２は第１の実施の形態における画像処理方法の全体フロー図である。
【図３Ａ】図３Ａは、トラッキング処理部の処理内容である。
【図３Ｂ】図３Ｂは、図３Ａの第１の追跡判断部（工程）の詳細を示す図である。
【図３Ｃ】図３Ｃは、図３Ａの第２の追跡判断部（工程）の詳細を示す図である。
【図３Ｄ】図３Ｄは、図３Ａの再テンプレートマッチング部（工程）の詳細を示す図である。
【図４】図４は相互標定部（工程）の詳細を示す図である。
【図５】図５はバンドル調整処理部（工程）の詳細を示す図である。
【図６】図６は他の構成によるバンドル調整処理部（工程）の詳細を示す図である。
【図７】図７は相互標定の原理を示す図である。
【図８】図８はＧＰＳデータを用いる場合の相互標定処理部（工程）の例である。
【図９】図９は本発明の概念を示す図である。
【符号の説明】
【００６７】
１ 操作部
２ 撮像画像取得部（カメラ）
３ 特徴抽出部
４ トラッキング部
４Ａ 特徴点追跡部
４Ｂ 第１の追跡判断部
４Ｃ 第２の追跡判断部
４Ｄ 再テンプレートマッチング部
５ 相互標定部
５Ａ 第１の相互標定処理部
５Ｂ 第２の相互標定処理部
５Ｃ 第３の相互標定処理部
６ バンドル調整部
６Ａ 第１のバンドル調整処理部
６Ｂ 選択・除去処理部
６Ｃ 最終バンドル調整部
７ 位置計測部
８ 表示部
９ 特徴点情報メモリ
１０ 動画像メモリ
１１ 外部出力部
１００ 位置計測装置
１０１ａ、１０１ｂ 特徴点
【技術分野】
【０００１】
本発明は位置計測装置及び位置計測方法に関する。詳しくは、対象物に対して撮影装置が相対的に移動して得られた対象物の動的画像から、撮影装置又は対象物の三次元座標を得る、位置計測装置及び位置計測方法に関する。
【背景技術】
【０００２】
対象物に対して撮像装置が相対的な移動をしながら対象物を連続的に撮影し、得られた画像を用い写真測量の手法により自己位置や対象物の位置を計測する技術がある。これは、ある距離を隔てて撮影した複数の撮影画像間の対応点を追跡（トラッキング）し、その中の２枚の撮影画像からステレオペアを組み、モデル画像を作成し、これらの複数のモデル画像をもとにバンドル調整をおこない最終的な三次元計測結果を得るものである。このような技術として、例えば、特許文献１のような技術が開示されている。
【０００３】
【特許文献１】特願２００５−３５５４７０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
上記従来の技術においては、得られた撮影画像から特徴点を抽出し、撮影画像前後に取得された複数の撮影画像についてリアルタイムに前記特徴点に対応する候補対応点をトラッキングし、その結果から対応点を確定し、確定された対応点を用いて撮影位置又は対象物の座標を演算する構成となっている。この過程において、後述するように、ステレオペアを組んだ２枚の撮影画像の各画像が満足すべき条件（共面条件、共線条件、縦視差除去法等）を満たすようにモデル座標系を求める相互標定、さらには、撮影装置の移動工程を求めるため複数のモデル座標を調整するバンドル調整等の操作を行うが、各処理・計算工程は、何れも、誤差を含む多数の観測データから最尤の座標値や座標変換パラメータを求める演算を伴う。従来、これらの演算では、最小二乗法が用いられることが多かった。
【０００５】
しかしながら、最終的に撮影装置の自己位置や撮影対象物の三次元座標を得るときに、特徴点に誤対応点があると、三次元座標が正確に求まらないだけでなく、場合によっては計算過程で解が収束せず、その結果、トラッキングができなくなる場合もある。
トラッキング部でのエラーは、例えば、誤対応点や計算に適切でない対応点（例えば、微小に動いている点（木の葉、その他）、擬似対応点：見かけ上一致しているような点）が含まれていた場合、その後段での相互標定演算等において、計算精度が悪化し、最悪の場合、解が収束しなくなる。さらに、それらが多く含まれているステレオペアを選択した場合も同様である。解の収束性が悪くなると演算速度が低下する。仮にモデル座標が計算でき、その後のバンドル調整に進んだ場合にそれらに誤差がまだ含まれていた場合、大誤差除去をしきれずに精度が不確かになる場合がある。また誤差が多く残っていたり、あるいは誤差の大きい点がいくつか残っていると、結果的に最小二乗法等の推定計算を行うので、収束するのに時間がかかりリアルタイムで処理できない、などの問題があった。
【０００６】
そこで、本願発明は、高速でかつ高精度に自己位置または対象物の三次元計測ができる装置および方法を提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記課題を解決するために、請求項１に記載の位置計測装置は、例えば、図１に記載のように、撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段（２）から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出部（３）と、複数の前記撮像画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡部（４Ａ）と、前記特徴点追跡部（４Ａ）の追跡結果から、一つの撮影画像フレームと他の一つの撮影画像フレームを選択し、当該２つの撮影画像フレーム間の各々の前記対応点の任意の複数の点を元に相互標定を行い、縦視差を求め、求められた縦視差に対してロバスト推定を行い、大きな誤差の影響を少なくして相互標定のパラメータの推定値を求める第１の相互標定処理部（５Ａ）と、前記第１の相互標定処理部（５Ａ）で推定された相互標定のパラメータに基づき、各対応点の前記２つの画像フレーム間で相互標定を行い、前記特徴点の縦視差を算出し、その縦視差に対して閾値処理を施し、誤対応点の除外又は撮影画像フレームの適否の判断を行う第２の相互標定処理部（５Ｂ）と、前記複数の撮影画像フレームを用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段（２）の撮影位置、又は、撮影姿勢を計測する位置計測部（７）とを備える。
【０００８】
ここにおいて、ロバスト推定とは、推定値の間に大きな誤差を含むものが観測データに含まれる場合にも比較的容易に推定が行えるもので、例えば、ＬＭｅｄＳ法、メディアン推定（Ｍ推定）法，ランザック（RANSAC）法である。閾値処理とは、予め設定した閾値と比較して、その大小によりデータを取捨選択する処理をいう。このように構成すると、各推定演算処理の前に大きな誤差を除去してあるので、演算の収束が速く、高精度な結果が得られる。
【０００９】
請求項２に記載の発明は、請求項１において、例えば図１のように、ＧＰＳデータから撮像画像取得手段の位置のデータ得るＧＰＳ位置データ取得部（１２）を備え、取得した位置データから十分な基線長を有するものから前記ステレオペアを選択するものである。このように構成すると、単にランダムにステレオペアを選択する場合に比べて、適切なステレオペアを的確に選定することができ、計測結果の収束時間を短くできる。
【００１０】
請求項３に記載の位置計測装置は、例えば、図１に記載のように、撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段（２）から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出部（３）と、複数の前記撮影画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡部（４Ａ）と、前記特徴点追跡部（４Ａ）の追跡結果から、複数の撮影画像フレームを選択し、当該複数の画像フレーム間の各々の前記特徴点の座標位置、または各フレームの撮影位置にバンドル調整処理を施し、得られた特徴点の座標位置データまたは、各フレームの撮影位置の残差に対してロバスト推定を行い、それらの推定データを求める第１のバンドル調整処理部（６Ａ）と、前記、第１のバンドル調整処理部（６Ａ）で推定された推定データに基づき、それらの閾値を決定し、決定された閾値により、誤対応点の除去又は撮影画像フレーム選択を行う選択・除去処理部（６Ｂ）と前記適切と判断された特徴点およびフレームを用いた前記バンドル調整結果を用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段の撮影位置、又は、撮影姿勢を計測する位置計測部（７）とを備える。
【００１１】
このように構成すると、複数のフレーム画像に基づいて特徴点、フレームの評価を行うので不適な特徴点やフレームを最終的なバンドル調整の前に除去することができる。また、各推定演算処理の前に大きな誤差を除去してあるので、演算の収束が速く、高精度な結果が得られる。
【００１２】
請求項４に記載の発明は、請求項３において、例えば図１のように、ＧＰＳデータから撮影画像取得手段の位置のデータを得るＧＰＳ位置データ取得部（１２）を備え、取得した位置データから前記撮影画像フレームを選択する。このように構成すると、単にランダムにステレオペアを選択する場合に比べて、撮影位置を的確に選定することができ、初期条件を推定値に近い値にすることができ計測結果の収束時間を短くできる。
【００１３】
請求項５に記載の発明は、請求項１乃至請求項４のロバスト推定が、ＬＭｅｄＳ法、メディアン推定（Ｍ推定）法，ランザック（RANSAC）法のいずれか一つであることを明示したものである。このように構成すると、各推定演算処理の前に大きな誤差を簡単な処理で除去できるので、演算の収束が速く、高精度な結果が得られる。
【００１４】
請求項６記載の発明は、請求項１に対応する方法の発明であり、例えば、図１、図４（主に、図４）に記載のように、撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段（２）から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出工程（３）と、複数の前記撮影画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡工程（４Ａ）と、前記特徴点追跡工程（４Ａ）の追跡結果から、一つの撮影画像フレームと他の一つの撮影画像フレームからなるステレオペアを選択し（Ｓ４２１）、当該２つの撮影画像フレーム間の各々の前記対応点の任意の複数の点を元に相互標定を行い（Ｓ４２３）、縦視差を求め、求められた縦視差に対してロバスト推定を行い、大きな誤差の影響を少なくして相互標定のパラメータの推定値を求める（Ｓ４２４，Ｓ４２５）、第１の相互標定処理工程（Ｓ４２）と、第１の相互標定処理工程（Ｓ４２）で推定された相互標定のパラメータに基づき、２つの画像フレーム間で相互標定を行い（Ｓ４３０、Ｓ４４１）、特徴点の縦視差を算出し（Ｓ４３１、Ｓ４４２）、その縦視差に対して閾値処理を施し（Ｓ４３２、Ｓ４４３）、誤対応点のを除外又は撮影画像の適否の判断を行う第２の相互標定処理工程（Ｓ４３、Ｓ４４）と、前記複数の撮影画像フレームを用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段の撮影位置、又は、撮影姿勢を計測する位置計測工程（Ｓ４５）とを；備える。
【００１５】
このように構成すると、各推定演算処理の前に大きな誤差を除去してあるので、演算の収束が速く、高精度な結果が得られる。
【００１６】
請求項７記載の発明は、請求項３に対応する方法の発明であり、例えば、図１、図６（主に、図６）に記載のように、撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段（２）から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出工程（３）と、複数の前記撮影画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡工程（４Ａ）と、前記特徴点追跡工程の追跡結果から、複数の撮影画像フレームを選択し（Ｓ６７１）、当該複数の画像フレーム間の各々の前記特徴点の座標位置または、各フレームの撮影位置にバンドル調整処理を施し（Ｓ６７４）、得られた特徴点の座標位置データまたは、各フレームの撮影位置の残差に対してロバスト推定を行い（Ｓ６７５，Ｓ６７６）、それらの推定データを求める第１のバンドル調整処理工程（Ｓ６７）と、前記、第１のバンドル調整工程（Ｓ６７）で推定された推定データに基づき、それらの閾値を決定し、決定された閾値により、誤対応点を除去又は撮影画像フレーム選択を行う選択・除去処理工程と（Ｓ６８）、適切と判断された特徴点およびフレームを用いたバンドル調整結果を用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段（２）の撮影位置、又は、撮影姿勢を計測する位置計測工程（Ｓ６９）とを備える。
【００１７】
このように構成すると、複数のフレーム画像に基づいて特徴点、フレームの評価を行うので不適な特徴点やフレームを最終的なバンドル調整の前に除去することができる。また、各推定演算処理の前に大きな誤差を除去してあるので、演算の収束が速く、高精度な結果が得られる。
【発明を実施するための最良の形態】
【００１８】
以下に図面に基づき本発明の実施の形態について説明する。
【００１９】
［第１の実施の形態］
図９は本実施の形態における概念を説明するための図である。自動車にカメラ２を装着し、自動車の位置を少しずつ変えて対象物である市街地を撮影し、これら複数の撮影画像における特徴ある目印（特徴点１０１ａ、１０１ｂ：例えば、特徴あるビルの特定の点等）のトラッキング結果から、カメラ２の位置座標すなわち自動車の軌跡を求める例である。これによりカーナビゲーションに自動車の位置を連続的に表示可能になるが、ＧＰＳ電波を受信できない区間で補完的に利用される意義も大きい。図のような自動車にカメラ２（以下、撮影画像取得部２ということがある）を搭載した場合、撮像画面が左右／上下、或いはある方向に回転するなど揺動する。また、その撮像画像には他の自動車、人など動くものが含まれることも多い。これらは全て誤差の要因となる。
【００２０】
[ハードウェアおよびソフトウェアモジュール構成]
図１に本実施の形態における位置計測装置１００の構成例を示す。図１は、ハードウェアと、それに搭載されるソフトウェアモジュールを模式的に示したものである。図において、操作部１は位置計測装置１００の各部を制御して、位置計測装置として機能せしめる部分であり、具体的には、撮影画像取得部２への撮影開始・停止の指示、特徴抽出部３の設定変更、トラッキング部４の設定変更、開始、停止の指示、相互標定部５、バンドル調整部６への処理開始指示、標定実行指示等を行う。
撮影画像取得部２は動画像等の撮影画像を順次取得する部分であり、典型的にはビデオ撮影カメラである。撮影画像の取得の他に特徴抽出部３への出力、動画像メモリ１０への撮影画像の保存等を行う。なお、保存データは撮影した全フレームであってもよいし、間引きしたデータであってもよい。また、位置計測装置１００は撮影画像取得部２を持たず、外部の撮影装置からケーブルや無線等の通信手段により撮影画像を取得しても良い。
【００２１】
特徴抽出部３は順次取得した少しずつ異なる撮影画像から画像内の特徴点を抽出するもので、撮影画像取得部２より入力された撮影画像から特徴点の抽出を行い、得られた各特徴点の周囲の画像の濃淡等の情報をトラッキング部４、及び相互標定部５へ出力する。
トラッキング部４は特徴抽出部３より入力された特徴点の濃淡情報等から別のフレーム画像をサーチして、その点の周囲がほぼ同じ濃淡情報を持つ点（対応点）を捜索し、特徴点の追跡を行う。又、このトラッキングの他に、トラッキング結果の相互標定部５への出力、実行開始指示、候補対応点の配置の判断と特徴抽出部３への特徴点の新設指示等を行う。
尚、以降の説明で、「特徴点」とは、特徴点抽出部から得られた点をいい、「対応点」とは、特徴点のうち、二つの以上のフレーム画像間で撮影対象が同一であると確認された特徴点どうしの互いに対応する点を言う。
【００２２】
図１において、撮影画像取得部２、特徴抽出部３、トラッキング部４における処理は、典型的には、画像データが入力された時にリアルタイムに処理を行う部分であり、次に述べる、相互標定部５、およびバンドル調整部６は、典型的には、前記特徴抽出部３、トラッキング部４で得られた複数画像にわたるデータをメモリに蓄積の上処理するもので、非リアルタイム処理である。但し、必要に応じて、前記トラッキングの処理を非リアルタイム処理としてもよいし、相互標定部５、およびバンドル調整部６の処理をリアルタイムで行うことも勿論可能である。
【００２３】
これらの処理の基準となる特徴点の情報は特徴点情報メモリ９に記憶されている。このメモリには、各フレームの特徴点の座標値、他のフレームの特徴点との対応関係（対応点）等の情報が処理の過程で随時書込み／消去できるようになっている。１０は撮影画像を記憶する動画メモリである。位置計測部７は、処理された特徴点（又は対応点）の座標値や外部標定要素等により最終的な三次元データを出力するものである。この結果は、表示部８や外部出力部１１に出力される。
表示部８は、例えば、三次元計測された対象物の画像を例えば立体的に表示する。
【００２４】
[全体のフロー]
図２に第１の実施の形態における画像処理方法の全体フロー例を示す。尚。各部分処理の詳細については後述する。まず撮影した動画データを順次取り込む（Ｓ１０）。これらは一連のフレーム画像である。次に、トラッキング処理を行う（Ｓ２０）。即ち、取り込んだ動画の一つのフレームの画像内で特徴点を抽出し、その特徴点の近傍を含む濃淡等のパターンを保存する。ここで、このフレーム画像内の特徴点のパターンをテンプレートとして、別のフレームの画像で同様のパターンの部分を求め対応点を求める。次に、このようにして求めた複数の対応点について適正な対応点か否かの評価を行い、誤対応点を特徴点情報メモリ９から除外かするか、又はその旨の情報を書き込む（以下単に「除外等」と言う）。Ｓ２０の処理は、典型的には、リアルタイム処理であり、入力されるフレームについて順次処理を行い、その都度対応点を保存し、誤対応点か否かの評価の結果で特徴点情報メモリ９の書き換えを行う。尚、入力されるフレームについては適宜間引いて処理することも出来ることは勿論である。以上により計測の基準点となる対応点（仮対応点）を決定する（Ｓ３０）。ここで、「仮」としたのは、以降の処理（相互標定、バンドル調整等）において対応点の適正性の再評価の結果、除外されることがあるからである。
【００２５】
次に、Ｓ４０において、相互標定処理を行い、２つの画像データの対応点の間で各座標軸を合わせる。即ち、相互標定のための適切なステレオペアを選択し、Ｓ３０で決定された対応点を元にそれらの間で例えば共面条件を満足するように画像の座標軸を調整して立体像が得られるようにする。この相互標定過程で判明する不適切な対応点があれば特徴点情報メモリを書き換えそのデータを除去する。この結果、充分な基線長を有する等、三次元座標決定に適切なステレオペアを決定する（Ｓ５０）。
【００２６】
次に、連続したモデル画像を得るため接続標定を行なう（Ｓ５５）。これは、各モデル間の傾き、縮尺を統一して同一座標系とする処理である。そして、画像上に含まれる、タイポイントやパスポイントを用いて、各画像の外部標定要素を最小二乗法により同時に決定する連続したモデル画像を得るためバンドル調整処理を行う（Ｓ６０）。ここでも、この処理の過程で判明する大きな誤差を与える対応点の除外等を行い、その後最終的なバンドル調整を行い、撮影画像取得部２の位置座標、姿勢、等の外部標定要素、および三次元座標を求める（Ｓ７０）。
以下、各処理について、更に詳細に説明する。
【００２７】
［トラッキング］
図３Ａにトラッキング処理部４の処理内容を示す。処理内容を図１を併用して説明する。図１に示すように、まず、撮影画像取得部２において撮影画像を取得する。自己の撮影装置で画像を撮影して取得しても良く、他の撮影装置で撮影した画像を通信回線やメモリ、テープ等を介して取得しても良い。次に、少しずつ変化する撮影画像を順次特徴抽出部３に供給する。本実施の形態では自動車にカメラ２を装着し、移動しながら撮影するので、少しずつ変化する撮影画像とは、時間的、従って、空間的に少しずつ変化する撮影画像であり、隣接するフレーム（或いは、ある枚数だけ間引いたフレーム）の画像の大部分で対象物が共通に含まれる。但し、カメラ２は自動車だけでなく、人等が持って歩いたり、自転車に載せて撮影したりしてもよい。
【００２８】
[特徴点抽出]
特徴抽出（図３ＡのＳ２１）は特徴抽出部３において行なわれる。典型的には初期フレームでは全画面から抽出を行い、次のフレームからは、初期フレームと重複しない新たな画面領域から抽出が行われる。あるいは、あるタイミングで、例えばテンプレートの点数が３〜５割減ったら、特徴抽出を行い、新たな特徴点を発生させる等の処理を行い、テンプレートを更新しながらトラッキングしてもよい。すなわち毎回特徴点を発生させる必要は必ずしもない。特徴点の抽出には、例えばＭＯＲＡＶＥＣオペレータ（Ｈ． Ｐ． Ｍｏｒａｖｅｃ． Ｔｏｗａｒｄｓ Ａｕｔｏｍａｔｉｃ Ｖｉｓｕａｌ Ｏｂｓｔａｃｌｅ Ａｖｏｉｄａｎｃｅ． Ｐｒｏｃ． ５ｔｈ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｉｎｔ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ａｒｔｉｆｉｃｉａｌ Ｉｎｔｅｌｌｉｇｅｎｃｅ， ｐｐ． ５８４， １９７７．）や、Ｈａｒｉｓｓ，Ｐｒｅｓｓｙ，Ｓｕｓａｎなどのオペレーターを適宜採用できる。
【００２９】
特徴抽出オペレータは、どのようなものを利用しても、画像上のわずかなノイズ（エッジ上のノイズなど）に反応しやすいという問題がある。この性質の改善のために、特徴抽出オペレータを使用する前に画像のノイズ除去処理を平均値フィルターなどのノイズフィルターを用いて行う。
【００３０】
また、撮影対象によっては、画像上のある部分（木や芝など）に特徴点が集中することがある。これは、画面の一部の情報から全体を評価することになり、後述のテンプレートマッチングなどに悪影響を及ぼすことがある。これを避けるために選点処理を行なう。更に、特徴点の最大数が予め指定される場合には、画像全体に均等に配置されるようにし、以降の処理の相互標定等を確実にできるようにする。このように、特徴点抽出に関しては、種々の処理を経て実行されるものであるが、本願発明とは直接関係がないのでこれ以上の説明は省略する。
【００３１】
[対応点追跡]
図３Ａを参照して説明を続ける。追跡処理（Ｓ２２）は、特徴点追跡部４で行われる処理で、特徴抽出処理により選点された各特徴点に対応する点をフレーム間に渡って追跡処理する。すなわち、特徴点に対応する別の画像の中の候補対応点を求める処理である。具体的には、隣接撮影画像について周囲の画像パターンが前記特徴点の周囲の濃淡パターンと近似している部分のサーチ（テンプレートマッチング）を行い、１つの特徴点に対応する候補対応点を求める。テンプレートマッチングとして、例えばＳＳＤＡ法（逐次残差法）などが使用できる。ＳＳＤＡ法（逐次残差法）とは、対応する点の濃淡等の差を用いてパターンの類似度を決定するもので、比較範囲（テンプレートサイズ）内の濃淡の差が最小となるような位置を候補対応点として求めるものである。また、テンプレートマッチングとして正規化相関法などの他の方式も採用することもできる（例えば、秋山実著、「写真測量」、株式会社山海堂発行、ＰＰ２３４−２３５、参照）。テンプレートマッチングには、テンプレートサイズと探索範囲を最適に選択することが重要であり、探索範囲については、ビデオカメラ２のフレームレート、移動速度などを基に最適な設定とする。
【００３２】
テンプレートマッチングより、各特徴点の候補対応点が求められるが、ミスマッチングも多く発生する。例えば、ＳＳＤＡテンプレートマッチング自体のエラー、撮影画面中に、走行中の自動車、飛鳥、落葉などの動くものに特徴点が付与された場合、またカメラの揺動が激しい場合などミスマッチングによる誤対応点が生じ得る。このような誤対応点は以降の相互標定、バンドル調整等で誤差の要因となり、処理時間の増加の原因となるので極力除外しておく必要がある。
【００３３】
Ｓ２３〜Ｓ２５は、求められた対応点から上記誤対応点を抽出し、対応特徴点から除外するもので、Ｓ２３の第１の追跡判断工程、Ｓ２４の第２の追跡判断工程と、Ｓ２５の再テンプレートマッチングで構成される。第１の追跡判断部（Ｓ２３）はトラッキング結果からの画面全体の動きから画面間の座標変換射影パラメータ（以下、単に「射影パラメータ」と言う）を推定するものである。具体的には、図３Ｂを参照して説明する。第２の追跡判断工程（Ｓ２４）は、Ｓ２３の結果を受けて処理するもので、第１の追跡判断工程で推定した射影パラメータを基に、各対応点の評価を行い誤対応点を特徴点情報メモリ９から除外等するもので、具体的には図３Ｃを参照して説明する。再テンプレートマッチング（Ｓ２５）は、誤対応点を除いた対応点について再度テンプレートマッチングを行い、その結果より再度対応点を評価し、不適当な対応点を更に除外するものである（Ｓ２６）。具体的な内容については図３Ｄを参照して説明する。
【００３４】
Ｓ２７は、これらの処理を繰り返して、ある一定以上のフレーム数の対応点修正が終了したら、次のＳ３０へ、仮の対応点として出力する。ここでのフレーム数は、システムの処理能力、速度によって適宜決める。
【００３５】
図３Ｂに前記第１の追跡判断工程の処理内容を示す。まず典型的な処理内容を説明し、その後、その他の具体例を説明する。
本処理は、一つの撮影画像フレーム（フレームＡと言う）と他の一つの撮像フレーム（フレームＢと言う）の２つのフレームに関する処理である。先ず、Ｓ２２でのテンプレートマッチングで求められた候補対応点のうちから複数点の組をランダムに選定する。組の数は次の処理（Ｓ２３２）の射影パラメータの数以上とする。尚、選定する候補対応点は誤差を少なくするため、画面内の一部に局在せずフレーム全体に渡って選択するのが望ましい。次に、フレームＡ、Ｂ間の射影変換行列の要素（射影パラメータ）を求める。射影変換として種々のものがあるが、通常、アフィン変換やヘルマート変換が用いられる。ここでは、並進変換と回転変換の双方が比較的取り扱いやすいアフィン変換の例で説明する。
【００３６】
対応するフレームの座標値を（ｘ’，ｙ’），（ｘ”，ｙ”）とすると、アフィン変換は以下の６個の射影パラメータを使って表される。
ｘ’=ａｘ”+ｂｙ”+ｃ （１ａ）
ｙ’=ｄｘ”+ｅｙ”+ｆ （１ｂ）
上記、ａ〜ｆの６個の射影パラメータは選定した６組以上の対応点の組の座標から求める。このパラメータと式（１）を用い、上記選定された対応点について一方を固定して他方の座標値（予想座標）を求める。次に、各対応点について、求められた予想座標と実際の対応点の座標との残差を求める（Ｓ２３３）。この残差は例えば、ｘ軸、ｙ軸の二つの座標点の距離の差としても良い。対応点が全て正しく認識されていればこの残差は非常に小さくなるはずである。上記選定した６組以上の対応点の中には誤対応点や誤対応点とまでは言えないが大きな誤差を含むものがある。このような対応点を選定すると正しい射影パラメータが求まらない。そこで、上記残差を基に選定対応点の評価を行う。本実施の形態として、以下の残差のメディアン値（中央値）を用いるものを説明する。メディアン値は周知の通り、複数のデータを大きさの順にソートし、その中心にあるもののデータの大きさであり、例えば、５個のデータがあるときは３番目のデータの値である。メディアン値で母集団の全体的な傾向を把握することができる。メディアン値は演算としてデータのソートを行えばよく、また、分散値の大きな観測データを無視するので、大きな残差を持つデータの影響を受けにくいという利点がある。本実施の形態では、上記各対応点の残差からメディアン値の大きさを求め（Ｓ２３４）、この値が所定の値（ε１）であるか判断する。尚、メディアン値の大きさとして、その２乗を用いてもよい。所定の閾値（ε１）より大きいときは、選定した対応点が不適切であるのでＳ２３１に戻り、新たな対応点を選定する。新たな対応点に関して同様の処理を行い、メディアン値の大きさが所定の閾値（ε１）以下になればその時の射影パラメータが求められる（Ｓ２３６）。この際の閾値は、例えば、１pixel（画素）に設定する。
【００３７】
また、本実施の形態では、メディアン値の大きさ、またはその２乗が最小となるようなアルゴリズム（ＬＭｅｄＳ法）について説明したが、これに限定されるものではなく、誤差の影響が小さい所謂ロバスト推定法（例えば、ランザック法、メディアン推定法（Ｍ推定法））を使用しても同じ効果が得られる。これらの推定法は周知であるのでここでは説明を省略する。
【００３８】
図３Ｃは、第２の追跡判断部の処理内容を説明したものである。前述したＳ２３の処理（図３Ｂ参照）で求められた射影パラメータから、フレームＡの各特徴点に対応するフレームＢにおける対応点を計算して予測値とし、全対応点について、各々の対応点のフレームＢにおける座標値との残差を求める。次に、各々の対応点について残差値の大きさが予め定めた閾値より大きなものと比較する閾値処理（Ｓ２４３）を行い、閾値より大きなものは誤対応点として除外等する（Ｓ２４４）。
【００３９】
図３Ｄは、Ｓ２５の再テンプレートマッチングの内容を示したものである。第１の追跡判断部で求めた射影パラメータを用い、誤対応点と判断された点について各対応点の推定座標を求め（Ｓ２３７）、この座標点付近について再度テンプレートマッチングを行う（Ｓ２３８）。この結果、推定座標から所定の距離以内の対応点が発見された場合はこの点を対応点として特徴点情報メモリ９に追加し、所定の距離より離れた点でマッチングした場合、または、マッチング点が発見できない場合は不適切な対応点として特徴点情報メモリ９から除外等する。但し、当該対応点情報が特徴点情報メモリ９にない場合はそのままとする。以上のように、第２の追跡判断部で除外されなかった対応点と、再テンプレートマッチング（Ｓ２５）で新しく求めた対応点をあわせて新しく対応点とする。
【００４０】
以上のトラッキング処理については、撮影した画像をリアルタイムに処理する例について説明したが、撮影した画像をメモリ等に蓄積しておき、撮影後その画像データを処理する非リアルタイム処理とすることも出来ることは勿論である。また、撮影した全フレームについて処理することなく、適当に間引くことも出来る。間引く画面については撮影装置の移動速度、他の手段からの位置データから適応的に決定することもできる。
【００４１】
[ロバスト推定を用いた相互標定による誤対応点除去およびステレオペアの選択ステレオペアの選定]
図４に具体的な処理な処理内容を示す。本処理は、ロバスト推定を用いた相互標定処理を行うことにより、誤対応点の除去を行い、さらに三次元計測に使用できる適切な視差の異なるフレームのペア（ステレオペア）を求めるもので、前記トラッキング処理で求めた対応点を入力データとして行う。先ず、ある程度の長さの基線長が得られるステレオペアの候補を複数選定する（Ｓ４１）。例えば、あるフレーム上で観測された特徴点の、以降のフレーム上で配置を調べ、十分な移動があった場合、あるいは配置が画面の片側に偏っているフレームをステレオペア候補として複数組選択する。これらは、フレーム数やスピード等から、あるいはトラッキングしている点の軌跡等を利用する。
次に第１の相互標定処理部（Ｓ４２）で前記ステレオペアの候補からランダムにステレオペアを選択し、２つの撮影画像フレーム間の各々の対応点の任意の複数の点を元に相互標定を行い、縦視差を求め、求められた縦視差に対してロバスト推定を行い、大きな誤差の影響を少なくして相互標定のパラメータの推定値を求める。
誤対応点の除去を行う場合は、第２の相互標定処理部（Ｓ４３）で、第１の相互標定処理部で推定された推定相互標定のパラメータ（外部標定要素）に基づき、再度相互標定を行い、特徴点の縦視差を算出しなおし、その縦視差に対して閾値処理を施して立体視が十分得られるステレオペアを選定し、第２の相互標定処理部で選定したステレオペア中の対応点の評価を行い、対応点として相応しくない点を誤対応点として除外等する（Ｓ４３）。次に、Ｓ４４で前記誤対応点を以外の対応点を使って相互標定（具体的内容は後述する）を実施し、選ばれたステレオペアの再評価を行い、相応しい場合はステレオペアとして採用し、相応しくない場合は再び第１の相互標定処理部（Ｓ４２）に戻り、別のステレオペアを選定し、同様の処理を行う。このように、第１の相互標定処理部で適切な相互標定パラメータ値を推定し、不適なステレオペアの排除、第２の相互標定処理部で誤対応点を除外しておき、その後の演算（Ｓ４４）で不適なステレオペアの排除を行う。本処理（Ｓ４１〜Ｓ４５）により適正なステレオペアの選択を行い、誤対応除去を行っておくことでこの段階での最適な相互標定パラメータ（外部標定要素）と対応点が得られる。
【００４２】
相互標定はステレオペア間の座標軸の相対的位置と傾きを撮影時と相似の関係にセットする処理であり、相互標定が完了すると、立体視可能なステレオ画像が作成できるようになる。この時、二つの画像間で２つの画像に写っている同一点の光路はすべて１直線上に乗るようになる（２点からできる空間直線が同一平面に含まれる：共面条件）。実際には不完全性のため直線上からずれる。このずれを縦視差といい、相互標定の完全性の評価尺度となる。逆に言えば、縦視差がなくなるように二つの画像間で座標軸および座標値を変換すれば、共面条件を満足するようになり、適切なステレオペアが得られる。
【００４３】
具体的な相互標定の手順について簡単に説明する。図７は相互標定の説明図である。相互標定では、以下の共面条件式により各パラメータを求める。
【数１】
図７に示すように、モデル座標系（撮影対象物の座標系）の原点を左側の投影中心にとり、右側の投影中心を結ぶ線をＸ軸にとるようにする。縮尺は、基線長を単位長さにとる。このとき求めるパラメータは、左側のカメラＯ１のＺ軸の回転角κ１、Ｙ軸の回転角φ１、右側のカメラＯ２のＺ軸の回転角κ２、Ｙ軸の回転角φ２、Ｘ軸の回転角ω２の５つの回転角となる。この場合左側のカメラＯ１のＸ軸の回転角ω１は０なので、考慮する必要がない。このような条件にすると、式２の共面条件式は式３のようになり、この式を解けば各パラメータが求まる。
【数２】
【００４４】
ここで、モデル座標系ＸＹＺとカメラ座標系ｘｙｚの間には、次に示すような座標変換の関係式が成り立つ。
【数３】
【００４５】
これらの式を用いて、例えば、次の手順により、未知パラメータを求める。
(1)初期近似値は通常０とする。
(2)共面条件式（２）を近似値のまわりにテーラー展開し、線形化したときの微分係数の値を式（４）、式（５）により求め、観測方程式をたてる。
(3)推定法をあてはめ、近似値に対する補正量を求める。
(4)近似値を補正する。
(5)補正された近似値を用いて、(2)〜(5)までの操作を収束するまで繰り返す。
【００４６】
次に、第１の相互標定処理部（Ｓ４２）の内容について図４を参照して詳しく説明する。この処理は、選定したステレオペアである画像フレーム間で相互標定を実施することにより適切な相互標定パラメータを推定するもので、その推定パラメータより、次の第２の相互標定処理部にて相互標定を行い（Ｓ４３０）、の誤対応点の検出／除外を行う。そしてその結果から再度相互標定を行い（Ｓ４４１）、最適なステレオペアかどうか評価選定する。誤対応点は前述のトラッキング処理（Ｓ２０）において既に除外されているが、あくまでも二次元的な処理であり、不適な対応点が除外されていないものがあるので相互標定処理を行い、縦視差を評価することで、擬似的に三次元的な評価を行い、さらに適切な対応点だけを残すようにする。
【００４７】
先ず、ステレオペアの候補（Ｓ４１）から１組のステレオペアをランダムに選択する（Ｓ４１ａ）。次に、その中から対応点を複数ランダムに選定する。対応点の数は５個以上である（Ｓ４２２）。次に、この５点以上の対応点を使って相互標定を行う（Ｓ４２３）。相互標定の結果、それら対応点を使った相互標定パラメータが求められ、さらに各対応点について相互標定の誤差の指標である縦視差（相互標定後の各対応点の基線と直交するｙ座標の差）が算出できる。縦視差の大きさは対応点が適切なものかの評価尺度となり、できるだけその大きさは小さい方がよい。ここでは、この評価を行うため、各対応点について縦視差のメディアン値を求め（Ｓ４２４）、その値が予め設定した閾値ε２（第２の所定値）と比較する（Ｓ４２５）。所定値より大きい場合はＳ４１ａに戻り、新たな対応点を設定する。この際の閾値は、例えば、１pixel（１画素）程度に設定する。所定値以下の時は、この段階での最適な相互標定パラメータが推定されたとして第２の相互評定処理部に移る。
【００４８】
ここでは、メディアン値を求め、それを最小値とするアルゴリズムＬＭｅｄＳ法を用いた場合について説明したが、これに限定されるものではなく、誤差のばらつきの影響が少ないいわゆるロバスト推定法が広く使用できることは明らかである。
【００４９】
次に、第２の相互標定処理部の処理内容について説明する。ここでは、Ｓ４２の結果を受け、推定された相互標定パラメータから相互標定を行い（Ｓ４３０）、当該ステレオペアのフレーム内の各対応点の縦視差を算出する（Ｓ４３１）。対応点はすべての点であってもよいし、一部であってもよいが、画面内の大部分が望ましい。これらの縦視差に閾値処理を行い（Ｓ４３２）、閾値（ε３）以上の対応点を誤対応点として特徴点情報メモリ９から除外する。この時の閾値（ε３）は、例えば、縦視差が０．５〜１pixel程度の値に設定する。尚、この閾値は所望精度や対象物により変更してもよい。これで、ステレオペア上の不適切な誤対応点は除去されたことになる。
【００５０】
次に、適切なステレオペアであったかどうか評価を行うために、誤対応点を除いた対応点のみで再度相互標定を実施し（Ｓ４４１）、相互標定パラメータを求め、各対応点の縦視差を求める（Ｓ４４２）。この縦視差のメディアン値と所定値ε４（第４の所定値）との比較を行い（Ｓ４４３）、所定値以上の時は不適切なステレオペアであるのでＳ４１ａに戻り、新たなステレオペアを選定する。所定値より小さい時は適切なステレオペアの候補とする。尚、ここで「候補」としたのは、次のバンドルによる大誤差除去処理で不適なステレオペアとされる場合があるからである。
【００５１】
ロバスト推定法として、ＲＡＮＳＡＣ推定やＭ推定法、あるいはそれらを変形したものも利用できる。これらは、評価方法やその評価関数がＬＭｅｄＳと異なるだけで、大きな観測誤差の影響を受けにくい点で同じである。
【００５２】
尚、ステレオペアの選択を行わず、誤対応点の除去だけを行ってもよい。その場合は、Ｓ４１ａ、Ｓ４４は行う必要がない。あるいは、適切なフレーム選択として、Ｓ４３の処理だけを行わなくともよい。この場合、４１ａとＳ４２の処理が、第１の相互標定処理部として、Ｓ４４の処理のみが第２の相互標定処理部として行われる。すなわち、Ｓ４２で相互標定パラメータを推定し、Ｓ４４で最適フレームの適否を判定する。これらは、システム全体の性能として、例えば速度やトラッキング部の誤対応除去能力、対象とする景観、計測対象などにより適切な構成とする。
【００５３】
[バンドル調整１]
次に、複数のフレーム間に渡る対応点の動きに注目し、異常な動きをする対応点及びフレームを除外する。具体的には、バンドル調整により複数のフレーム画像に対し、対応点、フレーム画像の評価を行う。
【００５４】
図５にその処理内容を示す。Ｓ４０／Ｓ５０は、図２、図４で説明した相互標定処理による誤対応点の除去と最適ステレオペアの選定処理である。本処理は、これら処理を本処理の前段階の処理とし、この処理を受けて行うものである。選定されたフレームについて複数のフレームを選択（Ｓ６１）する。次に、これらのフレームについて接続標定を行う。接続標定とは、ステレオペア毎に生成される立体モデル固有の座標系（モデル座標系）を、隣り合うモデル同士と関係付け、モデル間の傾き、縮尺を統一して、統一した座標系（コース座標系）で記述する処理を言う。具体的には、隣接する画像座標において、例えば右のモデル内の点を（Ｘｊｒ，Ｙｊｒ，Ｚｊｒ）、左のモデル内の対応する点を（Ｘｊｌ，Ｙｊｌ，Ｚｊｌ）、カメラ２の座標を（Ｘ０，Ｙ０，Ｚ０）とした時、以下のΔＸｊ，ΔＹｊ，ΔＺｊ，ΔＤｊを計算し、ΔＺｊおよびΔＤｊが十分小さければ（例えば、１／２０００以下）ならば、接続標定が正常に行われたと判断する。
ΔＸｊ＝（Ｘｊｒ−Ｘｊｌ）／（Ｚ０−Ｚｊｌ） （式６ａ）
ΔＹｊ＝（Ｙｊｒ−Ｙｊｌ）／（Ｚ０−Ｚｊｌ） （式６ｂ）
ΔＺｊ＝（Ｚｊｒ−Ｚｊｌ）／（Ｚ０−Ｚｊｌ） （式６ｃ）
ΔＤｊ＝（ΔＸｊ２＋ΔＹｊ２）1/2 （式６ｄ）
【００５５】
次に、選択された各フレーム画像を元にバンドル調整を行う（Ｓ６３）。バンドル調整とは複数（３個以上）のフレーム画像と三次元座標を関係付ける方程式を立てて各フレーム画像の外部標定要素を一括して得るものである。
具体的には、投影中心、写真像および地上の対象物が一直線上にあるというバンドル調整の基本式である以下の共線条件（式７）を用いる。式７に含まれる６つの外部標定要素Ｘ０，Ｙ０，Ｚ０、ω、φ、κを求める。即ち、３点以上の基準点の対象物座標と対応する画像座標から、逐次近似解法によってこれら６つの外部標定要素を算出する。
【数４】
ここで、
c：画面距離（焦点距離）、x,y：画像座標
X,Y,Z：対象空間座標（基準点、未知点）
X0,Y0,Z0：カメラ２の撮影位置
ａｉｊ：カメラ２の傾き（３×３回転行列の要素）
Δx,Δy：カメラの内部標定補正項。
具体的には、未知変量の近似値を与え、式７を近似値のまわりにテーラー展開して線形化し、最小二乗法により補正量を求めて近似値を補正し、同様の操作をくりかえし収束解を求める逐次近似解法を用いる。これによってこれら６つの外部標定要素を求める。更に各対応点の画像座標の残差を求め、残差が所定の閾値ε５（第５の所定値）と比較し、閾値ε５以上のものについてはその対応点を除外し（Ｓ６４）、再びＳ４０〜Ｓ５０の処理に戻り最適ステレオペアの選定（具体的には、相互標定処理による誤対応点除去と最適フレームの選択）を行う。すべての対応点の残差が所定値より小さい場合は処理を打ち切り（Ｓ６４）、三次元座標と、外部標定要素を出力し（Ｓ６５）処理を終了する。
【００５６】
[バンドル調整２]
図６に、他の方法によるバンドル調整による評価方法を示す。この方法により、各特徴点の座標位置データの誤差や各フレーム撮影位置の誤差を取り除くことにより、最終的な三次元座標や外部標定要素を得る。
Ｓ２０／Ｓ３０〜Ｓ６２’までは図５の場合と同様である。Ｓ６２’の接続標定は、図５の接続標定Ｓ６２と同様の処理である。
【００５７】
次に第１のバンドル調整処理部（Ｓ６７）で、得られた特徴点の座標位置データまたは、各フレームの撮影位置の残差に対してロバスト推定を行い、それらの推定データを求め、選択・除去処理部（Ｓ６８）で、第１のバンドル調整処理部で推定された推定データに基づき、それらの閾値を決定し、決定された閾値により、誤対応点を除去又は撮影画像フレーム選択を行う。そして、最終のバンドル調整を行い（Ｓ６８２）、その三次元座標と各フレームの外部標定要素を得る（Ｓ６９）。
【００５８】
第１のバンドル調整処理部（Ｓ６７）について、説明する。Ｓ６７１で使用するフレームの選択候補を選び、その中から複数（３個以上）のフレームをランダムに選択する（Ｓ６７２）。通常ビデオもしくは動画像の枚数はかなりの量になるので、全部を利用しても良いが、利用する候補を選択しておく方が速度やハードウェアの負荷を考えた場合良い。例えば、一定のフレーム間隔でサンプリングしたり、相互標定処理で選択したものやその周辺を選択したりする。次に対応点（特徴点）をランダムに選択する（Ｓ６７３）。前述したバンドル調整計算を行い（Ｓ６７４）、対応点座標推定位置データならびに各フレームの撮影推定位置（外部標定要素）を算出する（これらは同時に求まる）。次に、ロバスト推定基準を計算する。即ち、選択された点以外の座標残差を算出し（Ｓ６７５）、そのメディアン値を求めロバスト推定基準にする。この場合の評価値は推定された点の残差である。例えば、１pixel（画素）程度に設定する（Ｓ６７６）。本方法はいわゆるロバスト推定法で、具体的には前述したＬＭｅｄＳ法である。即ち、各対応点の画像座標の残差を求め、そのメディアン値の残差の大きさが所定の閾値ε６（第６の所定値）より以上大きいものがある場合はＳ６７３に戻り、新しい対応点を選択し、同様の処理を繰り返す。メディアン値の大きさとして閾値ε６以下のものが得られたら、選択・除去処理部（工程）（Ｓ６８）に移る。
【００５９】
選択・除去処理部（工程）（Ｓ６８）では、第１のバンドル調整処理部(工程)（Ｓ６７）で求められた推定座標位置データあるいは、撮影位置の残差の大きさを閾値ε７と比較し、それより大きい大誤差をもった座標位置データは、誤対応点として除外等する（Ｓ６８）。また、大きい大誤差をもった撮影位置に対応するフレームは、不適切な画像フレームとして除外する。閾値ε７としては、精度をよくしたければ、例えば０．５pixelとし、それほどの精度が必要でなければ３pixel程度とする。
次に、これらの誤対応点を除いてもう一度最終のバンドル調整を実施する（Ｓ６８２）、そしてＳ６８２の最終バンドル調整で算出された値を三次元座標と各フレームの外部標定要素として出力する（Ｓ６９）。
【００６０】
上記説明では、フレーム（撮影位置）の選択と大誤差点（座標位置データ）の除去をロバスト推定基準で行っているが、どちらか一方を行っても、あるいは両方同時に行ってもよい。例えば、処理が重ければ、フレームのロバスト推定を除外してもよい。
【００６１】
[第２の実施の形態]
近年ＧＰＳの利用が容易になっており、また、民生開放システムでもＤＧＰＳ等の相対測位方式、ＲＴＫ−ＧＰＳ等の干渉測位方式の開発により精度が向上しており、その計測精度は後者の場合、数ｃｍのオーダとなっている。本実施の形態はＧＰＳによる位置計測データの補完的に使用し、全体の精度の向上および計測時間の短縮を実現するものである。
【００６２】
具体的には、トラッキングにＧＰＳデータを適用するものがある。これは、各フレームに対応するカメラ２の位置のＧＰＳでの計測データとフレームを対応付けるものであり、具体的には、ＧＰＳで計測出力値と撮影画像フレームの同期がとれていれば、ＧＰＳの計測値が出力したときの撮影フレームが対応付けされ、同期が取れていない場合、もしくは同期がとれない場合は、ＧＰＳと撮影カメラのタイムスタンプを利用してお互いに一番近い時間のものを対応付ける。あるいはハード的に他にタイマーを用意させたりしてもよい。きちんと同期が取れているものが理想であるが、本発明では概略の値でもよい。
【００６３】
ＧＰＳデータは、ステレオペアの選択にも適用することが出来る。即ち、各フレームとカメラ２の位置が対応付けられているので適切な基線長のフレームを選択することが出来る。また、撮影カメラとＧＰＳ計測出力値に同期がきちんと取れていなくとも、本発明のフレームのロバスト推定選択を適用し処理することができる。図８に具体的な適用例を示す。図８は、図４のＳ４１ａの部分のランダムに選定するのに代えて、図８のＳ４１ａ’のようにＧＰＳによるカメラ２の位置データから十分な基線長を有するステレオペアを選定するものである。ここでは、先に説明したように、きちんと同期をとり対応付けがされてなくとも、基線長は概略値でもよい。このように構成すると、無駄なフレームを選択することがない。また、同期をとるための余計なハードウェアが無くとも適切なフレーム選択が可能となる。
【００６４】
更に、バンドル調整においては、ＧＰＳによるカメラ位置データをバンドル調整の初期値に使用する。このようにすると実際に近い値から計算を開始できるので収束の時間が飛躍的に短くできるという利点がある。
【００６５】
以上説明したように、本発明の位置計測装置および位置計測方法によれば、複数の写真のデジタルデータ処理による測量法において使用される推定演算に、第１段階として大きな観測誤差の影響を受けにくいロバスト推定法を適用し、その推定結果を基準に大きな誤差要因となる誤対応点や適切でないフレームを除去し、誤差要因を除いた上で第２段階である最終推定演算を行っているので、全体として収束時間が短く、高精度な計測結果が得られる。
【図面の簡単な説明】
【００６６】
【図１】図１は本発明のハードウェアおよびソフトウェアモジュール構成を示す図である。
【図２】図２は第１の実施の形態における画像処理方法の全体フロー図である。
【図３Ａ】図３Ａは、トラッキング処理部の処理内容である。
【図３Ｂ】図３Ｂは、図３Ａの第１の追跡判断部（工程）の詳細を示す図である。
【図３Ｃ】図３Ｃは、図３Ａの第２の追跡判断部（工程）の詳細を示す図である。
【図３Ｄ】図３Ｄは、図３Ａの再テンプレートマッチング部（工程）の詳細を示す図である。
【図４】図４は相互標定部（工程）の詳細を示す図である。
【図５】図５はバンドル調整処理部（工程）の詳細を示す図である。
【図６】図６は他の構成によるバンドル調整処理部（工程）の詳細を示す図である。
【図７】図７は相互標定の原理を示す図である。
【図８】図８はＧＰＳデータを用いる場合の相互標定処理部（工程）の例である。
【図９】図９は本発明の概念を示す図である。
【符号の説明】
【００６７】
１ 操作部
２ 撮像画像取得部（カメラ）
３ 特徴抽出部
４ トラッキング部
４Ａ 特徴点追跡部
４Ｂ 第１の追跡判断部
４Ｃ 第２の追跡判断部
４Ｄ 再テンプレートマッチング部
５ 相互標定部
５Ａ 第１の相互標定処理部
５Ｂ 第２の相互標定処理部
５Ｃ 第３の相互標定処理部
６ バンドル調整部
６Ａ 第１のバンドル調整処理部
６Ｂ 選択・除去処理部
６Ｃ 最終バンドル調整部
７ 位置計測部
８ 表示部
９ 特徴点情報メモリ
１０ 動画像メモリ
１１ 外部出力部
１００ 位置計測装置
１０１ａ、１０１ｂ 特徴点
【特許請求の範囲】
【請求項１】
撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出部と；
複数の前記撮影画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡部と；
前記特徴点追跡部の追跡結果から、一つの撮影画像フレームと他の一つの撮影画像フレームを選択し、当該２つの撮影画像フレーム間の各々の前記対応点の任意の複数の点を元に相互標定を行い、縦視差を求め、求められた縦視差に対してロバスト推定を行い、大きな誤差の影響を少なくして相互標定のパラメータの推定値を求める第１の相互標定処理部と；
前記第１の相互標定処理部で推定された推定相互標定のパラメータに基づき、前記２つの画像フレーム間で相互標定を行い、前記特徴点の縦視差を算出し、その縦視差に対して閾値処理を施し、誤対応点の除外又は撮影画像フレームの適否の判断を行う第２の相互標定処理部と；
前記複数の撮影画像フレームを用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段の撮影位置、又は、撮影姿勢を計測する位置計測部とを；備える
位置計測装置。
【請求項２】
ＧＰＳデータから撮像画像取得手段の位置のデータ得るＧＰＳ位置データ取得部を備え、取得した位置データから十分な基線長を有するものを前記ステレオペアとして選択する；
請求項１記載の位置計測装置。
【請求項３】
撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出部と；
複数の前記撮影画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡部と；
前記特徴点追跡部の追跡結果から、複数の撮影画像フレームを選択し、当該複数の画像フレーム間の各々の前記特徴点の座標位置または、各フレームの撮影位置にバンドル調整処理を施し、得られた特徴点の座標位置データまたは、各フレームの撮影位置の残差に対してロバスト推定を行い、それらの推定データを求める第１のバンドル調整処理部と；
前記第１のバンドル調整部で推定された推定データに基づき、それらの閾値を決定し、決定された閾値により、誤対応点の除去又は撮影画像フレーム選択を行う選択・除去処理部と；
前記適切と判断された特徴点およびフレームを用いた前記バンドル調整結果を用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段の撮影位置、又は、撮影姿勢を計測する位置計測部とを；備える
位置計測装置。
【請求項４】
ＧＰＳデータから撮影画像取得手段の位置のデータ得るＧＰＳ位置データ取得部を備え、取得した位置データから前記撮影画像フレームを選択する；
請求項３記載の位置計測装置。
【請求項５】
前記ロバスト推定は、ＬＭｅｄＳ法、メディアン推定（Ｍ推定），RANSACランザック法のいずれか一つであることを特徴とする請求項１乃至請求項４の位置計測装置。
【請求項６】
撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出工程と；
複数の前記撮影撮像画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡工程と；
前記特徴点追跡工程の追跡結果から、一つの撮影画像フレームと他の一つの撮影画像フレームからなるステレオペアを選択し、当該２つの撮影画像フレーム間の各々の前記対応点の任意の複数の点を元に相互標定を行い、縦視差を求め、求められた縦視差に対してロバスト推定を行い、大きな誤差の影響を少なくして相互標定のパラメータの推定値を求める第１の相互標定処理工程と；
前記第１の相互標定処理工程で推定された推定相互標定のパラメータに基づき、前記２つの画像フレーム間で相互標定を行い、前記特徴点の縦視差を算出し、その縦視差に対して閾値処理を施し、誤対応点のを除外又は撮影画像フレームの適否の判断を行う第２の相互標定処理工程と；
前記複数の撮影画像フレームを用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段の撮影位置、又は、撮影姿勢を計測する位置計測工程とを；備える
位置計測方法。
【請求項７】
撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出工程と；
複数の前記撮影画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡工程と；
前記特徴点追跡工程の追跡結果から、複数の撮影画像フレームを選択し、当該複数の画像フレーム間の各々の前記特徴点の座標位置または、各フレームの撮影位置にバンドル調整処理を施し、得られた特徴点の座標位置データまたは、各フレームの撮影位置の残差に対してロバスト推定を行い、それらの推定データを求める第１のバンドル調整処理工程と；
前記第１のバンドル調整処理工程で推定された推定データに基づき、それらの閾値を決定し、決定された閾値により、誤対応点を除去又は撮影画像フレーム選択を行う選択・除去処理工程と；
適切と判断された特徴点およびフレームを用いた前記バンドル調整結果を用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段の撮影位置、又は、撮影姿勢を計測する位置計測工程とを；備える
位置計測方法。
【請求項１】
撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出部と；
複数の前記撮影画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡部と；
前記特徴点追跡部の追跡結果から、一つの撮影画像フレームと他の一つの撮影画像フレームを選択し、当該２つの撮影画像フレーム間の各々の前記対応点の任意の複数の点を元に相互標定を行い、縦視差を求め、求められた縦視差に対してロバスト推定を行い、大きな誤差の影響を少なくして相互標定のパラメータの推定値を求める第１の相互標定処理部と；
前記第１の相互標定処理部で推定された推定相互標定のパラメータに基づき、前記２つの画像フレーム間で相互標定を行い、前記特徴点の縦視差を算出し、その縦視差に対して閾値処理を施し、誤対応点の除外又は撮影画像フレームの適否の判断を行う第２の相互標定処理部と；
前記複数の撮影画像フレームを用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段の撮影位置、又は、撮影姿勢を計測する位置計測部とを；備える
位置計測装置。
【請求項２】
ＧＰＳデータから撮像画像取得手段の位置のデータ得るＧＰＳ位置データ取得部を備え、取得した位置データから十分な基線長を有するものを前記ステレオペアとして選択する；
請求項１記載の位置計測装置。
【請求項３】
撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出部と；
複数の前記撮影画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡部と；
前記特徴点追跡部の追跡結果から、複数の撮影画像フレームを選択し、当該複数の画像フレーム間の各々の前記特徴点の座標位置または、各フレームの撮影位置にバンドル調整処理を施し、得られた特徴点の座標位置データまたは、各フレームの撮影位置の残差に対してロバスト推定を行い、それらの推定データを求める第１のバンドル調整処理部と；
前記第１のバンドル調整部で推定された推定データに基づき、それらの閾値を決定し、決定された閾値により、誤対応点の除去又は撮影画像フレーム選択を行う選択・除去処理部と；
前記適切と判断された特徴点およびフレームを用いた前記バンドル調整結果を用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段の撮影位置、又は、撮影姿勢を計測する位置計測部とを；備える
位置計測装置。
【請求項４】
ＧＰＳデータから撮影画像取得手段の位置のデータ得るＧＰＳ位置データ取得部を備え、取得した位置データから前記撮影画像フレームを選択する；
請求項３記載の位置計測装置。
【請求項５】
前記ロバスト推定は、ＬＭｅｄＳ法、メディアン推定（Ｍ推定），RANSACランザック法のいずれか一つであることを特徴とする請求項１乃至請求項４の位置計測装置。
【請求項６】
撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出工程と；
複数の前記撮影撮像画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡工程と；
前記特徴点追跡工程の追跡結果から、一つの撮影画像フレームと他の一つの撮影画像フレームからなるステレオペアを選択し、当該２つの撮影画像フレーム間の各々の前記対応点の任意の複数の点を元に相互標定を行い、縦視差を求め、求められた縦視差に対してロバスト推定を行い、大きな誤差の影響を少なくして相互標定のパラメータの推定値を求める第１の相互標定処理工程と；
前記第１の相互標定処理工程で推定された推定相互標定のパラメータに基づき、前記２つの画像フレーム間で相互標定を行い、前記特徴点の縦視差を算出し、その縦視差に対して閾値処理を施し、誤対応点のを除外又は撮影画像フレームの適否の判断を行う第２の相互標定処理工程と；
前記複数の撮影画像フレームを用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段の撮影位置、又は、撮影姿勢を計測する位置計測工程とを；備える
位置計測方法。
【請求項７】
撮影対象物との相対的な位置が時間的に変化する撮影画像取得手段から得られた一連で複数のフレームからなる撮影画像から、画像内の撮影対象物の複数の特徴点を抽出する特徴抽出工程と；
複数の前記撮影画像について、前記特徴点に対応する対応点を探索し、前記特徴点の追跡を行う特徴点追跡工程と；
前記特徴点追跡工程の追跡結果から、複数の撮影画像フレームを選択し、当該複数の画像フレーム間の各々の前記特徴点の座標位置または、各フレームの撮影位置にバンドル調整処理を施し、得られた特徴点の座標位置データまたは、各フレームの撮影位置の残差に対してロバスト推定を行い、それらの推定データを求める第１のバンドル調整処理工程と；
前記第１のバンドル調整処理工程で推定された推定データに基づき、それらの閾値を決定し、決定された閾値により、誤対応点を除去又は撮影画像フレーム選択を行う選択・除去処理工程と；
適切と判断された特徴点およびフレームを用いた前記バンドル調整結果を用いて、前記撮影対象物の座標、又は、前記撮影画像取得手段の撮影位置、又は、撮影姿勢を計測する位置計測工程とを；備える
位置計測方法。
【図１】
【図２】
【図３Ａ】
【図３Ｂ】
【図３Ｃ】
【図３Ｄ】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図２】
【図３Ａ】
【図３Ｂ】
【図３Ｃ】
【図３Ｄ】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【公開番号】特開２００９−１４６２９（Ｐ２００９−１４６２９Ａ）
【公開日】平成２１年１月２２日（２００９．１．２２）
【国際特許分類】
【出願番号】特願２００７−１７９０３７（Ｐ２００７−１７９０３７）
【出願日】平成１９年７月６日（２００７．７．６）
【出願人】（０００２２０３４３）株式会社トプコン (904)
【公開日】平成２１年１月２２日（２００９．１．２２）
【国際特許分類】
【出願日】平成１９年７月６日（２００７．７．６）
【出願人】（０００２２０３４３）株式会社トプコン (904)
[ Back to top ]