(57)【特許請求の範囲】
【請求項１】
データを位置合わせする方法であって、該データは３つの次元を有し、該方法は、
第１の座標系における前記データからプリミティブの第１の集合を選択するステップであって、該プリミティブの第１の集合は、少なくとも１つの面と、少なくとも１つの点と、点又は面のいずれかである第３のプリミティブとを含む、ステップと、
第２の座標系における前記データからプリミティブの第２の集合を選択するステップであって、該プリミティブの第２の集合は、少なくとも１つの面と、少なくとも１つの点と、前記プリミティブの第１の集合内の前記第３のプリミティブに対応する第３のプリミティブとを含む、ステップと、
点対点の誤差と面対面の誤差との組合せによって形成された誤差を最小にする、前記第１の座標系と前記第２の座標系の間での６−ＤＯＦ剛体変換［Ｒ，ｔ］を求めるステップであって、前記点対点の誤差は、前記６−ＤＯＦ剛体変換を用いて変換された、前記第１の座標系における点と前記第２の座標系における点との間の距離を含み、前記面対面の誤差は、前記６−ＤＯＦ剛体変換を用いて変換された、前記第１の座標系における面と前記第２の座標系における面との間の距離を含む、ステップと、
前記プリミティブの第１の集合及び前記プリミティブの第２の集合において、前記６−ＤＯＦ剛体変換を用いて、面を互いに位置合わせするとともに点を互いに位置合わせするステップであって、位置合わせされた点と位置合わせされた面とを含む、位置合わせされたプリミティブを得るものであり、前記位置合わせされた点の座標系は、互いにマッチングされ、前記位置合わせされた面の法線、および位置合わせされた面と前記第１の座標系および前記第２の座標系の原点との間の距離は、互いにマッチングされ、６−ＤＯＦ剛体変換のＲは、回転行列であり、ｔは、並進ベクトルである、ステップと、
を含み、
それぞれのステップは、位置推定及び地図作成の同時実行システムのプロセッサで実行される、データを位置合わせする方法。
【請求項２】
前記位置合わせすることは大域的である、請求項１に記載の方法。
【請求項３】
前記位置合わせすることはリアルタイムである、請求項１に記載の方法。
【請求項４】
前記データを奥行きマップとして取得することを更に含む、請求項１に記載の方法。
【請求項５】
前記奥行きマップは３Ｄセンサーによって取得される、請求項４に記載の方法。
【請求項６】
前記位置合わせされたプリミティブを大域マップ内に維持することを更に含む、請求項４に記載の方法。
【請求項７】
前記位置合わせすることはランダムサンプルコンセンサス手順を用いる、請求項４に記載の方法。
【請求項８】
位置及び記述子によって前記第１の集合および前記第２の集合の各点を表すとともに、単位法線ベクトルと、対応する座標系の原点に対する距離とによって前記第１の集合および前記第２の集合の各面を表すことを更に含む、請求項４に記載の方法。
【請求項９】
各面は、
３Ｄ点群における基準点の集合をランダムに選択することと、
ローカルウィンドウ内の近傍点を用いて基準点ごとに最適な面を求めることと、
前記奥行きマップの格子グラフに関して前記基準点との連結成分を形成する全てのインライアを求めることと、
所定の数の前記インライアを有する最適な面を特定することと、
前記インライアを前記３Ｄ点群から除去することと、
によって選択される、請求項４に記載の方法。
【請求項１０】
前記大域マップは、前記第１の集合および前記第２の集合の前記点及び前記面を用いてバンドル調整によって最適化される、請求項６に記載の方法。
【請求項１１】
前記位置合わせすることの間に、各座標系における、点間の距離、点と面との間の距離、又は２つの面の法線間の角度に基づいて誤合致を特定することを更に含む、請求項１に記載の方法。
【請求項１２】
前記第１の集合および前記第２の集合の各点は３Ｄキー点検出器によって選択される、請求項４に記載の方法。
【請求項１３】
前記バンドル調整は、前記第１の集合および前記第２の集合の前記面間の幾何学的制約を実施する、請求項１０に記載の方法。
【請求項１４】
請求項１に記載の方法であって、該方法の出力は、シーンの位置合わせされた点群及び面ベースの表現を含む、請求項１に記載の方法。
【請求項１５】
前記３Ｄセンサーは前記奥行きマップを取得中に移動可能である、請求項５に記載の方法。
【請求項１６】
前記奥行きマップに対応するテクスチャ画像を取得することを更に含む、請求項４に記載の方法。
【請求項１７】
前記第１の集合および前記第２の集合の各点は、前記テクスチャ画像に適用される２Ｄキー点検出器によって選択され、該２Ｄキー点検出器は、前記奥行きマップを用いて、検出されたキー点を逆投影する、請求項１６に記載の方法。
【請求項１８】
前記テクスチャ画像はカメラによって取得される、請求項１７に記載の方法。
【請求項１９】
前記誤差は、下式
【数１】
0005955406.tif 000052
を含み、上式において、ｉ、ｊは、正の整数であり、ｑ’は、前記第１の座標系の点であり、ｑは、前記第２の座標系の点であり、ｎ’は、前記第１の座標系の面であり、ｎは、前記第２の座標系の面であり、ｗは、対応する面の重みである、請求項１に記載の方法。
【請求項２０】
前記ｗは、それぞれの対応する面ごとに変更される、請求項１９に記載の方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、包括的には３次元（３Ｄ）データの位置合わせに関し、より詳細には、リアルタイム再構成用途のために３Ｄ点及び３Ｄ平面を互いに位置合わせすることに関する。
【背景技術】
【０００２】
３Ｄ再構成
シーンの対話型でリアルタイムの３Ｄ再構成は、多数の用途、例えばロボット工学、拡張現実、医用撮像及びコンピュータービジョンにおいて用いられる。リアルタイムの粗密３Ｄ再構成は、カメラ等のパッシブセンサーを用いることができる。しかしながら、パッシブセンサーは、テクスチャのない領域を再構成するのが困難である。
【０００３】
テクスチャのない領域を再構成する場合、アクティブ３Ｄセンサーを用いることができる。例えば、Ｍｉｃｒｏｓｏｆｔ  ＸｂｏｘのためのＫｉｎｅｃｔセンサーは、ＩＲパターンを用いてセンサーの視点からの奥行きマップとして３Ｄデータをリアルタイムで取得する。
【０００４】
他の問題は、処理時間、メモリ要件及び正確度に関する。視野及び分解能に起因して、３Ｄセンサーは通常、シーンの部分再構成を生成する。連続部分奥行きマップとシーンのモデルとを組み合わせることができる正確で高速な位置合わせ方法を提供することが望ましい。慣性センサーはドリフトしやすい。したがって、正確な位置合わせのためには、ＲＧＢ（テクスチャ）画像又は奥行きマップにおける特徴に頼る必要がある。加えて、奥行きマップは通常、雑音が多く、高次の空間制約を一切有しない。さらに、点群は非常に大きなメモリを必要とし、圧縮するのが困難である。
【０００５】
３Ｄ対３Ｄの位置合わせ
局所
３Ｄデータのアライメント又は位置合わせは、コンピュータービジョン用途における基本的な問題であり、これはいくつかの方法を用いて解決することができる。位置合わせ方法は局所的又は大域的とすることができる。局所的方法は、良好な初期化から開始し、比較的小さな反復運動を用いて２つの３Ｄ点群を位置合わせするべきである。これは、良好な初期解を用いて大域解に収束する非線形最小化方法に類似している。最も一般的な局所的方法は反復最近傍点（ＩＣＰ）法であり、この方法は、閉形式解を用いて対応する３Ｄ点及び運動を反復的に求める。
【０００６】
大域
大域的方法は通常、３Ｄ点群全体を検討し、いくつかの主要な幾何学的特徴（プリミティブ）を特定し、点群にわたって特徴をマッチングし、ランダムサンプルコンセンサス（ＲＡＮＳＡＣ）手順を用いて、対応の最小集合を用いた最適な仮説を生成する。大域的方法によって得られる粗い位置合わせの後、通常、局所的な非線形精緻化が続く。大域的方法は、局所的方法と異なり、初期化を必要としない。しかしながら、大域的方法は、誤った対応及び不十分な対応による害を被る可能性がある。大域的方法において通常用いられる幾何学的プリミティブは、点、線又は面のいずれかである。
【０００７】
同種の対応及び異種の対応を用いたいくつかの大域的位置合わせ方法が既知である。例えば、点対点、線対線、面対面、点対線、点対面又は線対面の対応を所与とした位置合わせの閉形式解を求めることが可能である。１つの方法は、分枝限定法を用いて、点対点、点対線、及び点対面の対応から大域最適解を得る。別の方法は、分枝限定法を用いて、最適な対応及び点対点位置合わせ問題への変換を得る。
【０００８】
３Ｄセンサーを用いたＳＬＡＭ
移動ロボット工学において、いくつかの３Ｄセンサーベースの方法は、センサーの動きを求め、シーン構造を再構成するために、位置推定及び地図作成の同時実行（ＳＬＡＭ）システムを用いる。これらの方法は通常、点プリミティブ、線プリミティブ又は面プリミティブ等の幾何学的特徴を用いる。２Ｄレーザースキャナー又は超音波センサー等の３Ｄデータの平面スライスを提供する３Ｄセンサーは、平面の３自由度（ＤＯＦ）の動きを求めるのに用いることができる。構造化光スキャナー、移動ステージに取り付けられた２Ｄレーザースキャナー及びＫｉｎｅｃｔセンサー等の、完全な３Ｄ点群を提供する３Ｄセンサーは、６ＤＯＦの動きを求めるのに用いることができる。
【０００９】
ＲＧＢ−Ｄマッピングは、ＲＧＢ画像からキー点を抽出し、奥行きマップを用いてこれらの点を３Ｄにおいて逆投影し、点対点の３つの対応を用いて、ＲＡＮＳＡＣ手順を用いて姿勢の初期推定値を求める。この初期推定値は、ＩＣＰ法を用いて更に精緻化される。
【００１０】
別の方法は、ＳＬＡＭシステムにおいて、３Ｄセンサーとの３つの面対面の対応を用いる。その方法は、面間の幾何学的制約を用いて対応問題に対処する。
【００１１】
別の方法は、面及び線分の双方をプリミティブとして用いる、ＳＬＡＭシステムのためのより小さな視野（ＦＯＶ）の３Ｄセンサー及びより大きなＦＯＶの２Ｄレーザースキャナーの組み合わせを用いる。その方法は、局所的な位置合わせ問題を解決する連続ＳＬＡＭシステムのために設計され、大域的な位置合わせを解くことはできない。
【００１２】
ＫｉｎｅｃｔＦｕｓｉｏｎは、粗密ＩＣＰ法を用いることによって、現在の奥行きマップを、大域的なトランケートされた符号付き距離関数（ＴＳＤＦ）表現から生成された仮想奥行きマップと位置合わせする。ＴＳＤＦ表現は、位置合わせされた全ての以前の奥行きマップを大域座標系に統合し、単一の画像を用いるよりも高品質の奥行きマップ生成を可能にする。
【００１３】
ＩＣＰ法のいくつかの他の変形が知られているが、これらの変形は依然として、２つの３Ｄ点群が別個であるときの極小値問題による害を被る。点のみに依拠する位置合わせ方法又はＳＬＡＭシステムは、テクスチャのない領域又は繰り返しパターンを有する領域における不十分な対応又は不正確な対応による害を被る。面に基づく技術は、不十分な数の平行でない面を含むシーンにおいて、縮退問題による害を被る。
【００１４】
Ｋｉｎｅｃｔセンサー等の３Ｄセンサーを用いると、奥行き不連続性付近の雑音が多い奥行き値又は欠落した奥行き値に起因して、線対応を得るのが困難である。
【発明の概要】
【００１５】
本発明の実施形態は、２つの異なる座標系においてデータを位置合わせする方法を提供する。本方法は、点及び面の双方を位置合わせプリミティブとして用いる。本方法は、３Ｄセンサーを用いる、位置推定及び地図作成の同時実行（ＳＬＡＭ）システムによって用いることができる。ＳＬＡＭシステムは本発明による位置合わせ方法の応用形態である。
【００１６】
本方法を用いると、少なくとも３つの点及び面プリミティブの任意の組み合わせ、すなわち、３面、２面及び１点、１面及び２点、並びに３点を用いて２つの異なる座標系における３Ｄデータ集合を位置合わせすることが可能である。本方法は、特に、少なくとも２つの３Ｄ点及び少なくとも１つの３Ｄ面、又は少なくとも１つの３Ｄ点及び少なくとも２つの３Ｄ面である混合（異種）プリミティブの集合に関する。
【００１７】
すなわち、本方法は、３つのプリミティブの最小集合と、ＲＡＮＳＡＣ手順とを用いて３Ｄデータを位置合わせし、シーンを再構成することができる。３Ｄデータにおいて、面数は点数よりも大幅に小さいので、好ましい実施形態によって用いられるＲＡＮＳＡＣ手順は、点よりも多くの面、例えば、最低限２つの面及び１つの点を含むプリミティブの組み合わせを用いる。
【００１８】
位置合わせのための主に点を用いる従来技術による方法と対照的に、本方法は以下の利点を有する。対応検索及び位置合わせは、面プリミティブがより少ないことに起因して、より高速であり、リアルタイムで実行することができる。本方法は、点ベースのモデルよりもコンパクトな面ベースの３Ｄモデルを生成する。本方法は、従来技術における局所的な位置合わせ方法におけるような極小値又は初期化問題による害を被ることのない大域的な位置合わせを提供する。
【図面の簡単な説明】
【００１９】
【図１】本発明の実施形態による位置合わせ方法の概略図である。
【図２】本発明の実施形態による位置合わせ方法を用いた位置推定及び地図作成の同時実行（ＳＬＡＭ）システムの流れ図である。
【図３】本発明の実施形態によって用いられるＲＡＮＳＡＣ手順の概略図である。
【図４】本発明の実施形態による面ベース及び点ベースの混合モデルの概略図である。
【発明を実施するための形態】
【００２０】
本発明の実施形態は、２つの異なる座標系において３Ｄデータを位置合わせするための方法を提供する。図１に示すように、本方法は、当該技術分野において既知のメモリ及び入／出力インターフェースに接続されたプロセッサ１５０において実行することができる。
【００２１】
図１は、位置合わせ方法１００の概略図を示している。この位置合わせ方法は、２つの異なる座標系において３Ｄデータから選択された少なくとも３つのプリミティブの２つの集合を用いることによって、２つの異なる座標系１０１及び１０２間の６−ＤＯＦの剛体変換（rigid body transformation：形状保存変換）１０５を求める。プリミティブは３Ｄ点１１０及び３Ｄ面１２０を含む。各集合内の３つのプリミティブは、少なくとも１つの３Ｄ面と、少なくとも１つの３Ｄ点と、点又は面のいずれかとすることができる第３のプリミティブとを含む。２つの集合内の面は面と位置合わせされ、点は点と位置合わせされる。位置合わせ方法は大域的とすることができ、リアルタイムで実行することができる。
【００２２】
図２に示すように、位置合わせ方法は、位置推定及び地図作成の同時実行（ＳＬＡＭ）システムによって用いることができる。ＳＬＡＭシステムは、本発明による位置合わせ方法の応用形態である。本システムは、当該技術分野において既知のメモリ及び入／出力インターフェースに接続されたプロセッサ２００において実施することができる。ＳＬＡＭシステムへの例示的な入力２０１は、１つ又は複数の奥行きマップ２０２の集合である。奥行きマップは、マップ内の各ピクセルの奥行き値（距離）を記述し、このため、マップ内のピクセルをそれらの奥行き値に従って逆投影することによって、３Ｄ点群を提供する。ＳＬＡＭシステムはオプションで、奥行きマップに対応する１つ又は複数の２Ｄテクスチャ画像２０３の集合を取得することができる。テクスチャ画像はグレースケールとすることもできるし、ＲＧＢカラーチャネルを有することもできる。奥行きマップは、３Ｄセンサー、例えばパッシブステレオカメラ２０４又はアクティブＩＲスキャナー若しくはアクティブレーザースキャナー２０５によって取得することができる一方、テクスチャ画像は、カメラ、例えばステレオカメラ２０４における単一のカメラ又は専用カメラ２０６によって取得することができる。３Ｄセンサー及びカメラは、人間が手で持つこともできるし、機械、例えばロボットアーム又は機械的に移動するステージによって保持することもできる。主要な概念は、可動センサーによって入力を取得することができ、本発明による位置合わせ方法は、センサーの動きを求めて、様々な視点において取得された３Ｄ点群を位置合わせすることによってシーンを再構成することである。
【００２３】
本発明によるＳＬＡＭシステムは、入力から３Ｄ点プリミティブ及び３Ｄ面プリミティブの集合（測定値２１１と呼ばれる）を選択し（２１０）、それらの点及び面を、大域マップ２２５における点プリミティブ及び面プリミティブの集合（ランドマーク２２１と呼ばれる）に対し位置合わせする（２２０）。各集合は、少なくとも１つの３Ｄ点と少なくとも１つの３Ｄ面とを含む。集合内の第３のプリミティブはともに点であるかともに面であるかのいずれかとすることができる。位置合わせは、２つの集合において点同士を位置合わせし、２つの集合において面同士を位置合わせすることによって実行される。
【００２４】
ＳＬＡＭシステムは、最小で３つのプリミティブを用いたランダムサンプルコンセンサス（ＲＡＮＳＡＣ）手順２２０により位置合わせ方法を実行する。追加の入力が取得されると、最適化２３０を大域マップに適用することができる。
【００２５】
３Ｄ点
本発明によるＳＬＡＭシステムは、３Ｄ点プリミティブを得るために、３Ｄ点群内の１つ又は複数のキー点を選択する。キー点は、テクスチャ画像を用いることなく３Ｄ点群から３Ｄキー点検出器を用いて選択することができる。例示的な３Ｄキー点検出器は、法線がアラインされたラジアル特徴（ＮＡＲＦ：Normal Aligned Radial Feature）及び３Ｄ高速化ロバスト特徴（ＳＵＲＦ）を含む。代替的に、システムは、２Ｄキー点検出器を用いて各テクスチャ画像から２Ｄキー点を選択し、対応する奥行き値を用いてキー点を逆投影し、３Ｄ点プリミティブを得ることができる。例示的な２Ｄキー点検出器は、スケール不変特徴変換（ＳＩＦＴ）及び高速化ロバスト特徴（ＳＵＲＦ）を含む。これらの３Ｄキー点検出器及び２Ｄキー点検出器は、検出されたキー点ごとの特徴記述子も提供する。
【００２６】
各点プリミティブは、キー点の位置ｐｍ及び記述子Ｄｍ、すなわち（ｐｍ，Ｄｍ）によって表される。記述子は、測定値とランドマークとの間で３Ｄ点プリミティブをマッチングするのに用いることができる。
【００２７】
３Ｄ面
３Ｄ点群から面を選択するために、以下の反復手順を用いる。
１．３Ｄ点群において基準点の集合をランダムに選択する；
２．ローカルウィンドウ内の近傍点を用いて基準点ごとに最適な面を求める；
３．奥行きマップの格子グラフに関して基準点との連結成分を形成する全てのインライアを求める；
４．十分な数の最大数のインライアを有する最適な面を特定する；及び
５．３Ｄ点群からインライアを除去する。
【００２８】
各面プリミティブは、面パラメーターπｍ及びインライアの集合Ｉｍによって表される。面パラメーターは４Ｄベクトル
【数１】
0005955406.tif 000002
を形成し、ここでｎｍは単位法線ベクトルであり、ｄｍは座標系の原点に対する距離である。
【００２９】
位置合わせ
センサーの姿勢２４０（すなわち、大域マップの座標系に対する６−ＤＯＦ剛体変換）は、測定値におけるプリミティブを、大域マップ内のランドマークにおけるプリミティブと位置合わせすることによって求められる。点及び面の双方を用いた本発明による位置合わせ方法及び本発明によるＲＡＮＳＡＣ手順が以下に説明される。
【００３０】
マップ更新
本発明によるＳＬＡＭシステムは、キーフレームから選択された点プリミティブ及び面プリミティブを合算することによって大域マップを更新する（２５０）。現在のフレームは、姿勢が以前のキーフレームと十分異なっている（例えば、現在のフレームの並進及び回転が以前のキーフレームの並進及び回転と所定の閾値よりも大きく異なっている）場合にのみキーフレームとみなされる。各点ランドマークは、位置ｐｌ及び記述子Ｄｌによって（ｐｌ，Ｄｌ）として表される。各面ランドマークは、面パラメーター
【数２】
0005955406.tif 000003
及びインライアの集合Ｉｌによって（πｌ，Ｉｌ）として表される。
【００３１】
マップ最適化
全てのキーフレーム間の位置合わせを共同で精緻化するために、本発明によるＳＬＡＭシステムは、点及び面の双方を用いてバンドル調整によって最適化２３０を実行する。バンドル調整は、位置合わせ方法を用いたカメラ姿勢の追跡のために、メインスレッドと別個のスレッドにおいて非同期的に実行される。バンドル調整の詳細は以下に説明される。
【００３２】
点及び面を用いた位置合わせ
本発明による位置合わせ方法は、点対点の対応及び面対面の対応を用い、閉形式解を与える。本方法は、３つ以上の対応に適用可能である。このため、本方法を用いて、本発明によるＲＡＮＳＡＣ手順において最小数の３つの対応を用いて仮説を生成し、ＲＡＮＳＡＣ手順によって与えられた初期仮説を全てのインライア対応を用いて精緻化することができる。
【００３３】
点対点の対応及び面対面の対応が混合している場合の閉形式解
｛ｐｉ｝及び｛ｐ’ｉ｝，ｉ＝１，．．．，Ｍを対応する３Ｄ点とし、
【数３】
0005955406.tif 000004
及び
【数４】
0005955406.tif 000005
，ｊ＝１，．．．，Ｎを２つの異なる座標系における対応する３Ｄ面とする。座標系間の６−ＤＯＦ剛体変換［Ｒ，ｔ］を求める。ここで、Ｒは３×３の回転行列であり、ｔは３×１の並進ベクトルである。
【００３４】
個々の事例の解
点対点対応の場合、回転成分及び並進成分は分離することができる。
【数５】
0005955406.tif 000006
及び
【数６】
0005955406.tif 000007
が３Ｄ点集合の重心であり、
【数７】
0005955406.tif 000008
及び
【数８】
0005955406.tif 000009
であるものとする。このとき、回転の四元数表現又は特異値分解（ＳＶＤ）を用いることによって、誤差を最小にする回転の最小二乗解が得られる。
【００３５】
【数９】
0005955406.tif 000010
【００３６】
推定される回転
【数１０】
0005955406.tif 000011
を用いると、並進は回転された重心間の差
【数１１】
0005955406.tif 000012
である。
【００３７】
面対面対応の場合、回転は誤差を最小にすることによって得られる。
【００３８】
【数１２】
0005955406.tif 000013
【００３９】
これは点対点対応の場合と同様に解くことができる。並進を求めるために、３つ以上の面について以下の線形制約をスタックし、連立一次方程式を解くことができる。
【００４０】
【数１３】
0005955406.tif 000014
【００４１】
ここで、Ｔは転置演算子である。
【００４２】
混合している場合の解
混合している場合、点対点対応及び面対面対応を有する。個々の事例において用いられる分解を利用して回転及び並進を求める。回転を求めるために、式（１）及び式（３）を以下のように組み合わせる。
【００４３】
【数１４】
0005955406.tif 000015
【００４４】
ここで、ｗｊは面の重みであり、面対応ごとに変更することができる。式（５）は、式（１）及び式（３）と同じ形式を共有し、最適な回転が同じように得られる。詳細には、３×３の相関行列Ｋを以下のように定義する。
【００４５】
【数１５】
0005955406.tif 000016
【００４６】
Ｋ＝ＵＤＶＴがＫの特異値分解（ＳＶＤ）であるものとする。このとき、最適な回転
【数１６】
0005955406.tif 000017
は以下となる。
【００４７】
【数１７】
0005955406.tif 000018
【００４８】
並進ｔを求めるために、以下の誤差を最小にする。
【００４９】
【数１８】
0005955406.tif 000019
【００５０】
これは、対角重み行列Ｗ＝ｄｉａｇ（Ｍ，Ｍ，Ｍ，ｗ１，．．．，ｗＮ）を用いて以下の連立一次方程式を定義することに対応する。
【００５１】
【数１９】
0005955406.tif 000020
【００５２】
このとき、重み付き最小二乗解はｔ＝（ＡＴＷＡ）−１ＡＴＷｂである。
【００５３】
縮退問題
Ｒ及びｔを一意に選択するために、式（６）における相関行列Ｋ及び式（９）における行列Ａは一定の条件を満たすべきである。Ｒを一意に求めるには、相関行列Ｋの階数が１よりも大きく、かつ以下の条件のうちの少なくとも１つが真であるべきである。
１．ｄｅｔ（ＵＶＴ）＝１
２．Ｋの最小特異値が単解である。
【００５４】
並進ｔを一意に求めるために、式（９）における行列Ａは階数３であるべきである。
【００５５】
行列Ｋ及びＡは、対応が以下のもの、すなわち３面と、２面及び１点と、１面及び２点と、３点とのうちの少なくとも１つを有する場合、上記の特性を満たす。本発明によるＳＬＡＭシステムにおけるＲＡＮＳＡＣ手順において、これらの最小数の対応を用いて仮説を生成する。
【００５６】
ＲＡＮＳＡＣ手順
２Ｄ画像空間のみにおける対応と対照的に、本発明による３Ｄプリミティブは、誤合致を特定するのに用いることができるいくつかの不変量を提供する。２つの異なる座標系における対応する点及び面を所与として、１つの座標系において求められる或る特定の幾何学的エンティティは、第２の座標系における対応するエンティティと実質的に合致するべきである。
【００５７】
各座標系において、以下の３つの不変量、すなわち、２点間の距離に基づくＩ１と、点と面との間の距離に基づくＩ２と、２つの面法線間の角度に基づくＩ３とを特定する。
【００５８】
対応する幾何学的プリミティブは、不変量ベクトルＩ＝（ｉ１，ｉ２，ｉ３）と関連付けることができ、ここで、ｉ１、ｉ２及びｉ３は、それぞれタイプＩ１、Ｉ２及びＩ３に関する不変量の数に対応する。
【００５９】
点及び面を含む全ての対応する三つ組が、合計で３つの不変量、すなわち、３点：Ｉ＝（３，０，０）と、１面及び２点：Ｉ＝（１，２，０）と、２面及び１点：Ｉ＝（０，２，１）と、３面：Ｉ＝（０，０，３）とを有する。
【００６０】
解釈木又は分枝限定法を用いて、これらの不変量を用いて誤合致を特定することができる。本発明によるＲＡＮＳＡＣ手順の場合、単純な解釈木ベースの剪定を用いる。
【００６１】
不変量に基づいて非合致を削除する前に、いくつかの初期対応を得る。点の場合、ＳＵＲＦ記述子を用いて対応を得る。面の場合、全ての可能な対応から開始して、不変量に基づいて非合致を削除する。３つの面を用いるＲＡＮＳＡＣ手順は、３つの点を用いるＲＡＮＳＡＣ手順よりもはるかに高速である。なぜなら、３Ｄデータ内の面数が通常、対応する点数よりもはるかに少ないためである。さらに、面は多くの点によって生成されるので、面は３Ｄデータにおける雑音によって受ける影響がより少なく、より正確な位置合わせにつながる。したがって、図３に示すように、入手可能な場合、プリミティブの３つ組を用いて、以下の好ましい順序、すなわち３面３０１、２面及び１点３０２、１面及び２点３０３、又は３点３０４の順序でＲＡＮＳＡＣ手順を初期化する。
【００６２】
ＲＡＮＳＡＣ手順の終了条件は、サンプリングされる所定の最小数の対応とすることができる。
【００６３】
点及び面を用いたバンドル調整
本発明によるＳＬＡＭシステムは、バンドル調整において以下の変数、すなわち、点ランドマーク
【数２０】
0005955406.tif 000021
と、面ランドマーク
【数２１】
0005955406.tif 000022
と、キーフレーム姿勢
【数２２】
0005955406.tif 000023
とを最適化する。
【００６４】
ここで、
【数２３】
0005955406.tif 000024
はｋ番目のキーフレームの並進の（ｘ，ｙ，ｚ）成分であり、
【数２４】
0005955406.tif 000025
は（ｘ，ｙ，ｚ）軸を中心とした回転を表す。ｋ番目のキーフレームの回転行列Ｒｋは
【数２５】
0005955406.tif 000026
によって表される。
【００６５】
各点／面ランドマークに関連付けられた点／面プリミティブを用いたヤコビ行列を以下のように求める。
【００６６】
点ランドマーク
点ランドマークの場合、点ランドマーク
【数２６】
0005955406.tif 000027
とｋ番目のキーフレームにおける関連する点
【数２７】
0005955406.tif 000028
との間のユークリッド距離誤差を最小にする。これは以下となる。
【００６７】
【数２８】
0005955406.tif 000029
【００６８】
ランドマークの現在の推定値
【数２９】
0005955406.tif 000030
及びキーフレーム姿勢の現在の推定値
【数３０】
0005955406.tif 000031
を用いることによって、式（１０）を以下のように線形化する。
【００６９】
【数３１】
0005955406.tif 000032
【００７０】
ここで、
【数３２】
0005955406.tif 000033
である。
【００７１】
式（１１）から、（ｘ，ｙ，ｚ）成分ごとに別個に３つの式が得られる。ｘ成分についての式は以下であり、
【数３３】
0005955406.tif 000034
ｙ成分及びｚ成分についての式も同様に得ることができる。
【００７２】
面ランドマーク
面ランドマークの場合、面ランドマークと、キーフレームにおける関連付けられた面からサンプリングされた３Ｄ点との間の距離の和によって定義される幾何学的誤差を最小にする。詳細には、面プリミティブ
【数３４】
0005955406.tif 000035
のインライア３Ｄ点から３Ｄ点
【数３５】
0005955406.tif 000036
を均一にサンプリングし、各サンプリングされた点と、関連付けられた面ランドマーク
【数３６】
0005955406.tif 000037
との間の距離を求める。このため、最小化する幾何学的誤差は以下となる。
【００７３】
【数３７】
0005955406.tif 000038
【００７４】
式（１４）を、面ランドマークの現在の推定値
【数３８】
0005955406.tif 000039
及びキーフレーム姿勢の現在の推定値
【数３９】
0005955406.tif 000040
を用いて以下のように線形化する。
【００７５】
【数４０】
0005955406.tif 000041
【００７６】
ここで、
【数４１】
0005955406.tif 000042
である。簡約すると、以下を得る。
【００７７】
【数４２】
0005955406.tif 000043
【００７８】
面ランドマーク間の幾何学的制約
本発明によるＳＬＡＭシステムはオプションで、バンドル調整における面ランドマーク間の幾何学的制約を実施することができる。幾何学的制約は平行性及び直交性を含む。直交する面又は平行な面は、人工的な環境において一般的に観察することができ、この人工的な環境では、幾何学的制約を実施することは、正確な位置合わせを得るのに有用である。
【００７９】
面ランドマーク
【数４３】
0005955406.tif 000044
及び
【数４４】
0005955406.tif 000045
の全ての対について、所定の閾値を用いて、２つの面の法線ベクトル間の角度が０度（平行）に十分近いか又は９０度（直交）に十分近いかをチェックする。真の場合、以下の制約を実施する。
【００８０】
【数４５】
0005955406.tif 000046
【００８１】
ここで、２つの面が平行に十分近い場合、Ｓ＝１である一方、２つの面が直交に十分近い場合、Ｓ＝０である。
【００８２】
２つの面ランドマークの現在の推定値
【数４６】
0005955406.tif 000047
及び
【数４７】
0005955406.tif 000048
を用いて、式（１７）を以下のように線形化する。
【００８３】
【数４８】
0005955406.tif 000049
【００８４】
簡約すると、式（１８）は以下となる。
【００８５】
【数４９】
0005955406.tif 000050
【００８６】
解
全てのランドマーク及びキーフレームについて式（１３）、式（１６）及び式（１９）をスタックすると、連立一次方程式ＪΔ＝−ε０が得られる。ここで、ヤコビ行列はＪであり、誤差ベクトルはε０であり、更新ベクトルは
【数５０】
0005955406.tif 000051
である。ガウス−ニュートン法を用いて最適化問題を解く。
【００８７】
実施態様
本発明によるリアルタイムのＳＬＡＭシステムは、６４０×４８０のピクセル分解能を有するテクスチャ画像及び奥行きマップを用いる。位置合わせに成功すると、現在の点群上に重ね合わされた点ランドマーク及び面ランドマークを視覚化する。本発明によるＳＬＡＭシステムは常に、大域マップに対しフレームごとに大域位置合わせを求める。このため、位置合わせの失敗は後続の入力に影響を与えない。本発明によるＳＬＡＭシステムは、シーンの面ベースの表現として、再構成された面ランドマークを提供する。これはよりコンパクトであり、点ベースのシーン表現よりも多くの意味的情報を提供する。
【００８８】
発明の効果
本発明は、２つの異なる座標系内の３Ｄデータを位置合わせするのに、点及び面の双方をプリミティブとして用いる位置合わせ方法を提供する。位置合わせ方法を用いて、位置合わせのために点対点及び面対面の双方の対応を用いる３ＤセンサーのためのリアルタイムＳＬＡＭシステムが開発される。双方のプリミティブを用いることによって、点のみを用いるよりも高速で正確な位置合わせが可能になる。本発明によるＳＬＡＭシステムは、３Ｄモデルを面の集合として生成し、これは厳密に点ベースの表現よりもコンパクトで意味的なシーン情報を提供する。
【００８９】
本発明は、３つの３Ｄ点／面プリミティブの任意の組み合わせが、２つの異なる座標系間の位置合わせを可能にすることを示している。本発明はまた、３Ｄ点及び３Ｄ面の双方を用いて複数の座標系間の位置合わせを共同で最適化するバンドル調整フレームワークを示している。バンドル調整フレームワークは、平行性及び直交性等の３Ｄ面間の幾何学的制約を実施して、人工的な環境の場合の位置合わせ正確度を更に改善することができる。
【００９０】
面を用いることによって、より高速な再構成及びコンパクトなモデル化の双方が可能になる。反復最近傍点（ＩＣＰ）法等の局所的方法は、３Ｄセンサーの高速運動下で極小値問題が生じやすい一方、本発明による位置合わせ方法は、大域解を与え、極小値問題を回避する。
【００９１】
本方法は、既に位置合わせされた３Ｄ点群から面プリミティブを抽出する方法と異なる。代わりに、本方法は、位置合わせのために個々の３Ｄ点群から選択された面プリミティブ及び点プリミティブを用いる。位置合わせにおいて点とともに面を用いることによって、点のみを用いるよりも効率的で正確な位置合わせが可能になる。
【００９２】
図４に示すように、本発明によるＳＬＡＭシステムの出力は、位置合わせされた点群４０１と、シーンの面ベースの表現４０２である。面ベースの表現は、点ベースの表現よりもコンパクトで意味的な情報を与える。表現はリアルタイムで生成される。
【００９３】
本発明は点及び面の双方を用いるので、従来技術において存在する失敗は回避される。この混合モードは新規である。
【００９４】
位置合わせに対する閉形式解は、点対点対応及び面対面対応について一体的に与えられる。
【図１】
0005955406.tif 000053
【図２】
0005955406.tif 000054
【図３】
0005955406.tif 000055
【図４】
0005955406.tif 000056