JP5027747B2 - 位置測定方法、位置測定装置、およびプログラム - Google Patents

Description

本発明は、動画像に基づいて、撮影した位置や姿勢と被写体の三次元位置とを測定する位置測定技術に係り、特に誤差を低減する位置測定技術に関する。

従来から写真測量の理論が研究されている。近年では、写真測量の理論を用いて、動画像からカメラの位置や姿勢と被写体の三次元位置とを測定する技術が開示されている（例えば、特許文献１）。

特許文献１に記載の発明は、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）によって位置測定ができない場合に、後方交会法によってカメラの位置を求め、前方交会法によって被写体の位置を求めるものである。この態様によれば、位置測定が中断されない。

しかしながら、画像からカメラの位置や被写体の位置を求める方法は、誤差が発生する傾向がある。この問題を解決する技術は、例えば、非特許文献２または特許文献３に開示されている。

非特許文献２に記載の発明は、推定された三次元位置を画像上へ投影した座標と画像上で検出された座標との誤差（再投影誤差）、および、ＧＰＳ測位値によって定義される位置誤差を同時に最小化するものである。

また、特許文献３に記載の発明は、ビデオ映像から求めたカメラの位置および三軸回転位置を示すカメラベクトル（ＣＶ）データと、移動体の位置計測データと、を基準時刻によって時刻同期させ、同一時間軸で比較し、相互に補完補正するものである。
特開２００７−１７１０４８号公報 情報処理学会論文誌Ｖｏｌ．０、Ｎｏ．１３、横地裕次著、ｐ１−１１ 特開２００６−２５０９１７号公報

このような背景を鑑み、本発明は、動画像に基づいて、撮影した位置や姿勢と被写体の三次元位置とを測定する位置測定技術において、撮影シーンの変化や撮影装置の変動による誤差を低減する技術を提供することを目的とする。

請求項１に記載の発明は、初期画像中の既知点または外部で測定された撮影位置および／または撮影姿勢に基づき、撮影位置と撮影姿勢とからなる外部標定要素の初期値を算出する初期値算出ステップと、前記初期値算出ステップで用いられた初期画像に続く動画像中の既知点または既に三次元座標が与えられた特徴点に基づいて、外部標定要素を算出する外部標定要素算出ステップと、前記外部標定要素算出ステップによって算出された外部標定要素を、特徴点の軌跡の方向、特徴点の軌跡の量、特徴点の分布率、特徴点のオーバーラップ率、および縦視差の少なくともいずれか一つに基づいて評価する外部標定要素評価ステップと、前記外部標定要素評価ステップによって修正された外部標定要素に基づき、１または複数の画像における外部標定要素および特徴点の三次元座標を同時にバンドル調整するバンドル調整ステップと、前記バンドル調整された外部標定要素に基づき、特徴点の密度が少なくなった領域で新たに検出された特徴点の三次元座標を算出する三次元座標算出ステップと、前記外部標定要素算出ステップから前記三次元座標算出ステップまでの処理を、最終画像になるまで繰り返す繰り返しステップと、を備えることを特徴とする位置測定方法である。

請求項１に記載の発明によれば、特徴点の軌跡の方向や量、特徴点の分布率、オーバーラップ率、および縦視差の少なくともいずれか一つに基づいて、撮影シーンの変化や撮影装置の変動を判断することができる。撮影シーンの変化や撮影装置の変動は、特徴点の追跡を困難にし、画像から算出された外部標定要素の精度を劣化させる。したがって、撮影シーンの変化や撮影装置の変動が相対的に大きい場合には、画像から算出された外部標定要素の信頼性が低いと評価し、撮影シーンの変化や撮影装置の変動が相対的に小さい場合には、画像から算出された外部標定要素の信頼性が高いと評価する。

画像から算出された外部標定要素の信頼性が低いと評価される場合には、外部で測定された撮影位置や撮影姿勢を利用し、画像から算出された外部標定要素は利用しない。この態様によって、撮影シーンの変化や撮影装置の変動による誤差が低減する。

請求項２に記載の発明は、請求項１に記載の発明において、前記特徴点の軌跡の方向もしくは特徴点の軌跡の量は、処理対象の画像内における、前画像の特徴点に対応する対応点の軌跡と、新たに検出された特徴点の軌跡との差に基づいて算出されることを特徴とする。

請求項２に記載の発明によれば、撮影シーンの変化や撮影装置の変動が容易に検知される。処理対象である現在の画像には、一つ前の画像の特徴点に対応する対応点と、新たに検出された特徴点とが存在する。撮影シーンの変化や撮影装置の変動がある場合には、対応点の軌跡と、新たに検出された特徴点の軌跡とが、異なる方向および異なる量となる。

請求項３に記載の発明は、請求項１に記載の発明において、前記特徴点の分布率もしくは特徴点のオーバーラップ率は、画像中心を原点とした特徴点のＸ座標および／またはＹ座標の合計値によって算出されることを特徴とする。

請求項３に記載の発明によれば、撮影シーンの変化や撮影装置の変動が容易に検知される。

請求項４に記載の発明は、請求項１に記載の発明において、前記外部標定要素評価ステップは、前記特徴点の軌跡の方向、特徴点の軌跡の量、特徴点の分布率、特徴点のオーバーラップ率、および縦視差の中から選ばれた複数の値を掛け合わせた値に基づいて、外部標定要素を評価することを特徴とする。

請求項４に記載の発明によれば、撮影シーンの変化や撮影装置の変動が一つのパラメータで検知される。

請求項５に記載の発明は、請求項１に記載の発明において、前記バンドル調整ステップは、外部から与えられる撮影位置および／または撮影姿勢の重み、外部標定要素および特徴点の三次元座標の重みを算出し、各データを重み付けしてバンドル調整することを特徴とする。

請求項５に記載の発明によれば、外部標定要素および特徴点の三次元座標に加えて、外部から与えられる撮影位置および／または撮影姿勢も同時にバンドル調整される。この際、各データは、重みが付けられてバンドル調整されるため、精度の悪いデータは軽く、精度の良いデータは重く扱われる。このため、外部標定要素および特徴点の三次元座標の誤差が低減する。

請求項６に記載の発明は、請求項５に記載の発明において、前記外部から与えられる撮影位置および／または撮影姿勢の重みは、画像の撮影タイミングと外部から与えられる撮影位置および／または撮影姿勢の取得タイミングとの差に基づいて算出されることを特徴とする。

請求項６に記載の発明によれば、画像の撮影タイミングと外部から与えられる撮影位置および／または撮影姿勢の取得タイミングが同期しない場合であっても、外部標定要素および特徴点の三次元座標の誤差が低減する。

請求項７に記載の発明は、請求項５に記載の発明において、前記外部標定要素および特徴点の三次元座標の重みは、特徴点の軌跡の方向、特徴点の軌跡の量、特徴点の分布率、特徴点のオーバーラップ率、および縦視差の少なくともいずれか一つに基づいて算出されることを特徴とする。

請求項７に記載の発明によれば、外部標定要素および特徴点の三次元座標の誤差が低減する。

請求項８に記載の発明は、請求項５に記載の発明において、前記バンドル調整ステップは、新たに検出された特徴点の三次元座標を算出する前、または、新たに検出された特徴点の三次元座標を算出した後に行われることを特徴とする。

請求項８に記載の発明によれば、新たに検出された特徴点の三次元座標を算出する前に、バンドル調整が行われる場合、新たに検出された特徴点の三次元座標の算出精度が向上する。新たに検出された特徴点の三次元座標が算出された後に、バンドル調整が行われた場合、評価後の外部標定要素に基づいて、新たに検出された特徴点の三次元座標が算出されるため、同様の効果を奏する。

請求項９に記載の発明は、請求項１に記載の発明において、特徴点の追跡によって算出された対応点の画像座標と、前記対応点の三次元座標を画像上に投影した画像座標との残差に基づいて、誤対応点を除去する誤対応点除去ステップと、を備えることを特徴とする。

請求項９に記載の発明によれば、特徴点の追跡結果に誤対応点が含まれていても、誤対応点が除去されるため、外部標定要素や特徴点の三次元座標の誤差が低減する。

請求項１０に記載の発明は、請求項１に記載の発明において、前後の画像における対応点の三次元座標をステレオ画像上に投影し、前後のステレオ画像における基線方向に直交する方向の座標値の差に基づいて、誤対応点を除去する誤対応点除去ステップと、を備えることを特徴とする。

