JP2017053795A - 情報処理装置、位置姿勢計測方法、及び位置姿勢計測プログラム - Google Patents

Abstract

【課題】環境地図の生成及び位置姿勢の計測における計算コストを低減する。【解決手段】情報処理装置において、時系列の画像を撮影する撮像手段と、前記撮像手段により撮影した画像から特徴点を抽出する特徴点抽出手段と、前記撮像手段で撮影した画像に対して前記特徴点抽出手段により抽出した特徴点の追跡を行う特徴点追跡手段と、前記特徴点抽出手段により抽出した特徴点から、位置又は姿勢を推定する基準となる制御点情報を抽出する制御点情報抽出手段と、前記特徴点追跡手段により追跡した画像上での特徴点の座標と、前記制御点情報抽出手段により抽出した制御点情報とから前記撮像手段における位置又は姿勢を計測する位置姿勢計測手段と、前記位置姿勢計測手段により得られた前記撮像手段の位置又は姿勢と、前記画像上の特徴点の座標とに基づいて環境地図を生成する環境地図生成手段と、を有することにより、上記課題を解決する。【選択図】図１

Description

本発明は、情報処理装置、位置姿勢計測方法、及び位置姿勢計測プログラムに関する。

従来では、車両等の移動体に取り付けたカメラ（撮像部）を用いて位置や姿勢を検出する手法があり、その一例としてＶｉｓｕａｌ−ＳＬＡＭ（Simultaneous Localization and Mapping；同時位置推定地図構築）が知られている。Ｖｉｓｕａｌ−ＳＬＡＭでは、例えば時系列の映像から、周囲の静止物体の３次元位置情報を含む環境地図の生成と、移動体の位置や姿勢の推定とを同時に行うことができる。Ｖｉｓｕａｌ−ＳＬＡＭは、環境地図の生成及び計測にカメラを用いるため、他のレーザレーダ等を用いる手段よりも安価なＳＬＡＭシステムを構築することができる。

また、Ｖｉｓｕａｌ−ＳＬＡＭの手法の１つとして、画像特徴点を用いた手法がある。画像特徴点を用いた手法では、環境地図は、３次元の座標を有する点の集合として定義される。車両の位置や姿勢は、その３次元の点の座標と、その３次元の点が観測された画像上の特徴点座標とから推定される。この推定による計測は、ＰｎＰ（Perspective n Points）問題と呼ばれている。

一般的に、移動体の自動運転等においては、位置や姿勢の計測が低遅延、高レート、高精度であるほど、移動体の制御が行いやすくなる。そのため、位置や姿勢を推定するＰｎＰ問題の解法は、可能な限り高速に計算が行えて、かつ精度が高いことが望ましい。

特許文献１には、ＳＬＡＭを処理するために、視覚センサ及びデッドレコニングセンサを使用し、新規のランドマークの作成プロセスや整合プロセスにおいて、より効率的な構成を実現するためにＳＬＡＭグラフを実時間で修正する手法が開示されている。

しかしながら、従来手法では、上述したＰｎＰ問題の計算コストが高い。また、計算コストを改善するためには、高パフォーマンスの計算機環境が必要となるため、高価なシステムが必要となったり、消費電力が大きくなってしまうという問題があった。

本発明の一実施形態は、環境地図の生成及び移動体の位置や姿勢の計測における計算コストを低減するための情報処理装置、位置姿勢計測方法、及び位置姿勢計測プログラムを提供することを目的とする。

上記目的を達成するため、一つの態様における情報処理装置は、時系列の画像を撮影する撮像手段と、前記撮像手段により撮影した画像から特徴点を抽出する特徴点抽出手段と、前記撮像手段で撮影した画像に対して前記特徴点抽出手段により抽出した特徴点の追跡を行う特徴点追跡手段と、前記特徴点抽出手段により抽出した特徴点から、位置又は姿勢を推定する基準となる制御点情報を抽出する制御点情報抽出手段と、前記特徴点追跡手段により追跡した画像上での特徴点の座標と、前記制御点情報抽出手段により抽出した制御点情報とから前記撮像手段における位置又は姿勢を計測する位置姿勢計測手段と、前記位置姿勢計測手段により得られた前記撮像手段の位置又は姿勢と、前記画像上の特徴点の座標とに基づいて環境地図を生成する環境地図生成手段と、を有する。

本発明の一実施形態によれば、環境地図の生成及び位置姿勢の計測における計算コストを低減することができる。

情報処理装置の機能構成の一例を示す図である。 情報処理装置のハードウェア構成の一例を示す図である。 本実施形態における位置姿勢計測処理の一例を示すフローチャートである。 位置姿勢計測処理を説明するための図である。 環境地図生成処理の一例示すフローチャートである。 制御情報抽出処理の一例を示すフローチャートである。 制御点情報を抽出するタイミングを説明するための図である。

以下、図面を用いて、本発明の一実施形態を詳細に説明する。

＜情報処理装置の機能構成＞
図１は、情報処理装置の機能構成の一例を示す図である。図１の例に示す情報処理装置１０は、撮像部１１と、画像特徴点抽出部１２と、画像特徴点追跡部１３と、記憶部１４と、位置姿勢計測部１５と、環境地図生成部１６と、制御点情報抽出部１７とを有する。

撮像部１１は、情報処理装置１０の周囲を撮影する撮像手段の一例である。撮像部１１は、例えば所定のカメラパラメータ（内部パラメータ）等に基づき、所定のポーズ（位置及び／又は姿勢（方向）（以下、「位置姿勢」という））や画角等における画像を取得する。また、撮像部１１は、所定の時間間隔（周期）で撮影を行い、時系列の画像フレームを生成する。例えば、情報処理装置１０が移動可能な移動体である場合、撮像部１１は、移動中の時系列画像を生成することができる。撮像部１１は、撮影した画像を、画像特徴点抽出部１２及び画像特徴点追跡部１３に出力する。撮像部１１は、例えばステレオカメラであってもよく、複数の単眼カメラであってもよい。

画像特徴点抽出部１２は、撮像部１１が撮影した画像から特徴点（画像特徴点）を抽出し、画像特徴点情報２１を出力する特徴点抽出手段の一例である。画像特徴点抽出部１２は、撮影した画像毎に特徴点を抽出する。画像特徴点抽出部１２は、例えば画像の輝度値を用いて特徴点を抽出したり、画像中の物体の形状等から特徴点を抽出したりする手法があるが、これらに限定されるものではない。

