特許 5987823 画像センサおよび運動または位置センサから生じたデータを融合するための方法およびシステム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】5987823

(24)【登録日】2016年8月19日

(45)【発行日】2016年9月7日

(54)【発明の名称】画像センサおよび運動または位置センサから生じたデータを融合するための方法およびシステム

(51)【国際特許分類】

   G01B 11/00 20060101AFI20160825BHJP

   G06T 7/60 20060101ALI20160825BHJP

   G01C 11/00 20060101ALI20160825BHJP

   G01B 11/26 20060101ALI20160825BHJP

【ＦＩ】

   G01B11/00 H

   G06T7/60 150P

   G01C11/00

   G01B11/26 H

【請求項の数】8

【全頁数】24

(21)【出願番号】特願2013-510540(P2013-510540)

(86)(22)【出願日】2011年4月19日

(65)【公表番号】特表2013-534616(P2013-534616A)

(43)【公表日】2013年9月5日

(86)【国際出願番号】EP2011056241

(87)【国際公開番号】WO2011144408

(87)【国際公開日】20111124

【審査請求日】2014年3月31日

(31)【優先権主張番号】1053803

(32)【優先日】2010年5月17日

(33)【優先権主張国】FR

(73)【特許権者】

【識別番号】510163846

【氏名又は名称】コミシリア ア レネルジ アトミック エ オ エナジーズ オルタネティヴズ

(74)【代理人】

【識別番号】100071054

【弁理士】

【氏名又は名称】木村 高久

(72)【発明者】

【氏名】ミショー、ジュリアン

(72)【発明者】

【氏名】バルトリ、アドリアン

(72)【発明者】

【氏名】ガスパール、フランソワ

【審査官】 梶田 真也

(56)【参考文献】

【文献】 特開２０１０−０１４４５０（ＪＰ，Ａ）

【文献】 特開２０１０−０１４４４３（ＪＰ，Ａ）

【文献】 特開２００８−０８９３１４（ＪＰ，Ａ）

【文献】 特開２００６−２１５９２４（ＪＰ，Ａ）

【文献】 特開２００５−０３３３１９（ＪＰ，Ａ）

【文献】 特開２００３−２８１５０４（ＪＰ，Ａ）

【文献】 Alexandre Eudes et al，Error Propagations for Local Bundle Adjustment，Computer Vision and Pattern Recognition, 2009. CVPR 2009. IEEE Conference on ，米国，２００９年 ６月２０日，2411-2418

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０１Ｂ １１／００ − １１／３０

Ｇ０１Ｃ １１／００

Ｇ０６Ｔ ７／００ − ７／６０

(57)【特許請求の範囲】

【請求項1】

第１の画像センサから、および運動および／または位置計測モジュールから生じたデータを融合するための方法において、前記第１の画像センサおよび前記運動および／または位置計測モジュールは一体型で、デバイスを形成し、前記方法は、前記デバイスの位置測定を行うために、処理モジュールによって実行され、少なくとも以下のステップ：
特定の時点ｔに関して、前記デバイス、画像カメラ、および運動および／または位置計測モジュールの現在の姿勢

【数1】

の定義を行うために、画像上の幾つかの点を取得することから成る第１のステップと、
前記同じ特定の時点ｔに関して、前記運動および／または位置計測モジュールからの計測結果を用いた、前記デバイス（カメラおよび運動／位置センサ）の予測姿勢

【数2】

の予測ステップと、
前記デバイス（カメラｃおよび運動センサｋ）の前記運動センサｋに関する関連の制約ε_ｋを決定するステップと、
前記制約の重み付け係数λ_ｔを自動的に決定するステップと、
前記デバイスの現在の姿勢ｐ_ｔの推定ステップと、
以下のコスト関数：

【数3】

であるコスト関数を用いたバンドル調整技術によって、前記デバイスのカメラおよびセンサの姿勢の同時最適化ステップと、
を含むことを特徴とする方法。

【請求項2】

請求項１に記載の方法において、所定数の第１の姿勢を用いて前記画像センサの環境を再構築し、３Ｄプリミティブの三角測量を選択された姿勢に関してそれらを構築する興味対象要素から実行することにより、キー姿勢およびシーン要素（３Ｄ）のデータベースを初期化するステップを含むことを特徴とする方法。

【請求項3】

請求項１に記載の方法において、前記重み付け係数の自動選択ステップは、前記デバイスの姿勢の精度向上ステップの結果を使用し、ビジョンによる姿勢の推定を行うためのモジュール［２０４、３０４］によって提供された前記デバイスの姿勢の第１の推定から開始され、以前に推定されたシーン要素を考慮に入れ、モジュール［２０９］において重み付けの自動選択モジュール［２０８］によって提供され、重み付けの自動選択モジュール［２０８、３０８］において学習された係数ラムダλまたはλ_ｔによって重み付けされた予測誤差（制約）をコスト項に加えることによって行われ、前記精度向上ステップの最後に、システムの現在の姿勢を姿勢のデータベース［２１２］に加えることを特徴とする方法。

【請求項4】

請求項１に記載の方法において、前記重み付けの自動選択ステップは、前記デバイスの姿勢の精度向上ステップ［３０９］を含み、前記コスト関数は、前記カメラ上のシーンの３Ｄプリミティブの再投影誤差のみから成り、前記カメラの姿勢は、現在精度向上が行われており、次に、前記制約の構築ステップが続き、その後、前記重み付けの自動選択ステップが続き、
前記精度向上ステップの最後に、システムの現在の姿勢を姿勢のデータベース（履歴）［３１２］に加えることを特徴とする方法。

【請求項5】

請求項１に記載の方法において、以下のステップ：第１の画像Ｉ１をキー画像として選択するステップと、次に、映像中で前記第１の画像Ｉ１から可能な限り離れて、かつ前記第１の画像Ｉ１に対して少なくともＭ個の興味対象対応点を有して第２の画像Ｉ２を選択するステップと、次に、第３の画像Ｉ３および前記第２の画像Ｉ２間で対応する少なくともＭ個の興味対象点が存在し、かつ前記第３の画像Ｉ３および前記第１の画像Ｉ１間で対応する少なくともＭ’個の興味対象点が存在するように、前記第２の画像Ｉ２から最も離れた前記第３の画像Ｉ３を選択するステップと、後に続く第ｎの画像Ｉｎに関して同様のステップとを含むキー画像の選択ステップを含み、同様に続けることで、第ｎ−２の画像Ｉｎ−２、第ｎ−１の画像Ｉｎ−１、および第ｎの画像Ｉｎが、同じ３Ｄプリミティブに対応する少なくともＭ個の興味対象点を有することを確認することを特徴とする方法。