請求項１０に記載の発明によれば、特徴点の追跡結果に誤対応点が含まれていても、誤対応点が除去されるため、外部標定要素や特徴点の三次元座標の誤差が低減する。

請求項１１に記載の発明は、請求項９または１０に記載の発明において、前記誤対応点除去ステップは、前記バンドル調整ステップの後、または、前記外部標定要素算出ステップの前に行われることを特徴とする。

請求項１１に記載の発明によれば、誤対応点の除去ステップが、バンドル調整の後に行われる場合には、次画像以降の外部標定要素の算出精度が向上する。一方、誤対応点の除去が、外部標定要素算出の前に行われる場合には、誤対応点に基づいて外部標定要素が算出されないため、外部標定要素の誤差が低減する。

請求項１２に記載の発明は、請求項９または１０に記載の発明において、請求項９または請求項１０に記載の誤対応点除去ステップは、撮影状況の変化に基づいて選択されることを特徴とする。

請求項１２に記載の発明によれば、撮影状況の変化に依存せずに、外部標定要素や特徴点の三次元座標を算出することができる。

請求項１３に記載の発明は、請求項９または１０に記載の発明において、前記誤対応点除去ステップは、ＬＭｅｄＳ推定によって閾値が推定されることを特徴とする。

請求項１３に記載の発明によれば、ＬｍｅｄＳ法は、最小二乗法と比べ、データに外れ値が多く含まれていても、ロバストに外れ値を推定することができる。このため、外部標定要素や特徴点の三次元座標の誤差を低減することができる。

請求項１４に記載の発明は、初期画像中の既知点または外部で測定された撮影位置および／または撮影姿勢に基づき、撮影位置と撮影姿勢とからなる外部標定要素の初期値を算出する初期値算出ステップと、前記初期値算出ステップで用いられた初期画像に続く動画像中の既知点または既に三次元座標が与えられた特徴点に基づいて、外部標定要素を算出する外部標定要素算出ステップと、前記外部標定要素算出ステップによって算出された外部標定要素を、特徴点の軌跡の方向、特徴点の軌跡の量、特徴点の分布率、特徴点のオーバーラップ率、および縦視差の少なくともいずれか一つに基づいて評価する外部標定要素評価ステップと、前記外部標定要素評価ステップによって修正された外部標定要素に基づき、１または複数の画像における外部標定要素および特徴点の三次元座標を同時にバンドル調整するバンドル調整ステップと、前記バンドル調整された外部標定要素に基づき、特徴点の密度が少なくなった領域で新たに検出された特徴点の三次元座標を算出する三次元座標算出ステップと、前記外部標定要素算出ステップから前記三次元座標算出ステップまでの処理を、最終画像になるまで繰り返す繰り返しステップと、を実行させるための位置測定プログラムである。

請求項１４に記載の発明によれば、特徴点の軌跡の方向や量、特徴点の分布率、オーバーラップ率、および縦視差の少なくともいずれか一つに基づいて、撮影シーンの変化や撮影装置の変動を判断することができる。このため、撮影シーンの変化や撮影装置の変動による誤差を低減することができる。

請求項１５に記載の発明は、対象物と撮影部とが少しずつ相対移動して撮影された動画像を取得する動画像取得部と、前記動画像取得部によって取得された画像を順次入力して、前記画像から特徴点の検出および追跡を行う特徴点検出追跡部と、前記動画像取得部の撮影位置を外部から測定する位置測定部と、前記動画像取得部の撮影姿勢を外部から測定する姿勢測定部と、前記動画像取得部によって取得された動画像、前記特徴点検出追跡部によって検出および追跡された特徴点、前記位置測定部によって測定された撮影位置、および前記姿勢測定部によって測定された撮影姿勢を入力して、前記動画像の撮影位置および撮影姿勢からなる外部標定要素および前記特徴点の三次元座標を計算する計算処理部と、を備えた位置測定装置であって、前記計算処理部は、初期画像中の既知点または前記位置測定部によって測定された撮影位置および／または前記姿勢測定部によって測定された撮影姿勢に基づき、撮影位置と撮影姿勢とからなる外部標定要素の初期値を算出する初期値算出部と、前記初期値算出部で用いられた初期画像に続く動画像中の既知点または既に三次元座標が与えられた特徴点に基づいて、外部標定要素を算出する外部標定要素算出部と、前記外部標定要素算出部によって算出された外部標定要素を特徴点の軌跡の方向、特徴点の軌跡の量、特徴点の分布率、特徴点のオーバーラップ率、および縦視差の少なくともいずれか一つに基づいて評価する外部標定要素評価部と、前記外部標定要素評価部によって修正された外部標定要素に基づき、１または複数の画像における外部標定要素および特徴点の三次元座標を同時にバンドル調整するバンドル調整部と、前記バンドル調整された外部標定要素に基づき、特徴点の密度が少なくなった領域で新たに検出された特徴点の三次元座標を算出する三次元座標算出部と、を備え、前記外部標定要素算出部から前記三次元座標算出部までの処理を、最終画像になるまで繰り返すことを特徴とする位置測定装置である。

請求項１５に記載の発明によれば、特徴点の軌跡の方向や量、特徴点の分布率、オーバーラップ率、および縦視差の少なくともいずれか一つに基づいて、撮影シーンの変化や撮影装置の変動を判断することができる。このため、撮影シーンの変化や撮影装置の変動による誤差を低減することができる。

本発明によれば、動画像に基づいて、撮影した位置や姿勢と被写体の三次元位置とを測定する位置測定技術において、撮影シーンの変化や撮影装置の変動による誤差が低減する。

１．第１の実施形態
以下、位置測定方法、位置測定装置およびプログラムの一例について、図面を参照して説明する。

（位置測定装置の構成）
図１は、位置測定装置の全体図であり、図２は、位置測定装置の機能ブロック図である。位置測定装置１は、動画像取得部２、特徴点検出追跡部３、位置測定部４、姿勢測定部５、基準時計部６、計算処理部７を備えている。

動画像取得部２は、ビデオカメラ、工業計測用のＣＣＤカメラ（Ｃｈａｒｇｅ Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ Ｃａｍｅｒａ）、ＣＭＯＳカメラ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｍｅｔａｌ Ｏｘｉｄｅ Ｓｅｍｉｃｏｎｄｕｃｔｏｒ Ｃａｍｅｒａ）などの動画像を取得するカメラで構成される。なお、動画像取得部２は、静止画を取得するカメラでもよい。この場合、撮影対象とカメラが相対的に少しずつ移動しながら撮影された複数枚の静止画像が取得される。

特徴点検出追跡部３は、特徴点検出部８、特徴点追跡部９で構成される。特徴点検出部８は、動画像取得部２が取得した動画像から特徴点を検出する。特徴点追跡部９は、特徴点検出部８が検出した特徴点に対応する対応点を他の画像内で追跡する。特徴点検出部８、特徴点追跡部９は、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）などファームウェアで実装される。なお、特徴点検出部８、特徴点追跡部９は、汎用のコンピュータ上で実行するソフトウェアで構成してもよい。

位置測定部４は、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機、慣性計測装置（ＩＭＵ：Ｉｎｔｅｒｎａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）などで構成される。位置測定部４は、カメラの位置（投影中心）の測定に利用される。撮影環境等に基づいて、ＧＰＳ、ＩＭＵを適宜組み合わせることで、外部標定要素の評価の信頼性を高めることができる。なお、位置測定部４の設置位置とカメラの設置位置との間の位置的なオフセットは、予め設定される。

姿勢測定部５は、ジャイロスコープなどの姿勢センサ、ＩＭＵ、加速時計、角速度計、角加速度計、およびこれらの組み合わせで構成される。撮影環境等に基づいて、これらを適宜組み合わせることで、外部標定要素の評価の信頼性を高めることができる。姿勢測定部５は、カメラの姿勢の測定に利用される。

基準時計部６は、水晶発振器、ルビジウム発振器、電波時計などで構成される。基準時計部６は、動画像撮影部２の時刻、位置測定部４の時刻、および姿勢測定部５の時刻を同一の時間軸に変換する。