特徴点の抽出手法としては、例えばＨａｒｒｉｓオペレータ、ＦＡＳＴ（Features from Accelerated Segment Test）オペレータを用いることができる。画像特徴点抽出部１２は、上述したＨａｒｒｉｓオペレータやＦＡＳＴオペレータ等を用いて画像上の特徴点座標を抽出する場合には、画像上の特徴点の座標と、その画像とを画像特徴点情報２１として記録する。

また、特徴点の抽出手法として、例えばＳＩＦＴ（Scale Invariant Feature Transform）特徴量記述子、ＳＵＲＦ（Speeded Up Robust Features）特徴量記述子を用いることができる。画像特徴点抽出部１２は、上述したＳＩＦＴやＳＵＲＦ等の特徴量記述を行う場合には、画像上の特徴点の座標と、その特徴量とを画像特徴点情報２１として記録する。

画像特徴点追跡部１３は、特徴点を時系列で追跡する特徴点追跡手段の一例である。画像特徴点追跡部１３は、例えば画像特徴点情報２１に記録された特徴点を、その特徴点を抽出した画像より後に撮像部１１が撮影した画像の中から探索することで各特徴点の追跡を行う。また、画像特徴点追跡部１３は、探索結果から追跡の成否（成功、失敗）を判断し、その判断結果に基づき、特徴点毎に画像特徴点情報２１の登録等を行う。

例えば、画像特徴点追跡部１３は、ブロックマッチングや特徴量等による探索手法を用いて特徴点の追跡を行うことができる。例えば、上述したブロックマッチングを用いて特徴点の追跡を行う場合、画像特徴点追跡部１３は、対象の画像と、その画像上での各特徴点の座標と、各特徴点における追跡の成否とを、画像特徴点情報２１に追加登録する。

また、上述した特徴量を用いた探索を行う場合には、画像特徴点追跡部１３は、画像特徴点抽出部１２と同様の特徴点記述処理等を行い、画像特徴点情報２１として記録されている特徴点と、相関の高い特徴点を探索する。また、画像特徴点追跡部１３は、上記の探索の結果、相関が所定の閾値を超えている場合には追跡成功と判断し、閾値以下の場合には追跡失敗と判断して、その画像上での各特徴点の座標と、各特徴点における追跡の成否とを、画像特徴点情報２１に追加登録する。

記憶部１４は、本実施形態における位置姿勢計測処理において必要な各種情報や処理結果等を記憶する記憶手段の一例である。記憶部１４は、例えば上述した画像特徴点情報２１、位置姿勢情報２２、環境地図２３、制御点情報２４等を記憶する。

位置姿勢情報２２は、特徴点を観測した画像を撮影した撮像部１１のポーズ（例えば、位置姿勢）等の情報である。この情報は、撮像部１１を有する情報処理装置１０の位置姿勢に対応させることができる。環境地図２３は、建物や壁等の物体、道路等がどこにあるか等の周辺環境の情報を記述した地図である。環境地図２３は、周囲の静止物体等の３次元位置情報であり、例えば上述した特徴点の３次元座標等が記録されている。なお、環境地図は、周囲の静止物体等の２次元の情報であってもよい。制御点情報２４は、撮像部１１や情報処理装置１０の位置や姿勢等を計測する際に使用される制御点に関する情報である。制御点情報２４は、例えば上述したＳＬＡＭにおけるＰｎＰ問題の高速解法として知られているＥＰｎＰアルゴリズムで利用する制御点の座標・重み行列等である（ＥＰｎＰアルゴリズムについては、例えば「Luis Ferraz、Xavier Binefa、Francesc Moreno-Noguer、"Very Fast Solution to the PnP Problem with Algebraic Outlier Rejection"、Conference in Computer Vision and Pattern Recognition (CVPR)、2014」を参照）。

なお、記憶部１４に記憶される情報については、これらに限定されるものではなく、例えば、撮像部１１により撮像された画像データ、本実施形態における位置姿勢計測処理の実行経過や実行結果、履歴情報等を記憶してもよい。

位置姿勢計測部１５は、制御点情報抽出部１７により抽出される制御点情報等に基づき、撮像部１１又は撮像部１１を有する情報処理装置１０の位置姿勢を計測する位置姿勢計測手段の一例である。位置姿勢計測部１５は、計測した位置姿勢等の情報を位置姿勢情報２２として記録部１４に記録する。位置姿勢計測部１５における具体的な処理内容については、後述する。

環境地図生成部１６は、画像特徴点情報２１と、位置姿勢情報２２と、環境地図２３とから、環境地図２３の生成及び更新（例えば、補正、削除、拡張）等を行う環境地図生成手段の一例である。環境地図生成部１６は、例えば環境地図２３の全体の再投影誤差の最小化や、複数回の追跡に成功した特徴点の環境地図２３への登録、環境地図２３にあるアウトライアの点の排除等を行う。環境地図生成部１６の具体的な処理内容については、後述する。

制御点情報抽出部１７は、画像特徴点抽出部１２により抽出した特徴点から、位置又は姿勢を推定する基準となる制御点情報を抽出する制御点情報抽出手段の一例である。制御点情報抽出部１７は、例えば環境地図２３からＥＰｎＰアルゴリズムで利用する制御点を抽出する。この制御点は、その後の位置姿勢計測等で使用される。制御点情報抽出部１７の具体的な処理内容については、後述する。

情報処理装置１０は、例えばタブレット端末、スマートフォン、撮像装置（カメラ）、車両や移動式ロボット、飛行機、船等の各種移動体であるが、これに限定されるものではなく、ＰＣ（Personal Computer）等であってもよい。また、情報処理装置１０は、車両やロボット等に組み込まれる部品や装置ユニット等の一つであってもよい。

＜情報処理装置１０のハードウェア構成例＞
ここで、本実施形態では、例えば上述した情報処理装置１０の各機能をコンピュータに実行させる実行プログラム（位置姿勢計測プログラム）を生成する。また、本実施形態は、生成した実行プログラムを、例えばＰＣ、タブレット端末、スマートフォン、カメラ、車両や移動型ロボット等の各種移動体に設けられた制御装置等にインストールすることで、本実施形態における位置姿勢計測処理等を実現することができる。

