WO2010001940A1

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

Info

Publication number: WO2010001940A1
Application number: PCT/JP2009/062067
Authority: WO
Inventors: 哲治穴井; 高地　伸夫; 暢之深谷
Original assignee: 株式会社トプコン
Priority date: 2008-07-01
Filing date: 2009-07-01
Publication date: 2010-01-07
Also published as: EP2309225A1; EP2309225A4; US8811718B2; US20110096957A1

Abstract

動画像に基づいて、撮影した位置や姿勢と被写体の三次元位置とを測定する位置測定技術において、誤差を低減する。位置測定方法は、ステップＳ１０によって算出された外部標定要素を、画像の撮影タイミングと外部から与えられる撮影位置および／または撮影姿勢の取得タイミングとの差に基づいて修正する外部標定要素修正ステップＳ１１と、ステップＳ１１によって修正された外部標定要素に基づき、１または複数の画像の外部標定要素および特徴点の三次元座標を同時にバンドル調整するバンドル調整ステップＳ１２と、バンドル調整された外部標定要素に基づき、画像中の特徴点の密度が少なくなった領域で新たに検出された特徴点の三次元座標を算出する三次元座標算出ステップＳ１３と、ステップＳ１０からステップＳ１３までの処理を、最終画像になるまで繰り返すステップとを備える。

Description

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

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

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

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

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

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

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

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

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

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

　請求項２に記載の発明は、請求項１に記載の発明において、前記外部標定要素修正ステップは、画像の撮影タイミングと外部から与えられる撮影位置および／または撮影姿勢の取得タイミングとの差と、外部から与えられる撮影位置および／または撮影姿勢の精度と、に基づいて外部標定要素を修正することを特徴とする。

　請求項２に記載の発明によれば、画像から算出した外部標定要素は、外部から与えられる撮影位置および／または撮影姿勢の固有の精度も考慮して修正される。

　請求項３に記載の発明は、請求項２に記載の発明において、前記外部から与えられる撮影位置の精度は、ＲＴＫ－ＧＰＳの各測位モードの測位精度であり、前記測位モードに基づいて外部標定要素を修正する閾値を変更するように構成されていることを特徴とする。

　請求項３に記載の発明において、ＲＴＫ－ＧＰＳ（Ｒｅａｌ　Ｔｉｍｅ　Ｋｉｎｅｍａｔｉｃ　ＧＰＳ）は、精度に影響を与え得るＧＰＳ衛星の位置、周辺環境によるマルチパス、および基準局からの補正情報などに基づいて、測位モードをリアルタイムに変更する。したがって、外部標定要素を修正する閾値を測位モードに基づいて変更することで、外部標定要素が適切に修正される。

　請求項４に記載の発明は、請求項２に記載の発明において、前記外部から与えられる撮影姿勢の精度は、撮影姿勢の計測時間に基づく精度であることを特徴とする。

　請求項４に記載の発明において、外部から与えられる撮影姿勢は、測定の原理上計測時間によって誤差が累積する傾向がある。したがって、撮影姿勢の計測時間に基づく精度を考慮することで、外部標定要素が適切に修正される。

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

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

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

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

　請求項７に記載の発明は、請求項５に記載の発明において、前記外部標定要素および特徴点の三次元座標の重みは、特徴点の追跡によって算出された対応点の画像座標と、前記対応点の三次元座標を画像上に投影した画像座標との残差に基づいて算出されることを特徴とする。

　請求項７に記載の発明において、画像から算出される外部標定要素や特徴点の三次元座標における精度は、特徴点の追跡精度に依存する傾向が強い。請求項７に記載の残差は、追跡精度を表す。このため、残差を外部標定要素および特徴点の三次元座標の重みとすることで、外部標定要素および特徴点の三次元座標の誤差が低減する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