計算処理部７は、パーソナルコンピュータ（ＰＣ：Ｐｅｒｓｏｎａｌ Ｃｏｍｐｕｔｅｒ）で構成される。計算処理部７は、制御演算部１０、記憶部１１を備える。制御演算部１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）であり、記憶部１１は、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）等の半導体記憶装置、磁気記憶装置などで構成される。記憶部１１は、オペレーティングシステムや位置測定プログラムを記憶し、制御演算部１０は、プログラムを読み出して実行する。

位置測定プログラムは、初期値算出部１２、外部標定要素算出部１３、画像判断部２０、外部標定要素評価部１４、バンドル調整部１５、三次元座標算出部１６、誤差点除去部１７、詳細計測部１８、外部標定要素・三次元座標出力部１９で構成される。なお、位置測定プログラムは、ＣＤＲＯＭなどの記録媒体によって提供が可能である。

初期値算出部１２は、トータルステーションやＧＰＳによって計測された基準点の三次元座標と、この基準点が撮影された画像における基準点の画像座標とに基づいて、カメラの内部標定要素（焦点距離、主点位置（光軸中心のずれ）、レンズ歪係数）、外部標定要素（カメラの位置、姿勢）の初期値を算出する。

外部標定要素算出部１３は、後方交会法あるいは相互標定法に基づいて、外部標定要素を算出する。後方交会法とは、未知点から３つ以上の既知点へ向かう方向を観測して、それらの方向線の交点として未知点の位置を定める方法である。後方交会法としては、単写真標定、ＤＬＴ法（Ｄｉｒｅｃｔ Ｌｉｎｅｒ Ｔｒａｎｓｆｏｒｍａｔｉｏｎ Ｍｅｔｈｏｄ）が挙げられる。

画像判断部２０は、撮影シーンの変化や撮影装置の変動を、特徴点の軌跡の方向、特徴点の移動量、特徴点の分布率、オーバーラップ率、縦視差などに基づいて判断する。

外部標定要素評価部１４は、画像判断部２０の判断結果に基づいて、画像から算出された外部標定要素を評価する。撮影シーンの変化が相対的に大きいと判断された場合には、位置測定部４が取得した位置データ、姿勢測定部５が取得した姿勢データで外部標定要素が修正される。撮影シーンの変化が相対的に小さいと判断された場合には、画像から算出された外部標定要素をそのまま選択する。

バンドル調整部１５は、評価された外部標定要素と、特徴点の三次元座標とを、１または複数枚または全部の画像間で同時調整する。

三次元座標算出部１６は、前方交会法に基づいて、新たに検出された特徴点の三次元座標を算出する。前方交会法とは、既知の２点以上から未知点へ向かう方向を観測して、それらの方向線の交点として未知点の位置を定める方法である。なお、バンドル調整は、新たに検出された特徴点の三次元座標を算出する前、もしくは新たに検出された特徴点の三次元座標を算出した後に行われる。

誤対応点除去部１７は、バックワードプロジェクションまたは相互標定より算出した縦視差を用いて、追跡された対応点のうち、誤差が相対的に大きい点を除去する。バックワードプロジェクションとは、後方交会法に基づいて、特徴点の三次元座標から算出した画像座標と、追跡された対応点の画像座標との残差から誤差点を除去するものである。また、相互標定とは、左右２枚の画像における６点以上の対応点によって外部標定要素を求めるものである。また、縦視差とは、ステレオ撮影された２枚の画像における対応点のｙ座標の差であり、ステレオ画像の基線方向に直交する方向の座標値の差のことを意味する。なお、ステレオ画像とは、２枚の画像を撮影したカメラの光軸が平行であり、かつ、基線方向に直交する方向が平行な画像対である。

詳細計測部１８は、三次元モデリングを行う。三次元モデリングとは、撮影対象の立体物を形成することを意味する。詳細計測部１５は、特徴点の三次元座標から輪郭線を算出してワイヤーフレームモデルの形成や、面を算出してサーフィスモデルの形成を行う。また、形成されたサーフィスモデルの表面には、２次元の画像がテクスチャマッピングされる。さらに、詳細計測部１５は、マッピングされたテクスチャに基づいて、テクスチャ解析を行い、テクスチャの特徴部分の三次元座標を算出する。テクスチャの特徴部分の三次元座標は、バンドル調整されて最適値に補正される。

外部標定要素・三次元座標出力部１９は、外部標定要素と、特徴点またはテクスチャ特徴部分の三次元座標のデータを出力する。

（位置測定装置の動作）
次に、位置測定装置の全体の動作について説明する。図３、図４は、位置測定装置の動作を示すフローチャートである。

まず、動画像取得部２によって、動画像が取得される（ステップＳ１）。動画像は、動画像取得部２と対象物とが少しずつ相対的に移動して撮影された複数の連続する画像で構成される。複数の連続する画像は、特徴点検出追跡部３に順次入力する。

特徴点検出部８は、特徴点検出追跡部３に入力した画像から特徴点を検出する（ステップＳ２）。特徴点の検出には、モラベック、ラプラシアン、ソーベルなどのフィルタが用いられる。次に、特徴点追跡部９が、特徴点に対応する対応点を次画像内で追跡する（ステップＳ３）。特徴点の追跡には、テンプレートマッチングが用いられる。テンプレートマッチングとしては、残差逐次検定法（ＳＳＤＡ：Ｓｅｑｕｅｎｔｉａｌ Ｓｉｍｉｌａｒｉｔｙ Ｄｅｔｅｃｔｉｏｎ Ａｌｇｏｒｉｔｈｍ）、相互相関係数法などが挙げられる。

特徴点が追跡された画像において、特徴点検出部８は、特徴点の密度が少なくなった領域で新たに特徴点を検出する。特徴点を検出するステップＳ２と特徴点を追跡するステップＳ３は、連続する画像内で繰り返される。特徴点に関する情報は、各画像内に埋め込まれる。そして、特徴点に関する情報が埋め込まれた画像は、計算処理部７に順次出力される。ステップＳ２、ステップＳ３の処理の詳細は、特願２００７−１４７４５７、特願２００７−１８３２５６などに記載されている。

次に、計算処理部７において、基準点が撮影された画像からカメラの内部標定要素と外部標定要素の初期値を算出する（ステップＳ４）。この作業に先立ち、作業者は、対象空間に基準点を設置し、その基準点を撮影しておく。また、作業者は、トータルステーションやＧＰＳを用いて、実空間における基準点の三次元座標を計測しておく。ステップＳ４の初期値算出においては、作業者は、基準点の三次元座標と画像座標とに基づいて、後方交会法によってカメラの内部標定要素と外部標定要素の初期値を算出する。または、外部標定要素の初期値は、トータルステーションやＧＰＳを用いて、特徴点の三次元座標値を直接与える代わりに、複数の撮影位置の三次元座標値を与え、複数の撮影位置で撮影されたステレオ画像から求めるようにしてもよい。

もしくは、作業者は、トータルステーションやＧＰＳを用いず、複数の基準点が描かれた基準板を移動しながら、複数枚の画像を撮影しておく。基準板に描かれた基準点間の距離は既知である。この場合、ステップＳ４の初期値算出において、基準点が撮影された複数枚の画像から基準点の画像座標が算出され、相互標定などを用いてカメラの位置、姿勢の初期値を得る。ただし、この場合、ローカル座標系となる。

次に、入力された各画像について、外部標定要素と、新たに検出された特徴点の三次元座標が算出される（ステップＳ５）。外部標定要素の算出には、後方交会法または相互標定法が用いられ、新たに検出された特徴点の三次元座標の算出には、前方交会法が用いられる。この処理は、全画像について繰り返される。この処理の詳細は、後述する。

さらに詳細に計測を行う場合には（ステップＳ６）、詳細計測が行われる（ステップＳ８）。詳細計測は、ワイヤーフレームモデルの形成、サーフィスモデルの形成、テクスチャマッピング、テクスチャ解析などが行われる。テクスチャ解析では、テクスチャの特徴部分の三次元座標が算出される。テクスチャの特徴部分の三次元座標は、ステップＳ５で算出した外部標定要素や特徴点の三次元座標と共に、バンドル調整されて最適値に補正される。算出された外部標定要素、三次元座標は、データ出力される（ステップＳ７）。

以下、本発明に係るステップＳ５の三次元座標・外部標定要素算出処理の詳細について説明する。図３に示すように、ステップＳ５の三次元座標・外部標定要素の算出処理は、外部標定要素の算出（ステップＳ１０）、算出した外部標定要素の評価（ステップＳ１１）、評価した外部標定要素と既知点の三次元座標のバンドル調整（ステップＳ１２）、新たに検出された特徴点の三次元座標の算出（ステップＳ１３）で構成される。