【請求項6】

請求項１に記載の方法において、前記制約は、以下の制約リスト：
位置制約

【数4】

配向制約

【数5】

式中、Ｒは、配向をパラメータ化する回転行列であり、Ｉは、恒等行列であり、スケール係数制約（または変換基準制約）

【数6】

式中、Ｔは、ｐの一部である、
から選択されることを特徴とする方法。

【請求項7】

請求項１に記載の方法において、前記デバイスの姿勢

【数7】

を予測するために、前記センサｋの最後の計測結果

【数8】

に対応する、時点ｔ’≦ｔにおける前記デバイスの状態

【数9】

の推定を提供する逆観測モデル

【数10】

を用い、

【数11】

このモデルは、前記運動／位置センサによって得られた計測結果

【数12】

および前の状態ｘ_ｔ−１から画像時間Ｉ_ｔに対応する時点ｔにおける前記デバイスの姿勢

【数13】

を推定し、式中、Ｆ_ｔは、その前の状態からデバイスの新しい状態ｗ_ｔを定義し、ｎは誤差ベクトルであることを特徴とする方法。

【請求項8】

第１の画像センサ（１０１）から、および運動および／または位置計測モジュールから生じたデータを融合するためのシステムであって、前記第１のセンサおよび前記運動および／または位置計測モジュール（１０２）は一体型で、デバイスを形成し、前記システムは、請求項１から７のいずれか一項に記載の方法の前記ステップを実行するのに適した処理装置（１０３）を含むシステム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明の対象は、カメラ等の視覚システムのロバストで、すなわち、外部摂動（突然の動き、照明等）に対してあまり敏感でなく、かつ正確な位置測定のために、画像センサおよび運動または位置センサから生じたデータの融合を可能にするシステムおよび方法に関する。

【0002】

例えば、これを、ある環境の三次元、つまり３Ｄでの位置測定およびモデリングに適用する。これにより、リアルタイムでの３Ｄの軌道の算出も可能となり、その結果、「拡張現実」技術の実施に利用することができる。

【0003】

本発明は、装着型であるか否かを問わずカメラ（例えば、動画でのＨＤカメラ）、および運動センサ（慣性）または他のあらゆる位置センサ（ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）、オドメータ等）を含むシステムに適用可能である。

【0004】

より一般的には、これは、コンピュータビジョン技術、拡張現実の領域、３Ｄ再構築およびモデリング、ロボット工学等に適用可能である。

【0005】

定義
姿勢という単語は、カメラまたはカメラと運動計測モジュールとを含むデバイスの位置および配向の概念を統合したものである。バンドル調整の領域におけるコスト関数の概念は、先行技術に従い、投影誤差を算出する最適化アルゴリズム（二次元観測（画像に対する計測）と理論上の再投影との距離の最小二乗の意味での最小化）に関連するものとして定義される。

【0006】

「最適化姿勢」という表現は、カメラまたはデバイスの位置および配向の最適化を意味する。

【0007】

シーンという単語は、特に三次元でのそれらの位置（例えば、３Ｄの点、３Ｄの線分等）によって特徴付けられる１組のプリミティブを指す。これらの要素またはプリミティブは、１つまたは複数のカメラによって撮影された実環境の再構築を形成する。要約すれば、「シーン」という単語は、再構築された環境に相当する。

【0008】

「デバイス」という用語は、本明細書において、運動計測モジュールまたは画像センサおよび運動または位置センサを組み込んだカメラによって形成されるアセンブリを指すために用いる。

【0009】

「システム」という単語は、上記に定義したデバイスに加えて、通信手段と共に情報またはデータを処理する手段を含むアセンブリを指す。

【0010】

システムは、（動作中である）デバイスの姿勢を算出する。処理装置がデバイスと同じ支持材上にない場合には、姿勢が異なるので、システムの姿勢の算出は行わない。実際、本発明による方法では、システムの移動要素の姿勢を決定する。

【0011】

各センサは、世界（シーン）を観測し、それ自体の変位（デバイスの変位ではない）の計測を行う。従って、デバイスの動きを得るためには、当業者には公知の座標系の単純な変化（本明細書において後に定義する関数Ｈｋにおいてもモデリングされる）を適用する。

【背景技術】

【0012】

本発明において提起される課題は、特に、様々なセンサに関連する不確定要素に対して先験的とならずに、物体の軌道を決定する、または３Ｄの環境を再構築する、あるいは、より一般的に、デバイスの位置測定を行うことを目的に、画像および慣性データを融合させるという課題を解決することである。

【0013】

実際、画像を曖昧にしてしまう雑音（画像、慣性、ＧＰＳ）の特徴または雑音センサによって提供された情報（さらにこれらは経時的に異なり得る）を得ることは必ずしも可能であるとは限らない。ビジョンおよび慣性データを統合する位置測定技術の大部分は、拡張カルマンフィルタを用いた、あるいは当業者に公知の粒子フィルタを用いた最適化方法に基づく。

【0014】

これらの方法は、ビジョンアルゴリズムを用いてシステムの姿勢を推定し、その後、拡張カルマンフィルタによって、この「ビジョン」計測結果を融合する。これらの異種データを融合するためには、これらの方法が異なる計測結果に対して雑音共分散因子を導入することが必要である。非常に重要なこの項は、多くの場合、推定が困難であり、一般的に、先験的に固定されている。位置測定の正確さは、このパラメータに非常に左右される。

【0015】

カルマンによる最適化の別の問題点は、かなり多量のシーンデータを扱う際のその非有効性である。これらの技術は、システムの状態および環境（シーン）のマップである２つの主要素を最適化する。システムステータスは、システムダイナミクスをモデリングするために選択された運動モデルに依存する。一般的に、その中には、必要に応じて速度や加速度と共に、一般座標系で表現されたシステム姿勢（姿勢という単語は、位置および配向の概念を統合したものである）が見られる。環境マップは、位置測定された要素、例えば、撮影されたシーンの点から成る。

【0016】

リアルタイムの状況では、カルマンに基づいた略称「ＳＬＡＭ」（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）でよく知られている、リアルタイムで環境の位置測定およびモデリングを行う方法は、マップの要素数を（数十（ａ ｆｅｗ ｔｅｎｓ）に）制限するものである。この削減の影響は、最終的な位置測定の精度を大幅に損なうことである。