図２は、情報処理装置のハードウェア構成の一例を示す図である。図２に示す情報処理装置１０のコンピュータ本体には、入力装置３１と、出力装置３２と、ドライブ装置３３と、カメラ３４と、位置計測装置３５と、補助記憶装置３６と、主記憶装置３７と、各種制御を行うＣＰＵ（Central Processing Unit）３８と、ネットワーク接続装置３９とを有するように構成され、これらはシステムバスＢで相互に接続されている。

入力装置３１は、ユーザ等が操作するキーボード、マウス等のポインティングデバイスやマイク等の音声入力デバイス等を有し、ユーザ等からのプログラムの実行等、各種操作信号を入力する。

出力装置３２は、本実施形態に係る処理を行うコンピュータ本体を操作するのに必要な各種ウィンドウやデータ等を表示するディスプレイを有し、ＣＰＵ３８が有する制御プログラムにより実行経過や結果等を表示する。なお、上述した入力装置３１と、出力装置３２とは、例えばタッチパネル等のように、入出力が一体型の構成であってもよい。

ドライブ装置３３は、例えば記録媒体４０等を着脱自在にセットすることができ、セットした記録媒体４０に記録された各種情報を読み込んだり、所定の情報を記録媒体４０に書き込むことができる。ここで、本実施形態においてコンピュータ本体にインストールされる実行プログラムは、例えば記録媒体４０等により提供される。記録媒体４０に含まれる実行プログラムは、記録媒体４０からドライブ装置３３を介して補助記憶装置３６にインストールされる。

カメラ３４は、一定の周期で撮影を行い、時系列の画像フレームを生成する。カメラ３４は、例えば上述した撮像部１１に対応する。位置計測装置３５は、上述した位置姿勢計測部１５に相当する。位置計測装置３５は、ＧＰＳ（Global Positioning System；全地球測位システム）機能等を用いて、撮像部１１の位置情報を取得する。

補助記憶装置３６は、例えばＨＤＤ（Hard Disk Drive）やＳＳＤ（Solid State Drive）等のストレージ装置等である。補助記憶装置３６は、ＣＰＵ３８からの制御信号に基づき、本実施形態における実行プログラムや、コンピュータに設けられた制御プログラム等を記憶し、必要に応じて入出力を行うことができる。補助記憶装置３６は、ＣＰＵ３８からの制御信号等に基づいて、記憶された各情報から必要な情報を読み出したり、書き込むことができる。

主記憶装置３７は、ＣＰＵ３８により補助記憶装置３６から読み出された実行プログラム等を格納する。主記憶装置３７は、例えばＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）等であるが、これに限定されるものではない。補助記憶装置３６及び主記憶装置３７は、例えば上述した記憶部１４等に対応する。

ＣＰＵ３８は、オペレーティングシステム等の制御プログラム、及び主記憶装置３７に格納されている実行プログラムに基づいて、各種演算や各ハードウェア構成部とのデータの入出力等、コンピュータ全体の処理を制御する。プログラムの実行中に必要な各種情報等は、例えば補助記憶装置３６から取得することができ、また実行結果等を補助記憶装置３６に格納することもできる。

ＣＰＵ３８は、例えば入力装置３１から得られるプログラムの実行指示等に基づき、補助記憶装置３６にインストールされたプログラムを実行させることにより、主記憶装置３７上でプログラムに対応する処理を行う。

例えば、ＣＰＵ３８は、位置姿勢計測プログラムを実行させることで、上述した撮像部１１による画像の取得、画像特徴点抽出部１２による画像特徴点の抽出、画像特徴点追跡部１３による画像特徴点の追跡等の処理を行う。また、ＣＰＵ３８は、位置姿勢計測プログラムを実行させることで、上述した位置姿勢計測部１５による位置姿勢計測、環境地図生成部１６による環境地図の生成、制御点情報抽出部１７による制御点情報の抽出等の各処理を行う。ＣＰＵ３８における処理内容は、これに限定されるものではない。ＣＰＵ３８により実行された内容は、必要に応じて補助記憶装置３６に記憶させることができる。

ネットワーク接続装置３９は、ＣＰＵ３８からの制御信号に基づき、インターネットや無線ＬＡＮ（Local Area Network）等の通信ネットワークと接続する。これにより、ネットワーク接続装置３９は、実行プログラムやソフトウェア、設定情報等を、通信ネットワークに接続されている外部装置等から取得する。また、ネットワーク接続装置３９は、プログラムを実行することで得られた実行結果又は本実施形態における実行プログラム自体を外部装置等に提供することができる。

記録媒体４０は、各種の実行プログラムやデータ等をコンピュータで読み取り可能な記録媒体である。記録媒体４０は、例えばＵＳＢ（Universal Serial Bus）メモリやＣＤ−ＲＯＭ等の可搬型の記録媒体であってもよく、フラッシュメモリ等の半導体メモリであってもよい。

本実施形態では、上述したコンピュータ本体のハードウェア構成に実行プログラム（例えば、位置姿勢計測プログラム等）をインストールすることで、ハードウェア資源とソフトウェアとが協働して本実施形態における位置姿勢計測処理等を実現することができる。また、上述した位置姿勢計測処理に対応する位置姿勢計測プログラムは、例えば装置上で常駐している状態であってもよく、起動指示により起動させてもよい。

＜位置姿勢計測処理例＞
次に、本実施形態における位置姿勢計測処理の一例について、フローチャートを用いて説明する。図３は、本実施形態における位置姿勢計測処理の一例を示すフローチャートである。図３の例において、撮像部１１は、予め設定されたカメラパラメータ等により所定のポーズ等で撮影された画像を取得する（Ｓ０１）。Ｓ０１の処理では、撮像部１１は、例えば、情報処理装置１０の周囲を撮影し、所定時間間隔における時系列の画像フレームを取得する。次に、画像特徴点抽出部１２は、Ｓ０１の処理で得られた画像から特徴点（画像特徴点）を抽出する（Ｓ０２）。次に、画像特徴点追跡部１３は、連続する画像フレーム間において、Ｓ０２の処理で得られた画像特徴点を追跡する（Ｓ０３）。

次に、位置姿勢計測部１５は、撮像部１１、又は、撮像部１１を有する情報処理装置１０の位置姿勢の計測を行う（Ｓ０４）。なお、Ｓ０４の処理では、後述するＳ０６の処理において抽出した制御点情報を用いて位置姿勢を計測することができる。