なお、バンドル調整（ステップＳ１２）は、新たに検出された特徴点の三次元座標を算出する前（図３）、もしくは新たに検出された特徴点の三次元座標を算出した後（図４）に行ってもよい。図３の場合には、新たに検出された特徴点の三次元座標の算出精度が向上する。図４の場合には、評価後の外部標定要素に基づいて、新たに検出された特徴点の三次元座標が算出されるため、同様の効果を奏する。

（ステップＳ１０）外部標定要素の算出
図５は、後方交会法を説明する説明図である。後方交会法とは、未知点Ｏから３つ以上の既知点Ｐ_１、Ｐ_２、Ｐ_３へ向かう方向を観測して、それらの方向線の交点として未知点Ｏの位置を定める方法である。まず、初期値として設定された基準点Ｐ_１、Ｐ_２、Ｐ_３の三次元座標（もしくは、前フレームで求められた三次元座標）に基づいて、後方交会法によって外部標定要素（Ｘ_０，Ｙ_０，Ｚ_０，ω，φ，κ）を算出する。以下、後方交会法として用いられる単写真標定、ＤＬＴ法について説明する。

［単写真標定］
単写真標定は、１枚の写真の中に写された基準点に成り立つ共線条件を用いて、写真を撮影したカメラの位置Ｏ（Ｘ_０，Ｙ_０，Ｚ_０）およびカメラの姿勢（ω，φ，κ）を求める。共線条件とは、投影中心、写真像および地上の対象点（Ｏｐ_１Ｐ_１，Ｏｐ_２Ｐ_２，Ｏｐ_３Ｐ_３）が、一直線上にあるという条件である。また、カメラの位置Ｏ（Ｘ_０，Ｙ_０，Ｚ_０）とカメラの姿勢（ω，φ，κ）は外部標定要素である。

まず、カメラ座標系をｘ，ｙ，ｚ、写真座標系ｘ，ｙ、地上座標系をＸ，Ｙ，Ｚとする。カメラを各座標軸の正方向に対して左回りにそれぞれω，φ，κだけ順次回転させた向きで撮影が行われたものとする。そして、４点の画像座標（図５では３点のみ記載）と対応する基準点の三次元座標を数１に示す２次の射影変換式に代入し、観測方程式を立ててパラメ−タｂ１〜ｂ８を求める。

数１のパラメータｂ１〜ｂ８を用いて、以下の数２から外部標定要素を求める。

［ＤＬＴ法］
ＤＬＴ法は、写真座標と対象空間の三次元座標との関係を３次の射影変換式で近似したものである。ＤＬＴ法の基本式は以下の数１３となる。なお、ＤＬＴ法の詳細については、「村井俊治：解析写真測量、ｐ４６−５１、ｐ１４９−１５５」等を参照する。

数３の式の分母を消去すると、数４の線形式を導き出せる。

さらに、数４を変形すると、以下の数５となる。

数５に６点以上の基準点の三次元座標を代入し、最小二乗法を用いて解くと、写真座標と対象点座標との関係を決定するＬ_１〜Ｌ_１１の１１個の未知変量を取得できる。なお、Ｌ_１〜Ｌ_１１には、外部標定要素が含まれる。

次に、相互標定法による外部標定要素の算出について説明する。相互標定は、既知点がなくとも相対的な外部標定要素が求められる方法である。また、既知点があれば、絶対標定を行うことで、絶対座標を求めることができる。

［相互標定］
図６は、相互標定を説明する説明図である。相互標定は、左右２枚の画像における６点以上の対応点によって外部標定要素を求める。相互標定では、投影中心Ｏ_１とＯ_２と基準点Ｐを結ぶ２本の光線が同一平面内になければならいという共面条件を用いる。以下の数６に、共面条件式を示す。

図６に示すように、モデル座標系の原点を左側の投影中心Ｏ_１にとり、右側の投影中心Ｏ_２を結ぶ線をＸ軸にとるようにする。縮尺は、基線長を単位長さとする。このとき、求めるパラメータは、左側のカメラのＺ軸の回転角κ_１、Ｙ軸の回転角φ_１、右側のカメラのＺ軸の回転角κ_２、Ｙ軸の回転角φ_２、Ｘ軸の回転角ω_２の５つの回転角となる。この場合、左側のカメラのＸ軸の回転角ω_１は０なので、考慮する必要はない。このような条件にすると、数６の共面条件式は数７のようになり、この式を解けば各パラメータが求められる。

ここで、モデル座標系ＸＹＺとカメラ座標系ｘｙｚの間には、次に示すような座標変換の関係式が成り立つ。

これらの式を用いて、次の手順により、未知パラメータ（外部標定要素）を求める。
（１）未知パラメータ（κ_１，φ_１，κ_２，φ_２，ω_２）の初期近似値は通常０とする。
（２）数７の共面条件式を近似値のまわりにテーラー展開し、線形化したときの微分係数の値を数８により求め、観測方程式をたてる。
（３）最小二乗法をあてはめ、近似値に対する補正量を求める。
（４）近似値を補正する。
（５）補正された近似値を用いて、（１）〜（４）までの操作を収束するまで繰り返す。

相互標定が収束した場合、さらに接続標定が行われる。接続標定とは、各モデル間の傾き、縮尺を統一して同一座標系とする処理である。この処理を行った場合、以下の数９で表される接続較差を算出する。算出した結果、ΔＺ_ｊおよびΔＤ_ｊが、所定値（例えば、０．０００５（１／２０００））以下であれば、接続標定が正常に行われたと判定する。

（ステップＳ１１）外部標定要素の評価
次に、外部標定要素が評価される。外部標定要素の算出は、基準点や既に三次元座標が求められた特徴点の三次元座標と、特徴点の追跡によって求められた画像座標に基づいている。したがって、外部標定要素の精度は、特徴点の追跡精度に依存する。また、特徴点の追跡精度は、撮影シーンの著しい変化や撮影装置の変動に依存している。このため、撮影シーンの変化や撮影装置の変動によって、誤差が発生したり、もしくは、計算が不能となる。

したがって、撮影シーンの変化や撮影装置の変動が大きく、画像から算出された外部標定要素が信頼できない場合には、位置測定部４から取得する位置データや姿勢測定部５から取得する姿勢データを利用する。この際、画像から算出された外部標定要素は利用しない。

撮影シーンの変化量や撮影装置の変動量は、特徴点の軌跡の方向や量、特徴点の分布率、オーバーラップ率、縦視差に基づいて判断される。この判断は、画像判断部２０によって行われる。以下、撮影シーンの変化の判断手法について説明する。

［特徴点の軌跡の方向と量による判断］
図７（Ａ）は、前フレームの特徴点の軌跡の方向と、新たに検出した特徴点の軌跡の方向が異なる画像を示す図であり、図７（Ｂ）は、前のフレームの特徴点の軌跡の量と、新たに検出した特徴点の軌跡の量が異なる画像を示す図である。

例えば、図７（Ａ）に示すように、カメラが右方向へ移動すると、特徴点の密度が少なくなる領域に新たに検出した特徴点が増えていく。この際、今までの軌跡の方向に対し、新しく検出された特徴点の軌跡の方向が、９０度、１８０度、・・・と大きく変化した場合には、撮影シーンが変化した、または、撮影装置が変動したと判断する。このような場合、画像から算出した外部標定要素は、信頼性が低いと評価する。そして、外部標定要素は、位置測定部４から取得する位置データや姿勢測定部５から取得する姿勢データに修正される。

特徴点の軌跡の方向の変化は、前フレームの特徴点の軌跡ベクトルａと、新たに検出した特徴点の軌跡ベクトルｂとのなす角θとして定義される。この角度は、数１０の内積の定義から求められる。

また、例えば、図７（Ｂ）に示すように、今までの軌跡の量に対し、新たに検出された特徴点の軌跡の量が、２倍、４倍、・・・と大きく変化した場合には、撮影シーンが変化したと判断する。このような場合、画像から算出した外部標定要素は、信頼性が低いと評価する。そして、外部標定要素は、位置測定部４から取得する位置データや姿勢測定部５から取得する姿勢データに修正される。