【0017】

さらに、先行技術において、拡張カルマンフィルタは、Ｇａｕｓｓ−Ｎｅｗｔｏｎ最適化（第１の反復）の近似であることが示されている。

【0018】

要するに、これらの方法によって提供される利点にもかかわらず、これらは、以下のデメリットを提示する。

【0019】

ビジョンのみによる位置測定は、あまりロバストではない（素早い動き、純回転等の重大な動き、カメラの一時的明滅等の問題）。さらに、カメラは、デキャリブレート（ｄｅｃａｌｉｂｒａｔｅｄ）状態となり得る（衝撃、温度変化等）。

【0020】

慣性データの統合による位置測定によれば、中期および長期の正確な位置測定が可能ではない（ドリフト、センサバイアス、雑音等の問題）。従って、正確にシステムの位置測定を行うためには、複数のセンサを用いる必要がある。

【発明の概要】

【課題を解決するための手段】

【0021】

定義
ｔ＝特定の時点または時間インデックス、
ｗ：カメラ（従って、ｗ＝ｃ）または運動センサ（ｗ＝ｋで、ｋはカメラの音声インデックス）のいずれかである任意のセンサのインデックス、
ｃ：カメラセンサを示すインデックス（あるいは、数個のセンサが存在する場合は、画像センサのインデックス）、
ｘ_ｔ：時点ｔにおけるシーン（またはその一部）およびカメラ姿勢（またはそれらの一部）の推定を含むベクトル、

【数1】

：時点ｔにおいてセンサｗによって行われた計測に関連する観測ベクトル、
ｓ：シーン（再構築された環境）、
ｐ：カメラの姿勢、これは、位置Ｔおよび配向Ｒを含む。

【0022】

本発明によるシステムおよび方法は、バンドル調整によって画像に関連するデータを用いて運動および／または位置データ間の自動重み付けを実施する新しいアプローチに基づく。

【0023】

本発明は、第１の画像センサ、および運動および／または位置計測モジュールから生じたデータを融合するための方法に関し、前記第１のセンサおよび前記運動および／または位置計測モジュールは一体型で、デバイスを形成し、この方法は、前記デバイスの位置測定を行うために処理モジュールによって実行され、この方法は、少なくとも以下のステップ：
特定の時点ｔに関して、デバイス（画像カメラ、および運動および／または位置計測モジュール）の姿勢

【数2】

の定義を行うために、画像上の幾つかの点を取得することから成る第１のステップと、
同じ特定の時点ｔに関して、運動および／または位置計測モジュールからの計測結果を用いた、デバイス（カメラおよび運動および／または位置センサ）の姿勢

【数3】

の予測ステップと、
デバイス（カメラおよびセンサ）ｋに関する関連の制約ε_ｋを決定するステップと、
前記制約の重み付け係数λ_ｔを自動的に決定するステップと、
前記デバイスの現在の姿勢ｐ_ｔの推定ステップと、
以下のコスト関数：

【数4】

であるコスト関数を用いたバンドル調整技術による同時最適化ステップと、
を含むことを特徴とする。

【0024】

この方法は、例えば、所定数の第１の姿勢を用いて画像センサの環境を再構築し、３Ｄプリミティブの三角測量を選択された姿勢に関してそれらを構築する興味対象要素から実行することによって、キー姿勢およびシーン要素（３Ｄ）のデータベースを初期化するステップを含む。

【0025】

ある実施形態によれば、重み付け係数の自動選択ステップは、デバイスの姿勢の精度向上ステップの結果を使用し、これは、ビジョンによって姿勢の推定を行うモジュールによって提供されたデバイスの姿勢の第１の推定から開始され、以前に推定されたシーン要素を考慮に入れ、重み付けの自動選択モジュールによって提供され、重み付けの自動選択モジュールにおいて学習した係数ラムダλまたはλ_ｔによって重み付けされた予測誤差（制約）をコスト項に加えることにより行われ、精度向上ステップの最後に、システムの現在の姿勢を姿勢のデータベース（履歴）に加える。

【0026】

重み付けの自動選択ステップは、例えば、デバイスの姿勢の精度向上ステップを含み、精度向上モジュールにおいて、コスト関数は、カメラ上のシーンの３Ｄプリミティブの再投影誤差のみから成り、このカメラの姿勢は、現在精度向上が行われており、次に、制約の構築ステップが続き、その後、重み付けの自動選択ステップが続き、精度向上ステップの最後に、システムの現在の姿勢を姿勢のデータベース（履歴）に加える。

【0027】

この方法は、以下のステップ：第１の画像Ｉ１をキー画像として選択するステップと、次に、映像中でＩ１から可能な限り離れて、かつＩ１に対して少なくともＭ個の興味対象対応点を有して第２の画像Ｉ２を選択するステップと、次に、Ｉ３およびＩ２間で対応する少なくともＭ個の興味対象点が存在し、かつＩ３およびＩ１間で対応する少なくともＭ’個の興味対象点が存在するように、Ｉ２から最も離れた第３の画像Ｉ３を選択するステップと、後に続く画像Ｉｎに関して同様のステップとを含むキー画像の選択ステップも含むことができ、同様に続けることで、画像Ｉｎ−２、Ｉｎ−１、およびＩｎが、同じ３Ｄプリミティブに対応する少なくともＭ個の興味対象点を有することを確認する。

【0028】

制約の値は、例えば、以下の制約リスト：
位置制約

【数5】

配向制約

【数6】

式中、Ｒは、配向をパラメータ化する回転行列であり、Ｉは、恒等行列である等
スケール係数制約（または変換基準制約）

【数7】

から選択される。

【0029】

デバイスの姿勢

【数8】

を予測するために、この方法は、センサｋの最後の計測結果

【数9】

に対応する、時点ｔ’≦ｔにおけるデバイスの状態

【数10】

の推定を提供する逆観測モデル

【数11】

を用いることが可能であり、

【数12】

このモデルは、運動／位置センサによって得られた計測結果

【数13】

および前の状態ｘ_ｔ−１から画像時間Ｉ_ｔに対応する時点ｔにおけるデバイスの姿勢

【数14】

を推定する。

【0030】

本発明は、第１の画像センサから、および運動および／または位置計測モジュールから生じたデータを融合するためのシステムにも関し、前記第１のセンサおよび前記運動および／または位置計測モジュールは一体型で、デバイスを形成し、前記システムは、上記方法のステップを実行するのに適した処理装置を含む。

【0031】

