(57)【特許請求の範囲】
【請求項１】
データを位置合わせする方法であって、
異なる視点におけるセンサーによってシーンから前記データを入手するステップと、
前記データから、３次元（３Ｄ）点及び３Ｄ線と、該３Ｄ点及び該３Ｄ線に関連付けられた記述子とを抽出するステップと、
前記センサーの第１の座標系に表されたプリミティブの第１の集合を選択するステップであって、該プリミティブの第１の集合は、少なくとも３つの３Ｄ点を含む、ステップと、
第２の座標系に表されたプリミティブの第２の集合を選択するステップであって、該プリミティブの第２の集合は、少なくとも３つのプリミティブを取得するための３Ｄ点及び３Ｄ線の任意の組み合わせを含む、ステップと、
前記プリミティブの第１の集合及び前記プリミティブの第２の集合を用い、前記３Ｄ点を互いに位置合わせするとともに、前記３Ｄ点を前記３Ｄ線と位置合わせして、位置合わせされたプリミティブを取得するステップであって、前記位置合わせされたプリミティブは、同時位置同定地図生成（ＳＬＡＭ）システムにおいて用いられる、ステップと、
を含み、それぞれのステップは、プロセッサにおいて実行される、方法。
【請求項２】
前記位置合わせされたプリミティブをＳＬＡＭ地図に記憶するステップと、
バンドル調整によって前記ＳＬＡＭ地図を最適化するステップと、
を更に含む、請求項１に記載の方法。
【請求項３】
前記センサーは、前記データを、前記シーンの前記異なる視点ごとのフレームのシーケンスとして入手し、各フレームは、ピクセル画像及び対応する深度地図を含み、それぞれのステップは、各フレームについて実行される、請求項１に記載の方法。
【請求項４】
前記データからキーポイントを抽出するステップであって、該キーポイントは、有効な深度を有する３Ｄキーポイントと、有効な深度を有しない２次元（２Ｄ）キーポイントとを含むステップと、
前記有効な深度を有する前記３Ｄキーポイントを背面投影して前記３Ｄ点を生成するとともに、無効な深度を有する前記２Ｄキーポイントを、前記センサーの固有パラメーターに基づいて該２Ｄキーポイントと前記センサーの光心とを通る線として表して前記３Ｄ線を生成するステップと、
を更に含む、請求項３に記載の方法。
【請求項５】
前記位置合わせするステップは、
記述子を照合することに基づいて、前記プリミティブの第１の集合と前記プリミティブの第２の集合との間の対応関係を求めて、点対点対応関係、点対線対応関係、及び線対線対応関係を生成するステップ、
を更に含む、請求項４に記載の方法。
【請求項６】
前記位置合わせするステップは、同時に行われる、請求項１に記載の方法。
【請求項７】
前記データは、複数のセンサーによって入手される、請求項１に記載の方法。
【請求項８】
前記３Ｄ点を前記位置合わせするステップは、３点透視手順を用いる、請求項１に記載の方法。
【請求項９】
前記プリミティブは３Ｄ平面を含む、請求項１に記載の方法。
【請求項１０】
位置合わせするステップは、全ての可能な平面対平面対応関係を考慮する、請求項９に記載の方法。
【請求項１１】
位置合わせするステップは、次の順序、すなわち、３つの平面対平面対応関係と、２つの平面対平面対応関係及び１つの点対点対応関係と、１つの平面対平面対応関係及び２つの点対点対応関係と、３つの点対点対応関係と、３つの点対線対応関係との順序で対応関係の三重項を用いて初期化されるランダムサンプルコンセンサス（ＲＡＮＳＡＣ）を介して実行される、請求項９に記載の方法。
【請求項１２】
データを位置合わせするシステムであって、
異なる視点において、シーンから前記データを入手するように構成されたセンサーと、
プロセッサであって、
前記データから、３次元（３Ｄ）点及び３Ｄ線と、該３Ｄ点及び該３Ｄ線に関連付けられた記述子とを抽出することと、
前記センサーの第１の座標系に表されたプリミティブの第１の集合を選択することであって、該プリミティブの第１の集合は、少なくとも３つの３Ｄ点を含むことと、
第２の座標系に表されたプリミティブの第２の集合を選択することであって、該プリミティブの第２の集合は、少なくとも３つのプリミティブを取得するための３Ｄ点及び３Ｄ線の任意の組み合わせを含むことと、
前記プリミティブの第１の集合及び前記プリミティブの第２の集合を用い、前記３Ｄ点を互いに位置合わせするとともに、前記３Ｄ点を前記３Ｄ線と位置合わせして、位置合わせされたプリミティブを取得することであって、前記位置合わせされたプリミティブは、同時位置同定地図生成（ＳＬＡＭ）システムにおいて用いられることと、
を行うように構成されたプロセッサと、
を備える、システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、２次元（２Ｄ）データ及び３次元（３Ｄ）データを位置合わせすることに関し、より詳細には、センサー姿勢推定、並びに３Ｄシーン再構成、ロボット工学、及び拡張現実等のコンピュータービジョンアプリケーションのために、ＲＧＢ−Ｄセンサーから取得された２Ｄデータ及び３Ｄデータを位置合わせすることに関する。
【背景技術】
【０００２】
コンピュータービジョン、ロボット工学、及び拡張現実における多くのアプリケーションに同時位置同定地図生成（ＳＬＡＭ：simultaneous localization and mapping：同時自己位置地図獲得手法）が用いられる。ＳＬＡＭシステムの不可欠な部分は、センサー位置同定である。これには、センサー座標系におけるプリミティブとＳＬＡＭ地図座標系におけるプリミティブとの位置合わせが必要とされる。単眼センサーを用いたＳＬＡＭがよく知られている。低コスト３Ｄセンサーの普及に伴い、多くのＳＬＡＭシステムは、Ｍｉｃｒｏｓｏｆｔ  Ｋｉｎｅｃｔ（登録商標）センサー等の赤色、緑色、青色、及び深度（ＲＧＢ−Ｄ）のセンサーを用いた色データ及び深度データの双方を利用する。
【０００３】
ＲＧＢ−Ｄセンサーは、位置合わせ精度を高めることができるが、そのようなセンサーは、通常、ハードウェアの限界及び雑音に起因して、限られた範囲、例えば、Ｋｉｎｅｃｔ（登録商標）の場合に０．５ｍ〜４ｍでしか正確な深度を提供しない。ほとんどのＲＧＢ−Ｄ  ＳＬＡＭシステムは、有効な深度を有するピクセルのみを用いる一方、センサーに過度に近いシーン点又はセンサーから過度に遠いシーン点に関連付けられたピクセルを無視する。これは、センサーによって提供された情報の効果的でない使用法であり、特に、大きな深度変化を有するシーンの場合、位置合わせの不正確さをもたらすおそれがある。
【０００４】
ほとんどの従来のＳＬＡＭシステムは、単一のタイプのプリミティブ、例えば、２Ｄ点又は３Ｄ点のいずれかを測定値として用いる。例えば、特徴ベース単眼ＳＬＡＭシステムは、２Ｄ点特徴部を抽出し、２Ｄ対２Ｄ点対応関係を用いて、三角測量によって３Ｄ点ランドマークを初期化し、次いで、２Ｄ点測定値と３Ｄ点ランドマークとの間の２Ｄ対３Ｄ対応関係を用いて、連続画像におけるセンサー姿勢を推定する。これについては、非特許文献１及び非特許文献２を参照されたい。
【０００５】
特徴ベースＲＧＢ−Ｄ  ＳＬＡＭシステムは、３Ｄ点特徴部を抽出し、３Ｄ対３Ｄ点対応関係を用いてセンサー姿勢を推定する。幾つかのＳＬＡＭシステムでは、平面特徴部も測定値として用いることができる。近年の高密度ＳＬＡＭシステムは、単眼及びＲＧＢ−Ｄの双方とも、特徴部抽出に依拠しておらず、逆に、画像内の全ての２Ｄ点又は３Ｄ点を利用し、直接的な位置合わせの測光誤差又は反復最近傍点（ＩＣＰ）コストを最小にする。しかしながら、それらのシステムは、依然として、２Ｄ点又は３Ｄ点のいずれかの単一のタイプのプリミティブしか用いない。
【０００６】
幾つかのＳＬＡＭシステムは、３Ｄ測定値のハイブリッドを用いる。１つのシステムは、平面対平面対応関係及び線対平面対応関係の双方を用いる。これについては、非特許文献３を参照されたい。別のシステムは、点対点対応関係及び平面対平面対応関係の双方を用いる。これについては、非特許文献４を参照されたい。しかしながら、それらのシステムにおいて用いられる全ての測定値は、３Ｄプリミティブである。
【０００７】
幾つかのシステムは、ＲＧＢ−Ｄ画像の幾つかの部分における深度測定値の欠落の問題に対処し、２Ｄ測定値及び３Ｄ測定値の双方を用いる。これについては、非特許文献５及び非特許文献６を参照されたい。Hu他は、利用可能な深度に従って２Ｄ対２Ｄ対応関係と３Ｄ対３Ｄ対応関係とをヒューリスティックに切り替え、したがって、単一の位置合わせにおいて双方の対応関係を用いない。Zhang他は、２Ｄ測定値及び３Ｄ測定値の双方を用いて、視覚オドメトリのための２つの画像を位置合わせするが、３Ｄ測定値は、２つの画像のうちの一方においてのみ想定され、したがって、２Ｄ対３Ｄ対応関係が用いられる。
【先行技術文献】
【非特許文献】
【０００８】
【非特許文献１】Davison他「MonoSLAM: Real-time single camera SLAM」IEEE Trans. Pattern Anal. Mach. Intell., vol. 29, no. 6, pp. 1052-1067, June 2007
【非特許文献２】Klein及びMurray「Parallel tracking and mapping for small AR workspaces」Proc. IEEE Int'l Symp. Mixed and Augmented Reality (ISMAR), pp. 1-10, Nov. 2007
【非特許文献３】Trevor他「Planar surface SLAM with 3D and 2D sensors」Proc. IEEE Int’l Conf. Robotics and Automation (ICRA), pp. 3041-3048, May 2012
【非特許文献４】Taguchi他「Point-plane SLAM for hand-held 3D sensors」Proc. IEEE Int'l Conf. Robotics and Automation (ICRA), pp. 5182-5189, May 2013
【非特許文献５】Hu他「A robust RGB-D SLAM algorithm」Proc. IEEE/RSJ Int'l Conf. Intelligent Robots and Systems (IROS), Oct. 2012
【非特許文献６】Zhang他「Real-time depth enhanced monocular odometry」Proc. IEEE/RSJ Int'l Conf. Intelligent Robots and Systems (IROS), Sept. 2014
【発明の概要】
【０００９】
本発明の実施の形態は、３Ｄ点及び３Ｄ線の同時位置合わせのシステム及び方法を提供する。本方法は、同時位置同定地図生成（ＳＬＡＭ）システムにおいて用いることができる。
【００１０】
位置合わせは、フレームから抽出された測定値が大域ＳＬＡＭ地図におけるランドマークに関して位置合わせされるＳＬＡＭシステムの最も不可欠な部分である。ＲＧＢ−Ｄセンサーを用いた従来のＳＬＡＭシステムは、ＲＧＢ−Ｄセンサーのハードウェア限界及び雑音に起因して、限られた範囲にしか深度を用いない。センサーに対して過度に遠いシーンにおける深度又は過度に近いシーンにおける深度は用いられない。そのようなシステムは、特に、大きな深度変化を有するシーンにおいて位置合わせ誤差をもたらす。
【００１１】
本発明の実施の形態は、深度を有する点測定値及び深度を有しない点測定値を用いる新規なＲＧＢ−Ｄ  ＳＬＡＭシステム及び方法を提供する。本システムは、ＲＧＢフレーム（画像）からキーポイントを抽出する。有効な深度を有するキーポイントは、３Ｄ点特徴部になる一方、有効な深度を有しないキーポイントは、２Ｄ点特徴部とみなされる。フレームの２Ｄ点は、センサーの姿勢及びセンサーの固有パラメーターに基づいてセンサーの光心（optical center）と２Ｄ点の２Ｄ画像座標とを通る３Ｄ線を用いて表される。
【００１２】
次に、点対点対応関係、線対点対応関係、及び線対線対応関係が求められる。線対点対応関係について、本発明では、２Ｄ点特徴部によって関連付けられた３Ｄ線を用いて、対応する３Ｄ点特徴部が「ピンポイント（正確に位置特定）」され、点対点対応関係のみを用いるよりも長い距離制約が生成される。線対線対応関係について、３Ｄ線が三角測量され、その後の処理において３Ｄ点特徴部として用いられる３Ｄ点が生成される。本明細書では、３Ｄ線及び２Ｄ点は、ともに有効な深度を有しないキーポイントを指すので、区別なく用いられる。本システムは、平面を抽出して３Ｄ平面特徴部を生成することもでき、点対点対応関係、線対点対応関係、及び線対線対応関係に加えて平面対平面対応関係を用いて位置合わせ精度を更に改善することもできる。
【００１３】
ハイブリッド対応関係は、オンラインＳＬＡＭ及びオフライン後処理の双方において用いられる。オンラインＳＬＡＭは、リアルタイム動作を得るために、連続した画像間の対応関係を求めることによって処理速度により多くの重点を置いている一方、オフライン後処理は、より高い精度を得るために、全ての画像間に、より多くの対応関係を生成する。
【図面の簡単な説明】
【００１４】
【図１Ａ】本発明の実施の形態による２Ｄ点測定値及び３Ｄ点測定値を概略的に示す図である。
【図１Ｂ】従来の３Ｄ測定値を概略的に示す図である。
【図２】本発明の実施の形態による、センサーによって入手された２Ｄデータ及び３Ｄデータのハイブリッド同時位置同定地図生成（ＳＬＡＭ）のシステム及び方法の概略図である。
【図３】図２のシステム及び方法の更なる詳細の概略図である。
【発明を実施するための形態】
【００１５】
本発明の実施の形態は、赤色、緑色、青色、及び深度（ＲＧＢ−Ｄ）のセンサーによって入手された２Ｄデータ及び３Ｄデータのハイブリッド同時位置同定地図生成のシステム及び方法を提供する。
【００１６】
図１Ａは、本発明の実施の形態によって用いられる、２Ｄ点から３Ｄ点を「ピンポイント」する基本となるアイデアを概略的に示している。三角形１００は、異なる姿勢（視点）、すなわち、異なる３Ｄ位置及び３Ｄ方位にあるセンサーを表している。代替的に、複数のセンサーを用いることができる。
【００１７】
点線１０１は、２Ｄ測定値に関連付けられた線（すなわち、無効な深度を有する２Ｄキーポイント）を表し、円１０２及び実線１０３は、３Ｄ測定値を表している。三角測量される３Ｄ点は、菱形１０４を用いて示されている。三角測量は、線１０１の交点を用いる。従来のＳＬＡＭシステムにおいて用いられる点対点対応関係に加えて、本発明では、線対点対応関係及び線対線対応関係を用いて、長距離制約が生成される。２Ｄ点（無効な深度を有するキーポイント）によって規定される線は、３Ｄ点、すなわち、有効な深度を有するキーポイント、又は線を交差させることによって三角測量された３Ｄ点を「ピンポイント」する。
【００１８】
図１Ｂは、点対点対応関係のみを用い、その結果、限られた深度範囲に起因して画像間の短距離制約が得られる従来のＳＬＡＭシステムを示している。
【００１９】
従来のＳＬＡＭシステムとは異なり、本発明のシステムは、画像から２Ｄ測定値及び３Ｄ測定値の双方を抽出し、この場合、有効な深度を有するキーポイントは、３Ｄ測定値になり、有効な深度を有しないキーポイントは、２Ｄ測定値として扱われる。その後、システムは、点対点対応関係、線対点対応関係、及び線対線対応関係を確立する。２Ｄ測定値を含む付加された対応関係は、画像間の長距離制約を提供する。すなわち、本発明では、２Ｄ点によって規定される線を用いて３Ｄ点が「ピンポイント」される。
【００２０】
本発明のシステムは、平面を付加的に抽出して３Ｄ平面特徴部を生成することができ、点対点対応関係、線対点対応関係、及び線対線対応関係に加えて平面対平面対応関係を用いて、位置合わせ精度を更に改善することができる。
【００２１】
本発明では、オンラインＳＬＡＭ及びオフライン後処理の双方において、ハイブリッド対応関係が用いられる。オンラインＳＬＡＭの場合、本発明では、リアルタイム処理のための近傍画像間の対応関係検索が制限される。この手法は、点対点対応関係しか用いない従来の手法よりも長い距離制約を生成するが、遠方の画像間に存在し得る対応関係を利用していない。
【００２２】
本発明の後処理は、そのような対応関係を、各画像と全ての画像からの測定値及び対応関係を用いて生成されるＳＬＡＭ地図との間のハイブリッド対応関係を反復的に求めることによって確立する。各反復において、これらの対応関係は、まず、画像の現在の姿勢に基づいて更新され、次に、バンドル調整手順を用いて、これらの姿勢を更新するのに用いられる。
【００２３】
ピンポイントＳＬＡＭ及び後処理
この節では、３Ｄ点及び３Ｄ線を用いた本発明の位置合わせ方法を説明する。本発明の方法は、任意の点特徴ベースＲＧＢ−Ｄ  ＳＬＡＭシステムに組み込むことができる。ここで、代表的なキーフレームをＳＬＡＭ地図に記憶するキーフレームベースＳＬＡＭシステムを用いた一例が提供される。本発明の位置合わせ方法の概略は、図２に見ることができ、この位置合わせ方法を用いた本発明のＳＬＡＭシステムは、図３に見ることができる。
【００２４】
本発明では、この方法は、オンラインＳＬＡＭ及びオフライン後処理の双方に用いられる。オンラインＳＬＡＭにおいて、本発明では、各入力フレームが一度処理され、このフレームは、キーフレームの以前の測定値及び対応関係を含むＳＬＡＭ地図３８０に位置合わせされる。オフライン後処理において、本発明では、ＳＬＡＭ地図における各キーフレームが処理され、このキーフレームが、ＳＬＡＭ地図におけるキーフレームの全てと位置合わせされ、キーフレームの姿勢が更新される限り、このプロセスが反復される。
【００２５】
表記
本発明では、測定値及びランドマークの従来の術語が用いられる。システムは、各ＲＧＢ−Ｄフレームから測定値を抽出し、大域ＳＬＡＭ地図にランドマークを生成する。
【００２６】
３Ｄ点測定値は（ｐｍ，Ｄｍ）であり、ここで、
【数１】
0006430064.tif 000002
は、その３Ｄ位置を示し、Ｄｍは、その関連付けられた記述子を示す。２Ｄ点測定値は、
【数２】
0006430064.tif 000003
であり、ここで、
【数３】
0006430064.tif 000004
は、２Ｄピクセル座標を表し、
【数４】
0006430064.tif 000005
は、その関連付けられた記述子であり、νｍ＝（ｃｍ，ｕｍ）は、センサーの固有パラメーター、すなわち、焦点距離（ｆｘ，ｆｙ）及び主点（ｃｘ，ｃｙ）に基づいて、
【数５】
0006430064.tif 000006
となるような、センサーの光心と２Ｄ点測定値とを通る３Ｄ線を表し、Ｔは、転置演算子である。
【００２７】
３Ｄ平面測定値は、平面パラメーターと、平面に関連付けられた３Ｄ内座層点（inlier points）の集合とを示す（πｍ，Ｉｍ）によって表される。
【００２８】
ランドマークは、測定値の集合体である。３Ｄ点ランドマークは、（ｐｌ，Ｄｌ）によって表され、ここで、
【数６】
0006430064.tif 000007
は、その３Ｄ位置を示し、Ｄｌは、このランドマークに関連付けられた記述子の集合を示す。３Ｄ平面ランドマークは、平面パラメーターπｌと、全ての関連付けられたフレームからの３Ｄ内座層点の集合Ｉｌとを有する（πｌ，Ｉｌ）である。
【００２９】
位置合わせ方法
本発明では、赤色、緑色、青色、及び深度（ＲＧＢ−Ｄ）のセンサー２０１を用いて３Ｄシーン２０２のデータ２０３が入手される（２１０）。これらのデータは、センサーのそれぞれ異なる姿勢（視点）ごとのフレームのシーケンスとして入手することもできるし、代替的に、複数のセンサーが用いられる。各フレーム内のデータから、本発明では、３Ｄ点２１１及び３Ｄ線２１２並びに関連付けられた記述子が抽出される。
【００３０】
本発明では、第１の座標系に表された少なくとも３つの３Ｄ点を含むプリミティブの第１の集合が選択される（２２０）。本発明では、第２の座標系に表されたプリミティブの第２の集合も選択される（２３０）。ここで、プリミティブの第２の集合は、全部で少なくとも３つのプリミティブを取得するための３Ｄ点及び３Ｄ線の任意の組み合わせを含む。
【００３１】
次に、本発明では、プリミティブの第１の集合及びプリミティブの第２の集合を用いて、３Ｄ点が互いに位置合わせされると同時に、３Ｄ点と３Ｄ線とが位置合わせされ（２４０）、位置合わせされたプリミティブ２５０が取得される。この位置合わせは、ランダムサンプルコンセンサス（ＲＡＮＳＡＣ）を用いることができる。位置合わせされたプリミティブは、任意のタイプのＳＬＡＭシステム２６０において用いることができる。これらのステップは、プロセッサ２００において実行される。
【００３２】
特徴部抽出
本発明のシステムは、各ＲＧＢ画像から２Ｄキーポイントを抽出する（３１１）。各キーポイントは、ロケーション（ｘ，ｙ）及び関連付けられた記述子を有する。２Ｄキーポイントは、例えば、スケール不変特徴変換（ＳＩＦＴ）キーポイント検出器及び記述子を用いることによって取得することができるが、任意の種類のキーポイント検出器及び記述子をこのステップに用いることができる。対応する深度が既定の範囲内にある場合、それらの深度は、有効であるとみなされる。有効な深度を有するキーポイントは、背面投影され、３Ｄ点測定値３２２として用いられる。無効な深度を有するキーポイントは、２Ｄ点測定値３２１とみなされ、センサーの光心と２Ｄキーポイントとを通る３Ｄ線として表される。
【００３３】
本発明では、例えば、凝集型階層クラスタリングを用いて、３Ｄ平面測定値３２３も抽出することができる（３１２）。これについては、Feng他「Fast plane extraction in organized point clouds using agglomerative hierarchical clustering」Proc. IEEE Int'l Conf. Robotics and Automation (ICRA), May 2014を参照されたい。
【００３４】
特徴部抽出ステップは、後処理の間は、省略される。なぜならば、後処理は、特徴部が既に抽出されている既存のＳＬＡＭ地図に適用されるからである。
【００３５】
キーポイントは、ＲＧＢ画像の代わりに強度画像から抽出することができることに留意されたい。したがって、本発明のシステムは、ＲＧＢ−Ｄフレームの代わりに強度フレーム及び深度フレームを入力として用いることができる。本明細書では、ピクセル画像は、強度画像又はＲＧＢ画像のいずれかを指すのに用いられる。
【００３６】
対応関係検索
特徴部抽出後、本発明では、現在のフレームから抽出されたキーポイントとＳＬＡＭ地図におけるキーポイントとの間の対応関係３３０が求められる。平面対応関係は検索されない。なぜならば、平面の数は、点の数よりもはるかに少ないからである。その代わりに、本発明のＲＡＮＳＡＣ位置合わせは、全ての可能な平面対応関係を考慮する。
【００３７】
オンラインＳＬＡＭ
本発明では、現在のフレームのキーポイントとＳＬＡＭ地図の直前のｋ個のキーフレームのキーポイントとの間で、全対全記述子照合が実行される。地図の全てのキーポイントの代わりにｋ個のキーフレームを考慮することによって、検索の時間が削減される。なぜならば、連続的なセンサーの動きを仮定すると、直前のキーフレームは、シーンにおいて現在のフレームと同じ領域を観測している可能性がより高いからである。この照合は、次の対応関係、すなわち、点対点対応関係、線対点対応関係、及び線対線対応関係を返す。これらの対応関係のそれぞれは、ＲＡＮＳＡＣ位置合わせフェーズにおいて特別に考慮される。線対点対応関係は、３Ｄ点ランドマークから２Ｄ点測定値又は２Ｄ点ランドマークから３Ｄ点測定値への２つの場合を考慮している。
【００３８】
オフライン後処理
本発明では、キーフレームの姿勢を予測として用いることによって、不要な記述子照合が回避される。本発明では、地図の全ての３Ｄ点が、予測された姿勢に基づいてフレームに投影される。フレーム内の測定値は、その投影がｒ個のピクセルからなる近傍の範囲内に入る場合、３Ｄ点ランドマークと整合する。これによって、点対点対応関係又は３Ｄランドマークから２Ｄ測定値への対応関係（すなわち、点対線対応関係）のいずれかが生成される。２Ｄランドマークと３Ｄ測定値との間にある点対線対応関係の場合、検索は、投影の方向の変化に伴って同じ方法で行われる。すなわち、フレームの３Ｄ点測定値は、２Ｄ点ランドマークを開始するキーフレームに投影される。線対線対応関係については、本発明では、２Ｄ点測定値を２Ｄ点ランドマークと照合するために、エピポーラ線までの距離がｒ個のピクセルよりも少ないことが検査される。
【００３９】
ＲＡＮＳＡＣ位置合わせ
ＲＡＮＳＡＣ位置合わせ３４０は、（ｉ）３つの平面、（ｉｉ）２つの平面＋１つの３Ｄ点、（ｉｉｉ）１つの平面＋２つの３Ｄ点、及び（ｉｖ）３つの３Ｄ点の順序で異なるタイプの仮定を試行する。本発明では、２Ｄ点測定値が用いられるので、本発明では、このリストに最後の仮定も追加される。この最後の仮定は、３つの線対点対応関係を考慮するものである。
【００４０】
本発明では、３点透視（Ｐ３Ｐ：perspective-three-point）手順が適用され、線対点対応関係を用いて位置合わせパラメーターが求められる。これについては、Haralick他「Review and analysis of solutions of the three point perspective pose estimation problem」Int'l J. Computer Vision, vol. 13, no. 3, pp. 331-356, Dec. 1994を参照されたい。線対点対応関係に加えて、本発明では、３Ｄ点のうちの１つの深度を無視することによって、点対点対応関係を線対点対応関係として扱うことができることに留意されたい。
【００４１】
ＲＡＮＳＡＣ内座層チェックが、対応関係のタイプに基づいて実行される。２つの３Ｄ点は、これらの点の間の距離が閾値未満であるときに内座層とみなされる。線対点対応関係の場合、本発明では、３Ｄ点と２Ｄ点に対応する線との間の距離が考慮される。３Ｄ点ランドマークｐｌと２Ｄ点測定値ｑｍとの間の距離
【数７】
0006430064.tif 000008
は、下式となる。
【数８】
0006430064.tif 000009
ここで、Ｔは、２Ｄ点測定値を含むキーフレームの姿勢であり、＜・，・＞は、ドット積を示す。上記距離が閾値未満である場合、対応関係は、内座層としてカウントされる。線対線対応関係の場合、本発明では、他方の点の対応するエピポーラ線までのピクセルの距離が閾値未満であることがチェックされる。
【００４２】
ＳＬＡＭ地図更新
ＲＡＮＳＡＣ位置合わせが成功した場合（３５０）、推定された姿勢及び内座層対応関係が用いられて、ＳＬＡＭ地図３８０が更新される（３７０）。
【００４３】
オンラインＳＬＡＭ
フレームの推定された姿勢が、地図における既存のキーフレームの姿勢と異なるとき、このフレームは、新たなキーフレーム３６０として地図に追加される。２Ｄ測定値が３Ｄ点ランドマークと整合する場合、この２Ｄ測定値の記述子を追加することによって、記述子の集合は、拡大される。線対線の整合の場合、本発明では、２つの線上の最も近い点の中点を求めることによって、三角測量が実行され、２Ｄ測定値の記述子を収集することによって、この中点が３Ｄ点ランドマークとして地図に追加される。本発明では、小さなカメラベースラインを用いた線対線の整合の三角測量は、雑音を導入するので無視される。３Ｄ点測定値が２Ｄ点ランドマークと整合する場合、このランドマークは、地図座標系に転送された３Ｄ点測定値の３Ｄ座標を介して３Ｄ点ランドマークとして更新される。全ての整合しない点測定値は、点ランドマークとして地図に追加される。
【００４４】
オフライン後処理
後処理は、既存の地図に適用されるので、全ての測定値は、ランドマークに既に関連付けられている。したがって、この段階は、キーフレームと地図との間の対応関係のみを更新する。ＲＡＮＳＡＣ内座層は、キーフレームの対応関係を精緻化するのに用いられる。測定値が、現在関連付けられているランドマークと異なるランドマークと整合する場合、この測定値は、現在のランドマークから除去され、新たな内座層ランドマークに関連付けられる。同様に、測定値が、その現在のランドマークと整合していない場合、この測定値はそのランドマークから分割され、新たなランドマークが生成される。
【００４５】
バンドル調整
三重項（ｋ，ｌ，ｍ）は、３Ｄ点ランドマークｐｌ（ｌ＝１，．．．，Ｌ）が姿勢Ｔｋ（ｋ＝１，．．．，Ｋ）を有するキーフレームｋ内の３Ｄ点測定値
【数９】
0006430064.tif 000010
又は２Ｄ点測定値
【数１０】
0006430064.tif 000011
に関連付けられている関連付けを示す。同様に、三重項（ｋ，ｌ’，ｍ’）は、３Ｄ平面ランドマークπｌ’（ｌ’＝１，．．．，Ｌ’）がキーフレームｋ内の３Ｄ平面測定値
【数１１】
0006430064.tif 000012
に関連付けられているような関連付けを示す。Ａ１、Ａ２、及びＡ３は、それぞれ地図における点対点関連付け、線対点関連付け、及び平面対平面関連付けを表す全ての三重項を含むことにする。その場合、バンドル調整３９０は、３Ｄ点ランドマーク座標、３Ｄ平面ランドマークパラメーター、及びキーフレーム姿勢に関する以下の誤差関数を最小にする。
【数１２】
0006430064.tif 000013
【００４６】
ここで、
【数１３】
0006430064.tif 000014
は、キーフレームｋ内の平面測定値の内座層点の集合（πｍ’，Ｉｍ’）からサンプリングされた３Ｄ点
【数１４】
0006430064.tif 000015
の平面ランドマークπｌまでの距離である。
【００４７】
オンラインＳＬＡＭ
バンドル調整は、非同期に実行される。本発明では、ループ閉鎖も実行される。ループ閉鎖は、まず、キーポイントに基づいて求められた局所集約記述子ベクトル（ＶＬＡＤ：vector of locally aggregated descriptors）記述子を用いて地図におけるキーフレームの全ての対の外観類似性をチェックする。これについては、Jegou他「Aggregating local image descriptors into compact codes」IEEE Trans. Pattern Anal. Mach. Intell., vol. 34, no. 9, pp. 1704-1716, Sept. 2012を参照されたい。オンラインＳＬＡＭは、キーフレームの対の間の類似性もチェックする。本発明では、キーフレームの現在の姿勢が過度に異なるとき、ループの閉鎖は、試行されない。類似性テストに合格したキーフレームの対について、本発明のシステムは、次に、ハイブリッド対応関係を用いてＲＡＮＳＡＣ位置合わせを実行し、ＲＡＮＳＡＣが成功した場合、内座層対応関係が用いられて、地図における関連付けが更新される。
【００４８】
オフライン後処理
後処理は、関連付けを更新するとともに地図におけるランドマークパラメーター及びキーフレーム姿勢を精緻化するために反復的に実行される。後処理の反復では、あらゆるキーフレームが、キーフレームの残りのものを含む地図と再位置合わせされ、その対応関係が更新される。すなわち、ランドマークの分割及びマージが必要なときに実行される。全ての対応関係が更新された後、本発明では、バンドル調整が再度実行され、ランドマークパラメーター及びキーフレーム姿勢が精緻化される。本発明では、キーフレーム姿勢の平均変化が閾値よりも大きい場合に、この反復が繰り返される。したがって、バンドル調整は、オフライン後処理において逐次的に実行される。
【００４９】
ＳＬＡＭシステムの要約
本発明では、３Ｄシーン２０２のフレーム２０３が、例えば、赤色、緑色、青色、及び深度（ＲＧＢ−Ｄ）のセンサー２０１を用いて入手される（３１０）。フレーム内のデータは、以下のように逐次的に処理される。
【００５０】
各フレームについて、本発明では、キーポイントが抽出される（３１１）。これらのキーポイントは、有効な深度を有しない２Ｄ点３２１と、有効な深度を有する３Ｄ点３２２とを含む。本発明では、３Ｄ平面３２３も抽出することができる（３１２）。次に、本発明では、現在のフレームと以前の処理済みフレームを含むＳＬＡＭ地図３８０との間の対応関係検索３３０が実行されて、点対応関係が取得され、入力フレームとＳＬＡＭ地図との間の全ての可能な平面対応関係が考慮される。次に、本発明では、点対点対応関係、線対点対応関係、線対線対応関係、及び平面対平面対応関係を用いて、ＲＡＮＳＡＣ位置合わせ３４０が実行される。
【００５１】
ＲＡＮＳＡＣが成功した場合（３５０）、フレームが新たなキーフレームであるか否かが判断され（３６０）、新たなキーフレームである場合、それに応じて、ＳＬＡＭ地図３８０が更新され（３７０）、そうでない場合、本発明では、次のフレームが入手される。本発明では、バンドル調整及びループ閉鎖手順３９０が非同期に実行されて、ＳＬＡＭ地図３８０における３Ｄ点ランドマーク、３Ｄ平面ランドマーク、及びキーフレーム姿勢が最適化される。バンドル調整は、シーン２０２の幾何形状を記述する３Ｄ座標を同時に精緻化する。
【００５２】
発明の効果
本発明の実施の形態は、ＳＬＡＭシステムの位置合わせされたプリミティブを提供する。本発明の１つの目的は、ＲＧＢ−Ｄセンサーによって入手された全てのデータを有効に用いることである。このシステムは、２Ｄ及び３ＤのＳＬＡＭのハイブリッドとみなすことができる。２Ｄ測定値は、センサーの光心と２Ｄ点とを通る３Ｄ線として表される。
【００５３】
３Ｄ点を関連付けられた３Ｄ線と照合することによって、フレーム間の位置合わせが改善される。２つの整合する２Ｄ測定値が三角測量され、地図に追加される。
【００５４】
このシステムは、フレーム間のより良好な対応関係を可能にし、位置合わせ精度の改善をもたらす。さらに、同じ手法は、再構成を精緻化することができるオフライン後処理手順に用いられる。
【００５５】
それらの結果、高い深度変化を有するシーンに対して、ピンポイントＳＬＡＭ及び後処理が、従来のシステムと比較して精度を向上させることが示されている。
【図１Ａ】
0006430064.tif 000016
【図１Ｂ】
0006430064.tif 000017
【図２】
0006430064.tif 000018
【図３】
0006430064.tif 000019