次に、環境地図生成部１６は、画像特徴点情報２１、Ｓ０４の処理で得られる位置姿勢情報２２等を用いて環境地図生成処理を行う（Ｓ０５）。なお、Ｓ０５の処理では、環境地図２３の生成だけでなく、環境地図の更新（例えば、補正、削除、拡張）等を行うこともできる。

次に、制御点情報抽出部１７は、環境地図２３等を用いて制御点情報抽出処理を行う（Ｓ０６）。Ｓ０６の処理で抽出された制御点情報２４を用いて、その位置姿勢計測等が行われる。次に、情報処理装置１０は、位置姿勢計測処理を終了するか否かを判断し（Ｓ０７）、処理を終了しない場合（Ｓ０７において、ＮＯ）、Ｓ０１の処理に戻る。また、情報処理装置１０は、ユーザ操作による終了指示により処理を終了する場合（Ｓ０７において、ＹＥＳ）、本実施形態における位置姿勢計測処理を終了する。

＜Ｓ０４；位置姿勢計測処理＞
次に、上述した位置姿勢計測部１５における撮像部１１を用いた位置姿勢計測処理（Ｓ０４）について具体的に説明する。図４は、位置姿勢計測処理を説明するための図である。なお、図４に示す撮像部１１の形状等については、これに限定されるものではない。

本実施形態における位置姿勢計測処理は、図４に示すように、「撮像部１１の座標系（カメラ座標系）とワールド座標系とを変換する回転行列Ｒ」、「カメラ座標系とワールド座標系とを変換する平行移動ベクトルｔ」の各パラメータを求める処理である。位置姿勢計測部１５は、ワールド座標系からカメラ座標系への変換行列を、位置姿勢の推定により求める。

ここで、Ｒ、ｔは、それぞれ以下の行列式（（１）式）で表される。

また、これらのパラメータを求めるために、「画像内にある特徴点のワールド座標系での３次元座標ｘ_ｉ ^ｗ」、「撮像部１１の内部パラメータ（カメラパラメータ）Ｐ」を利用する。ｘ_ｉ ^ｗ、Ｐは、それぞれ以下に示す（２）式、（３）式で表される。

なお、上述した（３）式において、ｆ_ｘはＸ方向の焦点距離を示し、ｆ_ｙはＹ方向の焦点距離を示し、ｏ_ｘはＸ方向の画像中心を示し、ｏ_ｙはＹ方向の画像中心を示す。

特徴点の３次元座標については、上述した環境地図生成処理で予め求めておく。また、カメラの内部パラメータについては、例えばカメラキャリブレーションによって予め求めておいてもよい。カメラキャリブレーションについては、例えばZhangの手法（Z.Zhang,"A flexible new technique for camera calibration",IEEE Transactions on Pattern Analysis and Machine Intelligence,22(11):1330-1334,2000）等を用いることができるが、これに限定されるものではない。

また、本実施形態において、位置姿勢計測部１５は、上述の処理以外に、撮像部１１の歪み補正を行ってもよい。なお、歪みのパラメータについてもZhangの手法で得ることができる。また、歪み補正については、特徴点抽出を行う前に画像自体の歪み補正を行ってもよく、特徴点を抽出した後に、歪みのパラメータ等を用いて補正してもよい。位置姿勢計測部１５は、上述した何れかの処理を行った後に、上述した位置姿勢の推定を行うことで、より高精度に撮像部１１又は情報処理装置１０の位置姿勢を計測することができる。

また、位置姿勢計測部１５は、上述したパラメータを利用して、再投影誤差を最小化にするＲ，ｔを求める。再投影誤差は、画像特徴点の３次元点を画像上に投影した点の座標と、実際にそれが観測された座標との差分により定義される。再投影座標は、例えば「３次元点のカメラ座標系への変換」及び「仮想スクリーンへの投影」により計測することができる。

３次元点のカメラ座標系への変換は、以下に示す（４）式で求めることができる。

なお、上述した（４）式において、カメラ座標系での３次元点の座標をｘ_ｉ ^ｃとし、ワールド座標系での３次元点の座標をｘ_ｉ ^ｗとして定義している。上述した（４）式により、カメラの座標系での３次元点の座標を得ることができる。また、仮想スクリーンへの３次元点の投影を再投影と呼び、以下に示す（５）式で求めることができる。

ここで、再投影点の座標をｘ_ｉ ^ｒ、ｙ_ｉ ^ｒとする。再投影誤差は、最投影点の座標、及び、観測座標の２乗誤差の総和として、以下に示す（６）式で定義される。

上述した（６）式において、ｎはあるフレームで撮影された３次元座標が既知の特徴点の点数である。ポーズ推定は、この再投影誤差を最小化するＲとｔを求める問題であり、以下に示す（７）式のように定式化することができる。

この最小化問題は、ＰｎＰ問題として知られており、この式をそのまま解くと非線形最小化問題となる。一般に、非線形最小化は、計算コストが高く、また適切な初期値を設定しないと適切な解が求まらないという課題がある。

そこで、本実施形態では、この問題を線形化して解くＥＰｎＰアルゴリズムを用いる。ＥＰｎＰアルゴリズムでは、４点の制御点を利用し、以下に示す（８）式のように、ワールド座標系での３次元点の座標を制御点Ｃ^ｗ _１，Ｃ^ｗ _２，Ｃ^ｗ _３，Ｃ^ｗ _４の重み付き和で、各点の座標を表現する。

上述した（８）式において、α_ｉ，ｊは３次元点ｉ、制御点ｊについての重みパラメータである。制御点は、任意の４点を選ぶことができるが、一般的にはポーズ推定で用いる点群の座標の重心と、点群の座標の主成分から設定される。

また、カメラ座標系での３次元点の座標は、上述と同様に、カメラ座標系での制御点の座標Ｃ^ｃ _１，Ｃ^ｃ _２，Ｃ^ｃ _３，Ｃ^ｃ _４を用いて以下に示す（９）式のように表すことができる。

これを上記の再投影誤差の評価式を用いると、以下に示す（１０）式のように、３次元点１点について２つの方程式を得ることができる。

したがって、画像フレームからｎ点の制御点が観測されれば、以下に示す（１１）式のように２ｎ行×１２列の行列となる。

ここで、上述した（１１）式のうち、左項の行列をＭとし、右項をｘとする。Ｍは、ランク落ちした行列であるため、Ｍを特異値分解し、特異値０に対応する右特異ベクトルの線形結合によってｘの解空間を求めることができる。