位置測定装置の全体図である。位置測定装置の機能ブロック図である。位置測定装置の動作を示すフローチャートである。位置測定装置の動作を示すフローチャートである。後方交会法を説明する説明図である。相互標定を説明する説明図である。画像の撮影タイミングとＧＰＳの位置データの取得タイミングが同期していない場合のタイミングチャート（Ａ）と、画像の撮影タイミングとＧＰＳの位置データの取得タイミングが同期している場合のタイミングチャート（Ｂ）である。画像の撮影タイミングとＧＰＳの位置データの取得タイミングとの差を考慮したＧＰＳの位置データが、設定した閾値の範囲内にある場合を示す図（Ａ）と、画像の撮影タイミングとＧＰＳの位置データの取得タイミングとの差を考慮したＧＰＳの位置データが、設定した閾値の範囲外にある場合を示す図（Ｂ）である。姿勢測定部の姿勢データの計測時間と累積誤差との関係を示す図（Ａ）と、画像の撮影タイミングと姿勢測定部の姿勢データの取得タイミングが同期していない場合のタイミングチャート（Ｂ）である。前方交会法の概念図である。位置測定装置の機能ブロック図である。位置測定装置の動作を示すフローチャートである。位置測定装置の動作を示すフローチャートである。前フレームの特徴点の軌跡の方向と、新たに検出した特徴点の軌跡の方向が異なる画像を示す図（Ａ）と、前のフレームの特徴点の軌跡の量と、新たに検出した特徴点の軌跡の量が異なる画像を示す図（Ｂ）である。特徴点の分布率が０の画像を示す図（Ａ）と、特徴点の分布率が負の画像を示す図（Ｂ）と、特徴点の分布率が正の画像を示す図（Ｃ）である。前後のフレームでオーバーラップしている特徴点が偏っている画像を示す図（Ａ）と、前後のフレームでオーバーラップしている特徴点が偏っていない画像を示す図（Ｂ）である。第１，２の実施形態に係るバンドル調整の概念図（Ａ）と、第３の実施形態に係る重み付きバンドル調整の概念図（Ｂ）である。誤対応点の除去を取り入れたフローチャートである。誤対応点の除去を取り入れたフローチャートである。誤対応点の除去を取り入れたフローチャートである。バックワードプロジェクションによる誤対応点の除去を行うフローチャート（Ａ）と、縦視差による誤対応点の除去を行うフローチャート（Ｂ）である。

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

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

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

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

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

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

　姿勢測定部５は、ジャイロスコープなどの姿勢センサ、ＩＭＵ、加速時計、角速度計、角加速度計、およびこれらの組み合わせで構成される。撮影環境等に基づいて、これらを組み合わせることで、画像から算出した外部標定要素を適切に修正することができる。姿勢測定部５は、カメラの姿勢の測定に利用される。

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

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

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

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

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

　外部標定要素修正部１４は、画像から算出した外部標定要素を、画像の撮影タイミングと位置測定部４が取得した位置データ、姿勢測定部５が取得した姿勢データの取得タイミングとの差を考慮して修正する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

（ステップＳ１１）外部標定要素の修正
　次に、外部標定要素を修正する。外部標定要素は、画像の撮影タイミングと位置測定部４の位置データや姿勢測定部５の姿勢データの取得タイミングとの差を考慮した位置測定部４の位置データや姿勢測定部５の姿勢データを評価した上で修正する。以下、外部標定要素（カメラの位置、姿勢）の修正方法について説明する。

［カメラ位置の修正］
　位置測定部４（ＧＰＳもしくはＩＭＵ）から取得した位置データは、各機器に応じた固有の精度がある。例えば、その精度をσとすると、２σ、３σなどをＧＰＳやＩＭＵの位置データの評価値（閾値）とする。すなわち、画像の撮影タイミングとＧＰＳやＩＭＵの位置データの取得タイミングとの差に基づく誤差を考慮したＧＰＳやＩＭＵの位置データが、その閾値の範囲外であれば、画像から算出したカメラの位置が選択される。一方、画像の撮影タイミングとＧＰＳやＩＭＵの位置データの取得タイミングとの差に基づく誤差を考慮したＧＰＳやＩＭＵの位置データが、その閾値の範囲内であれば、ＧＰＳやＩＭＵの位置データが選択される。なお、カメラの設置位置とＧＰＳもしくはＩＭＵの設置位置との間における位置オフセットは、予め補正される。