本発明の他の特徴および利点は、図面を添付した、一例として少しも限定的ではなく提供される１つまたは複数の実施形態例の以下の記載を読めば、より明白に分かるであろう。

【図面の簡単な説明】

【0032】

【図1】本発明による方法を実行可能なシステムアーキテクチャの一例である。

【図2】観測Ｏｉおよび再投影ＲＰｉの図である。

【図3】本発明による方法の概要の第１の例である。

【図4】本発明による方法の利用の第２の概要である。

【発明を実施するための形態】

【0033】

本発明において実施される原理およびモジュールのさらなる理解を提供するために、以下の例を、少しも限定的な意味ではなく、本方法の異なる実施形態のステップを実行するのに適した処理装置に接続された画像カメラおよび運動および／または位置センサを含むシステムの実例として提供する。場合によっては、自身の運動の計測を可能にするモジュールを備えたカメラを使用することが可能となる。

【0034】

要約すると、本発明による方法には、位置測定の適用（ＳＬＡＭ）における「バンドル調整」のステップにおいて重み付けされる姿勢間制約（ｉｎｔｅｒ−ｐｏｓｅ ｃｏｎｓｔｒａｉｎｔ）（すなわち、軌道計測デバイスに接続されたビジョンによって計測される姿勢に対する制約）が組み込まれる。運動モデルに関連する慣性データは、新しい画像の時点におけるシステムの姿勢の予測に役立つ。次に、この予測を用いて、姿勢間制約を構築する。学習理論から生じた技術は、効率的に、すなわち、自動的かつコンテキストに適して、リアルタイムでバンドル調整において直接的に重み付け係数（共分散信号の雑音比）を選択する。このように重み付けされた姿勢間制約は、次に、バンドル調整のコスト関数の範囲内で統合される。拡張バンドル調整は、特に数回のＧａｕｓｓ−Ｎｅｗｔｏｎ反復を行うので、カルマンタイプのアルゴリズムと比較してより正確である。

【0035】

図１は、本方法の実施が可能なシステムアーキテクチャの図の一例を示す。
本発明によるシステム１００は、以下を含む：
・ピンホールカメラ、全方向性、三次元ＴＯＦカメラ、ステレオカメラ、ウェブカメラ、赤外線等の画像センサ１０１、
・ＧＰＳ、オドメータ等の運動または位置センサ１０２；画像センサ１０１および運動センサは、同じ支持材上に存在してもよい；運動または位置センサ１０２は、それが生成する、または計測するデータを記憶するバッファメモリ１０７を組み込むことも可能である、
・同じ支持材上に存在する、あるいは正規の位置から離すことも可能な処理装置１０３。この処理装置が画像および位置センサから離れている場合には、これは、物理的通信手段（有線、インターネット）、または無線あるいはワイヤレス手段によって、情報またはデータのやりとりを行う、
・処理装置１０３は、例えば、本発明による方法のステップの実行に適したプロセッサ１０４と、例えばシステムの姿勢のデータベース１０５、シーンの３Ｄプリミティブと、３Ｄプリミティブおよび画像におけるそれらの観測を結び付ける関連性とのデータベース１０６であるデータの保存手段とから成る。運動センサによって生成されたデータを記憶するためのバッファメモリが運動センサに組み込まれていない場合には、このメモリは、処理装置１０３に挿入される。

【0036】

ＬＣＤスクリーンまたはＰＣモニタ等のオペレータによる可視化手段１０８、例えばロボットを自律的に操縦することを目的として（ロボットは、例えばロボットのモータを制御することによって、本発明による方法のおかげで移動する）、ドライバ（モータ等）の制御を行うモジュール１０９。

【0037】

各センサは、世界（シーン）を観測し、それ自体の変位（デバイスの変位ではない）の計測を行う。従って、カメラセンサおよび運動センサから成るデバイスの場合にデバイスの動きを得るためには、当業者には公知の座標系の単純な変化（後に記載する関数Ｈにおいてもモデリングされる）を適用する。

【0038】

処理デバイスを組み込んだカメラの場合には、処理装置１０３は、カメラと一体化される。

【0039】

本発明による方法によって実施される異なるステップを説明する前に、本発明による方法を理解するための幾つかの有用なリマインダーを示す。

【0040】

リマインダー
センサの融合の問題は、時間領域で離散化された逆問題として公式化することができる。観測プロセスは、センサｗに関して、以下の観測ベクトルの式：

【数15】

によって定義される。式中、

【数16】

は、特定の時点ｔにおいてセンサｗによって実行された投影関数である。この関数は、実物体または情報ｘ_ｔ（ここでは、システムおよびシーンの位置測定）を観測ベクトル

【数17】

に関連付ける。関数

【数18】

は、座標系の変化を含み、これにより、関係したセンサに特有の座標系で表現された観測を、本発明によるシステムの座標系に渡すことが可能となる。
当業者に公知の較正プロセスによってこれの推定を行うことができる。
誤差ベクトル

【数19】

は、通常、零平均（白色雑音）によって特徴付けられるガウス関数のベクトルで表される。
誤差ベクトル

【数20】

は、計測フェーズ中に導入された不正確さをモデリングする。

【0041】

逆問題の目的は、センサによって提供された複数の観測結果から未知の実ベクトルｘ_ｔを取り出すことである。
動的システムの場合、デバイスの経時的挙動を定義する運動モデルを使用する必要がある。使用されるモデルは、
ｘ_ｔ＝Ｆ_ｔ（ｘ_ｔ−１）＋ｗ_ｔの未知の実ベクトルであり、
式中、Ｆ_ｔは、その前の状態からデバイスの新しい状態ｗ_ｔ（特に姿勢を含む）を定義する。

【0042】

例えばシーンｓ等の静的システムは、定義上、経時的に一定であり、その結果、時間インデックスｔを用いることなく表現することができる。

【0043】

再帰的ベイズ推定
計測結果に雑音が存在し得ることが分かっていれば、不確かで、先験的な計測結果を最も有効にする統計フィルタリングは、動的システムの状態の推定から成る。この方法では、システムの状態（センサからの全ての計測結果）は、確率密度関数（ｐｄｆ）が求められる確率変数のベクトルである。

【0044】

再帰的ベイズ推定量の目的は、このｐｄｆを求め、コスト関数による期待値を最小にすることである。最もよく知られたコスト関数は、推定量と、その観測結果との二乗誤差を最小にしようとする平均二乗誤差、またはＭＳＥ関数である。
インデックスｋのセンサに関する平均二乗誤差は、