ここで、従来では、Ｍに対し左からＭ'をかけてＭ'Ｍについて特異値分解する手法が知られている。このＭ'Ｍの計算は、ＥＰｎＰアルゴリズムの中で最も計算コストのかかる処理である。上記の手法において、制御点を求めた後、Ｍを求めるために要する計算回数は、加減算が２ｎ回、乗算が８ｎ回となる。

また、Ｍ'Ｍの計算は、加減算７８×（２ｎ−１）回、乗算７８×２ｎ回となる。そのため、制御点を求めた後、Ｍ'Ｍの計算を行うまでには、加減算１５８ｎ−７８回、乗算１６４ｎ回を行う必要が生じる。

ＥＰｎＰは、ＰｎＰアルゴリズムの中では、計算量が少ないアルゴリズムであるが、計算機（情報処理装置１０）の環境によっては、更に計算量を少なくすることが望まれる。例えば、１０メガフロップス（ＭＦＬＯＰＳ）のＣＰＵにおいては、１０００個の制御点を利用してカメラのポーズ推定を行った場合、この計算に３２ｍｓｅｃの時間が必要となる。したがって、３０ｆｐｓ（frame per second）のカメラ（撮像部１１）を利用したとしても処理が追いつかない。そのため、上記の例では、３０Ｈｚでカメラのポーズ推定を行えないことになる。

また、処理時間を短縮するために、計算機環境をより高性能なものするか、Ｍ'Ｍの計算量を小さくするか、或いは、ポーズ推定に利用する点の数を減らす必要が生じる。計算機環境を高性能にした場合には、コストの増加や消費電力の増加が懸念される。また、ポーズ推定に利用する点の数を減らした場合には、ポーズ推定の精度やロバスト性が低下する可能性がある。そのため、処理時間を短縮するためには、Ｍ'Ｍの計算量を小さくして計算コストを低減することが望ましい。

そこで、本実施形態では、上述したＭの性質を利用して、Ｍ'Ｍの計算コストを下げる。ここで、Ｍ'Ｍを以下に示す（１２）式のように定義する。

上述した（１２）式において、１６個の各要素（ｍ_１１，ｍ_１２，ｍ_１３，ｍ_１４，ｍ_２１，ｍ_２２，ｍ_２３，ｍ_２４，ｍ_３１，ｍ_３２，ｍ_３３，ｍ_３４，ｍ_４１，ｍ_４２，ｍ_４３，ｍ_４４）は、それぞれＭ'Ｍの３×３の小行列を有する。この小行列は、Ｍ'Ｍの上からＫ番目左からＬ番目の小行列をｍ_ｋｌとすると、Ｍ'Ｍの対称性からｍ_ｋｌ＝ｍ_ｌｋという性質を持っている。また、ｍ_ｋｌの各要素は、以下に示す（１３）式のように、ｍ_ｋｌ自体も対称行列となる。

つまり、位置姿勢計測部１５は、上述した重みパラメータで表現された３次元点で、撮像部１１により撮影した画像から抽出された点について、各重みパラメータと、撮像部１１の焦点距離と、画像中心座標とを用いて、１２×１２の行列（所定の行列の一例）を生成する。また、位置姿勢計測部１５は、生成した１２×１２の行列を区分した１６個の３×３の小行列の上三角或いは下三角の要素の値を求める。また、位置姿勢計測部１５は、求めた要素の値に基づく小行列の対称性及び１２×１２の行列の対称性から、１２×１２の行列の要素を設定し、設定された１２×１２の行列を用いて位置姿勢の計測を行う。これにより、ＰｎＰ問題をＥＰｎＰアルゴリズムで解く方法において、１２×１２の行列を求める計算の計算量を従来手法よりも減らすことができる。

例えば、上述した（１３）式の行列ｍ_ｋｌの各要素の算出に要する加減算と乗算の数は、以下のようになる。
１行１列目：加減算ｎ−１回、乗算ｎ＋２回
１行２列目：加減算０回、乗算０回
１行３列目：加減算２ｎ−１回、乗算２ｎ＋１回
２行２列目：加減算ｎ−１回、乗算ｎ＋２回
２行３列目：加減算２ｎ−１回、乗算２ｎ＋１回
３行３列目：加減算４ｎ−１回、乗算４ｎ回
これらを合計すると、加算１０ｎ−５回、乗算６ｎ＋６回となる。これをｍ_１１，ｍ_１２，ｍ_１３，ｍ_１４，ｍ_２２，ｍ_２３，ｍ_２４，ｍ_３３，ｍ_３４，ｍ_４４の１０個の行列について計算すればよいため、トータルの計算量は、加減算１００ｎ−５０回、乗算６０ｎ＋６０回で行えることになる。そのため加減算と乗算の計算量の和は、上述した従来手法と比較して、約５０％程度の計算量を減らすことができる。

更に、本実施形態では、α_ｉｋ＊α_ｉｌと、ｄｘ_ｉと、ｄｙ_ｉと、ｄｘ_ｉ ^２＋ｄｙ_ｉ ^２とを予め求めておくことで、計算量を更に減らすことができる。

ここで、α_ｉｋ＊α_ｉｌを２次の重みパラメータ（２次重み）とする。α_ｉ１＊α_ｉ１，α_ｉ１＊α_ｉ２，α_ｉ１＊α_ｉ３，α_ｉ１＊α_ｉ４，α_ｉ２＊α_ｉ２，α_ｉ２＊α_ｉ３，α_ｉ２＊α_ｉ４，α_ｉ３＊α_ｉ３，α_ｉ３＊α_ｉ４，α_ｉ４＊α_ｉ４の１０個の２次重みの計算についてｎ点の計算量は、乗算１０ｎ回である。また、ｄｘ_ｉとｄｙ_ｉの計算は、加減算２ｎ回で行うことができる。また、ｄｘ_ｉ ^２＋ｄｙ_ｉ ^２の計算は、乗算２ｎ回と和ｎ回で計算できる。

これらを予め求めておくことで、ｍ_ｋｌの各要素の算出に要する加減算と乗算の数は、以下のようになる。
１行１列目：加減算ｎ−１回、乗算２回
１行２列目：加減算０回、乗算０回
１行３列目：加減算ｎ−１回、乗算ｎ＋１回
２行２列目：加減算ｎ−１回、乗算２回
２行３列目：加減算ｎ−１回、乗算ｎ＋１回
３行３列目：加減算ｎ−１回、乗算ｎ回
これらを合計すると、１つの小行列を計算するのに要する計算量は、加減算５ｎ−５回、乗算２ｎ＋６回となる。この計算を、小行列１０個について行うため、行列についての計算は、加減算５０ｎ−５０回、乗算２０ｎ＋６０回となる。