　以下、ＧＰＳの位置データにおける誤差の算出方法について説明する。一般に、ＧＰＳの時刻はカメラの時刻と同期していない。同期させるためには、巨額の投資が必要となり、実質的に実現が困難だからである。したがって、本発明においては、ＧＰＳの位置データの取得時刻と画像の撮影時刻は、基準時計６が出力する基準時刻に変換される。なお、変換された時刻は、位置データの伝送遅れや画像データの伝送遅れを考慮したものとなる。

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

　図７は、画像の撮影タイミングとＧＰＳの位置データの取得タイミングが同期していない場合のタイミングチャート（Ａ）と、画像の撮影タイミングとＧＰＳの位置データの取得タイミングが同期している場合のタイミングチャート（Ｂ）である。

　図７（Ａ）に示すように、画像の撮影タイミングとＧＰＳの位置データの取得タイミングとの差をΔｔとすれば、カメラが速度ｖで移動するとき、ＧＰＳの位置データの誤差Ｅｔは、以下の数１０のようになる。

　基準時計６がない場合には、数１０におけるΔｔは、一般に未知である。しかしながら、動画像のフレームレートを３０ｆｐｓ、ＧＰＳの位置データの取得レートを１ｆｐｓとするならば、図７（Ｂ）に示すように、Δｔは最大で±１／３０秒であると仮定することができる。なお、Δｔは、変更するように構成してもよい。

　基準時計６がある場合には、画像の撮影時刻とＧＰＳの位置データの取得時刻は、基準時計６によって同一の時間軸で表せるため、ＧＰＳの位置データが取得された時刻と、この時刻に最も近い時刻に撮影された画像との時間差がΔｔとなる。図７（Ａ）に示すΔｔは、画像の撮影時刻とＧＰＳの位置データの取得時刻とを差分することで得られる。

　また、速度ｖは、動画像内の特徴点の移動距離と動画像のフレームレートから求められる。以下の数１１は、速度ｖの算出式である。例えば、（Ｘ_ｉ－Ｘ_ｉ－１）＝０．１ｍ、フレームレートが３０ｆｐｓとすれば、ｖ＝３ｍ／ｓｅｃとなる。したがって、ＧＰＳの位置データの誤差Ｅｔは、３×（１／３０）＝０．１ｍとなる。

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

　したがって、ＧＰＳの測位精度ＧＰＳ_ｒｔｋｅを考慮すれば、画像の撮影タイミングとＧＰＳの位置データの取得タイミングとの差を考慮したＧＰＳの位置データの誤差合計Ｅｔ_ＡＬＬは、数１２となる。例えば、Ｅｔ＝０．１ｍ、ＲＴＫ　Ｆｉｘ測位モードでＧＰＳ_ｒｔｋｅ＝０．０３ｍだった場合、ＧＰＳの位置データの誤差合計は、Ｅｔ_ＡＬＬ＝０．１３ｍとなる。また、閾値をＲＴＫ　Ｆｉｘ測位モードの３σに設定した場合、閾値は、０．０３×３＝０．０９ｍとなる。この場合は、Ｅｔ_ＡＬＬ＞閾値となるため、画像から算出した外部標定要素（カメラの位置）を利用し、ＧＰＳの位置データを利用しない。

　なお、ＧＰＳの位置データが、表１に記載の測位モードのうち、どの測位モードで測定されたものかによって、外部標定要素（カメラの位置）を修正するか否かを選択してもよい。例えば、ＲＴＫ　Ｆｉｘ測位モード以外は利用しないと選択したとする。この場合、画像から算出した外部標定要素を選択する。