【数21】

として定義され、式中、

【数22】

は、残差計測のベクトルであり、

【数23】

は、その長さであり、

【数24】

である。
全ての計測結果が、共分散

【数25】

（Ｉは、恒等行列である）から互いに独立していると考えられる場合、

【数26】

が得られる。
センサ融合の問題は、センサの全ての平均二乗誤差を最小にすることから成る。ベイズ推定量の一例は、バンドル調整である。

【0045】

バンドル調整技術
バンドル調整技術またはＢＡ（Ｂａｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ）は、非線形最小二乗法（例えば、Ｇａｕｓｓ−Ｎｅｗｔｏｎ法、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法等）を解く方法に基づいた最適化技術である。最小化される目的Ｅ関数は、通常、画像（単位はピクセルである）に基づいたコスト項であり、例えば、平均二乗誤差ＭＳＥ：投影誤差

【数27】

（これは、画像ｔにおけるカメラｃの二次元観測結果

【数28】

と、画像におけるシーンの再投影との二乗距離の合計である）が用いられる。

【数29】

【0046】

バンドル調整技術は、最適化される入力変数に応じて異なる適用に使用される。通常、変数は、最後の姿勢ｐ_ｔのみとなり得るが、シーンｓのみ、あるいは、特定数の姿勢およびシーンの「姿勢精度向上」について述べ、次に、局所的または一般的バンドル調整（全ての姿勢の精度向上を行う場合）［ｐ_ｔｓ］について述べることとする。

【0047】

例えば、シーンおよびデバイスの姿勢の精度向上を行うためには、方程式：

【数30】

を解き、上付き文字ｃは、これらが、カメラによって行われた観測結果であることを示す。上付き文字＊は、最適化を表す。
この方程式は、Ｇａｕｓｓ−Ｎｅｗｔｏｎ法またはＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法等の先行技術の技術によって解くことができる。

【0048】

再投影誤差
カメラｋによって行われた観測結果に対する再投影誤差は、ここでは、カメラｃによって操作された画像のシーンの１組の３Ｄプリミティブの投影関数として、

【数31】

と定義され、この姿勢ｐ_ｔは、ｘ_ｔに含まれる。この関数は、システムに用いられるカメラのタイプおよび３Ｄプリミティブのタイプに特有のものである。
例えば、ピンホールタイプのカメラと、３Ｄの点によって形成されたプリミティブを用いることができる。
図２は、３Ｄの点からの投影、

【数32】

を示し、

【数33】

は、分かっている場合には（そうでなければ、

【数34】

観測結果ｚ_ｉｊの標準偏差である。
そして、

【数35】

は、時点ｔにおけるカメラｃの観測結果である。
他の種類のものが、領域中に存在し、当業者には公知のものである。

【0049】

システムアーキテクチャ用のモジュール
図３および図４は、本発明による方法の変形実施形態の２つの例を示す。これら２つの変形例において実施されるモジュールおよびそれらの機能性を説明する。

【0050】

ビジョン部による位置測定
［２０１、３０１］データ処理デバイスに配置されたビジョンによる位置測定モジュール
カメラの画像に基づいてシステムの姿勢を推定するために、このモジュールにおいて最先端の技術を用いる。このモジュールは、様々な最先端のアルゴリズムを含むことが可能であり、従って、その実施に従って変化し得る。
本発明による方法にとって重要なことは、ビジョン部の出力において：
・システムの姿勢の推定
・システム（またはその一部）の最後の姿勢の履歴
・シーンの３Ｄプリミティブおよび画像におけるそれらの観測結果（またはそれらの一部）の履歴
を有することである。

【0051】

［２０２、３０２］画像の取得
同期的または非同期的にカメラから提供された画像を取得するためのモジュールの考察を行う。

【0052】

［２０４、３０４］ビジョンによる姿勢の推定
センサが処理装置と同じ支持材上に存在しない場合には、上記で定義した画像を用いてデバイスの姿勢（位置および配向）を推定する。
姿勢の推定は、以下の方法によって行われる。

【0053】

二次元興味対象要素の検出
画像がモジュール［２０４、３０４］に届くと、本方法は、興味対象要素の検出を実行する。興味対象要素は、画像において明確に定義された位置を有し、かつ、空間点に対応し、視点または照明の変化時にロバストに検出可能である３Ｄプリミティブの観測結果である。数多くの検出器が当業者には公知である。

【0054】

興味対象要素の説明
次に、自身が互いに差別化を図ることを可能にし、後続の画像中にそれらを見つけ出すことを可能にする記述子によって、興味対象要素の特徴を定義する。記述子により、その近くの環境までずっと点を認識することが可能となる。
多数の記述子（Ｚｅｒｏ Ｍｅａｎ Ｎｏｒｍａｌｉｚｅｄ Ｃｒｏｓｓ Ｃｏｒｒｅｌａｔｉｏｎのアングロサクソン式の略称であるＺＮＣＣ等）が当業者には公知である。

【0055】

興味対象要素の相互相関
これより、異なる視点から取得された同じシーンの２つの画像を有する場合を検討する。これらの各々に関して、興味対象要素の検出、および対応する記述子の算出を行ったものと仮定する。

【0056】

実際の相関フェーズは、２つの画像間で類似特性を有する興味対象要素間の対応を確立することから成る。
幾つかの相関方法が当業者には公知である。

【0057】

対応に基づいた姿勢の算出
デバイスの姿勢は、一般座標系の三次元（ｘ，ｙ，ｚ）における位置Ｔ_ｔと、例えば回転Ｒ_ｔを有する行列の形式で保存されるデバイスの配向とによって定義される。この方法は変更可能で、例えば、５点アルゴリズムまたは８点アルゴリズム（コンピュータビジョンにおける古典的方法）を用いることが可能で、Ｎ個の画像からデバイスの姿勢を算出できる。以前に再構築されたシーン（当業者には公知のリセクションアルゴリズム）を用いることによってデバイスの姿勢を算出することもできる。姿勢を描写するパラメータも異なっていてもよい（回転行列の代わりに四元数等）。

【0058】

姿勢のロバスト推定
姿勢の第１の推定は、従来の方法による誤差によって混乱する場合があり、ここで、より正確かつロバストな方法でデバイスの姿勢の推定を行うことを提案する。これを行うためには、当業者は、公知の方法：ＲＡＮＳＡＣ、Ｍ−Ｅｓｔｉｍａｔｏｒ等を用いる。

【0059】