これを、予め求めたα_ｉｋ＊α_ｉｌと、ｄｘ_ｉと、ｄｙ_ｉと、ｄｘ_ｉ ^２＋ｄｙ_ｉ ^２の計算量と合わせると、加減算５３ｎ−５０回、乗算３４ｎ＋６０回で計算することができる。

これにより、本実施形態では、上述した従来手法と比較して約３０％程度の計算量になる。

上述したように、位置姿勢計測部１５は、２次の重みパラメータで表現された３次元点で、撮像部１１により撮影した画像から抽出された点について、各２次の重みパラメータと、撮像部１１の焦点距離と、画像中心座標とを用いて、１２×１２の行列（所定の行列の一例）を生成する。また、位置姿勢計測部１５は、生成した１２×１２の行列を区分した１６個の３×３の小行列の上三角或いは下三角の要素の値を求める。また、位置姿勢計測部１５は、求めた要素の値に基づく小行列の対称性及び１２×１２の行列の対称性から、１２×１２の行列の要素を設定し、設定された１２×１２の行列を用いて位置姿勢の計測を行う。これにより、ＰｎＰ問題をＥＰｎＰアルゴリズムで解く方法において、１２×１２の行列を求める計算の計算量を従来手法よりも減らすことができる。

＜Ｓ０５；環境地図生成処理＞
次に、上述した環境地図生成部１６における環境地図生成処理（Ｓ０５）について、フローチャートを用いて説明する。図５は、環境地図生成処理の一例示すフローチャートである。図５の例において、環境地図生成部１６は、環境地図全体の再投影誤差の最小化処理を行う（Ｓ１１）。Ｓ１１の再投影誤差の最小化処理は、バンドル調整ともいう。Ｓ１１の処理では、例えば、過去に撮影したＮ_ｊ個の撮影ポーズ（Ｒ_ｊ，ｔ_ｊ）とＮ_ｉ個の３次元点の座標ｘ_ｉをパラメータとして、再投影誤差が最小となるポーズとパラメータとを求める処理である。この問題は、例えば非線形の最小二乗問題に定式化することができる。したがって、これらの最適パラメータは、例えばＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法（ＬＭ法）等を用いて求めることができるが、これに限定されるものではない。

次に、環境地図生成部１６は、複数回の追跡に成功した特徴点（画像特徴点）を環境地図２３に登録する（Ｓ１２）。Ｓ１２の処理は、例えば３次元再構成処理と、信頼度計測処理とを有する。上述した、３次元再構成処理とは、例えば画像特徴点の３次元位置を求める処理である。３次元位置の算出は、例えば撮像部１１がステレオカメラである場合、そのステレオの測距情報から求めることができる。また、３次元再構成処理は、撮像部１１が単眼カメラである場合、２つの単眼カメラを用いてＤＬＴ（Direct Linear Transformation）法等を用いて３次元位置を求めることができる（ＤＬＴ法を用いた手法については、例えば「佐藤智和、"複数の動画像を用いたカメラパラメータ推定に基づく屋外環境の三次元モデル化に関する研究"、奈良先端科学技術大学院大学、博士論文、2003年3月24日」等を参照）。

また、上述した信頼度計測処理とは、再投影誤差のフィッシャー（Fisher）情報量の逆数を信頼度として、これが閾値より高い３次元点を信頼できる点として環境地図２３に登録する処理である。なお、上述の閾値は、予め設定された値としてもよく、また環境地図２３への追加候補点の全点のうち、上位から所定数の点を環境地図２３に追加するようにしてもよい。上述したような追加する点の抽出条件は、処理毎に設定してもよい。つまり、Ｓ１２の処理において、環境地図生成部１６は、３次元再構成処理に成功し、かつ信頼度計測により信頼度が高い点のみを環境地図２３に登録する。

次に、環境地図生成部１６は、環境地図２３にあるアウトライアの三次元点を削除する（Ｓ１３）。Ｓ１３の処理では、環境地図２３にある全点について、上述した信頼度を算出し、算出した信頼度が閾値より小さい点を環境地図２３から削除する。この閾値についても予め設定された値としてもよく、環境地図２３にある３次元点の全点のうち、下位から所定数の点を環境地図２３から削除してもよい。上述したような削除する点の抽出条件は、処理毎に設定してもよい。

＜Ｓ０６；制御点情報抽出処理＞
次に、上述した制御点情報抽出部１７における制御情報抽出処理（Ｓ０６）について、フローチャートを用いて説明する。図６は、制御情報抽出処理の一例を示すフローチャートである。図６の例において、制御点情報抽出部１７は、環境地図２３から３次元点群を抽出する（Ｓ２１）。Ｓ２１の処理では、制御点情報抽出部１７は、環境地図２３にある３次元点の中から次の撮影で観測される可能性が高い点を選択する。この処理は、例えば直前のカメラ（撮像部１１）のポーズと、カメラパラメータと、３次元点の座標とから再投影を行い、画像の範囲内の点が再投影されたか否かにより判定することができる。また、他の手法として、制御点情報抽出部１７は、直前の撮影で写っていた３次元点を、次の撮影で観測される可能性が高い点として選択してもよい。また、制御点情報抽出部１７は、ＰｎＰ問題の計算コストの上限を定めるために、抽出する３次元点の個数の上限を決めて、所定数の３次元点群を抽出してもよい。

次に、制御点情報抽出部１７は、抽出した３次元点群から制御点情報２４を抽出する（Ｓ２２）。制御点情報２４は、４つの制御点の座標と、上述した（８）式に示す各３次元点の４つの重みパラメ−タ、又は、上述した１０個の２次重みを含む情報である。

Ｓ２２の処理では、制御点情報抽出部１７は、Ｓ２１の処理により抽出した３次元点群の情報から制御点を４点選択する。この選択手法は、点群の点の重心１点と、点群の主成分と、その重みとから残りの３点を選ぶ手法が既に知られており、この手法を用いることができるが、これに限定されるものではない。