　ＧＰＳの位置データの誤差を算出した後、画像の撮影タイミングとＧＰＳの位置データの取得タイミングとの差を考慮したＧＰＳの位置データが評価される。図８（Ａ）は、画像の撮影タイミングとＧＰＳの位置データの取得タイミングとの差を考慮したＧＰＳの位置データが、設定した閾値の範囲内にある場合を示す図であり、図８（Ｂ）は、画像の撮影タイミングとＧＰＳの位置データの取得タイミングとの差を考慮したＧＰＳの位置データが、設定した閾値の範囲外にある場合を示す図である。図８（Ａ）に示すように、画像の撮影タイミングとＧＰＳの位置データの取得タイミングとの差を考慮したＧＰＳの位置データＥｔ_ＡＬＬが、設定した閾値の範囲内であれば、ＧＰＳの位置データが選択される。一方、図８（Ｂ）に示すように、画像の撮影タイミングとＧＰＳの位置データの取得タイミングとの差を考慮したＧＰＳの位置データＥｔ_ＡＬＬが、設定した閾値の範囲外であれば、画像から算出した外部標定要素（カメラの位置）が選択される。

［カメラ姿勢の修正］
　姿勢測定部５（姿勢センサやＩＭＵ）から取得した姿勢データは、その想定し得る精度σの２σ、３σなどを評価値（閾値）として評価される。すなわち、画像の撮影タイミングと姿勢センサやＩＭＵの姿勢データの取得タイミングとの差に基づく誤差を考慮した姿勢データが、その閾値の範囲外であれば、画像から算出したカメラの姿勢が選択される。一方、画像の撮影タイミングと姿勢センサやＩＭＵの姿勢データの取得タイミングとの差に基づく誤差を考慮した姿勢データが、その閾値の範囲内であれば、姿勢測定部５の姿勢データが選択される。

　なお、姿勢測定部５の姿勢データは、カメラ座標軸の３つの回転角（ω，φ，κ）で構成されるが、いずれか一つを評価してもよいし、二つでも三つ全部を評価してもよい。この際、姿勢センサやＩＭＵの精度によって使用するか否かを設定する。

　以下、姿勢センサの誤差の算出方法について説明する。図９（Ａ）は、姿勢センサの姿勢データの計測時間と累積誤差との関係を示す図である。一般に、姿勢センサの姿勢データは、原理上加速度の積分や二重積分などをとるため、経過時間によって誤差が累積する。したがって、初期値が設定されたときから、もしくはハードウェアリセットされたときからの計測時間をｔ、累積誤差が指数関数１．０５^ｔ－１に近似するとすれば、姿勢センサの姿勢データの誤差Ｅｘｔ、Ｅｙｔ、Ｅｚｔは、以下の数１３となる。例えば、ｔ＝１０ｓｅｃの場合、Ｅｘｔ、Ｅｙｔ、Ｅｚｔ＝０．６３°となる。なお、累積誤差は、姿勢センサやＩＭＵごと、計測軸ごとに異なるため、近似式は個別に設定される。

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

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

　図９（Ｂ）は、画像の撮影タイミングと姿勢センサの姿勢データの取得タイミングが同期していない場合のタイミングチャートである。図９（Ｂ）に示すように、画像の撮影タイミングと姿勢センサの姿勢データの取得タイミングとの差をΔｔ、その際のカメラの姿勢が角速度ｖ_ｘ、ｖ_ｙ、ｖ_ｚで変化するとすれば、姿勢センサの姿勢データの誤差Ｅｘｔ、Ｅｙｔ、Ｅｚｔは、さらに数１４のようになる。