キー姿勢の選択
２つの連続した画像間の動きは、エピポーラ幾何学の正確な算出、従って、２つの画像の姿勢の推定を行うのに十分な大きさである必要がある。従って、この方法は、比較的互いに距離があるが、十分な数の共通点を持つ画像を選択する。これを達成するためには、第１の画像Ｉ１は、常にキー画像として選択される。選択される第２の画像Ｉ２は、映像中の画像Ｉ１から可能な限り離して選択されるが、それは、Ｉ１に対して少なくともＭ個の興味対象対応点を有していなければならない画像である。次に、Ｉ３に関しては、Ｉ３およびＩ２間で対応する少なくともＭ個の興味対象点が存在し、かつＩ３およびＩ１間で対応する少なくともＭ’個の興味対象点が存在するように、Ｉ２から最も離れた画像を選択する。このように、カメラの動きを決定するのに十分な共通の興味対象点が画像間に存在する。後に続く画像Ｉｎに関して、同様に続行し、画像Ｉｎ−２、Ｉｎ−１、およびＩｎが、同じ３Ｄプリミティブに対応する少なくともＭ個の興味対象点を有することを確認する。これらの規則に従う場合、画像Ｉのみがキー画像Ｉｎとなる。キー画像のこの検出方法は異なっていてもよく、ここでは、一例を提案する。

【0060】

シーンの初期化
シーンの初期化は、シーケンスの開始時に一度だけ行われる。ここでは、それは、初回に環境を再構築するという問題である。これを達成するためには、例えば、最初の３つのキー姿勢を使用し、３つの画像において、それら（興味対象要素）の観測結果から３Ｄプリミティブを三角測量することができる（３Ｄ三角測量法）。この方法により、特に、キー姿勢およびシーン要素（３Ｄ）のデータベースの構築が可能となる。

【0061】

［２１２、３１２］キー姿勢のデータベース
システムの最後のＮ個（２〜全て）の姿勢を含むデータベース（現在のカメラ較正パラメータを有する）を所有する場合を検討する。

【0062】

［２１３、３１３］シーン要素のデータベース
再構築されたシーンの３Ｄプリミティブのデータベースを各キー画像におけるそれらの対応（二次元観測結果（ｘ，ｙ）−３Ｄプリミティブの関連性）と共に所有し、これらの対応において、要素（その姿勢（位置および配向）はデータベース［２１２］によって分かっている）が検出済みである場合を検討する。３Ｄプリミティブは、例えば、３Ｄ点（ｘ，ｙ，ｚ）であるが、他の要素（パッチ、セグメント等）でもよい。

【0063】

［２０９、３０９］拡張姿勢精度向上
「姿勢精度向上」および「拡張姿勢精度向上」モジュールは、コンピュータビジョンの領域において非常によく知られたアルゴリズム：バンドル調整に基づく。より正確には、この部分において、本方法は、モジュール［２０４、３０４］によって提供された姿勢の第１の推定から開始し、以前に推定されたシーン要素を考慮に入れて、デバイスの現在の姿勢のみを精度向上させる。

【0064】

モジュール［３０９］において、コスト関数は、現在その姿勢の精度向上を行っているカメラ上のシーンの３Ｄプリミティブの再投影誤差のみから成る。モジュール［２０９］では、モジュール［２０８］によって提供され、自動重み付け選択モジュール［２０８、３０８］において学習された係数ラムダλまたはλ_ｔによって重み付けされた予測誤差（制約）をコスト項に加える。

【0065】

精度向上の最後に、システムの現在の姿勢を姿勢のデータベース（履歴）［２１２、３１２］に加える。

【0066】

姿勢精度向上を以下に記載する詳細な方法で行うことができる。

【0067】

ＳｆＭインクリメンタル追跡法では、精度向上は、新しい画像またはキー画像が検出された際に、カメラの位置測定に関して実行される。

【0068】

従って、センサからの計測結果

【数36】

（時点ｔにおける全てのカメラセンサｃおよび運動センサｋに関する計測結果の完全集合体）を説明するデバイスの現在の姿勢ｐ_ｔを取り出すことが可能である。次に、カメラが一体型の運動計測モジュールを組み込んでいる場合に、センサｋまたはカメラによって提供される姿勢予測に基づいて選択された制約

【数37】

と共に視覚標準誤差項

【数38】

を用いて、コスト総計関数を構築する。第２の項は、本明細書において後に詳述する以前に選択した重み付け係数λ_ｔを用いて重み付けされる。

【0069】

次に、一般項ＭＳＥ（平均二乗誤差）は、公式

【数39】

（運動または位置センサｋ、カメラｃ、時点ｔに関する）
によって与えられる。

【0070】

次に、

【数40】

を解くために、コスト関数Ｅ_ｐ（ｘ_ｔ，Ｚ_ｔ）と共に、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔアルゴリズムを用いて現在の姿勢ｐ_ｔを最適化する。
「＊」は、ｐ_ｔの最良値を得ることを意味する。

【0071】

［２１０、３１０］３Ｄプリミティブの再構築
３Ｄプリミティブを再構築するためのこのモジュールは、姿勢の精度向上（［２０９、３０９］）の直後に、あるいは、その後しばらくしてから起動される。これは、シーンの３Ｄプリミティブ（例えば３Ｄ点）を、最後のＮ個の画像（例えば、Ｎ＝３）において行われたそれらの二次元観測から三角測量することから成る。これを行うためには、コンピュータビジョンの最新のアルゴリズムを利用する。計算の完了時に、２Ｄ／３Ｄデータベースに３Ｄプリミティブおよびそれらの対応（二次元観測結果−３Ｄプリミティブの関連性）を加える。

【0072】

位置／運動センサからのデータの統合による姿勢の推定部。
［２０５、３０５］位置／運動センサからのデータの取得
運動および／または位置センサからのデータを非同期的に取り出すことを可能にするシステム（センサおよびドライバリンクに対する処理モジュール）を検討する。運動センサによってデータ項目が生成されると、このデータ項目は、バッファメモリに保存するため、運動および／または位置センサあるいは処理モジュールに配置するために、プログラムによって取り出される。

【0073】