次に、制御点情報抽出部１７は、抽出した制御点情報２４に基づいて、重み情報（重みパラメータ）を算出する（Ｓ２３）。Ｓ２３の処理では、制御点情報抽出部１７は、上述した（８）式に示す各３次元点の４つの重みパラメ−タを重み情報とする場合、上述した（８）式に示す計算を、抽出された３次元点について行う。この場合、制御点情報２４は、環境地図２３に含まれるＮ点の３次元点（点群）から得た少なくとも４点の制御点の座標と、Ｎ点の各点の少なくとも４つの重みパラメータで表現した情報となる。これにより、ＰｎＰ問題をＥＰｎＰアルゴリズムで解く方法において、制御点の抽出と、各点を制御点の線形結合で表現した重みを求める処理をなくし、ＰｎＰ問題の解法の計算コストを減らすことができる。

また、制御点情報抽出部１７は、上述した１０個の２次重みパラメータを重み情報として定義している場合、抽出された３次元点について、２次重みを算出する。この場合、制御点情報２４は、環境地図２３に含まれるＮ点の３次元点から得た少なくとも４点の制御点の座標と、各点の重みパラメータから２つを有する重複の組み合わせの少なくとも１０通りに対して重みパラメータを乗算した２次重みとで表現した情報となる。これにより、上述したようにＰｎＰ問題をＥＰｎＰアルゴリズムで解く方法において、ＰｎＰ問題の解法の計算量を減らすことができる。

なお、上述した例では、環境地図２３が３次元点群を有しているが、環境地図２３が２次元点群であってもよい。この場合、制御点情報は、環境地図２３に含まれるＮ個の特徴点から得られた複数（少なくとも２点）の制御点の座標と、制御点に対応する重みパラメータとで表現した情報であってもよい。また、制御点情報は、環境地図２３に含まれるＮ個の特徴点から得られた複数の制御点の座標と、制御点に対応する重みパラメータのうち２つを有する重複の組み合わせに基づき生成された２次の重みパラメータとで表現した情報であってもよい。

＜制御点情報を抽出するタイミングについて＞
次に、上述した制御点情報を抽出するタイミングについて、図を用いて説明する。図７は、制御点情報を抽出するタイミングを説明するための図である。撮像部１１により撮像される動画像（時系列の画像フレーム）において、一般的に、連続して撮像された２つの画像（例えば、時刻ｔ−１で撮影した画像と、時刻ｔで撮影した画像）から観測される３次元点は、重複していることが多い。そのため、撮像部１１で撮影する度に、制御点を設定し直さなくても、精度への影響は少ない。そこで、本実施形態では、所定の条件を満たす場合に制御点情報を抽出し、抽出した内容を次の位置姿勢計測時に利用する。

例えば、情報処理装置１０は、図７に示すように、撮像部１１における撮像処理を所定のタイミングで行う。また、情報処理装置１０は、その撮影毎に画像の特徴点を抽出すると共に、またフレーム間において画像特徴点追跡部１３による特徴点の追跡を行う。また、情報処理装置１０は、位置姿勢計測部１５による位置姿勢計測処理を行い、計測処理結果に基づき、環境地図生成部１６による環境地図２３の生成を行う。なお、環境地図２３の生成は、必要に応じて更新（例えば、補正、削除、拡張）等を行ってもよい。また、環境地図を更新した場合は、それに対応して制御点情報も更新される。

ここで、情報処理装置１０は、所定の条件を満たす場合に、制御点情報抽出部１７による制御点情報２４の抽出を行う。所定の条件は、例えば画像特徴点抽出部１２により新規の特徴点が抽出された場合、制御点情報を抽出する処理を実行してから撮像部１１による撮影で一定の画像フレーム数が経過した場合、環境地図に更新があった場合、画像特徴点抽出部１２で抽出した特徴点の座標に変化があった場合等である。なお、所定の条件は、上述した各条件のうち、少なくとも１つを満たしていればよい。また、所定の条件については、上述した各条件に限定されるものではない。

情報処理装置１０は、制御点情報抽出部１７により、上述した所定の条件を満たすタイミングで制御点情報の抽出（制御点の再設定）を行う。つまり、本実施形態では、例えばＥＰｎＰアルゴリズムを用いてＰｎＰ問題を解く方法において、３次元点の座標が変わらない限りは、ＥＰｎＰアルゴリズムで利用する４点の制御点の座標・重み行列等の制御点情報を更新しないで求めておく。これにより、情報処理装置１０は、例えば制御点の座標と、制御点の重みα_ｉｋと、制御点の２次重みの値とを、複数のフレーム間で一定にすることができる。

例えば、情報処理装置１０は、画像フレームに含まれる３次元点の総数がＮ点であった場合、制御点での重み付き表現で上述したα_ｉｋ＊α_ｉｌを求めておき、その中で実際に観測されたｎ点について、上述した処理を行ってもよい。これにより、情報処理装置１０は、α_ｉ１＊α_ｉ１，α_ｉ１＊α_ｉ２，α_ｉ１＊α_ｉ３，α_ｉ１＊α_ｉ４，α_ｉ２＊α_ｉ２，α_ｉ２＊α_ｉ３，α_ｉ２＊α_ｉ４，α_ｉ３＊α_ｉ３，α_ｉ３＊α_ｉ４，α_ｉ４＊α_ｉ４の１０個の２次重みの計算について、ｎ点の計算量を乗算で１０ｎ回減らすことができる。

これにより、情報処理装置１０は、位置姿勢推定の精度を維持しつつ、計算コストを低減する（計算量を減らす）ことができる。

＜まとめ＞
上述したように、本実施形態によれば、環境地図の生成及び位置姿勢の計測における計算コストを低減することができる。例えば、本実施形態によれば、地図生成における精度を維持しつつ、ＰｎＰ問題を少ない計算量で解くことができる。また、本実施形態では、ＰｎＰ問題の解法の１つであるＥＰｎＰアルゴリズムにおいて、ＥＰｎＰアルゴリズムで利用する４点の制御点の座標・重み行列等の制御点情報の抽出を、画像撮影の度に行うのではなく、３次元点の座標に変化があった時にのみ求めておく。これにより、ＰｎＰ問題の解法の処理量を減らすことができる。したがって、本実施形態では、例えば計算能力の低いプロセッサであっても、リアルタイムでカメラポーズの推定等を行うＳＬＡＭ処理を実現することができる。

