(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2023-04-19
(45)【発行日】2023-04-27
(54)【発明の名称】３次元マップ生成方法及びシステム
(51)【国際特許分類】
G05D 1/02 20200101AFI20230420BHJP
G06T 7/73 20170101ALI20230420BHJP
【ＦＩ】
G05D1/02 H
G06T7/73
【請求項の数】
15
(21)【出願番号】P 2022030728
(22)【出願日】2022-03-01
(65)【公開番号】P2022134119
(43)【公開日】2022-09-14
【審査請求日】2022-03-01
(31)【優先権主張番号】10-2021-0027774
(32)【優先日】2021-03-02
(33)【優先権主張国・地域又は機関】KR
(73)【特許権者】
【識別番号】319012978
【氏名又は名称】ネイバーラボス コーポレーション
(74)【代理人】
【識別番号】100107766
【弁理士】
【氏名又は名称】伊東 忠重
(74)【代理人】
【識別番号】100070150
【弁理士】
【氏名又は名称】伊東 忠彦
(74)【代理人】
【識別番号】100135079
【弁理士】
【氏名又は名称】宮崎 修
(72)【発明者】
【氏名】イ ヨンハン
(72)【発明者】
【氏名】ヨン スヨン
(72)【発明者】
【氏名】リュー スヒョン
(72)【発明者】
【氏名】キム ドクファ
(72)【発明者】
【氏名】イ ドンハン
【審査官】影山 直洋
(56)【参考文献】
【文献】特開２００９－２３７８４５（ＪＰ，Ａ）
【文献】国際公開第２０１８／１３１１６５（ＷＯ，Ａ１）
【文献】国際公開第２０２１／００６６７７（ＷＯ，Ａ２）
【文献】Donghwan Lee, et.al.，Large-scale Localization Datasets in Crowded Indoor Spaces，CVPR 2021，3226-3235
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０５Ｄ １／０２
Ｇ０６Ｔ ７／７３
(57)【特許請求の範囲】
【請求項1】
収集装置に備えられるＬｉＤＡＲセンサ及びカメラセンサを用いて空間の空間データ及び画像データをそれぞれ収集するステップと、前記空間データを用いて前記ＬｉＤＡＲセンサの移動軌跡を推定するステップと、
前記画像データ及び前記移動軌跡を入力データとして用いてＳＦＭ（Structure-From-Motion）をベースとして前記空間に関する３次元構造データを生成するステップとを含むことを特徴とする３次元マップ生成方法。
【請求項2】
前記収集するステップにおいては、前記ＬｉＤＡＲセンサを用いたＬｉＤＡＲ ＳＬＡＭ（Simultaneous Localization and Mapping）により前記空間データ及び前記画像データを収集することを特徴とする請求項１に記載の３次元マップ生成方法。
【請求項3】
前記ＬｉＤＡＲ ＳＬＡＭの出力データは、前記画像データ及び前記移動軌跡を備え、前記ＬｉＤＡＲ ＳＬＡＭ及び前記ＳＦＭがパイプラインを形成するように、前記出力データが前記入力データとして用いられることを特徴とする請求項２に記載の３次元マップ生成方法。
【請求項4】
前記３次元構造データを生成するステップは、前記画像データから特徴点を抽出し、画像間で前記特徴点をマッチングするステップと、
前記移動軌跡を連続軌跡（Continuous Trajectory）に変換するステップと、
前記連続軌跡に基づいて前記画像データのポーズ情報を算出するステップとを含むことを特徴とする請求項１に記載の３次元マップ生成方法。
【請求項5】
前記画像データのポーズ情報が前記連続軌跡に整合するように、前記移動軌跡は、連続時間軌跡（Continuous-Time Trajectory）としてモデリングされることを特徴とする請求項４に記載の３次元マップ生成方法。
【請求項6】
前記生成された３次元構造データは、前記ＳＦＭの出力データとして、前記ポーズ情報が算出された画像及びポイントクラウドデータを備えることを特徴とする請求項４に記載の３次元マップ生成方法。
【請求項7】
前記３次元構造データを生成するステップにおいては、前記移動軌跡を連続３次スプライン（continuous cubic Spline）で表現してポーズエラーを定義することにより、地図最適化を行うことを特徴とする請求項１に記載の３次元マップ生成方法。
【請求項8】
前記生成された３次元構造データを用いて画像の深度情報を推定して深度マップを生成するステップをさらに含むことを特徴とする請求項１に記載の３次元マップ生成方法。
【請求項9】
前記推定された移動軌跡は、単一の地域で複数のデータとして算出され、前記複数のデータの整合を行うステップをさらに含むことを特徴とする請求項１に記載の３次元マップ生成方法。
【請求項10】
前記複数のデータの整合が行われるように、前記移動軌跡間で座標系変換を推定するか、又は前記移動軌跡間の相対ポーズを推定することを特徴とする請求項９に記載の３次元マップ生成方法。
【請求項11】
ＬｉＤＡＲセンサ及びカメラセンサを用いて空間の空間データ及び画像データをそれぞれ収集し、前記ＬｉＤＡＲセンサの移動軌跡を推定するステップと、前記画像データから特徴点を抽出し、画像間で前記特徴点をマッチングするステップと、
前記移動軌跡を連続軌跡（Continuous Trajectory）に変換し、前記連続軌跡に基づいて前記特徴点及び前記画像データのポーズ情報を最適化して前記空間の３次元特徴点マップを生成するステップとを含むことを特徴とする３次元マップ生成方法。
【請求項12】
前記３次元特徴点マップを生成するステップにおいては、前記移動軌跡を連続３次スプライン（continuous cubic Spline）で表現してポーズエラーを定義することにより、地図最適化を行うことを特徴とする請求項１１に記載の３次元マップ生成方法。
【請求項13】
前記画像データ及び前記移動軌跡は、ＬｉＤＡＲ ＳＬＡＭ（Simultaneous Localization and Mapping）の出力データとして出力され、前記３次元特徴点マップは、前記画像データ及び前記移動軌跡を入力データとして用いてＳＦＭ（Structure-From-Motion）をベースとして生成されることを特徴とする請求項１１に記載の３次元マップ生成方法。
【請求項14】
ＬｉＤＡＲセンサ及びカメラセンサを備える収集装置と、前記収集装置を用いて空間の３次元特徴点マップを生成するマッピング装置とを含み、
前記収集装置は、前記ＬｉＤＡＲセンサ及び前記カメラセンサを用いて前記空間の空間データ及び画像データをそれぞれ収集し、
前記マッピング装置は、前記空間データを用いて前記ＬｉＤＡＲセンサの移動軌跡を推定し、前記画像データ及び前記移動軌跡を入力データとして用いてＳＦＭ（Structure-From-Motion）をベースとして前記空間に関する３次元構造データを生成することを特徴とするマップ生成システム。
【請求項15】
電子機器で１つ以上のプロセスにより実行され、コンピュータ可読媒体に格納可能なプログラムであって、前記プログラムは、
収集装置に備えられるＬｉＤＡＲセンサ及びカメラセンサを用いて空間の空間データ及び画像データをそれぞれ収集するステップと、
前記空間データを用いて前記ＬｉＤＡＲセンサの移動軌跡を推定するステップと、
前記画像データ及び前記移動軌跡を入力データとして用いてＳＦＭ（Structure-From-Motion）をベースとして前記空間に関する３次元構造データを生成するステップとを実行させるコマンドを含むことを特徴とするコンピュータ可読媒体に格納可能なプログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、３次元マップ生成方法及びシステムに関し、特に、空間で測位に活用できる３次元マップを生成する方法及びシステムに関する。
【背景技術】
【0002】
測位技術は、ＧＰＳベース、慣性センサベース、画像ベース、ＳＬＡＭ（Simultaneous Localization and Mapping）、ＶＬＣ（Visible Light Communication）などの様々な方式のシステムが互いに密接な関係を有して発展してきている。
【0003】
このような測位技術のうち、画像ベースの測位（Visual Localization）は、撮影した写真を用いて位置を検出する技術であり、ＧＰＳと比較すると、誤差が少なく、ユーザが見る方向までも正確に測定できるという利点がある。このような画像ベースの測位のためには、撮影した写真と比較して位置を特定するためのマップを構築しなければならない。
【0004】
一方、マップ構築の対象が室内空間であるか、室外環境であるかによって考慮すべき点が異なる。例えば、室内空間は、一般的に室外環境より小さい空間であるので、カメラの小さな動きによっても視点の大きな変化がもたらされる。また、大型ショッピングモールや地下鉄駅などの大規模な室内空間の実際の環境では、時間帯に応じた混雑度の変化や、テクスチャ（質感）がない空間などにより、カメラポーズを正確に推定することが困難である。
【0005】
このような問題により、室内空間では、ＳＦＭ（Structure-From-Motion）が効果的なツールとして用いられる室外環境とは異なり、大規模でＳＦＭを適用することが困難である。室内空間では、ＲＧＢＤカメラやレーザスキャナなどを用いてマップを構築する方法が多く用いられているが、これは相対的に小さい室内空間に適している。ＲＧＢＤカメラは測定距離が短く、レーザスキャナは色情報がなく、動く物体が多い場合に脆弱であるので、大規模な室内空間ではあまり用いられない。
【0006】
よって、大規模な室内空間で画像ベースの測位用マップを生成するために、新たな形態のパイプラインを用いる方法が考慮される。
【発明の概要】
【発明が解決しようとする課題】
【0007】
本発明は、大規模な室内空間に適用できる画像ベースの測位用３次元マップを生成する方法及びシステムを提供するものである。
【0008】
より具体的には、本発明は、新たな形態のパイプラインを用いて画像ベースの測位に活用できる３次元特徴点マップを生成する方法を提供する。
【0009】
また、本発明は、所定期間に時間間隔をおいて取得したデータを用いて大規模な室内空間のより正確なマップを生成する方法を提供する。
【課題を解決するための手段】
【0010】
上記課題を解決するために、本発明による３次元マップ生成方法及びシステムにおいては、大規模な空間に対して、所定期間に収集したシーケンスデータに基づいてＬｉＤＡＲセンサの軌跡を活用したビジュアルマッピング（Visual Mapping）を行うことにより、３次元マップを構築する。例えば、本発明は、ＬｉＤＡＲ ＳＬＡＭ（Simultaneous Localization and Mapping）からＳＦＭ（Structure-From-Motion）までのパイプラインを用いて空間の３次元マップを生成するプロセスを用いる。
【0011】
具体的には、本発明による３次元マップ生成方法は、収集装置に備えられるＬｉＤＡＲセンサ及びカメラセンサを用いて空間の空間データ及び画像データをそれぞれ収集するステップと、前記空間データを用いて前記ＬｉＤＡＲセンサの移動軌跡を推定するステップと、前記画像データ及び前記移動軌跡を入力データとして用いてＳＦＭ（Structure-From-Motion）をベースとして前記空間に関する３次元構造データを生成するステップとを含む。
【0012】
本発明の一実施形態において、前記収集するステップにおいては、前記ＬｉＤＡＲセンサを用いたＬｉＤＡＲ ＳＬＡＭ（Simultaneous Localization and Mapping）により前記空間データ及び前記画像データを収集するようにしてもよい。
【0013】
前記ＬｉＤＡＲ ＳＬＡＭの出力データは、前記画像データ及び前記移動軌跡を備え、前記ＬｉＤＡＲ ＳＬＡＭ及び前記ＳＦＭがパイプラインを形成するように、前記出力データが前記入力データとして用いられるようにしてもよい。
【0014】
本発明の他の実施形態において、前記３次元構造データを生成するステップは、前記画像データから特徴点を抽出し、画像間で前記特徴点をマッチングするステップと、前記移動軌跡を連続軌跡（Continuous Trajectory）に変換するステップと、前記連続軌跡に基づいて前記画像データのポーズ情報を算出するステップとを含む。
【0015】
前記画像データのポーズ情報が前記連続軌跡に整合するように、前記移動軌跡は、連続時間軌跡（Continuous-Time Trajectory）としてモデリングされるようにしてもよい。前記生成された３次元構造データは、前記ＳＦＭの出力データとして、前記ポーズ情報が算出された画像及びポイントクラウドデータを備えるようにしてもよい。
【0016】
本発明のさらに他の実施形態において、前記３次元構造データを生成するステップにおいては、前記移動軌跡を連続３次スプライン（continuous cubic Spline）で表現してポーズエラーを定義することにより、地図最適化を行うようにしてもよい。
【0017】
本発明のさらに他の実施形態において、前記３次元マップ生成方法は、前記生成された３次元構造データを用いて画像の深度情報を推定して深度マップ（depthmap）を生成するステップをさらに含むようにしてもよい。
【0018】
本発明のさらに他の実施形態において、前記推定された移動軌跡は、単一の地域で複数のデータとして算出され、前記３次元マップ生成方法は、前記複数のデータの整合を行うステップをさらに含むようにしてもよい。前記複数のデータの整合が行われるように、前記移動軌跡間で座標系変換を推定するか、又は前記移動軌跡間の相対ポーズを推定するようにしてもよい。
【0019】
また、本発明は、ＬｉＤＡＲセンサ及びカメラセンサを用いて空間の空間データ及び画像データをそれぞれ収集し、前記ＬｉＤＡＲセンサの移動軌跡を推定するステップと、前記画像データから特徴点を抽出し、画像間で前記特徴点をマッチングするステップと、前記移動軌跡を連続軌跡（Continuous Trajectory）に変換し、前記連続軌跡に基づいて前記特徴点及び前記画像データのポーズ情報を最適化して前記空間の３次元特徴点マップを生成するステップとを含む、３次元マップ生成方法を開示する。
【0020】
さらに、本発明は、ＬｉＤＡＲセンサ及びカメラセンサを備える収集装置と、前記収集装置を用いて空間の３次元特徴点マップを生成するマッピング装置とを含み、前記収集装置は、前記ＬｉＤＡＲセンサ及び前記カメラセンサを用いて前記空間の空間データ及び画像データをそれぞれ収集し、前記マッピング装置は、前記空間データを用いて前記ＬｉＤＡＲセンサの移動軌跡を推定し、前記画像データ及び前記移動軌跡を入力データとして用いてＳＦＭ（Structure-From-Motion）をベースとして前記空間に関する３次元構造データを生成する、マップ生成システムを開示する。
【発明の効果】
【0021】
本発明による３次元マップ生成方法及びシステムは、ＬｉＤＡＲ ＳＬＡＭからＳＦＭまでのパイプラインにより、大規模な空間で所定期間に収集したデータを用いる場合もより正確な室内マッピングを行うことができる。
【0022】
このように、新たなパイプラインを用いて正確な３次元マップを生成することにより、大規模な室内空間、例えばデパートや地下鉄駅でクエリ画像を用いた画像ベースの測位を行うことができる。
【0023】
また、本発明は、ＬｉＤＡＲセンサの軌跡を活用したビジュアルマッピングを行うことにより、室外環境に適したＳＦＭベースのアルゴリズムを用いて大規模な室内空間の正確なマッピングを実現することができる。
【0024】
特に、ＬｉＤＡＲセンサ（又は収集装置、プラットフォーム、マッピングロボット）の軌跡を連続時間軌跡で表現し、カメラセンサで取得した画像のポーズ情報を対応する連続軌跡とより精巧に整合することができる。
【図面の簡単な説明】
【0025】
【図1a】本発明による３次元マップを構築する動作を説明するための概念図である。
【図2】本発明による３次元マップ生成システムを示す概念図である。
【図4】本発明による３次元マップ生成方法を示すフローチャートである。
【図7】ＬｉＤＡＲ ＳＬＡＭにおいてデータ整合を行う方法の一例を示す概念図である。
【図8】ＳＦＭにおいてデータ整合を行う方法の一例を示すフローチャートである。
【図9】本発明による３次元マップ生成方法により生成された３次元マップの実際の構築結果を示す図である。
【図10】本発明による３次元マップ生成方法により生成された３次元マップの実際の構築結果を示す図である。
【発明を実施するための形態】
【0026】
以下、添付図面を参照して本明細書に開示される実施形態について詳細に説明するが、図面番号に関係なく同一又は類似の構成要素には同一又は類似の符号を付し、その説明は省略する。以下の説明で用いる構成要素の接尾辞である「モジュール」及び「部」は、明細書の作成を容易にするために付与又は混用されるものであり、それ自体が有意性や有用性を有するものではない。また、本明細書に開示される実施形態について説明するにあたって、関連する公知技術についての具体的な説明が本明細書に開示される実施形態の要旨を不明にする恐れがあると判断される場合は、その詳細な説明を省略する。なお、添付図面は本明細書に開示される実施形態を容易に理解できるようにするためのものにすぎず、添付図面により本明細書に開示される技術的思想が限定されるものではなく、本発明の思想及び技術範囲に含まれるあらゆる変更、均等物又は代替物が本発明に含まれるものと理解されるべきである。
【0027】
第１、第２などのように序数を含む用語は様々な構成要素を説明するために用いられるが、上記構成要素は上記用語により限定されるものではない。上記用語は１つの構成要素を他の構成要素と区別する目的でのみ用いられる。
【0028】
ある構成要素が他の構成要素に「連結」又は「接続」されていると言及された場合は、他の構成要素に直接連結又は接続されていてもよく、中間にさらに他の構成要素が存在してもよいものと解すべきである。それに対して、ある構成要素が他の構成要素に「直接連結」又は「直接接続」されていると言及された場合は、中間にさらに他の構成要素が存在しないものと解すべきである。
【0029】
単数の表現には、特に断らない限り複数の表現が含まれる。
【0030】
本明細書において、「含む」や「有する」などの用語は、明細書に記載された特徴、数字、ステップ、動作、構成要素、部品、又はそれらの組み合わせが存在することを指定しようとするもので、１つ又はそれ以上の他の特徴、数字、ステップ、動作、構成要素、部品、又はそれらの組み合わせの存在や付加可能性を予め排除するものではないと理解すべきである。
【0031】
［１．大規模室内空間マッピングシステムについての説明］
本発明は、３次元マップ生成方法及びシステムに関し、具体的には、建物、施設物、道路、歩道などで測位を行う場合に活用できる３次元マップを生成する方法及びシステムに関する。
本発明は、３次元マップ生成方法及びシステムに関し、具体的には、建物、施設物、道路、歩道などで測位を行う場合に活用できる３次元マップを生成する方法及びシステムに関する。
【0032】
ここで、測位とは、位置を決定するか、又は位置に関する情報を取得することをいい、ＧＰＳや無線ネットワークの基地局の位置を活用するか、又は様々なセンサ情報を用いて、モバイル機器の正確な位置を把握する技術をいう。ここで、正確な位置は、絶対的位置、相対的位置、概念的位置など、様々な形態で存在する。特に、本発明における測位には、ＧＰＳ信号を受信できない室内で位置を取得する、室内位置確認技術を適用することができる。
【0033】
また、本発明におけるモバイル機器には、手に持ったり身につけて簡単に持ち運べる移動端末機と、自ら移動できるように構成される自律走行機器がどちらも含まれる。
【0034】
例えば、移動端末機には、携帯電話、スマートフォン（smart phone）、ノートパソコン（laptop computer）、デジタル放送端末、ＰＤＡ（Personal Digital Assistants）、ＰＭＰ（Portable Multimedia Player）、ナビゲーション、スレートＰＣ（slate PC）、タブレットＰＣ（tablet PC）、ウルトラブック（ultrabook）、ウェアラブルデバイス（例えば、スマートウォッチ（smartwatch）、スマートグラス（smart glass）、ＨＭＤ（Head Mounted Display））などが含まれる。また、自律走行機器には、掃除ロボット、案内ロボット、配達ロボットなど、移動しながら位置ベースのサービスを提供するロボットや、建物内で自律的に移動するドローンなどがある。
【0035】
なお、本発明の実施形態においては、空間で位置を取得するか、空間を移動しながら位置を取得する新たな方式の測位方法及びシステムについて、移動端末機や自律走行機器の位置を基準に説明するが、本発明は、必ずしもこれに限定されるものではない。移動端末機や自律走行機器は、ユーザの位置を測定する１つの手段であり、他の機器を本発明に適用することもできる。
【0036】
この場合、測位には、モバイル機器が位置するゾーン（zone）を検出するか、モバイル機器のリアルタイム位置を検出するなど、様々な方式を適用することができる。なお、本発明における測位は、ローカライゼーション（Localization）、位置決定（Location Determination）、ポジショニング（Positioning）などともいう。
【0037】
このような室内空間の測位技術のうち、画像ベースの測位は、撮影した写真を用いて位置を検出する技術であり、与えられたクエリ画像のカメラポーズ情報を推定してモバイル機器の位置を測定する。
【0038】
本発明における画像は、カメラセンサを用いて撮影した写真や画像などを含む意味で用いられる。例えば、モバイル機器が空間を移動しながらカメラセンサにより撮影画像を取得して３次元マップデータと比較するプロセスにより、モバイル機器の位置をリアルタイムで追跡することができる。
【0039】
このように、本発明における画像ベースの測位は、ユーザに現在の位置情報を提供し、そのために、画像を用いて生成された３次元マップを用いる。
【0040】
【0041】
図１ａに示すように、データを収集する収集装置としてロボットＲが室内空間１１に位置する。ロボットＲは、３次元マップデータを構築するためにデータを収集するマッピングロボットであり得る。ただし、ロボットＲが走行する空間の種類には制限がなく、ロボットＲは、必要に応じて室内空間及び室外空間の少なくとも一方を走行することができる。本例において、ロボットＲは、大型ショッピングモールや地下鉄駅などの大規模な室内空間１１で３次元マップを生成するために、カメラセンサを用いてデータを収集しながら自律走行するように構成される。
【0042】
しかし、大規模な室内空間１１の実際の環境では、時間の経過に応じて大きな変化が生じるので、カメラセンサの正確なポーズ情報を推定することは非常に難しい。例えば、テクスチャ（質感）がない空間（textureless areas）１２やサイネージ１３でコンテンツの変化などにより生じる外観の変化（changing appearance）、時間帯に応じた混雑度（crowdedness）やオクルージョン（occlusions）の変化、動的環境（dynamic environment）の変化などにより、画像に対するカメラポーズの推定が難しくなる。
【0043】
本発明においては、大規模な室内空間に対応するために、１００日以上収集したデータを用いて３次元特徴点マップを生成し、そのために、新たな形態の３次元マップ生成方法を提示する。
【0044】
図１ｂに示すように、本発明による３次元マップ生成方法により、画像及びポイントクラウドを含む３次元マップのデータセットを構築することができる。前記データセットは、正しく整列された３次元モデル及びカメラポーズ情報を含む高密度の画像サンプリングを提供することができる。
【0045】
ここで、前記３次元マップは、３次元の特徴点に関するデータを有するマップであり、フィーチャマップ、特徴点マップ又は３次元特徴点マップともいう。なお、本例においては、前記空間として室内空間を例示するが、本発明は、必ずしもこれに限定されるものではなく、室外空間、室内空間及び室外空間などにも適用することができる。
【0046】
以下、前記空間に対して前記３次元マップのデータセットを構築するシステムについてまず説明し、その後、前記３次元マップを生成する方法と構築されたデータの例について図面を参照してより詳細に説明する。
【0047】
【0048】
前記３次元マップのデータセットは、図２に示す３次元マップ生成システム１００により実現することができる。
【0049】
３次元マップ生成システム１００は、収集装置１１０及びマッピング装置１２０を備えてもよい。
【0050】
ここで、収集装置１１０は、データを収集する装置であり、モバイルプラットフォームであってもよい。収集装置１１０は、ＬｉＤＡＲセンサ及びカメラセンサを用いて空間の空間データ及び画像データをそれぞれ収集することができる。
【0051】
同図に示すように、収集装置１１０は、駆動部１１１、センシング部１１２及び通信部１１３の少なくとも１つを備える。
【0052】
駆動部１１１は、収集装置１００が空間を移動できるようにする手段を提供する。そのために、駆動部１１１は、モータ及び複数の車輪を含んでもよい。例えば、図３に示すように、駆動部１１１は、モータ及び複数の車輪が組み合わせられ、収集装置１００を走行、方向転換、回転させる機能を実行する。
【0053】
本発明においては、このような駆動部１１１として、２つの車輪及び４つのキャスタから構成された差動ドライブを例示する。ここで、前記車輪には、走行距離測定データを取得するためのエンコーダが備えられてもよい。
【0054】
センシング部１１２は、ＬｉＤＡＲセンサ１１２ａ及びカメラセンサ１１２ｄを含む。
【0055】
ＬｉＤＡＲセンサ１１２ａは、遠隔探知方式で周辺の空間に関する３次元情報を取得するセンサであり、レーザを照射するレーザスキャナであってもよい。
【0056】
前記レーザスキャナは、レーザパルスを発射し、発射したレーザパルスが周囲の対象物体から反射されて戻るとそれを受け、物体までの距離などを測定することにより、周辺の形状を精密に描き出す装置であってもよい。
【0057】
本実施形態においては、ＬｉＤＡＲセンサ１１２ａとして３次元スキャニングＬｉＤＡＲを例示し、それにより３次元空間データを生成する。ただし、本発明は、必ずしもこれに限定されるものではなく、ＬｉＤＡＲセンサ１１２ａとして、例えば２次元レーザスキャナやレーザを照射しないセンサを適用してもよい。
【0058】
図３に示すように、ＬｉＤＡＲセンサ１１２ａは、複数のＬｉＤＡＲセンサからなるようにしてもよい。第１ＬｉＤＡＲセンサ１１２ｂは、収集装置１１０の上端に垂直に配置され、第２ＬｉＤＡＲセンサ１１２ｃは、収集装置１１０の上端と下端間（例えば、中央部）に水平に配置されるようにしてもよい。この場合、垂直方向の第１ＬｉＤＡＲセンサ１１２ｂは、プッシュブルームスキャン（push-broom scanning）技法により高密度の３次元ポイントクラウドを構成し、水平方向の第２ＬｉＤＡＲセンサ１１２ｃは、室内空間でポーズを推定する上で必要な情報を水平方向に最大化する。
【0059】
カメラセンサ１１２ｄは、６自由度（６ＤｏＦ）を有するカメラであり、様々なタイプの装置を含むようにしてもよい。その例として、カメラセンサ１１２ｄは、産業用カメラ及びスマートフォンカメラを含んでもよい。同図において、カメラセンサ１１２ｄは、産業用カメラであり、収集装置１１０の上端には、スマートフォンカメラ（図示せず）の据え付けのための据付台１１２ｅが備えられる。
【0060】
本例においては、複数の産業用カメラが収集装置１１０の外周に沿って間隔で配列され、よって、水平方向に３６０度全方位で画像を撮影することができる。この場合、画像内での地面のグラフィックを低減するように、各産業用カメラは、角度で上側を向くように配置されてもよい。一方、クエリ時間での実際の環境をより正確に反映するために、産業用カメラより上方に複数のスマートフォンカメラが配置され、周辺を撮影するようにしてもよい。このとき、収集装置１１０とセンシング部１１２とは、例えばＮＴＰ（Network Time Protocol）サーバを用いて同期化するようにしてもよい。
【0061】
一方、センシング部１１２は、ＬｉＤＡＲセンサ１１２ａ及びカメラセンサ１１２ｄの他に、様々なデータを収集できるセンサを備えてもよい。一例として、センシング部１１２は、障害物を認識するために、赤外線センサ、超音波センサ、姿勢センサ、衝突センサ、光センサなどを備えてもよい。他の例として、センシング部１１２は、空間内の異なる位置で無線信号（例えば、Ｗｉ－Ｆｉ信号、Ｂｌｕｅｔｏｏｔｈ信号、ＺｉｇＢｅｅ信号など）を測定するセンサを備えてもよい。
【0062】
この場合、センシング部１１２で取得したデータは、通信部１１３によりマッピング装置１２０に送信することができる。通信部１１３は、収集装置１１０とマッピング装置１２０間で無線通信を行う。ここで、マッピング装置１２０は、収集装置１１０の通信部１１３との無線通信のために、通信部１２２を備えてもよい。ただし、本発明は、必ずしも無線通信に限定されるものではなく、通信部１１３と通信部１２２とは、有線通信を行うようにしてもよい。また、通信部１１３は、収集装置１１０と他の収集装置間、収集装置１１０とモバイル機器間、又は収集装置１１０と通信ネットワーク間で無線通信を行うことができる。このような無線通信を行うために、通信部１１３は、無線インターネットモジュール、近距離通信モジュール、位置情報モジュールなどを備えてもよい。
【0063】
なお、本発明においては、収集装置１１０とマッピング装置１２０が別の装置である場合を例示するが、収集装置１１０とマッピング装置１２０は、単一の機器に備えられてもよい。この場合、前記データは、内部のデータ伝送経路で送受信する。
【0064】
マッピング装置１２０は、収集装置１１０を用いて空間の３次元特徴点マップを生成する。この場合、マッピング装置１２０は、前記空間データを用いてＬｉＤＡＲセンサ１１２ａの移動軌跡を推定し、前記画像データ及び前記移動軌跡を入力データとして用いてＳＦＭをベースとして前記空間に関する３次元構造データを生成することができる。
【0065】
一方、収集装置１１０は、センシング部１１２を用いてデータを収集する場合、ＳＬＡＭを行うように構成される。すなわち、収集装置１１０は、リアルタイムで自身の位置を計測（測定）すると共に周辺環境の地図を作成するように構成される。前記ＳＬＡＭは、収集装置１１０のプロセッサで行われるようにしてもよく、マッピング装置１２０の制御部１２１で行われるようにしてもよい。
【0066】
前記ＳＬＡＭが制御部１２１で行われる場合、マッピング装置１２０は、マップクラウドサーバであってもよい。この場合、収集装置１１０とマップクラウドサーバとが１つのデータ収集装置を構成する。
【0067】
一方、制御部１２１は、無線通信を制御し、収集装置１１０のセンシング部１１２を用いて画像を撮影し、駆動部１１１を制御する動作と共に、通常、収集装置１１０の全般的な動作を制御する。
【0068】
前記ＳＬＡＭのためのデータ処理の例として、制御部１２１がセンサデータに基づいて同時位置推定及び地図作成機能を実行し、収集装置１１０が移動した経路を推定するようにしてもよい。例えば、収集装置１１０でセンシングされたデータに基づいてループ閉じ込み（loop closure）を検出し、ループ閉じ込みが検出されると位置データを生成し、位置データに基づいて移動経路情報及びターゲット建物の室内地図情報を生成するようにしてもよい。
【0069】
この場合、制御部１２１は、保存部１２３と連携し、前述したデータセットを生成、保存及び更新する一連のプロセスを制御する。
【0070】
保存部１２３には、前記データセットとして３次元マップのデータが保存され、前記３次元マップには、座標情報、ポイントクラウド情報、画像情報、ポーズ情報などが備えられる。
【0071】
このようなデータセットを構築するために、本発明による３次元マップ生成方法においては、大規模な空間に対して、所定期間に収集したシーケンスデータに基づいてＬｉＤＡＲ ＳＬＡＭからＳＦＭまでのパイプラインを用いて空間の３次元マップを生成するプロセスを用いる。
【0072】
以下、このような３次元マップを生成する方法について、図面を参照してより詳細に説明する。
【0073】
［２．パイプラインの全体プロセスについての説明］
図４は、本発明による３次元マップ生成方法を示すフローチャートである。
図４は、本発明による３次元マップ生成方法を示すフローチャートである。
【0074】
本発明による３次元マップ生成方法においては、ＬｉＤＡＲ ＳＬＡＭ１３１の出力値をＳＦＭ１３２の入力値として用いて、ＬｉＤＡＲ ＳＬＡＭ１３１からＳＦＭ１３２につながる新たなパイプラインを提示する。
【0075】
この場合、前記パイプラインは、ＬｉＤＡＲ ＳＬＡＭパイプライン及びＳＦＭパイプラインからなるようにしてもよい。前記ＬｉＤＡＲ ＳＬＡＭパイプラインは、統合座標系に基づいてカメラポーズを推定するポーズグラフ最適化（pose-graph optimization）に基づいて構築する。
【0076】
また、前記ＳＦＭパイプラインは、ＬｉＤＡＲ ＳＬＡＭ１３１の初期画像ポーズ（又はカメラポーズ）を改善し、ＬｉＤＡＲポイントクラウドに整列された３次元特徴点マップを再建する。
【0077】
この場合、ＬｉＤＡＲ ＳＬＡＭ１３１及びＳＦＭ１３２により、全ての画像に関する実際の資料（実際の写真、グラウンドトゥルース（Ground Truth））のポーズを生成することができる。ＬｉＤＡＲ ＳＬＡＭ１３１では、制御部１２１（図２参照）が各シーケンスに収集装置のポーズを生成し、その後同じ空間のシーケンスを組み合わせて前記シーケンスが統合された座標系を有するようにデータを処理する。収集装置はプラットフォームともいう。以下、説明の便宜上、収集装置をプラットフォームで統一して説明する。
【0078】
この場合、ポイントクラウド及びカメラポーズ情報は、プラットフォームポーズ及び補正パラメータに基づいて算出することができる。その後、前記カメラポーズ情報を改善（refine）するために、ＳＦＭ１３２を用いることができる。前述したプロセスによれば、別途の手動のカメラキャリブレーションがなくても、前記カメラセンサの外部変数のカメラキャリブレーションが可能になる。
【0079】
ここで、前記カメラポーズは、位置（position）を示す座標と、オリエンテーション（orientation）を示す姿勢とを含んでもよい。この場合、前記座標は、地面の座標に対して所定の高さ、例えばロボットの高さや人の目の高さなどを加えた座標に特定されるようにしてもよい。また、前記姿勢は、任意に特定されるが、ロボットや人が画像ベースの測位を用いる場合に実際にクエリ画像を送信する状況を仮定してそれに類似した姿勢に特定されるようにしてもよい。その例として、前記姿勢は、地面に水平な方向、看板を見る方向、徒歩の進行方向などを基準に特定されるようにしてもよい。
【0080】
より具体的な例として、本発明におけるＬｉＤＡＲ ＳＬＡＭパイプラインにおいては、まず、プラットフォームに備えられるＬｉＤＡＲセンサ及びカメラセンサを用いて空間の空間データ及び画像データをそれぞれ収集するステップ（Ｓ１１１）が行われる。
【0081】
ここで、前記ＬｉＤＡＲセンサ及び前記カメラセンサとしては、図３を参照して説明したプラットフォームのＬｉＤＡＲセンサ１１２ａ及びカメラセンサ１１２ｄを備えてもよい。
【0082】
前記空間データは、点群データ又はスキャンデータであってもよい。この場合、前記点群データ又は前記スキャンデータが収集されると、その次のステップで、ＬｉＤＡＲ ＳＬＡＭが点群データを用いて他の空間データや移動軌跡を導出するようにしてもよい。
【0083】
ここで、前記空間データは、３次元ポイントクラウド、各シーケンスにおけるプラットフォームのポーズなどを備えてもよい。また、前記画像データは、前記カメラセンサにより撮影した撮影画像であってもよい。
【0084】
このように、前記データを収集するステップ（Ｓ１１１）においては、前記ＬｉＤＡＲセンサを用いたＬｉＤＡＲ ＳＬＡＭにより前記空間データ及び前記画像データを収集する。
【0085】
次に、ＬｉＤＡＲ ＳＬＡＭ１３１では、前記空間データを用いて前記ＬｉＤＡＲセンサの移動軌跡を推定するステップ（Ｓ１１２）が行われる。
【0086】
例えば、前記プラットフォームの軌跡（又は、ＬｉＤＡＲセンサやカメラセンサの軌跡）は、ＬｉＤＡＲベースのポーズグラフＳＬＡＭを適用して推定するようにしてもよい。この場合、前記ＬｉＤＡＲ ＳＬＡＭのポーズは、同じ空間内の様々な軌跡に対するカメラポーズを算出するＳＦＭプロセスの事前情報として用いるようにしてもよい。
【0087】
また、本例においては、ＬｉＤＡＲによる空間データの導出（マッピング）と移動軌跡の推定（ローカライゼーション）とが順次行われるか又は同時に行われるＬｉＤＡＲ ＳＬＡＭアルゴリズムを用いることができる。
【0088】
前述したように、本発明による３次元マップ生成方法は、ＬｉＤＡＲセンサ及びカメラセンサを用いて空間の空間データ及び画像データをそれぞれ収集するステップ（Ｓ１１１）と、前記ＬｉＤＡＲセンサの移動軌跡を推定するステップ（Ｓ１１２）とを含む。
【0089】
次に、ＬｉＤＡＲ ＳＬＡＭ１３１からＳＦＭ１３２につながる新たなパイプラインを形成するために、前記画像データ及び前記移動軌跡を入力データとして用いてＳＦＭをベースとして前記空間に関する３次元構造データを生成するステップ（Ｓ１１３）が行われる。このように、ＬｉＤＡＲ ＳＬＡＭパイプラインで取得した画像データ及び移動軌跡を入力データとして用いるＳＦＭパイプラインにより、大規模な室内空間に対するマッピングが可能になる。
【0090】
前述したように、本発明においては、ＬｉＤＡＲ ＳＬＡＭ及びＳＦＭアルゴリズムを用いることにより、大規模な室内空間の環境が変化しても、正確なカメラポーズを推定することができる。
【0091】
以下、本発明による３次元マップ生成方法の各ステップについて、例を挙げてより詳細に説明する。
【0092】
図５及び図６は、図４のフローチャートの一実施形態を示す詳細フローチャートであり、図７は、ＬｉＤＡＲ ＳＬＡＭにおいてデータ整合を行う方法の一例を示す概念図であり、図８は、ＳＦＭにおいてデータ整合を行う方法の一例を示すフローチャートであり、図９及び図１０は、本発明による３次元マップ生成方法により生成された３次元マップの実際の構築結果を示す図である。
【0093】
［３．各ステップの詳細についての説明－ＬｉＤＡＲ ＳＬＡＭ］
まず、図５及び図６に示すように、ＬｉＤＡＲ ＳＬＡＭ１３１では、スキャンデータ（scan data）を用いて中間データ（intermediate data）を算出する。
まず、図５及び図６に示すように、ＬｉＤＡＲ ＳＬＡＭ１３１では、スキャンデータ（scan data）を用いて中間データ（intermediate data）を算出する。
【0094】
前記スキャンデータは、前記データを収集するステップ（Ｓ１１１）により収集され、前記中間データは、前記移動軌跡を推定するステップ（Ｓ１１２）により算出される。
【0095】
前記スキャンデータは、単一のスキャン軌跡（Scan Trajectory）を用いて連続的に取得したデータであってもよい。ここで、前記スキャンデータは、ＬｉＤＡＲスキャンの生データ（LiDAR raw scan）と、カメラセンサの画像データ（Camera images）とを備えてもよい。ＬｉＤＡＲ ＳＬＡＭ１３１では、前記生データを処理してスキャンデータの軌跡を復元する。この場合、前記中間データは、前記画像データと共に復元されたＬｉＤＡＲセンサの軌跡（LiDAR sensor trajectory）を備えてもよい。
【0096】
一方、ＬｉＤＡＲ ＳＬＡＭ１３１では、ポイントクラウドの生成、ポーズグラフ及びグラフの組み合わせの過程で、前記プラットフォームのポーズ情報を取得することができる。
【0097】
＜歪みのないポイントクラウドの生成（Undistorted point cloud generation）＞
ポイントクラウドの生成において、１つのノードで用いられるポイントクラウドデータの量を増やすために、複数、例えば２つのＬｉＤＡＲセンサで収集したポイントクラウドを組み合わせてもよい。ここで、異なるＬｉＤＡＲ座標系で表現されたポイントクラウドは、プラットフォーム座標系に変換されて互いに関連付けられる。
ポイントクラウドの生成において、１つのノードで用いられるポイントクラウドデータの量を増やすために、複数、例えば２つのＬｉＤＡＲセンサで収集したポイントクラウドを組み合わせてもよい。ここで、異なるＬｉＤＡＲ座標系で表現されたポイントクラウドは、プラットフォーム座標系に変換されて互いに関連付けられる。
【0098】
また、プラットフォームの移動速度が速くなるほど、動いている間、ポイントクラウドの歪みが引き起こされ、それを補うために、ホイールオドメトリ（wheel odometry）技術が適用される。よって、初期軌跡の推定及び動きによるＬｉＤＡＲデータの歪みを補正することができる。この場合、プラットフォーム座標系のポイントクラウドでのポイントの位置は下記式（１）により算出される。
【0099】
【数1】
ここで、Ｌｐは、ＬｉＤＡＲ座標系で与えられたポイントクラウドでのポイントの位置であり、
[Ｒｏｄｏｍ，ｔｏｄｏｍ]は、ポイントクラウドの基本時間とポイント取得時間間の相対的なプラットフォームポーズであり、ホイールオドメトリから計算され、
[ＲＲＬ，ｔＲＬ]は、プラットフォームからＬｉＤＡＲセンサまでの相対ポーズである。
【0100】
＜ポーズグラフＳＬＡＭ（Pose-graph SLAM）＞
歪みのない順次ポイントクラウドを用いて、推定された相対ポーズとしてグラフのエッジを設定することができる。しかし、順次ノード間の情報だけで累積されたポーズエラーを補償することは困難であるので、時間的には距離があるが空間的には隣接するノード間に追加エッジが算出され、ポーズグラフに追加されるようにしてもよい。
歪みのない順次ポイントクラウドを用いて、推定された相対ポーズとしてグラフのエッジを設定することができる。しかし、順次ノード間の情報だけで累積されたポーズエラーを補償することは困難であるので、時間的には距離があるが空間的には隣接するノード間に追加エッジが算出され、ポーズグラフに追加されるようにしてもよい。
【0101】
一方、この場合、前記推定された移動軌跡は、単一の地域で複数のデータとして算出されるようにしてもよい。前記複数のデータの整合のために、本発明による３次元マップ生成方法は、複数のデータの整合を行うステップを含むようにしてもよい。
【0102】
前記複数のデータの整合を行うステップにおいては、前記移動軌跡間で座標系変換を推定すること、又は前記移動軌跡間の相対ポーズを推定することにより、前記複数のデータの整合を行うようにしてもよい。前記移動軌跡間で座標系変換を推定することは、ＬｉＤＡＲ ＳＬＡＭ１３１で行われ、前記移動軌跡間の相対ポーズを推定することは、ＳＦＭ１３２で行われるようにしてもよい。以下、前記移動軌跡間で座標系変換を推定する整合過程についてまず説明し、相対ポーズを推定する方法についてはＳＦＭ１３２の説明で後述する。
【0103】
＜軌跡間の整合＞
前述した過程により、推定されたプラットフォームのポーズは、各シーケンスに対して独立した座標系で表現される。例えば、図７の（ａ）を参照すると、第１シーケンスのデータセット１４１と第２シーケンスのデータセット１４２とはそれぞれ独立した座標系で表現される。この場合、図７の（ｂ）のように、レジストレーション（registration）技術により、空間的に隣接するキーフレーム間の整合を行うことができる。より具体的には、異なるシーケンスでＩＣＰ（Iterative Closest Point）アルゴリズムを用いて相対ポーズを算出するように、時間的には距離があるが空間的には隣接するノードを再び検索することができる。従来のグラフと算出された相対ポーズとが新たなグラフを形成し、グラフ最適化により単一の座標系で表現されたプラットフォームポーズを取得することができる。
前述した過程により、推定されたプラットフォームのポーズは、各シーケンスに対して独立した座標系で表現される。例えば、図７の（ａ）を参照すると、第１シーケンスのデータセット１４１と第２シーケンスのデータセット１４２とはそれぞれ独立した座標系で表現される。この場合、図７の（ｂ）のように、レジストレーション（registration）技術により、空間的に隣接するキーフレーム間の整合を行うことができる。より具体的には、異なるシーケンスでＩＣＰ（Iterative Closest Point）アルゴリズムを用いて相対ポーズを算出するように、時間的には距離があるが空間的には隣接するノードを再び検索することができる。従来のグラフと算出された相対ポーズとが新たなグラフを形成し、グラフ最適化により単一の座標系で表現されたプラットフォームポーズを取得することができる。
【0104】
この場合、前記ＬｉＤＡＲセンサとしては、前述したように、２つのＬｉＤＡＲセンサを用いてもよい。２つのＬｉＤＡＲセンサ間の補正のために、幾何学的特徴を用い、ＩＣＰアルゴリズムを用いて相対ポーズを推定することができる。また、プラットフォームのベースとカメラ間の補正のために、ＳＦＭによるオンライン補正を用いることができる。
【0105】
このように、異なる座標系を１つの座標系に統合するためにデータセット間の整合が行われ、それにより、図７の（ｃ）のように、軌跡座標系が整合した中間データが算出される。
【0106】
前述したように、前記ＬｉＤＡＲ ＳＬＡＭの出力データは、中間データであって、前記画像データ及び前記移動軌跡を備えてもよい。そして、前記ＬｉＤＡＲ ＳＬＡＭの出力データは、前記ＬｉＤＡＲ ＳＬＡＭ及び前記ＳＦＭがパイプラインを形成するように、ＳＦＭ１３２の入力データとして用いられる。
【0107】
ＳＦＭ１３２は、ビジュアルマッピングと最適化を行うステップであり、前記中間データの入力を受けてＳＦＭデータを出力する。
【0108】
前記生成された３次元構造データは、前記ＳＦＭの出力データとして、前記ポーズ情報が算出された画像及びポイントクラウドデータを備えてもよい。例えば、前記ＳＦＭデータは、ポーズ情報を有する画像と、ポイントクラウドデータ（ＰＣＤ）マップとを備えてもよい。
【0109】
［４．各ステップの詳細についての説明－ＳＦＭ］
図５及び図６に示すように、前記３次元構造データを生成するステップ（Ｓ１１３）は、特徴点をマッチングするステップ（Ｓ１１４）と、連続軌跡に変換するステップ（Ｓ１１５）と、ポーズ情報を算出するステップ（Ｓ１１６）とを含むようにしてもよい。
図５及び図６に示すように、前記３次元構造データを生成するステップ（Ｓ１１３）は、特徴点をマッチングするステップ（Ｓ１１４）と、連続軌跡に変換するステップ（Ｓ１１５）と、ポーズ情報を算出するステップ（Ｓ１１６）とを含むようにしてもよい。
【0110】
前記特徴点をマッチングするステップ（Ｓ１１４）においては、前記画像データから特徴点を抽出し、画像間で前記特徴点をマッチングする。例えば、それぞれの画像に対して画像の特徴点を抽出し、Ｇｕｉｄｅｄ Ｆｅａｔｕｒｅ Ｍａｔｃｈｉｎｇにより、位置及び角度が近い画像対（Image Pair）に対して特徴点間のマッチングを行う。
【0111】
次に、前記移動軌跡を連続軌跡に変換し、前記連続軌跡に基づいて前記特徴点及び前記画像データのポーズ情報を最適化して前記空間の３次元特徴点マップを生成する。
【0112】
以下、特徴点マッチングとマップ最適化についてより具体的に説明する。
【0113】
＜特徴点マッチング（Local feature matching）＞
例えば、各画像毎に繰り返し可能で信頼できるＲ２Ｄ２（Reliable and Repeatable Detectors and Descriptors）特徴点が抽出される。その後、距離が近い画像対を用いて特徴点のマッチングが行われる。近い画像対を選択するために、２つの画像間の距離と視野角の差が閾値として設定されるようにしてもよい。
例えば、各画像毎に繰り返し可能で信頼できるＲ２Ｄ２（Reliable and Repeatable Detectors and Descriptors）特徴点が抽出される。その後、距離が近い画像対を用いて特徴点のマッチングが行われる。近い画像対を選択するために、２つの画像間の距離と視野角の差が閾値として設定されるようにしてもよい。
【0114】
この場合、前記マッチングされた特徴点は、トライアンギュレーション（triangulation）技法により３次元ポイントを生成することができる。
【0115】
次に、前記移動軌跡を連続軌跡に変換するステップ（Ｓ１１５）が行われる。
【0116】
この場合、前記画像データのポーズ情報が前記連続軌跡に整合するように、前記移動軌跡は、連続時間軌跡としてモデリングされるようにしてもよい。また、前記ポーズ情報を算出するステップ（Ｓ１１６）においては、前記連続軌跡に基づいて前記画像データのポーズ情報を算出するようにしてもよい。
【0117】
このような連続軌跡に変換するステップ（Ｓ１１５）及びポーズ情報を算出するステップ（Ｓ１１６）は、マップ最適化により行うことができる。前記マップ最適化は、ＬｉＤＡＲ軌跡を連続時間軌跡で表現又はモデリングし、画像のポーズ情報が対応する連続軌跡と整合するように最適化するステップにしてもよい。この場合、ＳＦＭ１３２は、従来のビジュアルマッピング（従来のＳＦＭ）とは異なり、事前に取得した移動軌跡を連続フォーム（Continuous Form）で表現して活用するので、精巧な最適化を行う。すなわち、事前のＬｉＤＡＲ ＳＬＡＭで取得したポーズを連続軌跡であるスプライン（spline）で表現し、その後それを制約条件としてビジュアルマップの最適化を行う。
【0118】
＜マップ最適化（Map optimization）＞
まず、センサ間のタイムスタンプの差を説明するために、ＳＥ（３）で各プラットフォームの軌跡は連続３次スプラインで表現する。具体的には、前記移動軌跡を連続３次スプラインで表現してポーズエラーを定義することにより、地図最適化を行う。
まず、センサ間のタイムスタンプの差を説明するために、ＳＥ（３）で各プラットフォームの軌跡は連続３次スプラインで表現する。具体的には、前記移動軌跡を連続３次スプラインで表現してポーズエラーを定義することにより、地図最適化を行う。
【0119】
プラットフォーム軌跡スプライン、ＴＷＲ（ｔ）、カメラに対する外部ポーズ、Ｃｉ、ＴＲＣｉを用いると、ポーズエラーｅｓｐｌｉｎｅは、下記式（２）のように表される。
【0120】
【数2】
ここで、
【数3】
ＴＷＣｉは、画像ポーズであり、ｔｉは、対応するタイムスタンプである。
【0121】
プラットフォーム軌跡の連続的な表現により、ＬｉＤＡＲ ＳＬＡＭ１３１からの非同期式の事前ポーズをＳＦＭ１３２で用いることができる。スプラインを介した提案されたポーズエラーは、下記式（３）のように特徴点再投影エラーで最適化することができる。
【0122】
【数4】
このように、修正されたバンドル調整方法により、画像のポーズ情報及び特徴点を再建することができる。
【0123】
ここで、ＳＦＭモデルは、バンドル調整とトライアンギュレーション技法を繰り返すことにより生成することができる。このようなＳＦＭパイプラインは、ＬｉＤＡＲ ＳＬＡＭから出力されるデータを統合し、視覚的に目立たないか人が多い画像でも正確に登録できるようにする。
【0124】
また、カメラ補正パラメータは、ＳＦＭパイプラインを用いて各データセットの３次元モデルを再構成する間、自動的に推定することができる。
【0125】
なお、本発明による３次元マップ生成方法においては、密度が高いマップを生成するために、ＭＶＳ（Multi-View Stereo）技法を適用することもできる。その例として、図５及び図６に示すように、本発明による３次元マップ生成方法は、ＭＶＳ１３３を適用し、深度マップを生成するステップ（Ｓ１１７）を含むようにしてもよい。
【0126】
前記深度マップを生成するステップ（Ｓ１１７）においては、前記生成された３次元構造データを用いて画像の深度情報を推定して深度マップを生成する。
【0127】
ＭＶＳ１３３は、ＳＦＭ１３２で算出されるＳＦＭデータから高密度のビジュアルマップ（Dense Visual Map）を生成するＭＶＳアルゴリズムであってもよい。ＭＶＳ１３３の深度マップを生成する過程で、ＭＶＳデータは、密度が高く、色があり得るポイントクラウドマップを備えてもよい。
【0128】
図９及び図１０は、実際に構築したＳＦＭデータの低密度で再建（Sparse Reconstruction）された特徴点マップ、及びＭＶＳデータの高密度で再建（Dense Reconstruction）された特徴点マップをそれぞれ示す写真である。図９の（ａ）においては、江南駅の地下１階の３次元マップの実際の事例を示し、図９の（ｂ）においては、現代デパートの４階の３次元マップの実際の事例を示す。図１０においては、ＭＶＳ１３３の結果として、密度が高くなり、色がより明確になった３次元マップを示す。
【0129】
前述した３次元マップ生成方法においては、新たなパイプラインを用いて正確な３次元マップを生成することにより、大規模な室内空間、例えばデパートや地下鉄駅でクエリ画像を用いた画像ベースの測位を行うことができる。
【0130】
［５．ＳＦＭでのデータ整合についての説明］
前述したように、本発明において、データ整合は、相対ポーズを推定する方法でＳＦＭ１３２で行うこともできる。そのために、図８に示すように、特徴点をマッチングするステップ（Ｓ１１４）の後に相対ポーズを推定するステップ（Ｓ１１８）を行うようにしてもよい。
前述したように、本発明において、データ整合は、相対ポーズを推定する方法でＳＦＭ１３２で行うこともできる。そのために、図８に示すように、特徴点をマッチングするステップ（Ｓ１１４）の後に相対ポーズを推定するステップ（Ｓ１１８）を行うようにしてもよい。
【0131】
スキャンデータ（又は中間データ）の軌跡がそれぞれ処理されると、ＬｉＤＡＲ ＳＬＡＭが正確であってもデータ間で座標系が異なることがある。単一の地域に対してデータを整合するために、軌跡間の座標系変換を推定しなければならない。この場合、座標系変換は、データ間の６－ＤＯＦ Ｒｅｌａｔｉｖｅ Ｐｏｓｅ（３－ＤＯＦ ｐｏｓｉｔｏｎ＋３－ＤＯＦ ｏｒｉｅｎｔａｔｉｏｎ）を求める問題として表される。
【0132】
このとき、最適化プロセス中にプラットフォームでＬｉＤＡＲセンサ及びカメラセンサ間の配置（すなわち、相対ポーズ（Relative Pose））を共に解くことができる。また、最適化中に中間データに含まれるＬｉＤＡＲセンサの軌跡間の座標系変換を共に解き、中間データを互いに整合することもできる。
【0133】
より具体的には、座標系変換の初期値計算で、ＬｉＤＡＲ ＳＬＡＭが算出するそれぞれのデータ毎のポイントクラウドデータマップをＩＣＰアルゴリズムで互いに整合して座標系変換を算出することができる。このとき、ＳＦＭで中間データに含まれる画像間のマッチングを行うので、それにより相対ポーズを推定することができる。
【0134】
座標系変換の最適化において、初期値が与えられると最適化が可能になる。ＳＦＭ１３２で画像のポーズ情報とマップのポイントの最適化を行う際に、軌跡間の相対ポーズをさらなる変数として設定すると、同時に最適化を行うことができる。ここで、軌跡間の相対ポーズが実際とは異なると、事前に取得した軌跡（Prior Trajectory）が間違ってしまうので、最適化コスト（Cost）に影響を与えるようになる。よって、相対ポーズに対する最適化コストのヤコビアン（Jacobian）を最適化フレームワーク（Framework）が計算し、正確な相対ポーズを算出することができる。
【0135】
前述したＬｉＤＡＲセンサの軌跡を活用したビジュアルマッピングにより、室外環境に適したＳＦＭベースのアルゴリズムを用いて大規模な室内空間の正確なマッピングを行うことができる。
【0136】
特に、ＬｉＤＡＲセンサ（又は収集装置、プラットフォーム、マッピングロボット）の軌跡を連続時間軌跡で表現し、カメラセンサで取得した画像のポーズ情報を対応する連続軌跡とより精巧に整合することができる。
【0137】
また、前述した方法により構築された３次元マップは、ユーザのモバイル機器で画像ベースの測位サービスで活用することができる。例えば、ユーザは、徒歩移動中に空間で自身の位置を確認するために、スマートフォンで画像ベースの測位サービスに関連するアプリケーションを実行し、周辺の街頭を撮影する。前記アプリケーションは、撮影した写真の特徴点と３次元マップの特徴点とを比較し、前記モバイル機器の３次元位置及びポーズを推定する。
【0138】
このように推定した３次元位置及びポーズを用いて、前記モバイル機器の正確な位置を推定することができる。この場合、前記モバイル機器では前記位置をベースとする様々なサービスを実行することができる。
【0139】
前述した３次元マップ生成方法及びシステムは、上記実施形態の構成や方法に限定されるものではなく、上記実施形態に様々な変形が行われるように、各実施形態の全部又は一部を選択的に組み合わせて構成してもよい。
【符号の説明】
【0140】
１００ ３次元マップ生成システム
１１０ 収集装置
１１１ 駆動部
１１２ センシング部
１１２ａ ＬｉＤＡＲセンサ
１１２ｂ 第１ＬｉＤＡＲセンサ
１１２ｃ 第２ＬｉＤＡＲセンサ
１１２ｄ カメラセンサ
１１２ｅ 据付台
１１３ 通信部
１２０ マッピング装置
１２１ 制御部
１２２ 通信部
１２３ 保存部
１１０ 収集装置
１１１ 駆動部
１１２ センシング部
１１２ａ ＬｉＤＡＲセンサ
１１２ｂ 第１ＬｉＤＡＲセンサ
１１２ｃ 第２ＬｉＤＡＲセンサ
１１２ｄ カメラセンサ
１１２ｅ 据付台
１１３ 通信部
１２０ マッピング装置
１２１ 制御部
１２２ 通信部
１２３ 保存部
【図1a】
【図1b】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