［２０６、３０６］慣性センサに基づいた姿勢予測
要求が予測モジュール（２０６、３０６）にアドレッシングされると（ビジョンによる姿勢の推定を行うためのモジュール［２０４、３０４］によって）、予測モジュールは、同期が必要とする時点の前に、センサの観測結果を取り出し、それらをバッテリまたはバッファメモリから取り除き、次に、これらの計測結果および運動モデル（任意であるが、本方法では、これを用いるとより正確である）からデバイスまたはシステムの姿勢（またはその一部）を算出する。デバイス（カメラおよび運動センサから成る）が例えばＰＣである処理手段とは別個のものである場合を考えると、この方法は、ＰＣを組み込んだシステムの姿勢ではなく、デバイスの姿勢を算出する。

【0074】

予測モデルの仮定
バンドル調整技術は、最適化速度を提供しないので、これらの量は、上流で推定される必要がある、あるいは、第２の運動および／または位置センサによって直接提供される必要がある。センサｋの最後の計測結果

【数41】

に対応する、時点ｔ’≦ｔにおけるデバイス

【数42】

の状態の推定を提供する逆観測モデル

【数43】

の存在も仮定する。従って、デバイスの運動予測モデルを用いると、時間または時点ｔにおける予測姿勢は以下の通りである：

【数44】

このモデルは、運動／位置センサによって得られた計測結果

【数45】

および前の状態ｘ_ｔ−１から画像時間Ｉ_ｔに対応する時点ｔにおけるデバイス（例えば、カメラおよび運動および／または位置センサ）の姿勢

【数46】

を推定する。従って、誤差ベクトル

【数47】

およびｗ_ｔは、ゼロであると仮定する。

【0075】

［２０７、３０７］制約定義モジュール
このモジュールは、精度向上コスト関数に加える制約の種類（または誤差の種類）を定義する。

【0076】

システムの第１のバージョン（図３）では、この方法は、モジュール［２０９］および［２１４］のコスト関数を変更するが、図４に示す方法の第２のバージョンの実施では、この項が姿勢調整［３１４］のコスト関数にのみ加えられる。

【0077】

この項（コスト関数）は、通常、システムまたはデバイスの現在の姿勢（またはその一部）（これの最適化を行う）と、モジュール［２０６、３０６］によって予測された姿勢との距離である。誤差の選択は、設計者によって指定されるものであり、運動または位置センサによって提供される観測の種類によって決まる。例えば、３Ｄジャイロスコープを用いる場合、後者は、角速度Ωの情報を提供する。この情報は、運動モデル（例えば、一定角速度）との組み合わせで、キー画像（これの現在の配向はｑ^ｃである）と同じ時点において、配向ｑ^ｋの予測の構築を可能にする。誤差は、例えば、２つの四元数 ||ｑ^ｋ−ｑ^ｃ ||間の距離でもよい。

【0078】

制約の種類は、センサが取得する計測結果の性質によって決まる。例えば、センサに対して幾つかの制約を提案することができる。
位置制約

【数48】

配向制約：

【数49】

（Ｒは、配向をパラメータ化する回転行列であり、Ｉは、恒等行列である等）
スケール係数制約（または変換基準制約）

【数50】

【0079】

各制約は、位置測定システムに対して異なる影響を持ち、対応するドリフトを修正する傾向がある。本発明による方法は一般的なものであるので、ユーザの定義した制約は、バンドル調整技術において最適化される変数を制約する任意の距離項（最小二乗法における）となることが可能である。

【0080】

［２０８］重み付けの自動選択：共分散比の推定
［２１０］新しい３Ｄプリミティブを選択するためのモジュール
制約構築モジュールによって定義される誤差または制約は、その数例を以下に示す特定の技術によって選択される、我々がラムダλと呼ぶ係数によって重み付けされる必要がある。

【0081】

この特定の例では、この重み付け係数を算出するための方法を幾つか提案する。これらの方法は、この問題を解くために互いに独立して利用することができる。

【0082】

これを行うためには、ビジョンに基づく誤差項を用い、かつラムダによって重み付けされた融合項（前の制約）を用いて、一般コスト関数を構築する。次に、異なるラムダセット（例えば、０．１〜１００までの範囲の１００のラムダの静的セット）に対して、図３のモジュール［２０９］を数回起動する、または図４のモジュール［３１４］を数回起動することによって、幾つかの最適化を実行する。

【0083】

以下に詳述するように、重み付けパラメータであるラムダの選択基準は変化する。Ｌカーブ法（これは、ある特定の曲線の最大曲率を決定する問題である）を利用する、あるいはクロス確認として知られる方法を利用し、再投影の予測が最も効果的である最良ラムダ値を求める。

【0084】

基準（Ｌ正接ノルム、Ｒｅｇｉｎｓｋａ等）または学習（クロス確認等など）による他の方法を利用することもできる。

【0085】

最良ラムダが選択されると、この重み付けを用いて最後の最適化（［２０９］または［３１４］）に進み、この値および制約パラメータの保存を、この目的で設けられたデータベース［２１１、３１１］において行う。

【0086】

当業者に公知の他の方法を実施することも可能であることは分かっているが、重み付けパラメータを決定する２つの方法を例として提供する。

【0087】

Ｌカーブ基準による選択方法
Ｌカーブ基準は、本来、モデルの第１のコスト項およびパラメータ数を共に最小にすることが可能な有効正則化パラメータの推定を求める正則化問題において使用されたものである。
さらに、この方法は、幾つかの目的で、非線形最小二乗の最適化の問題において用いられたものであり、この場合、これは、各コスト項の重み付け係数ラムダを推定することによって異なる目的間で妥協点を見つけ出し、それによって、１組の目的の最良の最小化を求めるという問題であった。
従って、Ｌカーブ基準は、対数−対数スケールで、２つのコスト項間の妥協点を表す。
この基準は、通常、Ｌ字形状を有し、垂直部分から水平部分を分離する「角」は、最小化される２つのコスト項（目的）の一方の優勢に対応する。Ｌカーブ基準の曲率ｋ（ｌ）により、求められる角を見つけ出すことが可能となる：

【数51】

式中、「’」および「"」は、ラムダとの関連で第１および第２の導関数を表す。「良好な」重み付けパラメータラムダは、基準の角付近に位置し、そこでは、曲率ｋが最大値である。そこから、

【数52】

を求める。
Ｌカーブを推定するためには、インデックスｃを有するカメラおよびインデックスｋを有するセンサに関する数ペアの制約を算出する必要がある（異なるラムダ値に関して最小化される２つの目的（ε_ｃ、ε_ｋ）を表す）。これは、ラムダの各値に関して、デバイスの最後の姿勢を最適化する［２０９］、またはシーンの一部を用いて最後のＮ個の姿勢を最適化する［３１４］ことを意味する。次に、上記ペアを用いてＬカーブを構築し、最大曲率（ｋ ｍａｘ）に位置する最良ラムダを推定する。