軌跡の量の変化は、前フレームの特徴点の軌跡ベクトルａの大きさと、新たに検出した特徴点の軌跡ベクトルｂの大きさとの差である。この差は、以下の数１１から求める。なお、特徴点の軌跡の向きや量に対する閾値は、調整可能とする。

［特徴点の分布率、オーバーラップ率による判断］
撮影シーンの変化や撮影装置の変動が大きい場合、特徴点が追跡不能となり、画像内の点の配置が偏ることがある。図８（Ａ）は、特徴点の分布率が０の画像を示す図であり、図８（Ｂ）は、特徴点の分布率が負の画像を示す図であり、図８（Ｃ）は、特徴点の分布率が正の画像を示す図である。

カメラが水平方向へ移動する場合、水平方向における特徴点の分布率は、画像中心を原点（０，０）とすると、特徴点のＸ座標の合計値として求められる。例えば、図８（Ａ）の場合、特徴点が画像全体に均一に分布しているため、Ｘ座標の合計値は、０となる。また、図８（Ｂ）の場合、特徴点が画像左側に分布しているため、Ｘ座標の合計値は、負となる。図８（Ｃ）の場合、特徴点が画像右側に分布しているため、Ｘ座標の合計値は、正となる。

なお、カメラが鉛直方向へ移動する場合、鉛直方向における特徴点の分布率は、画像中心を原点（０，０）とすると、特徴点のＹ座標の合計値として求められる。したがって、特徴点のＹ座標の合計値によって、鉛直方向における特徴点の分布率を求めてもよい。

また、撮影シーンの変化や撮影装置の変動が大きい場合、前後のフレームでオーバーラップしている特徴点が偏ることがある。オーバーラップしている特徴点とは、特徴点の追跡によって求められた点のことである。図９（Ａ）は、前後のフレームでオーバーラップしている特徴点が偏っている画像を示す図であり、図９（Ｂ）は、前後のフレームでオーバーラップしている特徴点が偏っていない画像を示す図である。

図９（Ａ）に示すように、前後のフレームでオーバーラップしている特徴点が偏っている場合には、画像から算出した外部標定要素は、信頼性が低いと評価する。そして、外部標定要素は、位置測定部４から取得する位置データや姿勢測定部５から取得する姿勢データに修正される。また、図９（Ｂ）に示すように、前後のフレームでオーバーラップしている特徴点が偏っていない場合には、画像から算出した外部標定要素は、信頼性が高いと評価する。そして、画像から算出された外部標定要素を利用する。

オーバーラップしている特徴点の偏り具合（オーバーラップ率）は、特徴点の分布率の算出方法を用いる。なお、特徴点の分布率やオーバーラップ率に対する閾値は、調整可能とする。

［縦視差による判断］
縦視差とは、ステレオ撮影された２枚の画像における対応点のｙ座標の差のことをいう。すなわち、ステレオ画像の基線方向に直交する方向の座標値の差のことである。ステレオ画像は、２枚の画像を撮影したカメラの光軸が平行であり、かつ、基線方向に直交する方向が平行な画像対である。したがって、２枚の画像の対応点のｙ座標は等しいため、ステレオ画像の縦視差は通常０画素となる。

しかしながら、撮影装置の変動があった場合には、縦視差が０画素でなくなる。したがって、縦視差が１画素以上ある場合は、画像から算出された外部標定要素を利用せず、位置測定部４から取得する位置データや姿勢測定部５から取得する姿勢データを利用する。

縦視差は、前フレームの特徴点のｙ座標と、現フレームの特徴点のｙ座標との差を算出することで求められる。なお、縦視差に対する閾値は、調整可能とする。

なお、撮影シーンの変化や撮影装置の変動は、特徴点の軌跡の方向や量、特徴点の分布率、オーバーラップ率、および縦視差のうち、２つ、３つ、４つ、または全てを掛け合わせた値に基づいて判断してもよい。

（ステップＳ１２）バンドル調整
外部標定要素を評価した後、１枚または複数の画像、または全画像における外部標定要素および特徴点の三次元座標を同時にバンドル調整する。バンドル調整は、対象空間の点、画像上の点、投影中心の３点を結ぶ光束（バンドル）が同一直線上になければならないという共線条件に基づき、各画像の光束１本毎に観測方程式を立て、最小二乗法によって同時調整する方法である。共線条件式は以下の数１２のようになる。

バンドル調整は、逐次近似解法を用いる。具体的には、基準点や特徴点の三次元座標、外部標定要素の真値を、各々の近似値（Ｘ’，Ｙ’，Ｚ’）、（Ｘ_０’，Ｙ_０’，Ｚ_０’，ω’，φ’，κ’）に補正量を加えたもの（Ｘ’＋ΔＸ，Ｙ’＋ΔＹ，Ｚ’＋ΔＺ）、（Ｘ_０’＋ΔＸ_０，Ｙ_０’＋ΔＹ_０，Ｚ_０’＋ΔＺ_０，ω’＋Δω，φ’＋Δφ，κ’＋Δκ）とし、基準点や特徴点に対応する画像座標の真値を、計測値（ｘ’，ｙ’）に誤差を加えたもの（ｘ’＋ｄｘ，ｙ’＋ｄｙ）とする。そして、補正量を加えた近似値と、誤差を含む計測値とを、１または複数の画像の共線条件式（数１２）に代入し、近似値回りにテーラー展開して線形化し、最小二乗法により補正量を求める。求めた補正量によって近似値を補正し、同様の操作を繰り返し、収束解を求める。

なお、Δｘ、Δｙは、内部標定要素（焦点距離、主点位置、レンズ歪係数）の補正項を表す。以下の数１４は、レンズが放射方向の歪みを有する場合の内部標定要素の補正モデルである。数１４によって、内部標定要素も同時に調整することができる。いわゆる、セルフキャリブレーション付きバンドル調整である。

（ステップＳ１３）特徴点の三次元座標算出
バンドル調整された外部標定要素に基づいて、特徴点の密度が少なくなった領域で新たに検出された特徴点の三次元座標を算出する。算出方法は、前方交会法、もしくは、ＤＬＴが挙げられる。なお、図４に示すように、ステップＳ１３の三次元座標の算出は、バンドル調整（ステップＳ１２）に先立って処理してもよい。

図１０は、前方交会法を説明する説明図である。前方交会法とは、既知の２点（Ｏ_１，Ｏ_２）以上から未知点Ｐへ向かう方向を観測して、それらの方向線の交点として未知点Ｐの位置を定める方法である。

図１０において、対象空間の座標系をＯ−ＸＹＺとする。前フレーム画像のカメラの投影中心Ｏ_１の座標（Ｘ_０１，Ｙ_０１，Ｚ_０１）およびカメラ座標軸の傾き（ω_１，φ_１，κ_１）と、現フレーム画像のカメラの投影中心Ｏ_２の座標（Ｘ_０２，Ｙ_０２，Ｚ_０２）およびカメラ座標軸の傾き（姿勢）（ω_２，φ_２，κ_２）は、既知である。また、内部標定要素（焦点距離、主点位置、レンズ歪係数）も既知である。この際、前フレーム画像上の点ｐ_１（ｘ_１，ｙ_１）と、これに対応する現フレーム画像上の点ｐ_２（ｘ_２，ｙ_２）が分かれば、対象空間の未知点Ｐ（Ｘ，Ｙ，Ｚ）は、光線Ｏ_１ｐ_１と光線Ｏ_２ｐ_２の交点として定まる。ただし、実際には誤差があり、２本の光線が交わらないため、最小二乗法によって交点位置を求める。

具体的には、２本の共線条件式（数１２）を立て、これに既知の外部標定要素、内部標定要素、対応点の画像座標を代入する。さらに、この共線条件式に未知点Ｐの近似値（Ｘ’，Ｙ’，Ｚ’）に補正量を加えたもの（Ｘ’＋ΔＸ，Ｙ’＋ΔＹ，Ｚ’＋ΔＺ）を代入する。近似値回りにテーラー展開して線形化し、最小二乗法により補正量を求める。求めた補正量によって近似値を補正し、同様の操作を繰り返し、収束解を求める。この操作によって、特徴点の三次元座標Ｐ（Ｘ，Ｙ，Ｚ）が求められる。

（ステップＳ１４）最終フレーム判定
新たに検出した特徴点の三次元座標が算出された後、現在処理している画像が最終画像であるか否かを判定する。現在処理している画像が最終画像でない場合には、ステップＳ１０に戻って、次画像の外部標定要素を算出する。以後、ステップＳ１０〜ステップＳ１４までを最終画像まで繰り返す。現在処理している画像が最終画像である場合には、処理を終了する。

