(57)【要約】【課題】１つのカメラを用いて天井を撮影し、天井までの高さが予め求められていなくても、撮影した天井画像をつなげて天井地図を作成可能にする。【解決手段】同一のオブジェクトが写っている互いにコビジブルな関係にある天井画像に対し、位置情報から得られるカメラ６の位置の移動量と両天井画像におけるオブジェクトの移動量との比で表されるスケールを推定するステップ（ＳＴ１６）と、複数の天井画像を、対応するスケールに応じて天井地図に適合する大きさに変換した上で貼り合わせ（ＳＴ８４）、天井地図を作成するステップ（ＳＴ２）とを含む。【選択図】図５
2019036804.tif 000002
【特許請求の範囲】
【請求項１】
床面の上を移動可能なカメラにより、前記カメラを移動させつつ天井を撮影して複数の天井画像を作成すると共に、撮影時の前記カメラの位置情報を作成し、前記位置情報に基づいて、複数の前記天井画像を合成して天井地図を作成する天井地図作成方法であって、
同一のオブジェクトが写っている互いにコビジブルな関係にある前記天井画像に対し、前記位置情報から得られる前記カメラの位置の移動量と両天井画像における前記オブジェクトの移動量との比で表されるスケールを推定するステップと、
複数の前記天井画像を、対応する前記スケールに応じて前記天井地図に適合する大きさに変換した上で貼り合わせ、前記天井地図を作成するステップとを含むことを特徴とする天井地図作成方法。
【請求項２】
前記スケールを推定するステップは、
互いにコビジブルな関係にある前記天井画像に対し、変換行列を用いた複数種類のマッチングによるスケール推定を実行するステップと、
複数種類の前記スケール推定のうち、テンプレートマッチングによるスコアが最も高い変換行列によって得られる結果を前記スケールに選定するステップとを含むことを特徴とする請求項１に記載の天井地図作成方法。
【請求項３】
複数種類の前記スケール推定が、局所特徴点の対応点マッチング結果を用いて推定されるホモグラフィ行列を用いた推定を含むことを特徴とする請求項２に記載の天井地図作成方法。
【請求項４】
複数種類の前記スケール推定が、局所特徴点の対応点マッチング結果を用いて推定される剛体変換行列を用いた推定を含むことを特徴とする請求項２に記載の天井地図作成方法。
【請求項５】
複数種類の前記スケール推定が、テンプレートマッチングを用いて推定される並進変換行列を用いた推定を含むことを特徴とする請求項２に記載の天井地図作成方法。
【請求項６】
複数種類の前記スケール推定が、直近過去の前記天井画像の前記スケールに基づく画像間の並進変換行列を用いた推定を含むことを特徴とする請求項２に記載の天井地図作成方法。
【請求項７】
前記スケールを推定するステップは、撮影の時系列順に並べた複数の前記天井画像の前記スケールに対し、平滑化処理を実行するステップを含むことを特徴とする請求項１〜請求項６のいずれかに記載の天井地図作成方法。
【請求項８】
前記天井画像を貼り合わせるステップは、
前記天井地図を複数のブロックに分割するステップと、
ｉ番目の前記天井画像の画像座標系から天井地図座標系へのアフィン変換行列Ａｉを算出するステップと、
前記天井地図の前記ブロックの中心座標に最も近い前記天井画像を、当該ブロックに貼り合わせるステップとを含むことを特徴とする請求項１〜請求項７のいずれかに記載の天井地図作成方法。
【請求項９】
床面の上を移動可能なカメラが移動しながら天井を撮影して作成された複数の天井画像及び、撮影時の前記カメラの位置を検出して作成された位置情報を取得するステップと、
同一のオブジェクトが写っている互いにコビジブルな関係にある前記天井画像に対し、前記位置情報から得られる前記カメラの位置の移動量と両天井画像における前記オブジェクトの移動量との比で表されるスケールを推定するステップと、
複数の前記天井画像を、対応する前記スケールに応じて天井地図に適合する大きさに変換した上で貼り合わせ、前記天井地図を作成するステップと、を実行するように構成されている天井地図作成装置。
【請求項１０】
床面の上を移動可能なカメラにより、移動しながら天井を撮影して作成された複数の天井画像及び、天井撮影時の前記カメラの位置を検出して作成された位置情報を取得するステップと、
同一のオブジェクトが写っている互いにコビジブルな関係にある前記天井画像に対し、前記位置情報から得られる前記カメラの位置の移動量と両天井画像における前記オブジェクトの移動量との比で表されるスケールを推定するステップと、
複数の前記天井画像を、対応する前記スケールに応じて天井地図に適合する大きさに変換した上で貼り合わせ、前記天井地図を作成するステップと、コンピュータに実行させることを特徴とする天井地図作成プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本開示は、カメラにより作成された複数の天井画像を貼り合わせて天井地図を作成する天井地図作成方法、天井地図作成装置及び天井地図作成プログラムに関する。
【背景技術】
【０００２】
天井面を撮像可能なカメラ及び、このカメラが撮像した画像データを処理して位置を検出する位置検出装置を備え、自律移動可能な移動体において、位置検出装置が、天井画像と位置情報を含む教示画像情報を予め記憶しており、カメラが撮像した画像データから天井の画像を抽出し、抽出した天井の画像と教示画像情報とに基づいて、自己位置を認識するものが開示されている（特許文献１）。
【０００３】
特許文献１記載の移動体では、教示画像情報を記憶する教示段階において、２台のカメラを用いて天井方向画像を撮影し、天井方向画像を複数の部分に分割して、部分ごとに移動体からの高さ方向の距離を求めて距離画像を生成する。そして、距離画像の高さ方向の距離と、移動体から天井までの予め求められている高さ方向の天井距離とを比較し、両者が略同じ値であればその部分の距離画像を天井近傍とみなし、距離マスクを生成し、撮像画像のうちの一方の画像を天井画像として使用する。自己位置を検出する位置検出段階では、２つの画像データから距離画像を生成し、距離画像の距離と天井距離とを比較して生成した距離マスク等から天井画像マスクを生成した上で、画像マッチングを行って教示画像情報に基づいて、移動体の位置を検出している。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００４−１３３５６７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１記載の移動体では、移動体から天井までの距離が予め求められていないと天井画像を作成することができない。また、天井画像が１つの撮像画像に納まらない広い空間で天井画像を作成する場合に、撮像画像をどのようにつなげるかが問題となる。
【０００６】
本発明は、このような背景に鑑み、１つのカメラを用いて天井を撮影し、天井までの高さが予め求められていなくても、撮影した天井画像をつなげて天井地図を作成可能にすることを課題とする。
【課題を解決するための手段】
【０００７】
このような課題を解決するために、本発明のある実施形態は、床面（２）の上を移動可能なカメラ（６）により、前記カメラを移動させつつ天井（５）を撮影して複数の天井画像を作成すると共に、撮影時の前記カメラの位置情報を作成し、前記位置情報に基づいて、複数の前記天井画像を合成して天井地図を作成する天井地図作成方法であって、同一のオブジェクトが写っている互いにコビジブルな関係にある前記天井画像に対し、前記位置情報から得られる前記カメラの位置の移動量と両天井画像における前記オブジェクトの移動量との比で表されるスケールを推定するステップ（ＳＴ１６）と、複数の前記天井画像を、対応する前記スケールに応じて前記天井地図に適合する大きさに変換した上で貼り合わせ（ＳＴ８４）、前記天井地図を作成するステップ（ＳＴ２）とを含むことを特徴とする。
【０００８】
この構成によれば、天井までの高さが予め求められていなくても、撮影した天井画像をつなげて天井地図を作成することができる。
【０００９】
また、上記構成において、前記スケールを推定するステップ（ＳＴ１６）は、互いにコビジブルな関係にある前記天井画像に対し、変換行列を用いた複数種類のマッチングによるスケール推定を実行するステップ（ＳＴ７５）と、複数種類の前記スケール推定のうち、テンプレートマッチングによるスコアが最も高い変換行列によって得られる結果を前記スケールに選定するステップ（ＳＴ７６）とを含むとよい。
【００１０】
この構成によれば、互い異なるマッチングの性質を相補的に用いるにより、互いにコビジブルな関係にある天井画像のスケールを、より精度よく推定することができる。
【００１１】
また、上記構成において、複数種類の前記スケール推定が、局所特徴点の対応点マッチング結果を用いて推定されるホモグラフィ行列を用いた推定を含むとよい。
【００１２】
この構成によれば、より広角な画像を用いてより少ない枚数の画像により天井画像を作成することができる。
【００１３】
また、上記構成において、複数種類の前記スケール推定が、局所特徴点の対応点マッチング結果を用いて推定される剛体変換行列を用いた推定を含むとよい。
【００１４】
この構成によれば、高さが一体の天井において精度よくスケールを推定することができる。
【００１５】
また、上記構成において、複数種類の前記スケール推定が、テンプレートマッチングを用いて推定される並進変換行列（式（２））を用いた推定を含むとよい。
【００１６】
この構成によれば、高さが変化するような天井においても精度よくスケールを推定することができる。
【００１７】
また、上記構成において、複数種類の前記スケール推定が、直近過去の前記天井画像の前記スケールに基づく画像間の並進変換行列（式（５））を用いた推定を含むとよい。
【００１８】
この構成によれば、高さの変化が少ない天井において少ない計算量でスケールを高速に推定することができる。
【００１９】
また、上記構成において、前記スケールを推定するステップ（ＳＴ１６）は、撮影の時系列順に並べた複数の前記天井画像の前記スケールに対し、平滑化処理を実行するステップ（ＳＴ７８）を含むとよい。
【００２０】
この構成によれば、スケール推定の誤差を抑制し、天井画像をより円滑につなげることができる。
【００２１】
また、上記構成において、前記天井画像を貼り合わせるステップ（ＳＴ２）は、前記天井地図を複数のブロックに分割するステップ（ＳＴ８３）と、ｉ番目の前記天井画像の画像座標系から天井地図座標系へのアフィン変換行列Ａｉを算出するステップ（ＳＴ１２１）と、前記天井地図の前記ブロックの中心座標に最も近い前記天井画像を、当該ブロックに貼り合わせるステップ（ＳＴ１２３）とを含むとよい。
【００２２】
この構成によれば、天井画像の中で撮影角度が垂直に近い部分の複数の画像をつなげて歪みの少ない天井地図を作成することができる。
【００２３】
また、課題を解決するために、本発明のある実施形態は、天井地図作成装置であって、床面（２）の上を移動可能なカメラ（６）が移動しながら天井（５）を撮影して作成された複数の天井画像及び、撮影時の前記カメラの位置を検出して作成された位置情報を取得するステップ（ＳＴ００）と、同一のオブジェクトが写っている互いにコビジブルな関係にある前記天井画像に対し、前記位置情報から得られる前記カメラの位置の移動量と両天井画像における前記オブジェクトの移動量との比で表されるスケールを推定するステップ（ＳＴ１６）と、複数の前記天井画像を、対応する前記スケールに応じて天井地図に適合する大きさに変換した上で貼り合わせ（ＳＴ８４）、前記天井地図を作成するステップ（ＳＴ２）と、を実行するように構成されていることを特徴とする。
【００２４】
この構成によれば、天井までの高さが予め求められていなくても、撮影した天井画像をつなげて天井地図を作成することができる。
【００２５】
また、課題を解決するために、本発明のある実施形態は、天井地図作成プログラムであって、床面（２）の上を移動可能なカメラ（６）により、移動しながら天井（５）を撮影して作成された複数の天井画像及び、天井撮影時の前記カメラの位置を検出して作成された位置情報を取得するステップ（ＳＴ００）と、同一のオブジェクトが写っている互いにコビジブルな関係にある天井画像に対し、前記位置情報から得られる前記カメラの位置の移動量と両天井画像における前記オブジェクトの移動量との比で表されるスケールを推定するステップ（ＳＴ１６）と、複数の前記天井画像を、対応する前記スケールに応じて天井地図に適合する大きさに変換した上で貼り合わせ（ＳＴ８４）、前記天井地図を作成するステップ（ＳＴ２）と、コンピュータに実行させることを特徴とする。
【００２６】
この構成によれば、天井までの高さが予め求められていなくても、撮影した天井画像をつなげて天井地図をコンピュータに作成させることができる。
【発明の効果】
【００２７】
このように本発明によれば、１つのカメラを用いて天井を撮影し、天井までの高さが予め求められていなくても、撮影した天井画像をつなげて天井地図を作成することができる。
【図面の簡単な説明】
【００２８】
【図１】実施形態に係る天井地図作成ユニットの構成図
【図２】（Ａ）キャリブレーション前、（Ｂ）キャリブレーション後の撮影画像を示すカメラキャリブレーションの説明図
【図３】位置姿勢情報ファイルのイメージ図
【図４】天井地図作成処理の手順を示すフローチャート
【図５】キーフレーム作成処理の手順を示すフローチャート
【図６】入力データ取得処理の手順を示すフローチャート
【図７】初期キーフレーム選択処理の手順を示すフローチャート
【図８】グラフマップのイメージ図
【図９】ＯＲＢ特徴点の抽出処理の手順を示すフローチャート
【図１０】キーフレームの選択処理の手順を示すフローチャート
【図１１】キーフレームのグラフマップのイメージ図
【図１２】可視性グラフの作成処理の手順を示すフローチャート
【図１３】可視性グラフのイメージ図
【図１４】キーフレームのスケール推定処理の手順を示すフローチャート
【図１５】メディアンフィルタ適用前後のスケール値を示すグラフ
【図１６】天井パノラマ地図作成処理の手順を示すフローチャート
【図１７】座標軸の算出処理の手順を示すフローチャート
【図１８】天井パノラマ地図のサイズ決定処理の手順を示すフローチャート
【図１９】天井パノラマ地図のブロック分割処理の手順を示すフローチャート
【図２０】貼り合わせる天井画像の選択処理の手順を示すフローチャート
【図２１】コビジブルな天井画像間の対応点決定処理の手順を示すフローチャート
【図２２】バンドル調整による全体最適化処理の手順を示すフローチャート
【図２３】バンドル調整後の天井パノラマ地図の例を示す図
【図２４】天井地図の出力処理の手順を示すフローチャート
【発明を実施するための形態】
【００２９】
以下、図面を参照して、本発明の実施形態について詳細に説明する。
≪全体構成≫
【００３０】
図１は、天井地図作成ユニット１の構成図である。図１に示されるように、天井地図作成ユニット１は、走行車輪３を備え、床面２の上を走行によって移動可能な移動体４として構成されている。移動体４は、床面２の上を自律走行するものであってもよく、人力や他の装置の牽引等によって走行するものであってもよい。或いは、移動体４は、走行車輪３の代わりに歩行用の足を備えたロボットであってもよい。移動体４には、天井５を撮影するための撮影装置として、単一の魚眼レンズを備えた電子カメラである魚眼レンズカメラ（以下、単にカメラ６という）が搭載されている。また、移動体４には、カメラ６の世界座標系の位置及び姿勢を検出する位置姿勢検出ユニット７が搭載されている。位置姿勢検出ユニット７は、世界座標系における位置を高精度に検出するＧＰＳと、カメラ６の角速度から姿勢を検出するジャイロセンサとにより構成されている。
【００３１】
移動体４には、カメラ６により撮影された天井画像ファイル及び、位置姿勢検出ユニット７により検出された天井撮影時のカメラ６の位置情報及び姿勢情報（以下、位置姿勢情報という）を記憶する記憶装置８が搭載されている。また、移動体４には、記憶装置８に記憶された天井画像ファイル及び位置姿勢情報に基づいて天井地図を作成する天井地図作成装置１０が搭載されている。記憶装置８や天井地図作成装置１０は、移動体４に搭載されている必要はなく、有線や無線でデータを受信してもよい。
【００３２】
天井地図作成装置１０は、ＣＰＵ、ＲＡＭ、ＲＯＭ等を含む電子回路ユニットにより構成されたコンピュータであり、天井画像ファイル及び位置姿勢情報に基づいて天井地図を作成する演算処理を実行するように構成されている。なお、天井地図作成装置１０が演算処理を実行するように構成されているとは、天井地図作成装置１０を構成する演算処理装置（ＣＰＵ）が、記憶装置８から必要なデータを読み取り、ＲＯＭに格納されたアプリケーションソフトウェアを読み込んで当該ソフトウェアに従って所定の演算処理を実行するようにプログラムされていることを意味する。
【００３３】
天井地図作成装置１０は、カメラキャリブレーション機能を備えている。カメラ６を用いて天井を撮影するにあたり、魚眼レンズにおけるカメラパラメータとして、内部パラメータ及び歪み係数ベクトルを推定するカメラキャリブレーションを行う。まず、カメラキャリブレーションの準備として、精度のよいカメラパラメータを推定するために、魚眼レンズを用いてチェスボードを撮影した図２（Ａ）に示されるような複数のチェスボード画像を用意する。
【００３４】
カメラキャリブレーションは、次の手順で行う。即ち、カメラキャリブレーション用の全てのチェスボード画像に対し、チェスボードのコーナー検出を行う。チェスボードのコーナー検出は、チェスボード画像を読み込んで画像データを取得し、取得した画像データのコーナー検出をサブピクセル精度で行う。コーナー検出の後、全てのチェスボード画像のコーナー検出結果を用いて、カメラ６の内部パラメータ及び歪み係数ベクトルを公知の手法により推定する。このようにして得たカメラ６の内部パラメータ及び歪み係数ベクトルはカメラパラメータファイルに記録される。カメラ６の内部パラメータ及び歪み係数ベクトルを用いることにより、魚眼レンズで撮影された画像の歪みを図２（Ｂ）に示されるように補正することが可能となる。以下、特に断りをしない場合、天井画像というときは、歪み補正後の画像を指す。
≪天井撮影≫
【００３５】
カメラ６による天井撮影は次のようにして行われる。即ち、カメラ６を天井５に向くように移動体４に取り付け、天井５をスキャンしてゆくように移動体４を移動させながら、位置姿勢検出ユニット７によってカメラ６の位置及び姿勢を検出しつつ、カメラ６で天井５を撮影する。これにより、複数の天井画像ファイルが作成されると同時に、撮影を行ったカメラ６の位置姿勢情報ファイルが作成される。また、位置姿勢情報と天井画像ファイル名とを紐付けするタイムスタンプ情報を含んだタイムスタンプ情報ファイルが作成される。図３は位置姿勢情報ファイルのイメージ図である。図３に示されるように、移動体４は実線で示されるように移動し、撮影を行ったカメラ６の世界座標系の位置情報及び姿勢情報が移動体４の軌跡上に現れている。
≪天井地図作成≫
【００３６】
次に、天井地図作成装置１０の天井地図作成機能について説明する。天井地図作成装置１０に天井地図作成機能を実行させるにあたり、カメラ６の内部パラメータ及び歪み係数ベクトル、天井画像ファイル群、位置姿勢情報ファイル及び、タイムスタンプ情報ファイルを用意し、これらのファイルの情報を入力データとして天井地図作成装置１０に入力する（ステップＳＴ００、図示省略）。また、天井地図サイズやバンドル調整回数等、処理に必要なパラメータを入力した天井地図作成用設定ファイルを用意し、天井地図作成用設定ファイルの情報を設定データとして天井地図作成装置１０に入力する。
【００３７】
図４は、天井地図作成装置１０による天井地図作成処理の手順を示すフローチャートである。天井地図作成処理では、天井地図作成装置１０は、図４に示される手順で各処理を実行する。即ち、天井地図作成装置１０は、キーフレーム作成処理を行い（ステップＳＴ１）、天井パノラマ地図作成処理を行う（ステップＳＴ２）。以上により、天井地図作成処理が終了する。
≪キーフレーム作成処理≫
【００３８】
以下、図４中ステップＳＴ１のキーフレーム作成処理について説明する。複数枚の天井画像ファイルを同一の天井地図座標系に適切に貼り合わせるには、天井地図において天井画像がどこに位置するか、又は、どのように天井画像を変形して貼り合わせるか（合成するか）を定める必要がある。いずれの場合においても、天井画像ファイル群の位置姿勢情報から得られる世界座標系における両天井画像間の並進と、画像座標系における両天井画像間の並進との対応により、天井地図座標系における天井画像の局所的なスケールを推定する必要がある。
【００３９】
なお、本明細書において、画像座標系における並進とは、２枚の天井画像における同一オブジェクトの移動量（差分ベクトル）と定義する。一方、世界座標系における並進とは、カメラ６の両天井画像を撮影した位置の移動量（差分ベクトル）と定義する。また、スケールとは、カメラ６の両天井画像を撮影した位置の移動量に対する２枚の天井画像における同一オブジェクトの移動量の比であって、世界座標系で１メール進んだときに天井地図座標系で何ピクセル進むかを表す量と定義し、単位には「ピクセル／メートル」が与えられる。
【００４０】
ところで、全ての画像に対して局所的なスケールを推定する処理を行うと計算量が膨大になる一方、天井５が一定の高さに設けられている場合が多いことから、互いに近傍にある複数の天井画像は同じスケールを持つ場合が多い。そのため、計算の効率化を図る上では、全ての天井画像のスケールを推定する必要はなく、天井画像を適切に間引くことが重要であると考えられる。そこで、間引き処理がなされた代表的な天井画像をキーフレームとし、各キーフレームの局所的なスケールを推定してゆく。
【００４１】
キーフレームの局所的なスケールを精度よく推定するためには、天井画像間の距離が近く、且つ、同一のオブジェクトが写っている、いわゆるコビジブル（covisible：双可視）な複数の天井画像をキーフレームに選ぶ必要がある。キーフレームとする天井画像が互いにコビジブルであることの要請は、距離が近くても互いに壁の向こう側の領域の天井画像を連続しているかのように誤マッチングすることを防止することを目的としている。ここでは、天井画像間の局所特徴点マッチングを実行し、特徴点に占める対応点の割合が大きい天井画像をコビジブルなキーフレームとし、コビジブルなキーフレーム間の並進から局所的なスケールを推定する。
【００４２】
局所特徴点マッチングは、公知のアルゴリズムを用いればよく、例えば、Ｈ．Ｓｔｒａｓｄａｔ，Ａ．Ｊ．Ｄａｖｉｏｎ，Ｊ．Ｍ．Ｍ．Ｍｏｎｔｉｅｌ，Ｋ．Ｋｏｎｏｌｉｇｅ，"Ｄｏｕｂｕｌｅ  ｗｉｎｄｏｗ  ｏｐｔｉｍｉｓａｔｉｏｎ  ｆｏｒ  ｃｏｎｓｔａｎｔ  ｔｉｍｅ  ｖｉｓｕａｌ  ＳＬＡＭ，"２０１１や、Ｒａｕｌ  Ｍｕｒ−Ａｒｔａｌ，Ｊ．Ｍ．Ｍ．Ｍｏｎｔｉｅｌ，Ｊｕａｎ  Ｄ．Ｔａｒｄｏｓ，"ＯＲＢ−ＳＬＭ：ａ  Ｖｅｒｓａｔｉｌｅ  ａｎｄ  Ａｃｃｕｒａｔｅ  Ｍｏｎｏｃｕｌａｒ  Ｓｌａｍ  Ｓｙｓｔｅｍ，"２０１５に開示されたアルゴリズムを用いてよい。ここでは、ＯＲＢ（Ｏｒｉｅｎｔｅｄ  ＦＡＳＴ  ａｎｄ  Ｒｏｔａｔｅｄ  ＢＲＩＥＦ）アルゴリズムを用いて抽出する特徴点（以下、ＯＲＢ特徴点という）を用いてマッチングを実行する。
【００４３】
図５は、図４中ステップＳＴ１のキーフレーム作成処理の手順を示すフローチャートである。キーフレーム作成処理では、天井地図作成装置１０は、図５に示される手順で各処理を実行する。即ち、天井地図作成装置１０は、入力データ取得処理を実行する（ステップＳＴ１１）。天井地図作成装置１０は、取得した入力データの天井画像の中から初期キーフレームとする複数の天井画像を選択する初期キーフレーム選択処理を実行する（ステップＳＴ１２）。天井地図作成装置１０は、選択した初期キーフレームからＯＲＢ特徴点を抽出するＯＲＢ特徴点抽出処理を実行する（ステップＳＴ１３）。天井地図作成装置１０は、初期キーフレームを間引いて複数のキーフレームを選択するキーフレームの選択処理を実行する（ステップＳＴ１４）。天井地図作成装置１０は、複数のキーフレームを接続した可視性グラフ（ｃｏｖｉｓｉｂｉｌｉｔｙ  ｇｒａｐｈ）を作成する可視性グラフ作成処理を実行する（ステップＳＴ１５）。天井地図作成装置１０は、各キーフレームのスケールを推定するスケール推定処理を実行する（ステップＳＴ１６）。以上により、キーフレーム作成処理が終了する。
【００４４】
以下、ステップＳＴ１１からステップＳＴ１６の各処理について順に詳細に説明する。
＜入力データ取得処理＞
【００４５】
図６は、図５中ステップＳＴ１１の入力データ取得処理の手順を示すフローチャートである。入力データ取得処理では、天井地図作成装置１０は、図６に示される手順で各処理を実行する。即ち、天井地図作成装置１０は、天井地図作成用設定ファイルを読み込み（ステップＳＴ２１）、天井地図作成装置１０は、位置姿勢情報ファイルを読み込み（ステップＳＴ２２）、タイムスタンプ情報ファイルを読み込む（ステップＳＴ２３）、カメラパラメータファイルを読み込む（ステップＳＴ２４）。
＜初期キーフレーム選択処理＞
【００４６】
図７は、図５中ステップＳＴ１２の初期キーフレーム選択処理の手順を示すフローチャートである。初期キーフレーム選択処理では、天井地図作成装置１０は、キーフレーム探索時の計算の高速化を図るため、世界座標系の天井画像の位置情報のｋｄ木（ｋ  ｄｉｍｅｎｓｉｏｎ  ｔｒｅｅ）を作成し（ステップＳＴ３１）、位置情報をカバーする領域にて世界座標系を格子に分割する（ステップＳＴ３２）。ここでは、天井５の高さやスケールに応じて０．１５〜０．５０メートル間隔の格子に分割する。次に、天井地図作成装置１０は、初期キーフレームとするために、ステップＳＴ３２で作成した各格子内に位置する天井画像を、ｋｄ木を用いて探索する（ステップＳＴ３３）。格子上に複数の天井画像が存在する場合は、格子の中心に最も近い天井画像を初期キーフレームとする。天井地図作成装置１０は、初期キーフレームを時系列順に接続してグラフマップを作成する（ステップＳＴ３４）。
【００４７】
図８は初期キーフレームを時系列順に接続したグラフマップのイメージ図である。グラフマップは、図３に示される位置姿勢情報ファイルの情報に基づいて、図８に示されるように作成される。
＜ＯＲＢ特徴点抽出処理＞
【００４８】
図９は、図５中ステップＳＴ１３のＯＲＢ特徴点の抽出処理の手順を示すフローチャートである。ＯＲＢ特徴点の抽出処理では、天井地図作成装置１０は、図５に示される手順で全ての初期キーフレームに対して処理を実行する。天井地図作成装置１０は、初期キーフレームの天井画像ファイルを読み込み（ステップＳＴ４１）、画像データの前処理を行う（ステップＳＴ４２）。具体的には、天井地図作成装置１０は、天井画像作成用設定ファイルにて指定されたサイズの倍率に画像を縮小する。また、天井地図作成装置１０は、色彩（Ｈｕｅ）、彩度（Ｓａｔｕｒａｔｉｏｎ）及び明度（Ｖａｌｕｅ）の３つの成分からなる色空間であるＨＳＶ色空間（ＨＳＢ空間ともいう）に画像を変換し、明度の平坦化を行うことでコントラストの強調を行う。その後、天井地図作成装置１０は、ＯＲＢ特徴点を抽出する（ステップＳＴ４３）。ＯＲＢ特徴点の抽出は、例えば、Ｅ．Ｒｕｂｌｅｅ，Ｖ．Ｒａｂａｕｄ，Ｋ．Ｋｏｎｏｌｉｇｅ，Ｇ．Ｂｒａｄｓｋｉ，"ＯＲＢ：ａｎ  ｅｆｆｉｃｉｅｎｔ  ａｌｔｅｒｍａｔｉｖｅ  ｔｏ  ＳＩＦＴ  ｏｒ  ＳＵＲＦ，"２０１１に開示された公知の手法で行えばよい。その後、天井地図作成装置１０は、次の初期キーフレームがあるか否かを判定し（ステップＳＴ４４）、ある場合には（Ｙｅｓ）、上記ステップＳＴ４１以降の手順を繰り返し、ない場合には（Ｎｏ）、本処理を終了する。
＜キーフレームの選択処理（初期キーフレームの間引き）＞
【００４９】
図１０は、図５中ステップＳＴ１４のキーフレームの選択処理の手順を示すフローチャートである。キーフレームの選択処理では、天井地図作成装置１０は、全ての初期キーフレームに対し時系列順に、次の手順で処理を実行する。天井地図作成装置１０は、まず、現在の初期キーフレームと直近過去の初期キーフレームとの特徴点マッチングを行う（ステップＳＴ５１）。次に、天井地図作成装置１０は、次式（１）で定義されるマッチング率を算出する（ステップＳＴ５２）。
【数１】
2019036804.tif 000003
続いて、天井地図作成装置１０は、算出されたマッチング率に応じてＯＲＢ特徴点を抽出する。具体的には、天井地図作成装置１０は、マッチング率が第１所定値（例えば１８％）より大きいか否かを判定し（ステップＳＴ５３）、第１所定値より大きい場合には（Ｙｅｓ）、現在の初期キーフレームを削除し（ステップＳＴ５４）、第１所定値以下の場合には（Ｎｏ）、現在の初期キーフレームと直近過去の初期キーフレームとを接続してグラフマップを作成する（ステップＳＴ５５）。その後、天井地図作成装置１０は、次の初期キーフレームがあるか否かを判定し（ステップＳＴ５６）、ある場合には（Ｙｅｓ）、上記ステップＳＴ５１以降の手順を繰り返し、ない場合には（Ｎｏ）、本処理を終了する。これにより、初期キーフレームが間引かれる。
【００５０】
図１１は、キーフレームの選択処理によって初期キーフレームが間引かれ、残ったキーフレームを時系列方向に接続したグラフマップのイメージ図である。
＜可視性グラフの作成処理＞
【００５１】
図１２は、図５中ステップＳＴ１５の可視性グラフの作成処理の手順を示すフローチャートである。可視性グラフの作成処理では、天井地図作成装置１０は、初期キーフレームが間引かれ、残ったことで選択された全てのキーフレームに対し、次の処理を実行する。即ち、天井地図作成装置１０は、キーフレームとその近傍（例えば、３ｍ以内）のキーフレームとを収集し、キーフレームと距離が近いものから順に特徴点マッチングを実行する（ステップＳＴ６１）。次に、天井地図作成装置１０は、上式（１）でマッチング率を算出し（ステップＳＴ６２）、算出したマッチング率が、第１所定値よりも小さな第２所定値（例えば５％）より大きいか否かを判定し（ステップＳＴ６３）、第２所定値以下の場合には（Ｎｏ）、対応点の個数が所定個数（例えば、１５０個）以上あるか否かを判定する（ステップＳＴ６４）。マッチング率が第２所定値より大きい場合（ステップＳＴ６３：Ｙｅｓ）及び、対応点の個数が所定個数以上ある場合（ステップＳＴ６４：Ｙｅｓ）、天井地図作成装置１０は、キーフレームとその近傍のキーフレームとをコビジブルな関係として接続する（ステップＳＴ６５）。マッチング率が第２所定値以下であり（ステップＳＴ６３：Ｎｏ）、且つ対応点の個数が所定個数未満である（ステップＳＴ６４：Ｎｏ）場合、天井地図作成装置１０は、キーフレームを接続しない。その後、天井地図作成装置１０は、次のキーフレームがあるか否かを判定し（ステップＳＴ６６）、ある場合には（Ｙｅｓ）、上記ステップＳＴ６１以降の手順を繰り返し、ない場合には（Ｎｏ）、本処理を終了する。これにより、可視性グラフが作成される。
【００５２】
図１３は、可視性グラフの作成処理によって作成された可視性グラフのイメージ図である。可視性グラフでは、近傍のキーフレーム同士が接続されている。
＜キーフレームのスケール推定処理＞
【００５３】
図１４は、図５中ステップＳＴ１６のキーフレームのスケール推定処理の手順を示すフローチャートである。キーフレームのスケール推定処理では、天井地図作成装置１０は、図１４に示される手順で全てのキーフレームに対して処理を実行する。天井地図作成装置１０は、キーフレームの天井画像ファイルを読み込み（ステップＳＴ７１）、読み込んだ画像データの前処理を行う（ステップＳＴ７２）。具体的には、天井地図作成装置１０は、ステップＳＴ４２と同様に、画像を指定されたサイズに縮小し、ＨＳＶ色空間に変換し、明度の平坦化を行うことでコントラスト調整を行う。その後、天井地図作成装置１０は、姿勢情報から得られる移動体４の姿勢θに対し、画像中心回りに−θだけ回転させ、画像の姿勢を位置方向に揃える。この処理により、近似の範囲内で画像間のマッチングを行う際には、回転を気にする必要はなくなり、並進の情報のみを取り扱うことが可能になる。画像データの前処理の後、天井地図作成装置１０は、コビジブルな関係にあるキーフレームの天井画像ファイルを読み込み（ステップＳＴ７３）、ステップＳＴ７２と同様に、読み込んだ画像データの前処理を行う（ステップＳＴ７４）。
【００５４】
その後、天井地図作成装置１０は、キーフレームとこれとコビジブルな関係にあるキーフレームとに対し、４種類のマッチングによるスケール推定を実行する（ステップＳＴ７５）。具体的には、天井地図作成装置１０は、これらのフレームに対し、次の第１〜第４の４種類の画像間の変換行列を算出する。
【００５５】
第１の変換行列は、局所特徴点の対応点マッチング結果を用いて推定される、３行３列のホモグラフィ行列である。ホモグラフィ行列の推定には、例えば、ＯｐｅｎＣＶのｃｖ：：ｆｉｎｄＨｏｍｏｇｒａｐｙ関数を用いることができる。ホモグラフィ行列を用いることにより、より広角な画像を用いてより少ない枚数の画像により天井画像を作成することができる。
【００５６】
第２の変換行列は、局所特徴点の対応点マッチング結果を用いて推定される、３行３列の剛体変換行列である。剛体変換行列の推定には、例えば、ＯｐｅｎＣＶのｃｖ：：ｅｓｔｉｍａｔｅＲｉｇｉｄＴｒａｎｓｆｏｒｍ関数を用いることができる。剛体変換行列を用いることにより、高さが一体の天井５において精度よくスケールを推定することができる。
【００５７】
第３の変換行列は、テンプレートマッチングを用いて推定される並進変換行列である。テンプレートマッチングにより得られる画像座標系における画像間の並進が（ΔＸ，ΔＹ）で与えられる場合、画像間の並進変換行列は次式（２）で与えられる。
【数２】
2019036804.tif 000004
テンプレートマッチングを用いて推定される並進変換行列を用いることにより、高さが変化するような天井５においても精度よくスケールを推定することができる。
【００５８】
第４の変換行列は、直近過去のキーフレームのスケールに基づいて推定される並進変換行列である。直近過去のスケールをｓｐｒｅｖ、移動体４の姿勢からのカメラ６の姿勢のズレを（−ψ）としたとき、世界座標系から画像座標系への変換行列は、次式（３）で与えられる。
【数３】
2019036804.tif 000005
従って、キーフレーム間の並進（ΔＸ，ΔＹ）は、世界座標系での並進（Δｘ，Δｙ）を用いて次式（４）で与えられる。
【数４】
2019036804.tif 000006
これより、第３の変換行列と同様に、画像間の並進変換行列は次式（５）で与えられる。
【数５】
2019036804.tif 000007
直近過去のキーフレームのスケールに基づいて推定される並進変換行列を用いることにより、高さの変化が少ない天井５において少ない計算量でスケールを高速に推定することができる。
【００５９】
このように複数種類の変換行列を算出するのは、特徴が多い領域に対しては局所特徴点マッチングの精度がよいのに対し、特徴が少ない領域においてはテンプレートマッチングが有効であるといった、互い異なるマッチングの性質を相補的に用いるためである。これにより、互いにコビジブルな関係にある天井画像のスケールを、より精度よく推定することができる。
【００６０】
その後、天井地図作成装置１０は、ステップＳＴ７５で算出した画像間の変換行列の中から採用する推定結果を選定する（ステップＳＴ７６）。具体的には、天井地図作成装置１０は、各変換行列をそれぞれ片方の画像に適用して画像中心付近を抽出し、もう片方の画像中心付近に対し、同じ大きさの画像を抽出し、テンプレートマッチングを実行する。そして、天井地図作成装置１０は、マッチングのスコアが最も高い変換行列によって得られるスケールを、キーフレームの局所的なスケールｓとする。
【００６１】
スケールｓは、画像中心に対してステップＳＴ７５で求めた最もよい変換行列を適用して得られる画像座標系での並進（ΔＸｃ，ΔＹｃ）とキーフレーム間の世界座標系での並進（Δｘ，Δｙ）とを用いて、次式（６）により算出される。
【数６】
2019036804.tif 000008
【００６２】
なお、多くの建物等では、天井画像は床とほぼ平行にあり、キーフレーム間の並進が剛体変換となることが多いため、ステップＳＴ７６にて第２の変換行列として推定した剛体変換行列が最もよいテンプレートマッチング結果を示す傾向にある。
【００６３】
続いて、天井地図作成装置１０は、次のキーフレームがあるか否かを判定し（ステップＳＴ７７）、ある場合には（Ｙｅｓ）、上記ステップＳＴ７１以降の手順を繰り返す。ステップＳＴ７７でし、次のキーフレームがない場合には（Ｎｏ）、天井地図作成装置１０は、スケールの外れ値を除去するために、撮影の時系列順に並べたキーフレームのスケールに対し、平滑化処理を実行する（ステップＳＴ７８）。ここでは、ある時刻ｔのキーフレームに対し、前後７つのキーフレームとコビジブルな関係にあるキーフレームのスケールの中間値を採用する、メディアンフィルタを適用し、時刻ｔのキーフレームのスケールを定める。以上により、本処理を終了する。
【００６４】
図１５は、メディアンフィルタ適用前後のスケール値を示すグラフである。図示されるように、メディアンフィルタの適用により、スケール値が平滑化される。これにより、スケール推定の誤差が抑制され、天井画像がより円滑につながる。
≪天井パノラマ地図作成処理≫
【００６５】
次に、図４中ステップＳＴ２の天井パノラマ地図作成処理について説明する。天井パノラマ地図作成処理は、キーフレームの局所的なスケールを基に画像を貼り合わせ、１枚の天井パノラマ地図を作成するものである。局所的なスケールをそれぞれ有する画像を等倍サイズで貼り合わせてゆくと、世界座標系の座標と天井パノラマ地図の座標が１対１に対応しない。そこで、局所的なスケールに応じて画像を拡大・縮小して貼り合わせることで、世界座標系の座標と天井パノラマ地図を１対１に対応、つまり、「１メートルが何ピクセルに対応しているか」というスケールを１つの値に定めることができる。
【００６６】
図１６は、ステップＳＴ２の天井パノラマ地図作成処理の手順を示すフローチャートである。天井パノラマ地図作成処理では、天井地図作成装置１０は、図１６に示される手順で各処理を実行する。即ち、天井地図作成装置１０は、天井地図座標系と世界座標系の座標軸の角度差ωを算出する座標軸の算出処理を実行する（ステップＳＴ８１）。天井地図作成装置１０は、天井パノラマ地図のＸ方向及びＹ方向のサイズを決定する天井パノラマ地図のサイズ決定処理を実行する（ステップＳＴ８２）。天井地図作成装置１０は、天井パノラマ地図を複数のブロックに分割する天井パノラマ地図のブロック分割処理を実行する（ステップＳＴ８３）。天井地図作成装置１０は、天井パノラマ地図に貼り合わせる天井画像を選択する天井画像の選択処理を実行する（ステップＳＴ８４）。天井地図作成装置１０は、コビジブルな天井画像間の対応点を探索するコビジブルな天井画像間の対応点探索処理を実行する（ステップＳＴ８５）。天井地図作成装置１０は、バンドル調整による全体最適化処理を実行する（ステップＳＴ８６）、天井地図の出力処理を実行する（ステップＳＴ８７）。以上により、天井パノラマ地図作成処理が終了する。
【００６７】
以下、ステップＳＴ８１からステップＳＴ８７の各処理について順に詳細に説明する。
＜座標軸の算出処理＞
【００６８】
図１７は、図１６中ステップＳＴ８１の座標軸の算出処理の手順を示すフローチャートである。座標軸の算出処理では、天井地図作成装置１０は、図１７に示される手順で全てのキーフレームに対して時系列順に処理を実行する。天井地図作成装置１０は、まず、現在のキーフレームｉの地図上の画像中心座標と、１つ過去の時刻のキーフレーム（ｉ−１）の地図上の画像中心座標との２次元の差分ベクトルΔＸｌを算出する（ステップＳＴ９１）。なお、本文中ではベクトルに矢印は付さず、数式においてはベクトルに矢印を付して示す。続いて、天井地図作成装置１０は、現在のキーフレームｉの世界座標系での座標と、１つ過去の時刻のキーフレーム（ｉ−１）の世界座標系での座標との２次元の差分ベクトルΔｘｌを算出する（ステップＳＴ９２）。その後、天井地図作成装置１０は、次式（７）で地図上の差分ベクトルの方向と世界座標系の差分ベクトルの成す角（座標軸の角度差ωｉ）を算出する（ステップＳＴ９３）。座標軸の角度差ωｉは、次式（７）で求められる。
【数７】
2019036804.tif 000009
その後、天井地図作成装置１０は、次のキーフレームがあるか否かを判定し（ステップＳＴ９４）、ある場合には（Ｙｅｓ）、上記ステップＳＴ９１以降の手順を繰り返し、ない場合には（Ｎｏ）、ステップＳＴ９１〜ステップＳＴ９３で求めた角度差ωｉの平均値を算出し、天井地図座標系と世界座標系の座標軸の角度差ωとする（ステップＳＴ９５）。以上により、本処理を終了する。
＜天井パノラマ地図のサイズ決定処理＞
【００６９】
図１８は、図１６中ステップＳＴ８２の天井パノラマ地図のサイズ決定処理の手順を示すフローチャートである。天井パノラマ地図のサイズ決定処理では、天井地図作成装置１０は、図１８に示される手順で処理を実行する。即ち、天井地図作成装置１０は、キーフレームの天井地図上での初期座標を決める（ステップＳＴ１０１）。具体的には、天井地図作成装置１０は、全てのキーフレームで最小となるスケールｓｍｉｎを探索し、対応する世界座標系での位置情報（ｘｉ，ｙｉ）を用いた次式（８）により、キーフレームｉの初期座標（Ｘｉｉｎｉ，Ｙｉｉｎｉ）を算出する。
【数８】
2019036804.tif 000010
次に、天井地図作成装置１０は、次のキーフレームがあるか否かを判定し（ステップＳＴ１０２）、ある場合には（Ｙｅｓ）、上記ステップＳＴ１０１の手順を繰り返し、ない場合には（Ｎｏ）、全てのキーフレームの天井地図上での座標に対し、Ｘ方向の最小値Ｘｍｉｎｉｎｉ、最大値Ｘｍａｘｉｎｉ、Ｙ方向の最小値Ｙｍｉｎｉｎｉ、最大値Ｙｍａｘｉｎｉを求め、天井パノラマ地図のＸ方向のオフセットＸｏｆｆｓｅｔ、Ｙ方向のオフセットＹｏｆｆｓｅｔを、次式（９）、（１０）により算出する（ステップＳＴ１０３）。
Ｘｏｆｆｓｅｔ＝Ｘｍｉｎｉｎｉ−２Ｂ  ・・・（９）
Ｙｏｆｆｓｅｔ＝Ｙｍｉｎｉｎｉ−２Ｂ  ・・・（１０）
ここで、Ｂは天井パノラマ地図をブロック分割する際のブロックのオフセットの長さを表す。その後、天井地図作成装置１０は、天井地図座標系における天井パノラマ地図画像のサイズ、即ち幅Ｗ及び高さＨを、次式（１１）、（１２）により算出する（ステップＳＴ１０４）。
Ｗ＝Ｘｍａｘｉｎｉ＋２Ｂ−Ｘｏｆｆｓｅｔ＝Ｘｍａｘｉｎｉ−Ｘｍｉｎｉｎｉ＋４Ｂ  ・・・（１１）
Ｈ＝Ｙｍａｘｉｎｉ＋２Ｂ−Ｙｏｆｆｓｅｔ＝Ｙｍａｘｉｎｉ−Ｙｍｉｎｉｎｉ＋４Ｂ  ・・・（１２）
＜天井パノラマ地図のブロック分割処理＞
【００７０】
図１９は、図１６中ステップＳＴ８３の天井パノラマ地図のブロック分割処理の手順を示すフローチャートである。天井パノラマ地図のブロック分割処理では、天井地図作成装置１０は、図１９に示される手順で処理を実行する。即ち、天井地図作成装置１０は、全てのキーフレームから局所的なスケールが最小のものｓｍｉｎ、及び、最大のものｓｍａｘを取得し（ステップＳＴ１１１）、次式（１３）より、天井パノラマ地図のブロックの１辺の長さＢｌ［ピクセル］を算出する（ステップＳＴ１１２）。
【数９】
2019036804.tif 000011
ここでは、ｋ１＝１０００、ｋ２＝２００００とする。また、ブロックの１辺の長さＢｌが大きい場合、ブロックとブロックの境界付近での自己位置推定の精度が悪くなるため、上式（１３）により算出したブロックの１辺の長さＢｌの値の範囲を［３０，１００］とし、上限を超えた場合は１００、下限を下回った場合は３０に丸める。次に、天井地図作成装置１０は、図１８中ステップＳＴ１０４で算出した天井パノラマ地図の幅Ｗ、高さＨから、ブロックの行数Ｂｒｏｗｓ、列数Ｂｃｏｌｓを次式（１４）、（１５）により算出する（ステップＳＴ１１３）。
Ｂｒｏｗｓ＝［Ｈ／Ｂｌ］  ・・・（１４）
Ｂｃｏｌｓ＝［Ｗ／Ｂｌ］  ・・・（１５）
ここで、[]はガウス記号を表す。その後、天井地図作成装置１０は、ブロック分割の原点を表すオフセット（Ｂｏｆｆｓｅｔ＿Ｘ，Ｂｏｆｆｓｅｔ＿Ｙ）を、次式（１６）、（１７）により算出する（ステップＳＴ１１４）。
Ｂｏｆｆｓｅｔ＿Ｘ＝［（Ｗ−ＢｃｏｌｓＢ）／２］  ・・・（１６）
Ｂｏｆｆｓｅｔ＿Ｙ＝［（Ｈ−ＢｒｏｗｓＢ）／２］  ・・・（１７）
最後に、天井地図作成装置１０は、ステップＳＴ１１４で算出したオフセット（Ｂｏｆｆｓｅｔ＿Ｘ，Ｂｏｆｆｓｅｔ＿Ｙ）を、ブロック分割の原点とし、各ブロックの中心座標を算出する（ステップＳＴ１１５）。
＜貼り合わせる天井画像の選択処理＞
【００７１】
図２０は、図１６中ステップＳＴ８４の貼り合わせる天井画像の選択処理の手順を示すフローチャートである。貼り合わせる天井画像の選択処理では、天井地図作成装置１０は、図２０に示される手順で処理を実行し、ブロックに属するキーフレームを探索する。まず、天井地図作成装置１０は、キーフレームｉの画像座標系から天井地図座標系へのアフィン変換行列Ａｉを、次式（１８）により算出する（ステップＳＴ１２１）。
【数１０】
2019036804.tif 000012
ここで、Ｓｉは、ｓｍｉｎ／ｓｉで与えられるスケール比を表しており、θｉは姿勢情報で得られる移動体４の姿勢を表す。この処理を全てのキーフレームに対して行うべく、天井地図作成装置１０は、次のキーフレームがあるか否かを判定し（ステップＳＴ１２２）、ある場合には（Ｙｅｓ）、上記ステップＳＴ１２１の手順を繰り返す。これにより、全てのキーフレームの天井画像が、スケールに応じ、天井パノラマ地図の座標系である天井地図座標系に適合した大きさに変換される。
【００７２】
ステップＳＴ１２２で次のキーフレームがない場合（Ｎｏ）、天井地図作成装置１０は、ブロックに貼り合わせる画像として選択するべく、ＳＴ１２１の変換を行ったときに、各ブロックの中心座標に最も近いキーフレームを探索する（ステップＳＴ１２３）。この処理を全てのブロックに対して行うべく、天井地図作成装置１０は、次のブロックがあるか否かを判定し（ステップＳＴ１２４）、ある場合には（Ｙｅｓ）、上記ステップＳＴ１２３の手順を繰り返し、ない場合には（Ｎｏ）、本処理を終了する。
【００７３】
このように天井地図作成装置１０が、ＳＴ８３で天井パノラマ地図を複数のブロックに分割し、ステップＳＴ１２１でｉ番目の天井画像の画像座標系から天井地図座標系へのアフィン変換行列Ａｉを算出し、ステップＳＴ１２３で天井パノラマ地図のブロックの中心座標に最も近い天井画像を、そのブロックに貼り合わせて合成することにより、天井画像の中で撮影角度が垂直に近い部分の複数の画像がつながって歪みの少ない天井パノラマ地図が作成される。
＜コビジブルな天井画像間の対応点決定処理＞
【００７４】
図２１は、図１６中ステップＳＴ８５のコビジブルな天井画像間の対応点決定処理の手順を示すフローチャートである。コビジブルな天井画像間の対応点決定処理では、天井地図作成装置１０は、図２１に示される手順で、貼り合わせに使用する全てのキーフレームに対して処理を実行する。天井地図作成装置１０は、キーフレームに対し、図９中ステップＳＴ４２で説明した前処理を行い（ステップＳＴ１３１）、前処理を行ったキーフレームに対し、ＯＲＢ特徴点抽出を行う（ステップＳＴ１３２）。この処理を全てのキーフレームに対して行うべく、天井地図作成装置１０は、次のキーフレームがあるか否かを判定し（ステップＳＴ１３３）、ある場合には（Ｙｅｓ）、上記ステップＳＴ１３１以降の手順を繰り返し、ない場合には（Ｎｏ）、貼り合わせる天井画像とコビジブルな関係にあるキーフレームとの間の特徴点の対応点を探索、決定する（ステップＳＴ１３４）。その後、天井地図作成装置１０は、貼り合わせる天井画像が更にあるか否かを判定し（ステップＳＴ１３５）、ある場合には（Ｙｅｓ）、ステップＳＴ１３４の処理を繰り返し、ない場合には（Ｎｏ）、本処理を終了する。
＜バンドル調整による全体最適化処理＞
【００７５】
図２２は、図１６中ステップＳＴ８６のバンドル調整による全体最適化処理の手順を示すフローチャートである。バンドル調整による全体最適化処理は、図２０中ステップＳＴ１２１で求めたキーフレームのアフィン変換行列Ａｉに対してバンドル調整を行うことにより、位置情報の誤差と局所スケール推定の誤差との同時最適化を図るための処理である。なお、バンドル調整は、Ｅ．Ｒｕｂｌｅｅ，Ｖ．Ｒａｂｕａｄ，Ｋ．Ｋｏｎｏｌｉｇｅ，Ｇ．Ｂｒａｄｓｋ，"Ｖｉｓｕａｌ−ｉｎｅｒｔｉａｌ  ｍｏｎｏｃｕｌａｒ  ＳＬＡＭ  ｗｉｔｈ  ｍａｐ  ｒｅｕｓｅ，"２０１６や、Ｂ．Ｔｒｉｇｇｓ，Ｐ．Ｆ．ＭｃＬａｕｃｈｌａｎ，Ｒ．Ｉ．Ｈａｒｔｌｅｙ，Ａ．Ｗ．Ｆｉｔｚｇｉｂｂｏｎ，"Ｂｕｎｄｌｅ  ａｄｊｕｓｔｍｅｎｔ  ａ  ｍｏｄｅｒｎ  ｓｙｎｔｈｅｓｉｓ，"２０００に開示された公知の手法で行えばよい。
【００７６】
枚数の多い天井画像群を用いた天井地図作成処理では、１回のバンドル調整の実行に要する時間が長くなるため、プログラムにてバンドル調整を繰り返す回数（例えば、２０回）を、天井地図作成用設定ファイルにて設定するとよい。バンドル調整による全体最適化処理では、天井地図作成装置１０は、図２２に示される手順で処理を実行する。即ち、天井地図作成装置１０は、キーフレームｉとこれとコビジブルな関係にあるキーフレームｊとの天井地図座標系における誤差を算出する（ステップＳＴ１４１）。具体的には、天井地図作成装置１０は、キーフレームｉ及びキーフレームｊのそれぞれｋ個目の対応点をそれぞれｐｉｋ、ｐｊｋとおき、両キーフレームの天井地図座標系における誤差を次式（１９）により算出する。
【数１１】
2019036804.tif 000013
次に、天井地図作成装置１０は、ステップＳＴ１４１で算出した各キーフレームの誤差の総和Ｅを最小にする画像座標系−天井地図座標系のアフィン変換行列Ａｉを、レベンバーグ・マーカート法を用いて次式（２０）により算出する。
【数１２】
2019036804.tif 000014
このとき、Ａｉの初期値は、図２０のステップＳＴ１２１で算出したアフィン変換行列の成分を初期値として用い、各キーフレームのアフィン変換行列に対し、次式（２１）で与えられる６個のパラメータを更新し、補正する（ステップＳＴ１４２）。
【数１３】
2019036804.tif 000015
その後、天井地図作成装置１０は、貼り合わせるキーフレームに対し、ステップＳＴ１４２で更新したアフィン変換行列を適用し、キーフレームが属するブロックの領域内にクロップして貼り合わせる（ステップＳＴ１４３）。これを全てのブロックに対して実行する。これにより１回のバンドル調整が終了する。その後、天井地図作成装置１０は、バンドル調整の繰り返しが必要か否か（天井地図作成用設定ファイルで指定された回数行ったか否か）を判定し（ステップＳＴ１４４）、必要ある場合には（Ｙｅｓ）、上記ステップＳＴ１４１以降の手順を繰り返し、ない場合には（Ｎｏ）、本処理を終了する。
【００７７】
図２３は、バンドル調整後の天井パノラマ地図の例を示している。図示されるように、各ブロックには、天井画像が隣の天井画像に連続するように貼り合わされる。
＜天井地図の出力処理＞
【００７８】
図２４は、図１６中ステップＳＴ８７の天井地図の出力処理の手順を示すフローチャートである。天井地図の出力処理では、天井地図作成装置１０は、図２４に示される手順で処理を実行し、自己位置推定に必要となる天井地図に関する情報を出力する。即ち、天井地図作成装置１０は、天井地図の基本的な情報を記述した、天井地図の情報ファイルを出力し（ステップＳＴ１５１）、天井パノラマ地図に使用したキーフレームの情報を記述した、キーフレームの情報ファイルを出力し（ステップＳＴ１５２）、天井パノラマ地図の画像データを天井地図画像ファイルとして出力する（ステップＳＴ１５３）。これにより、本処理が終了する。
【００７９】
このように、本実施形態に係る天井地図作成方法は、図５のステップＳＴ１６において、同一のオブジェクトが写っている互いにコビジブルな関係にある天井画像に対してスケールを推定し、図４のステップＳＴ２の天井パノラマ地図作成処理のうち、図１６のステップＳＴ８４において、複数の天井画像を、対応するスケールに応じて天井パノラマ地図に適合する大きさに変換した上で貼り合わせる。そのため、１つのカメラ６を用いて天井５を撮影し、天井５までの高さが予め求められていなくても、撮影した天井画像をつなげて天井地図を作成することができる。
【００８０】
また、本実施形態に係る天井地図作成装置１０は、複数の天井画像及び、撮影時のカメラ６の位置情報を取得し、図５のステップＳＴ１６において、同一のオブジェクトが写っている互いにコビジブルな関係にある天井画像に対してスケールを推定し、図４のステップＳＴ２の天井パノラマ地図作成処理のうち、図１６のステップＳＴ８４において、複数の天井画像を、対応するスケールに応じて天井パノラマ地図に適合する大きさに変換した上で貼り合わせるように構成されている。そのため、天井５までの高さが予め求められていなくても、撮影した天井画像をつなげて天井地図を作成することができる。
【００８１】
また、本実施形態に係る天井地図作成プログラムは、複数の天井画像及び、撮影時のカメラ６の位置情報を取得し、図５のステップＳＴ１６において、同一のオブジェクトが写っている互いにコビジブルな関係にある天井画像に対してスケールを推定する処理と、図４のステップＳＴ２の天井パノラマ地図作成処理のうち、図１６のステップＳＴ８４において、複数の天井画像を、対応するスケールに応じて天井パノラマ地図に適合する大きさに変換した上で貼り合わせる処理とを、コンピュータである天井地図作成装置１０に実行させる。そのため、天井５までの高さが予め求められていなくても、撮影した天井画像をつなげて天井地図を作成することができる。
【００８２】
このように統一したスケールで作成された天井地図は、床面２を移動する移動体４が自己位置を推定する際に利用することができる。例えば、１つのカメラ６を備えた移動体４は、床面２を移動した際にコビジブルな関係の天井画像を撮影し、天井地図作成時と同様の手法でスケールを推定して天井地図と同じスケールに変換した天井画像を、天井地図の中から検索することにより、天井地図上の自己位置を推定することができる。
【００８３】
以上で具体的実施形態の説明を終えるが、本発明は上記実施形態に限定されることなく幅広く変形実施することができる。例えば、各部材や部位の具体的構成や配置、数量、角度、手順など、本発明の趣旨を逸脱しない範囲であれば適宜変更可能である。一方、上記実施形態に示した各構成要素は必ずしも全てが必須ではなく、適宜選択することができる。
【符号の説明】
【００８４】
１      天井地図作成ユニット
２      床面
３      走行車輪
４      移動体
５      天井
６      カメラ
７      位置姿勢検出ユニット
８      記憶装置
１０      天井地図作成装置
【図１】
2019036804.tif 000016
【図２】
2019036804.tif 000017
【図３】
2019036804.tif 000018
【図４】
2019036804.tif 000019
【図５】
2019036804.tif 000020
【図６】
2019036804.tif 000021
【図７】
2019036804.tif 000022
【図８】
2019036804.tif 000023
【図９】
2019036804.tif 000024
【図１０】
2019036804.tif 000025
【図１１】
2019036804.tif 000026
【図１２】
2019036804.tif 000027
【図１３】
2019036804.tif 000028
【図１４】
2019036804.tif 000029
【図１５】
2019036804.tif 000030
【図１６】
2019036804.tif 000031
【図１７】
2019036804.tif 000032
【図１８】
2019036804.tif 000033
【図１９】
2019036804.tif 000034
【図２０】
2019036804.tif 000035
【図２１】
2019036804.tif 000036
【図２２】
2019036804.tif 000037
【図２３】
2019036804.tif 000038
【図２４】
2019036804.tif 000039