【0088】

学習による選択方法
クロス確認
クロス確認法は、統計的考察に基づいた非常に良く知られた手段である：
適切なモデル（ここでは、重み付け係数ラムダ）は、どのような欠測データも正確に予測することができるべきである。より正確には、データセットから２〜３個の計測結果を除去した場合、推定された解（学習フェーズ：モジュール［２０９または３１４］）は、これらの観測結果を（テストフェーズ中に）予測できるべきである。
クロス確認には幾つかの異なる型が存在し、これらは、主に、それらのテストおよび学習セットの扱いが異なる。現在のケースの場合、限られた数のカメラの観測結果により、「１つ除外する」（"ｌｅａｖｅ ｏｎｅ ｏｕｔ"）という表現でよく知られる観測結果を再利用する方法の使用が生じるが、全く同様に、別の異なる型を用いることが可能である。
クロス確認スコアＣｖｌｏｏ（ｌ）は、ある特定の重み付けパラメータラムダに関する予測の質を反映する。これは、３Ｄ点ｊの実観測ｙｊと、予測

【数53】

（式中、姿勢

【数54】

は、点ｊを用いずに最適化される）との平均差を表す。

【数55】

【数56】

は、点ｊを用いないモジュール［２０９または３１４］の解である。クロス確認の目的は、ラムダに関してモジュール［２０９または３１４］によって行われる予測の質を最大限に高めることであり、それによって、予測誤差を最小限に抑えることである。

【数57】

【0089】

［２１１、３１１］重み付き制約のデータベース
各重み付き制約は、キー画像の選択ステップが実施されない場合には画像ごとに、あるいは、キー画像ごとにデバイスの姿勢ｐ_ｔ^ｃと関連付けられ、その後、データベース２１１、３１１に保存される。この制約（重み付けを有する）は、デバイスの関連姿勢が調整モジュール［２１４、３１４］において精度向上されるたびに再使用される。制約要素をデータベース２１１、３１１に保存する：重み付けλ_ｋおよびデバイスの姿勢の予測

【数58】

であって、例えば

【数59】

（Ｔは、表記に指定されるようにｐの一部である）の制約全体ではない。

【0090】

［２１４、３１４］拡張バンドル調整
バンドル調整は、特定数のデバイスの姿勢（データベース［２１２、３１２］から取り出された全てまたは一部）を、（データベース［２１３、３１３］に保存された）特定数のシーン３Ｄプリミティブ（例えば、現在最適化を行っている姿勢が見る全てのプリミティブ）と共に同時に最適化することから成る。

【0091】

最小化される一般コスト関数は、一方では、ビジョンを用いて形成され（画像のシーン要素の再投影と、画像において取得されたそれらの実際の計測結果との距離）、他方では、システムの異なるキー姿勢に関連する全ての重み付き制約（データベース［２１１、３１１］）によって形成される。

【0092】

この最適化問題を解くためには、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法と呼ばれる最適化アルゴリズムを起動する。当業者に公知の他のアルゴリズム（条件付け有りまたは無しの勾配降下法、Ｇａｕｓｓ−Ｎｅｗｔｏｎ法等）を用いることもできる。

【0093】

この方法は、通常、最後の３つのキーカメラまたはカメラの最後の３つの姿勢のみを最適化する（ただし、この数は変化し得る）。

【0094】

局所的バンドル調整（ＬＢＡ）
インクリメンタルＳＬＡＭの累積誤差を低減するために、シーンおよびシステムの位置測定に対して周期的に精度向上を行う。局所的最適化によって（すなわち、最後の姿勢のみを精度向上させることによって）良い結果が得られる。
従って、例えば、少なくとも最後の２つの姿勢（可能であればキー姿勢）の姿勢を最適化することが可能である。簡単な手順方法は、前の制約をＬＢＡコスト関数に投入することである。Ｘ_ｔ＝［ｐ_ｔ−９・・・・・・・ｐ_ｔｓ’］を定義し、ベクトルは、固定姿勢（ｐ_ｔ−９，・・・・ｐ_ｔ−３）、精度向上を望むカメラの姿勢（ｐ_ｔ−２，ｐ_ｔ−１，ｐ_ｔ）、および例えば最後のＮ個の姿勢に見られる３Ｄ点のセットである、シーンｓの一部ｓ’の現在の推定を含む。
例えば、Ｎ＝３、Ｍ＝９とする。
この考えは、姿勢を最適化することであり、すなわち、カメラおよびデバイスの位置および配向を最適化することである。

【0095】

制約を有するバンドル調整コスト関数は、

【数60】

となる。
この方程式の第１の要素は、例えば、ａ＝３、ｂ＝９、ｄ＝２で姿勢が固定されたカメラの再投影の合計に相当する。

【0096】

方程式の第１の部分は、姿勢が精度向上されていない（従って、固定された）カメラの再投影誤差に相当する。

【0097】

この場合、全ての姿勢を制約する必要はなく、オリジナルの再投影の誤差のみを考慮に入れる。

【0098】

方程式の第２の部分は、カメラのＮ個の最新の姿勢に対する再投影誤差および制約から成る。

【0099】

次に、この方法は、

【数61】

を解くためにＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔアルゴリズムを用いて、システム（カメラの最後のＮ個）の位置測定およびシーンの一部を最適化する。

【0100】

従って、本発明による方法およびシステムにより、データ間で効率的、自動的、かつリアルタイムの融合を達成することが可能となる。
バンドル調整から直接的に融合を統合する能力を引き出すこのような融合は、リアルタイムでの重み付け項の最適化推定が行えない先行技術の方法では不可能であった。これは、バンドル調整法の範囲内で、データの直接的統合による正確かつ迅速な融合を提供し、実際、拡張カルマンフィルタを用いた方法と比較して、より高い精度を提供する。
この技術によれば、計測の最初の共分散を推定する、あるいは、システム状態に共分散を伝える必要はなく、その理由は、本発明による方法は、自動的に自身を適応させるからである（重み付けパラメータラムダのリアルタイムでの推定）。
重み付け係数の推定は、動的かつリアルタイムである。これは、新しい最適化ごとに計算され、これにより、前の状態の履歴を用いた、より完全な最適化方法、すなわち、先行技術によるカルマンフィルタを用いた方法とは異なり、デバイスの最後のＮ個の姿勢の精度向上を行う方法が提供される。

【図1】

【図2】

【図3】

【図4】