（第１の実施形態の優位性）
第１の実施形態によれば、特徴点の軌跡の方向や量、特徴点の分布率、オーバーラップ率、および縦視差の少なくともいずれか一つに基づいて、撮影シーンの変化や撮影装置の変動が判断される。撮影シーンの変化や撮影装置の変動は、特徴点の追跡を困難し、画像から算出された外部標定要素の精度を劣化させる。したがって、撮影シーンの変化や撮影装置の変動が相対的に大きい場合には、画像から算出された外部標定要素の信頼性が低いと評価し、撮影シーンの変化や撮影装置の変動が相対的に小さい場合には、画像から算出された外部標定要素の信頼性が高いと評価する。

画像から算出された外部標定要素の信頼性が低いと評価される場合には、位置測定部４から取得する位置データや姿勢測定部５から取得する姿勢データを利用し、画像から算出された外部標定要素は利用しない。この態様によって、撮影シーンの変化や撮影装置の変動による誤差が低減する。

２．第２の実施形態
以下、第１の実施形態の変形例について説明する。第２の実施形態は、重み付きバンドル調整を行うものである。

図１１（Ａ）は、第１の実施形態に係るバンドル調整の概念図である。図１１（Ａ）に示すように、第１の実施形態においては、カメラから算出された外部標定要素が、特徴点の軌跡の方向や量、特徴点の分布率、オーバーラップ率、および縦視差の少なくともいずれか一つに基づいて評価される。そして、バンドル調整される外部標定要素は、ＧＰＳの位置データ、姿勢センサの姿勢データ、カメラから算出された外部標定要素の中から最良なデータが選択され、選択された外部標定要素に基づいてバンドル調整が行われる。したがって、バンドル調整は、図１１（Ａ）の点線枠で示すデータに基づいて行われる。

一方、第２の実施形態では、外部標定要素が最良なデータに修正されるのではなく、ＧＰＳの位置データ、姿勢センサの姿勢データ、カメラから算出された外部標定要素は、重み付けされてバンドル調整される。図１１（Ｂ）は、第２の実施形態に係る重み付きバンドル調整の概念図である。図１１（Ｂ）に示すように、精度の悪いデータは重みが軽くなり、精度の良いデータは重みが重くなる。この結果、外部標定要素、特徴点の三次元座標は、図１１（Ｂ）の実線枠の範囲にバンドル調整される。

重みは、各データの精度に基づいて算出される。各データの精度をσとすると、重みｗは、数１５によって算出される。以下、各データの精度について説明する。

［ＧＰＳの位置データ、姿勢センサの姿勢データの精度］
ＧＰＳの位置データ、姿勢センサの姿勢データの精度は、画像の撮影タイミングとＧＰＳの位置データや姿勢センサの姿勢データの取得タイミングとの差に基づいた誤差と、ＧＰＳの位置データ、姿勢センサの姿勢データの固有の精度とに基づく。

一般に、ＧＰＳや姿勢センサの時刻はカメラの時刻と同期していない。したがって、本発明においては、ＧＰＳの位置データの取得時刻と画像の撮影時刻は、基準時計６が出力する基準時刻に変換される。なお、変換された時刻は、位置データや姿勢データの伝送遅れや画像データの伝送遅れを考慮したものとなる。

また、動画像のフレームレートとＧＰＳや姿勢センサの取得レートが一致していないため、画像の撮影タイミングとＧＰＳの位置データや姿勢センサの姿勢データの取得タイミングも同期していない。このため、ＧＰＳや姿勢センサの精度は、画像の撮影タイミングとＧＰＳの位置データや姿勢センサの姿勢データの取得タイミングとの差に基づく誤差を考慮する。

画像の撮影タイミングとＧＰＳの位置データや姿勢センサの姿勢データの取得タイミングとの差をΔｔとすれば、カメラが速度ｖ（角速度ｖ_ｘ、ｖ_ｙ、ｖ_ｚ）で移動するとき、ＧＰＳの位置データの誤差Ｅｔは、数１６となり、姿勢センサの姿勢データの誤差Ｅｘｔ、Ｅｙｔ、Ｅｚｔは、数１７となる。

画像の撮影時刻とＧＰＳの位置データや姿勢センサの姿勢データの取得時刻は、基準時計６によって同一の時間軸で表せるため、Δｔは、ＧＰＳの位置データや姿勢センサの姿勢データが取得された時刻と、この時刻に最も近い時刻に撮影された画像との時間差となる。

また、速度ｖは、数１８に示すように動画像内の特徴点の移動距離と動画像のフレームレートから求められる。また、角速度ｖ_ｘ、ｖ_ｙ、ｖ_ｚは、姿勢センサやＩＭＵから取得した値を用いる。

次に、ＧＰＳや姿勢センサの固有の精度について説明する。ＧＰＳの測位精度は、衛星位置（ＤＯＰ：Ｄｉｌｕｔｉｏｎ ｏｆ Ｐｒｅｃｉｓｉｏｎ）、マルチパス、リンククオリティ（ＬＱ：Ｌｉｎｋ Ｑｕａｌｉｔｙ）、測位演算の収束評価などの値によって変わる。例えば、ＲＴＫ−ＧＰＳ（Ｒｅａｌ Ｔｉｍｅ Ｋｉｎｅｍａｔｉｃ ＧＰＳ）システムの場合、ＧＰＳ衛星の位置、周辺環境によるマルチパスの影響、基準局からの補正情報などを考慮して、測位モードがリアルタイムに変化する。以下の表１は、各測位モードにおける測位精度の概略を示している。

ＧＰＳの測位精度をＧＰＳ_ｒｔｋｅとすると、ＧＰＳの位置データの精度の合計Ｅｔ_ＡＬＬは、数１９となる。

また、姿勢センサは、原理上加速度の積分や二重積分などをとることから、経過時間によって誤差が累積する。したがって、初期値が設定されたときから、もしくは、ハードウェアリセットされたときからの計測時間をｔ、累積誤差が指数関数１．０５^ｔ−１に近似するとすれば、姿勢センサの精度は、数２０となる。また、これに姿勢センサ固有の精度ＩＭＵｅを考慮すると、姿勢センサの精度の合計Ｅｘｔ_ＡＬＬ、Ｅｙｔ_ＡＬＬ、Ｅｚｔ_ＡＬＬは、数２１となる。

［外部標定要素および特徴点の三次元座標の精度］
外部標定要素および特徴点の三次元座標の精度は、第１の実施形態における、特徴点の軌跡の方向や量、特徴点の分布率、オーバーラップ率、および縦視差の少なくともいずれか一つに基づき求められる。すなわち、特徴点の軌跡の方向の変化は、２つのベクトルａ、ベクトルｂのなす角θであり（数１０）、特徴点の軌跡の量の変化は、２つのベクトルａ、ベクトルｂの差｜ａ−ｂ｜であるため（数１１）、精度σは、θまたは｜ａ−ｂ｜となる。

また、特徴点の分布率やオーバーラップ率は、画像中心を原点とする特徴点のＸ座標および／またはＹ座標の合計値であるため、精度σは、分布率やオーバーラップ率の絶対値となる。また、縦視差は、ステレオ画像における特徴点のｙ座標の差であるため、精度σは、特徴点のｙ座標の差の絶対値となる。

なお、外部標定要素および特徴点の三次元座標の精度σは、特徴点の軌跡の方向や量、特徴点の分布率、オーバーラップ率、および縦視差のうち、２つ、３つ、４つ、または全てを掛け合わせた値としてもよい。

［重み付きバンドル調整］
重み付きバンドル調整における各補正量は、以下の関数Ｇを最小にする値として求めることができる。例えば、ｗ_２がカメラによる三次元座標の重み、ｗ_３およびｗ_４が外部標定要素の重み、となる。

また、ＧＰＳやＩＭＵによる位置データ、姿勢センサの姿勢データが加わった場合には、以下の数２３となり、これを最小にする。例えば、ｗ_５がＧＰＳによる位置データの重み、ｗ_６がＩＭＵの位置データの重み、ｗ_７が姿勢センサの重み、となる。