以上、本発明の好ましい実施形態について詳述したが、本発明は係る特定の実施形態に限定されるものではなく、特許請求の範囲に記載された本発明の要旨の範囲内において、種々の変形、変更が可能である。

また、上述した各実施形態で述べたフローチャートは、矛盾の無い限り順序を入れ替えてもよい。また、上述した各実施形態の全部又は一部を組み合わせることができる。また、上述した各実施形態の全部又は一部は、対応するプログラム（例えば、位置姿勢計測プログラム）をコンピュータ（例えば、情報処理装置１０等）にインストールすることによって実装され得る。また、このプログラムは、記録媒体（記録メディア）に格納することができる。

１０ 情報処理装置
１１ 撮像部（撮像手段の一例）
１２ 画像特徴点抽出部（特徴点抽出手段の一例）
１３ 画像特徴点追跡部（特徴点追跡手段の一例）
１４ 記憶部（記憶手段の一例）
１５ 位置姿勢計測部（位置姿勢計測手段の一例）
１６ 環境地図生成部（環境地図生成手段の一例）
１７ 制御点情報抽出部（制御点情報抽出手段の一例）
２１ 画像特徴点情報
２２ 位置姿勢情報
２３ 環境地図
２４ 制御点情報
３１ 入力装置
３２ 出力装置
３３ ドライブ装置
３４ カメラ
３５ 位置計測装置
３６ 補助記憶装置
３７ 主記憶装置
３８ ＣＰＵ
３９ ネットワーク接続装置
４０ 記録媒体

特開２０１４−２２２５５１号公報

Claims (9)

1. 時系列の画像を撮影する撮像手段と、
   前記撮像手段により撮影した画像から特徴点を抽出する特徴点抽出手段と、
   前記撮像手段で撮影した画像に対して前記特徴点抽出手段により抽出した特徴点の追跡を行う特徴点追跡手段と、
   前記特徴点抽出手段により抽出した特徴点から、位置又は姿勢を推定する基準となる制御点情報を抽出する制御点情報抽出手段と、
   前記特徴点追跡手段により追跡した画像上での特徴点の座標と、前記制御点情報抽出手段により抽出した制御点情報とから前記撮像手段における位置又は姿勢を計測する位置姿勢計測手段と、
   前記位置姿勢計測手段により得られた前記撮像手段の位置又は姿勢と、前記画像上の特徴点の座標とに基づいて環境地図を生成する環境地図生成手段と、
   を有することを特徴とする情報処理装置。
2. 前記制御点情報は、前記環境地図に含まれるＮ個の特徴点から得られた複数の制御点の座標と、前記制御点に対応する重みパラメータとで表現した情報であることを特徴とする請求項１に記載の情報処理装置。
3. 前記位置姿勢計測手段は、
   前記重みパラメータで表現された３次元点で、前記撮像手段により撮影した画像から抽出された点について、前記重みパラメータと、前記撮像手段の焦点距離と、画像中心座標とを用いて、所定の行列を生成し、生成した行列を区分した小行列の対称性及び前記所定の行列の対称性から、前記所定の行列の要素を設定して前記位置又は姿勢の計測を行うことを特徴とする請求項２に記載の情報処理装置。
4. 前記制御点情報は、前記環境地図に含まれるＮ個の特徴点から得られた複数の制御点の座標と、前記制御点に対応する重みパラメータのうち２つを有する重複の組み合わせに基づき生成された２次の重みパラメータとで表現した情報であることを特徴とする請求項１に記載の情報処理装置。
5. 前記位置姿勢計測手段は、
   前記２次の重みパラメータで表現された３次元点で、前記撮像手段により撮影した画像から抽出された点について、前記２次の重みパラメータと、前記撮像手段の焦点距離と、画像中心座標とを用いて、所定の行列を生成し、生成した行列を区分した小行列の対称性及び前記所定の行列の対称性から、前記所定の行列の要素を設定して前記位置又は姿勢の計測を行うことを特徴とする請求項３に記載の情報処理装置。
6. 前記制御点情報抽出手段は、
   所定の条件を満たす場合に、前記制御点情報を抽出することを特徴とする請求項１乃至５の何れか１項に記載の情報処理装置。
7. 前記所定の条件は、
   前記特徴点抽出手段により新規の特徴点が抽出された場合、前記制御点情報を抽出してから前記撮像手段による撮影で一定の画像フレーム数が経過した場合、前記環境地図に更新があった場合、及び、前記特徴点抽出手段により抽出した特徴点の座標に変化があった場合のうち、少なくとも１つを含むことを特徴とする請求項６に記載の情報処理装置。
8. 撮像手段により撮影された時系列の画像を取得する画像取得ステップと、
   前記画像取得ステップにより取得した画像から特徴点を抽出する特徴点抽出ステップと、
   前記撮像手段から取得した画像に対して前記特徴点抽出ステップにより抽出した特徴点の追跡を行う特徴点追跡ステップと、
   前記特徴点抽出ステップにより抽出した特徴点から、位置又は姿勢を推定する基準となる制御点情報を抽出する制御点情報抽出ステップと、
   前記特徴点追跡ステップにより追跡した画像上での特徴点の座標と、前記制御点情報抽出ステップにより抽出した制御点情報とから前記撮像手段における位置又は姿勢を計測する位置姿勢計測ステップと、
   前記位置姿勢計測ステップにより得られた前記撮像手段の位置又は姿勢と、前記画像上の特徴点の座標とに基づいて環境地図を生成する環境地図生成ステップと、
   を有することを特徴とする位置姿勢計測方法。
9. コンピュータを、
   時系列の画像を撮影する撮像手段、
   前記撮像手段により撮影した画像から特徴点を抽出する特徴点抽出手段、
   前記撮像手段で撮影した画像に対して前記特徴点抽出手段により抽出した特徴点の追跡を行う特徴点追跡手段、
   前記特徴点抽出手段により抽出した特徴点から、位置又は姿勢を推定する基準となる制御点情報を抽出する制御点情報抽出手段、
   前記特徴点追跡手段により追跡した画像上での特徴点の座標と、前記制御点情報抽出手段により抽出した制御点情報とから前記撮像手段における位置又は姿勢を計測する位置姿勢計測手段、及び、
   前記位置姿勢計測手段により得られた前記撮像手段の位置又は姿勢と、前記画像上の特徴点の座標とに基づいて環境地図を生成する環境地図生成手段、
   として機能させるための位置姿勢計測プログラム。

Images