　基準時計６がある場合、画像の撮影時刻と姿勢センサの姿勢データの取得時刻は、基準時計６によって同一の時間軸で表せるため、姿勢センサの姿勢データが取得された時刻と、この時刻に最も近い時刻に撮影された画像との時間差がΔｔとなる。図９（Ｂ）に示すΔｔは、画像の撮影時刻と姿勢センサの姿勢データの取得時刻とを差分することで得られる。また、角速度ｖ_ｘ、ｖ_ｙ、ｖ_ｚは、姿勢センサやＩＭＵから取得した値を用いる。例えば、ｖ_ｘ＝１．２５°／ｓｅｃ、ｖ_ｙ＝３．８６°／ｓｅｃ、ｖ_ｚ＝１０．１２°／ｓｅｃ、Δｔ＝１／１２０ｓｅｃとすれば、Ｅｘｔ＝０．６４°、Ｅｙｔ＝０．６６°、Ｅｚｔ＝０．７１°となる。

　さらに、姿勢センサの固有の精度ＩＭＵｅも考慮すると、姿勢センサの姿勢データの誤差合計Ｅｘｔ_ＡＬＬ、Ｅｙｔ_ＡＬＬ、Ｅｚｔ_ＡＬＬは、数１５のようになる。

　姿勢センサの姿勢データの誤差Ｅｘｔ_ＡＬＬ、Ｅｙｔ_ＡＬＬ、Ｅｚｔ_ＡＬＬが、設定した閾値の範囲外であれば、画像から算出した外部標定要素（カメラの姿勢）が選択される。姿勢センサの姿勢データの誤差Ｅｘｔ_ＡＬＬ、Ｅｙｔ_ＡＬＬ、Ｅｚｔ_ＡＬＬが、設定した閾値の範囲内であれば、姿勢センサの姿勢データが選択される。

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

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

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

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

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

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

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

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

（第１の実施形態の優位性）
　第１の実施形態によれば、画像の撮影タイミングと位置測定部４の位置データや姿勢測定部５の姿勢データの取得タイミングとの差は、位置測定部４の位置データや姿勢測定部５の姿勢データの誤差とみなされる。画像の撮影タイミングと位置測定部４の位置データや姿勢測定部５の姿勢データの取得タイミングとの差を考慮した位置測定部４の位置データや姿勢測定部５の姿勢データが、設定した閾値の範囲内か否かによって、外部標定要素が修正される。画像の撮影タイミングと位置測定部４の位置データや姿勢測定部５の姿勢データの取得タイミングが同期しない場合であっても、外部標定要素は適切に修正されるため、誤差が低減する。

　また、位置測定部４の位置データまたは姿勢測定部５の姿勢データの誤差は、時間差Δｔに基づく誤差に、位置測定部４の位置データや姿勢測定部５の姿勢データにおける固有の精度が加味される。このため、画像から算出した外部標定要素は適切に修正される。

　特に、ＲＴＫ－ＧＰＳ（Ｒｅａｌ　Ｔｉｍｅ　Ｋｉｎｅｍａｔｉｃ　ＧＰＳ）の場合は、精度に影響を与え得るＧＰＳ衛星の位置、周辺環境によるマルチパス、および基準局からの補正情報などに基づいて、測位モードをリアルタイムに変更する。したがって、外部標定要素を修正する閾値を測位モードに基づいて変更することで、外部標定要素が適切に修正される。

　一方、姿勢測定部５の姿勢データは、測定の原理上、計測時間ｔによって誤差が累積する傾向がある。したがって、姿勢データの計測時間ｔに基づく精度を考慮することで、外部標定要素が適切に修正される。

２．第２の実施形態
　以下、第１の実施形態の変形例について説明する。第２の実施形態は、特徴点の軌跡の方向や量、特徴点の分布率、オーバーラップ率、および縦視差の少なくともいずれか一つに基づいて、撮影シーンの変化や撮影装置の変動を判断するものである。

（位置測定装置の構成）
　図１１は、位置測定装置の機能ブロック図である。位置測定プログラムは、図２と比べ、画像判断部２０をさらに備えている。また、図２の外部標定要素修正部１４に替わり、外部標定要素評価部２１を備えている。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Claims

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