（第２の実施形態の優位性）
第２の実施形態によれば、外部標定要素および特徴点の三次元座標に加えて、位置測定部４の位置データや姿勢測定部５の姿勢データも同時にバンドル調整される。この際、各データは、重みが付けられてバンドル調整されるため、精度の悪いデータは軽く、精度の良いデータは重く扱われる。このため、外部標定要素および特徴点の三次元座標の誤差が低減する。

また、位置測定部４の位置データや姿勢測定部５の姿勢データの重みは、画像の撮影タイミングと位置測定部４の位置データや姿勢測定部５の姿勢データの取得タイミングとの差に基づいて算出される。このため、画像の撮影タイミングと位置測定部４の位置データや姿勢測定部５の姿勢データの取得タイミングが同期しない場合であっても、外部標定要素および特徴点の三次元座標の誤差が低減する。

さらに、外部標定要素および特徴点の三次元座標の重みは、特徴点の軌跡の方向や量、特徴点の分布率、オーバーラップ率、および縦視差の少なくともいずれか一つに基づいて算出される。画像から算出される外部標定要素や特徴点の三次元座標における精度は、撮影シーンの変化や撮影装置の変動に依存する傾向が強い。このため、撮影シーンの変化や撮影装置の変動の判断材料となる特徴点の軌跡の方向や量、特徴点の分布率、オーバーラップ率、および縦視差を外部標定要素および特徴点の三次元座標の重みとすることで、外部標定要素および特徴点の三次元座標の誤差が低減する。

３．第３の実施形態
以下、第１〜第３の実施形態の変形例について説明する。第４の実施形態は、追跡（トラッキング）された対応点のうち相対的に誤差の大きい点（誤対応点）を除去し、外部標定要素や特徴点の三次元座標の算出精度を向上させるものである。

図１２〜図１４は、誤対応点の除去を取り入れたフローチャートである。誤対応点の除去は、バンドル調整の後（図１２、図１３）、もしくは外部標定要素算出の前（図１４）に行う。図１２の場合、次フレーム以降における外部標定要素の算出精度が向上する。また、図１３の場合、除去された誤対応点の三次元座標は算出されないため、計算処理が効率化する。また、図１４の場合、現フレームにおける外部標定要素の算出精度が向上する。

誤対応点の除去法には、バックワードプロジェクション、もしくは相互標定によって算出した縦視差を利用する。これらの除去法は、撮影状況によって使い分けを行ったり、双方を行ったりする。例えば、単写真標定を用いたバックワードプロジェクションによる誤対応点除去は、撮影シーンの変化が大きい場合に利用し、相互標定による誤対応点除去は、撮影シーンの変化が少ない場合に利用する。

また、誤差判定の閾値の推定に、ロバスト推定を用いてもよい。例えば、ＬＭｅｄＳ推定（Ｌｅａｓｔ Ｍｅｄｉａｎ ｏｆ Ｓｑｕａｒｅｓ推定）、ＲＡＮＳＡＣ推定（ＲＡＮｄｏｍ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ推定）、Ｍ推定が利用される。特に、ＬＭｅｄＳ推定は、ロバスト性に優れており、誤差範囲が不明でも自動的に誤対応点を除去することができる。

なお、誤差判定の閾値は、ロバスト推定に依らず、調整可能な固定閾値としてもよい。この態様によれば、演算処理が簡易であるため、処理速度が向上する。

以下、図１５のフローチャートに従って説明する。図１５（Ａ）は、バックワードプロジェクションによる誤対応点の除去を行うフローチャートであり、図１５（Ｂ）は、縦視差を用いた誤対応点の除去を行うフローチャートである。

［バックワードプロジェクションによる誤対応点の除去］
まず、追跡された対応点がランダムにサンプリングされる（ステップＳ２０）。次に、サンプリングされた点ごとに、後方交会法（単写真標定またはＤＬＴ）によって算出された外部標定要素と追跡された対応点の三次元座標に基づいて、対応点の画像座標（ｘ’，ｙ’）を算出する。例えば、単写真標定の場合、数６の共線条件式に外部標定要素（Ｘ_０，Ｙ_０，Ｚ_０，ω，φ，κ）と対応点の三次元座標（Ｘ，Ｙ，Ｚ）を代入することで、画像座標（ｘ’，ｙ’）が逆算される（ステップＳ２１）。

そして、サンプリングされた点ごとに、後方交会法によって得られた画像座標（ｘ’，ｙ’）と、追跡された対応点の画像座標（ｘ，ｙ）との残差の二乗を求め、二乗残差の中央値を算出する（ステップＳ２２）。その後、ステップＳ２０〜ステップＳ２２を繰り返し、中央値の最小値（ＬＭｅｄＳ）を求める（ステップＳ２３）。以下の数２４にＬＭｅｄＳ基準を示す。

なお、ステップＳ２０〜ステップＳ２２の繰り返しに必要な回数は、ｑ回のランダムサンプリングで少なくとも一回は例外値が全く含まれない確率を考えることで決定できる。例えば、全データ中の例外値の割合をｃとすると、この確率Ｐは、以下の数２５に示すようになる。例えば、ｃ＝０．３、ｎ＝３とすると、Ｐ＝０．０１になるためには、ｑ＝１１回となる。

ステップＳ２３で求めたＬＭｅｄＳ値を閾値として誤差の大きい点を除去する（ステップＳ２４）。

［縦視差を用いた誤対応点の除去］
まず、前後の画像における対応点がランダムにサンプリングされる（ステップＳ３０）。次に、図６に示すように、サンプリングされた対応点（Ｐ_１，Ｐ_２）の実空間上の点Ｐの三次元座標は、相互標定の共面条件式（数６）によって、点線で示されるステレオ画像上に再投影される（ステップＳ３１）。

縦視差は、再投影された２つの対応点のｙ座標の差として求められる。縦視差とは、ステレオ撮影された２枚の画像における対応点のｙ座標の差であり、ステレオ画像の基線方向に直交する方向の座標値の差のことを意味する。なお、ステレオ画像とは、２枚の画像を撮影したカメラの光軸が平行であり、かつ、基線方向に直交する方向が平行な画像対である。次に、縦視差の二乗を求め、その中央値を算出する（ステップＳ３２）。その後、ステップＳ３０〜ステップＳ３２を繰り返し、中央値の最小値（ＬＭｅｄＳ）を求める（ステップＳ３３）。ＬＭｅｄＳ基準は数２２による。

ステップＳ３３で求めたＬＭｅｄＳ値を閾値として誤差の大きい点を除去する（ステップＳ３４）。

（第３の実施形態の優位性）
特徴点の追跡結果には、多くの誤対応点が含まれている。したがって、誤対応点に基づいて算出された外部標定要素や特徴点の三次元座標は、精度が劣る。しかしながら、誤対応点を除去することで、外部標定要素や特徴点の三次元座標の算出精度が向上し、誤差が低減する。

また、撮影状況の変化に基づいて、誤対応点の除去法（バックワードプロジェクションによって算出した残差または相互標定によって算出した縦視差）が選択されることで、撮影状況の変化に依存せずに、外部標定要素や特徴点の三次元座標を算出することができる。

さらに、ＬｍｅｄＳ法は、最小二乗法と比べ、データに外れ値が多く含まれていても、ロバストに外れ値を推定することができる。このため、外部標定要素や特徴点の三次元座標の誤差を低減することができる。

本発明は、動画像に基づいて、撮影した位置や姿勢と被写体の三次元位置とを測定する位置測定方法、位置測定装置、およびそのプログラムに利用することができる。

位置測定装置の全体図である。 位置測定装置の機能ブロック図である。 位置測定装置の動作を示すフローチャートである。 位置測定装置の動作を示すフローチャートである。 後方交会法を説明する説明図である。 相互標定を説明する説明図である。 前フレームの特徴点の軌跡の方向と、新たに検出した特徴点の軌跡の方向とが異なる画像を示す図（Ａ）と、前のフレームの特徴点の軌跡の量と、新たに検出した特徴点の軌跡の量が異なる画像を示す図（Ｂ）である。 特徴点の分布率が０の画像を示す図（Ａ）と、特徴点の分布率が負の画像を示す図（Ｂ）と、特徴点の分布率が正の画像を示す図（Ｃ）である。 前後のフレームでオーバーラップしている特徴点が偏っている画像を示す図（Ａ）と、前後のフレームでオーバーラップしている特徴点が偏っていない画像を示す図（Ｂ）である。 前方交会法の概念図である。 第１の実施形態に係るバンドル調整の概念図（Ａ）と、第２の実施形態に係る重み付きバンドル調整の概念図（Ｂ）である。 誤対応点の除去を取り入れたフローチャートである。 誤対応点の除去を取り入れたフローチャートである。 誤対応点の除去を取り入れたフローチャートである。 バックワードプロジェクションによる誤対応点の除去を行うフローチャート（Ａ）と、縦視差による誤対応点の除去を行うフローチャート（Ｂ）である。

