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

Description

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

図１１（Ａ）は、第１の実施形態に係るバンドル調整の概念図である。図１１（Ａ）に示すように、第１の実施形態においては、カメラから算出された外部標定要素が、ＧＰＳの位置データまたは姿勢センサの姿勢データの時間差Δｔに基づく誤差を基準として修正される。そして、バンドル調整される外部標定要素は、ＧＰＳの位置データ、姿勢センサの姿勢データ、カメラから算出された外部標定要素の中から最良なデータが選択され、選択された外部標定要素に基づいてバンドル調整が行われる。したがって、バンドル調整は、図１１（Ａ）の点線枠で示すデータに基づいて行われる。

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

重みは、それぞれのデータの誤差に基づいて算出される。ＧＰＳの位置データ、姿勢センサの姿勢データの誤差は、第１の実施形態における誤差の算出方法と同様である。すなわち、画像の撮影タイミングとＧＰＳの位置データや姿勢センサの姿勢データの取得タイミングとの差Δｔに基づいた誤差が加味される。この誤差をσとすると、重みｗは、以下の数１９によって算出される。

また、カメラから算出された外部標定要素や三次元座標の誤差は、バックワードプロジェクションによる残差や、相互標定による縦視差が用いられる。この処理の詳細については後述する。この誤差をσとして、数１９から重みｗを算出する。

そして、重み付きバンドル調整における各補正量は、以下の関数Ｇを最小にする値として求めることができる。例えば、ｗ_２がカメラによる三次元座標の重み、ｗ_３が姿勢センサによる姿勢データの重み、ｗ_４がＧＰＳによる位置データの重み、となる。

また、ＩＭＵによるカメラ位置、カメラ２による外部標定要素が加わった場合には、以下の数２１となり、これを最小にする。例えば、ｗ_５がＩＭＵによるカメラ位置の重み、ｗ_６がカメラ２によるカメラ位置の重み、ｗ_７がカメラ２によるカメラ姿勢の重み、となる。

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

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

さらに、外部標定要素および特徴点の三次元座標の重みは、バックワードプロジェクションによって算出した残差または相互標定によって算出した縦視差に基づいて算出される。画像から算出される外部標定要素や特徴点の三次元座標における精度は、特徴点の追跡精度に依存する傾向が強い。このため、追跡精度（残差や縦視差）を外部標定要素および特徴点の三次元座標の重みとすることで、外部標定要素および特徴点の三次元座標の誤差が低減する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

符号の説明

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

Claims (15)

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

Images