符号の説明

１…位置測定装置、２…動画像取得部、３…特徴点検出追跡部、４…位置測定部、５…姿勢測定部、６…基準時計、７…計算処理部、８…特徴点検出部、９…特徴点追跡部、１０…制御演算部、１１…記憶部、１２…初期値算出部、１３…外部標定要素算出部、１４…外部標定要素評価部、１５…バンドル調整部、１６…三次元座標算出部、１７…誤対応点除去部、１８…詳細計測部、１９…外部標定要素・三次元座標出力部、２０…画像判断部。

Claims (15)

1. 初期画像中の既知点または外部で測定された撮影位置および／または撮影姿勢に基づき、撮影位置と撮影姿勢とからなる外部標定要素の初期値を算出する初期値算出ステップと、
   前記初期値算出ステップで用いられた初期画像に続く動画像中の既知点または既に三次元座標が与えられた特徴点に基づいて、外部標定要素を算出する外部標定要素算出ステップと、
   前記外部標定要素算出ステップによって算出された外部標定要素を、特徴点の軌跡の方向、特徴点の軌跡の量、特徴点の分布率、特徴点のオーバーラップ率、および縦視差の少なくともいずれか一つに基づいて評価する外部標定要素評価ステップと、
   前記外部標定要素評価ステップによって修正された外部標定要素に基づき、１または複数の画像における外部標定要素および特徴点の三次元座標を同時にバンドル調整するバンドル調整ステップと、
   前記バンドル調整された外部標定要素に基づき、特徴点の密度が少なくなった領域で新たに検出された特徴点の三次元座標を算出する三次元座標算出ステップと、
   前記外部標定要素算出ステップから前記三次元座標算出ステップまでの処理を、最終画像になるまで繰り返す繰り返しステップと、を備えることを特徴とする位置測定方法。
2. 前記特徴点の軌跡の方向もしくは特徴点の軌跡の量は、前画像の特徴点に対応する現画像の対応点の軌跡と、現画像で新たに検出された特徴点の軌跡との差に基づいて算出されることを特徴とする請求項１に記載の位置測定方法。
3. 前記特徴点の分布率もしくは特徴点のオーバーラップ率は、画像中心を原点とした特徴点のＸ座標および／またはＹ座標の合計値によって算出されることを特徴とする請求項１に記載の位置測定方法。
4. 前記外部標定要素評価ステップは、前記特徴点の軌跡の方向、特徴点の軌跡の量、特徴点の分布率、特徴点のオーバーラップ率、および縦視差の中から選ばれた複数の値を掛け合わせた値に基づいて、外部標定要素を評価することを特徴とする請求項１に記載の位置測定方法。
5. 前記バンドル調整ステップは、外部から与えられる撮影位置および／または撮影姿勢の重み、外部標定要素および特徴点の三次元座標の重みを算出し、各データを重み付けしてバンドル調整することを特徴とする請求項１に記載の位置測定方法。
6. 前記外部から与えられる撮影位置および／または撮影姿勢の重みは、画像の撮影タイミングと外部から与えられる撮影位置および／または撮影姿勢の取得タイミングとの差に基づいて算出されることを特徴とする請求項５に記載の位置測定方法。
7. 前記外部標定要素および特徴点の三次元座標の重みは、特徴点の軌跡の方向、特徴点の軌跡の量、特徴点の分布率、特徴点のオーバーラップ率、および縦視差の少なくともいずれか一つに基づいて算出されることを特徴とする請求項５に記載の位置測定方法。
8. 前記バンドル調整ステップは、新たに検出された特徴点の三次元座標を算出する前、または、新たに検出された特徴点の三次元座標を算出した後に行われることを特徴とする請求項５に記載の位置測定方法。
9. 特徴点の追跡によって算出された対応点の画像座標と、前記対応点の三次元座標を画像上に投影した画像座標との残差に基づいて、誤対応点を除去する誤対応点除去ステップと、を備えることを特徴とする請求項１に記載の位置測定方法。
10. 前後の画像における対応点の三次元座標をステレオ画像上に投影し、前後のステレオ画像における基線方向に直交する方向の座標値の差に基づいて、誤対応点を除去する誤対応点除去ステップと、を備えることを特徴とする請求項１に記載の位置測定方法。
11. 前記誤対応点除去ステップは、前記バンドル調整ステップの後、または、前記外部標定要素算出ステップの前に行われることを特徴とする請求項９または１０に記載の位置測定方法。
12. 請求項９または請求項１０に記載の誤対応点除去ステップは、撮影状況の変化に基づいて選択されることを特徴とする請求項９または１０に記載の位置測定方法。
13. 前記誤対応点除去ステップは、ＬＭｅｄＳ推定によって閾値が推定されることを特徴とする請求項９または１０に記載の位置測定方法。
14. 初期画像中の既知点または外部で測定された撮影位置および／または撮影姿勢に基づき、撮影位置と撮影姿勢とからなる外部標定要素の初期値を算出する初期値算出ステップと、
    前記初期値算出ステップで用いられた初期画像に続く動画像中の既知点または既に三次元座標が与えられた特徴点に基づいて、外部標定要素を算出する外部標定要素算出ステップと、
    前記外部標定要素算出ステップによって算出された外部標定要素を、特徴点の軌跡の方向、特徴点の軌跡の量、特徴点の分布率、特徴点のオーバーラップ率、および縦視差の少なくともいずれか一つに基づいて評価する外部標定要素評価ステップと、
    前記外部標定要素評価ステップによって修正された外部標定要素に基づき、１または複数の画像における外部標定要素および特徴点の三次元座標を同時にバンドル調整するバンドル調整ステップと、
    前記バンドル調整された外部標定要素に基づき、特徴点の密度が少なくなった領域で新たに検出された特徴点の三次元座標を算出する三次元座標算出ステップと、
    前記外部標定要素算出ステップから前記三次元座標算出ステップまでの処理を、最終画像になるまで繰り返す繰り返しステップと、を実行させるための位置測定プログラム。
15. 対象物と撮影部とが少しずつ相対移動して撮影された動画像を取得する動画像取得部と、
    前記動画像取得部によって取得された画像を順次入力して、前記画像から特徴点の検出および追跡を行う特徴点検出追跡部と、
    前記動画像取得部の撮影位置を外部から測定する位置測定部と、
    前記動画像取得部の撮影姿勢を外部から測定する姿勢測定部と、
    前記動画像取得部によって取得された動画像、前記特徴点検出追跡部によって検出および追跡された特徴点、前記位置測定部によって測定された撮影位置、および前記姿勢測定部によって測定された撮影姿勢を入力して、前記動画像の撮影位置および撮影姿勢からなる外部標定要素および前記特徴点の三次元座標を計算する計算処理部と、を備えた位置測定装置であって、
    前記計算処理部は、初期画像中の既知点または前記位置測定部によって測定された撮影位置および／または前記姿勢測定部によって測定された撮影姿勢に基づき、撮影位置と撮影姿勢とからなる外部標定要素の初期値を算出する初期値算出部と、
    前記初期値算出部で用いられた初期画像に続く動画像中の既知点または既に三次元座標が与えられた特徴点に基づいて、外部標定要素を算出する外部標定要素算出部と、
    前記外部標定要素算出部によって算出された外部標定要素を、特徴点の軌跡の方向、特徴点の軌跡の量、特徴点の分布率、特徴点のオーバーラップ率、および縦視差の少なくともいずれか一つに基づいて評価する外部標定要素評価部と、
    前記外部標定要素評価部によって修正された外部標定要素に基づき、１または複数の画像における外部標定要素および特徴点の三次元座標を同時にバンドル調整するバンドル調整部と、
    前記バンドル調整された外部標定要素に基づき、特徴点の密度が少なくなった領域で新たに検出された特徴点の三次元座標を算出する三次元座標算出部と、を備え、
    前記外部標定要素算出部から前記三次元座標算出部までの処理を、最終画像になるまで繰り返すことを特徴とする位置測定装置。
    

Images