JP7249919B2 - 推定装置、推定方法及びプログラム - Google Patents

Description

本発明の実施形態は推定装置、推定方法及びプログラムに関する。

移動体に設置されたカメラ等で撮影した画像から、撮像時のカメラ、つまり移動体の位置を推定する推定装置が知られている。移動体の自己位置推定は種々の原因で失敗することがある。このとき、移動体は自己位置推定に失敗してもその事実を認識することなくそのまま移動を続ける。そのため、移動体が、例えば本来移動不可能な領域に侵入する可能性がある。また、自己位置推定に失敗した場合、再び推定が成功するまでの間の区間は「何も推定されていない」区間となり、これにより移動体の移動軌跡が途切れることがある。

特開２０１７－２０７９４２号公報 特許第６０６１７７０号公報 特許第６２４０５９５号公報

しかしながら、従来の技術では、カメラ以外のセンサを用いずに、移動軌跡を途切れさせることなく自己位置を推定することが難しかった。

実施形態の推定装置は、取得部と第１の推定部と第１の算出部と第２の推定部と第２の算出部と判定部とを備える。取得部は、カメラによって撮像された画像を取得する。第１の推定部は、前記画像から前記カメラの位置及び姿勢の少なくとも一方を示す第１の推定情報を推定する。第１の算出部は、前記第１の推定情報の信頼度を示す第１の信頼度を算出する。第２の推定部は、前記カメラの位置及び姿勢の少なくとも一方を示す第２の推定情報を推定する。第２の算出部は、前記第２の推定情報の信頼度を示す第２の信頼度を算出する。判定部は、前記第１及び第２の信頼度に基づき、前記第１及び第２の推定情報から前記カメラの位置及び姿勢の少なくとも一方を決定し、決定された位置及び姿勢の少なくとも一方を判定処理結果として出力する。前記第２の推定部は、前記判定部による過去の判定処理結果に基づいて前記第２の推定情報を推定する。

第１実施形態の移動体の例を示す図。 第１実施形態の移動体の機能構成の例を示す図。 第１実施形態の推定方法の例を示すフローチャート。 第２実施形態の推定装置の処理部の機能構成の例を示す図。 第２実施形態の推定方法の例を示すフローチャート。 第３実施形態の推定装置の処理部の機能構成の例を示す図。 第３実施形態の推定方法の例を示すフローチャート。 第１乃至第３実施形態の推定装置のハードウェア構成の例を示す図。

以下、添付図面を参照して実施形態の推定装置、推定方法及びプログラムについて説明する。

（第１実施形態）
第１実施形態の推定装置は、例えば移動体に搭載される。

［移動体の例］
図１は第１実施形態の移動体１０の例を示す図である。

移動体１０は、推定装置２０、出力部１０Ａ、カメラ１０Ｂ、センサ１０Ｃ、動力制御部１０Ｇ及び動力部１０Ｈを備える。

移動体１０は任意でよい。移動体１０は、例えば車両、台車及び移動ロボット等である。車両は、例えば自動二輪車、自動四輪車及び自転車等である。また、移動体１０は、例えば人による運転操作を介して走行する移動体でもよいし、人による運転操作を介さずに自動的に走行（自律走行）可能な移動体でもよい。

推定装置２０は、例えば専用又は汎用のコンピュータハードウェアにより実現される。推定装置２０は、移動体１０の位置及び姿勢の少なくとも一方を示す自己位置を推定する。具体的には、推定装置２０は、複数の異なる方法で得られた自己位置推定結果に対してそれぞれ信頼度を算出し、予め設定した閾値との比較により、自己位置推定結果を選択又は統合する。これにより、移動体１０の追跡の途切れをなくすことが可能となる。

なお、推定装置２０は、移動体１０に搭載された形態に限定されない。推定装置２０は、静止物に搭載されていてもよい。静止物は、例えば地面に固定された物等の移動不可能な物である。地面に固定された静止物は、例えばガードレール、ポール、駐車車両及び道路標識等である。また例えば、静止物は、地面に対して静止した状態の物である。また、推定装置２０は、クラウドシステム上で処理を実行するクラウドサーバに搭載されていてもよい。

動力部１０Ｈは、移動体１０に搭載された駆動デバイスである。動力部１０Ｈは、例えば、エンジン、モータ及び車輪等である。

動力制御部１０Ｇは、動力部１０Ｈを制御する。動力制御部１０Ｇの制御によって動力部１０Ｈが駆動する。

出力部１０Ａは情報を出力する。第１実施形態では、出力部１０Ａは、推定装置２０により推定されたカメラ１０Ｂの運動の推定結果を示す推定結果情報を出力する。

出力部１０Ａは、例えば、推定結果情報を送信する通信機能、推定結果情報を表示する表示機能、及び、推定結果情報を示す音を出力する音出力機能等を備える。出力部１０Ａは、例えば通信部１０Ｄ、ディスプレイ１０Ｅ及びスピーカ１０Ｆの少なくとも１つを備える。なお、第１実施形態では、出力部１０Ａは、通信部１０Ｄ、ディスプレイ１０Ｅ及びスピーカ１０Ｆを備えた構成を例にして説明する。

通信部１０Ｄは、推定結果情報を他の装置へ送信する。例えば、通信部１０Ｄは、通信回線を介して推定結果情報を他の装置へ送信する。ディスプレイ１０Ｅは、推定結果に関する情報を表示する。ディスプレイ１０Ｅは、例えばＬＣＤ（Ｌｉｑｕｉｄ Ｃｒｙｓｔａｌ Ｄｉｓｐｌａｙ）、投影装置及びライト等である。スピーカ１０Ｆは、推定結果に関する情報を示す音を出力する。

カメラ１０Ｂは、例えば単眼カメラ、ステレオカメラ、魚眼カメラ及び赤外線カメラ等である。カメラ１０Ｂの数は任意でよい。また、撮像される画像はＲＧＢの３チャネルで構成されたカラー画像であってもよく、グレースケールで表現された１チャネルのモノクロ画像であってもよい。カメラ１０Ｂは、移動体１０周辺の時系列の画像を撮像する。カメラ１０Ｂは、例えば移動体１０の周辺を時系列に撮像することにより、時系列の画像を撮像する。移動体１０の周辺は、例えば当該移動体１０から予め定められた範囲内の領域である。この範囲は、例えばカメラ１０Ｂの撮像可能な範囲である。

第１実施形態では、カメラ１０Ｂが、移動体１０の前方を撮影方向として含むように設置されている場合を例にして説明する。すなわち、第１実施形態では、カメラ１０Ｂは、移動体１０の前方を時系列に撮像する。

センサ１０Ｃは、測定情報を測定するセンサである。測定情報は、例えば移動体１０の速度、並びに、移動体１０のハンドルの舵角を含む。センサ１０Ｃは、例えば慣性計測装置（ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）、速度センサ及び舵角センサ等である。ＩＭＵは、移動体１０の三軸加速度及び三軸角速度を含む測定情報を測定する。速度センサは、タイヤの回転量から速度を測定する。舵角センサは、移動体１０のハンドルの舵角を測定する。

次に、第１実施形態の移動体１０の機能構成の例について詳細に説明する。

［機能構成の例］
図２は第１実施形態の移動体１０の機能構成の例を示す図である。

移動体１０は、推定装置２０、出力部１０Ａ、カメラ１０Ｂ、センサ１０Ｃ、動力制御部１０Ｇ及び動力部１０Ｈを備える。推定装置２０は、処理部２０Ａ及び記憶部２０Ｂを備える。出力部１０Ａは、通信部１０Ｄ、ディスプレイ１０Ｅ及びスピーカ１０Ｆを備える。

処理部２０Ａ、記憶部２０Ｂ、出力部１０Ａ、カメラ１０Ｂ、センサ１０Ｃ及び動力制御部１０Ｇは、バス１０Ｉを介して接続されている。動力部１０Ｈは、動力制御部１０Ｇに接続されている。

なお、出力部１０Ａ（通信部１０Ｄ、ディスプレイ１０Ｅ及びスピーカ１０Ｆ）、カメラ１０Ｂ、センサ１０Ｃ、動力制御部１０Ｇ及び記憶部２０Ｂは、ネットワークを介して接続されていてもよい。接続に使用されるネットワークの通信方式は、有線方式であっても無線方式であってもよい。また、接続に使用されるネットワークは、有線方式と無線方式とを組み合わせることにより実現されていてもよい。

記憶部２０Ｂは情報を記憶する。記憶部２０Ｂは、例えば半導体メモリ素子、ハードディスク及び光ディスク等である。半導体メモリ素子は、例えばＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）及びフラッシュメモリ等である。なお、記憶部２０Ｂは、推定装置２０の外部に設けられた記憶装置であってもよい。また、記憶部２０Ｂは、記憶媒体であってもよい。具体的には、記憶媒体は、プログラムや各種情報を、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）やインターネットなどを介してダウンロードして記憶または一時記憶したものであってもよい。また、記憶部２０Ｂを、複数の記憶媒体から構成してもよい。

処理部２０Ａは、取得部２１、第１の推定部２２、第１の算出部２３、第２の推定部２４、第２の算出部２５、及び、判定部２６を備える。取得部２１、第１の推定部２２、第１の算出部２３、第２の推定部２４、第２の算出部２５、及び、判定部２６は、例えば１又は複数のプロセッサにより実現される。

処理部２０Ａは、例えばＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等のプロセッサにプログラムを実行させること、すなわちソフトウェアにより実現されてもよい。また例えば、処理部２０Ａは、専用のＩＣ（Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）等のプロセッサ、すなわちハードウェアにより実現されてもよい。また例えば、処理部２０Ａは、ソフトウェア及びハードウェアを併用することにより実現されてもよい。

なお、実施形態において用いられる「プロセッサ」の文言は、例えば、ＣＰＵ、ＧＰＵ（Ｇｒａｐｈｉｃａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、特定用途向け集積回路（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ：ＡＳＩＣ）、及び、プログラマブル論理デバイスを含む。プログラマブル論理デバイスは、例えば単純プログラマブル論理デバイス（Ｓｉｍｐｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ：ＳＰＬＤ）、複合プログラマブル論理デバイス（Ｃｏｍｐｌｅｘ Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ：ＣＰＬＤ）、及び、フィールドプログラマブルゲートアレイ（Ｆｉｅｌｄ Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ：ＦＰＧＡ）等を含む。

プロセッサは、記憶部２０Ｂに保存されたプログラムを読み出し実行することで、処理部２０Ａを実現する。なお、記憶部２０Ｂにプログラムを保存する代わりに、プロセッサの回路内にプログラムを直接組み込むよう構成してもよい。この場合、プロセッサは回路内に組み込まれたプログラムを読み出し実行することで、処理部２０Ａを実現する。

処理部２０Ａの各機能について説明する。取得部２１は、カメラ１０Ｂによって撮像された画像を取得する。

第１の推定部２２は、取得部２１から画像を受け付けると、当該画像からカメラ１０Ｂの位置及び姿勢の少なくとも一方を推定し、推定された位置及び姿勢の少なくとも一方を第１の推定情報として出力する。

第１の算出部２３は、第１の推定情報の信頼度を示す第１の信頼度を算出する。

第２の推定部２４は、判定部２６による過去の判定処理結果に基づいて、カメラ１０Ｂの位置及び姿勢の少なくとも一方を推定し、推定された位置及び姿勢の少なくとも一方を第２の推定情報として出力する。例えば、第２の推定部２４は、判定部２６により出力された過去の判定処理結果にカルマンフィルタリングを適用して、現在のカメラ１０Ｂの位置及び姿勢を線形補間することによって推定する。

第２の算出部２５は、第２の推定情報の信頼度を示す第２の信頼度を算出する。

判定部２６は、第１の信頼度と第２の信頼度とに基づき、第１の推定情報と第２の推定情報とから、カメラ１０Ｂの位置及び姿勢の少なくとも一方を決定し、決定された位置及び姿勢の少なくとも一方を判定処理結果として出力する。

［推定方法の例］
図３は第１実施形態の推定方法の例を示すフローチャートである。なお、図３の説明では、推定装置２０が、カメラ１０Ｂの位置及び姿勢の両方を推定する場合について説明するが、推定装置２０はカメラ１０Ｂの位置及び姿勢の一方を推定してもよい。すなわち、推定装置２０は、カメラ１０Ｂの位置及び姿勢の少なくとも一方を推定してもよい。後述する第２及び第３実施形態についても同様である。

はじめに、取得部２１が、少なくとも１つ以上のカメラ１０Ｂによって撮像された画像を取得する（ステップＳ１０１）。

次に、第１の推定部２２が、ステップＳ１０１の処理により撮像された画像を使用して、上述の第１の推定情報を推定する（ステップＳ１０２）。

具体的には、第１の推定部２２は、例えば画像中から複数個の特徴点を抽出して画像間で対応付けを行うことによって、それぞれの画像が撮像されたときのカメラ１０Ｂの位置及び姿勢を推定する。具体的には、移動体１０に設置されたカメラ１０Ｂから得られる複数の画像から、ＳｆＭ（Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ）を適用することにより、それぞれの画像が撮像されたときのカメラ１０Ｂの位置及び姿勢を推定可能である。

また例えば、第１の推定部２２は、カメラ１０Ｂによって取得された画像にＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）を適用することにより、画像を撮像したときのカメラ１０Ｂの位置及び姿勢を推定してもよい。具体的には、複数の特徴点のカメラ座標の組を用いて、基本行列や基礎行列、射影変換行列に基づいて位置及び姿勢を算出可能となる。複数の特徴点組は、画像間で特徴点の対応付けを行うことにより求めることが可能である。対応付けは、例えば、テンプレートマッチングや、Ｌｕｃａｓ－Ｋａｎａｄｅ法、ＳＩＦＴ等を用いることで実現可能である。なお、この場合は、絶対的なスケールを求めるために、並進ベクトルのノルムを１に正規化する等の処理を実施してもよいし、加速度センサやジャイロセンサによりスケールを決定してもよい。

また、カメラ１０Ｂの位置及び姿勢の表現方法は任意でよい。カメラ１０Ｂの位置及び姿勢は、例えば３次元空間中の座標と回転を表す４元数の組み合わせで表現してよい。また例えば、床面での移動等、移動が平面上で行われると仮定できる場合には、並進に関する２自由度と、回転に関する１自由度を合わせた３自由度で表現することができる。また例えば、三次元空間中の姿勢を表現するために、四元数のかわりに回転行列、回転軸と回転方向を表すベクトル、オイラー角などを用いてもよい。

また、カメラ姿勢のその他の推定方法として、特徴点を経由せずに推定する方法を用いてもよい。具体的な方法として、例えば入力画像から深度を推定するネットワークと、カメラ姿勢を推定するネットワークとを有する深層学習モデルによるカメラ姿勢推定を用いることで実現可能である。

次に、第１の算出部２３が、上述の第１の信頼度を算出する（ステップＳ１０３）。

第１の信頼度の算出方法は、例えば推定されたカメラ１０Ｂの姿勢の回転成分を４成分で表現したクォータニオンを用いることで実現可能である。このとき、回転成分のうちのｑｗ（＝ｃｏｓ＾２（θ／２）または１－ｑｗの値が高いほど信頼度が高いというように定義してよい。

また、推定されたカメラ１０Ｂの姿勢の並進成分を３変数で表現したトランスレーションを用いてもよく、その場合は、世界座標系におけるｘ方向とｚ方向に対する２次元での移動に着目し、移動量に基づいてフィッティングしたｎ次式の第１項の係数が大きければ第１の信頼度が高いというように定義してよい。

次に、判定部２６が、判定部２６による過去の判定処理結果があるか否かを判定する（ステップＳ１０４）。

過去の判定処理結果が存在しない場合（ステップＳ１０４，Ｎｏ）、判定部２６は、ステップＳ１０１の処理により取得された画像から直接、カメラ１０Ｂの位置及び姿勢を決定する（ステップＳ１０５）。カメラ１０Ｂの位置及び姿勢を決定する方法は、例えば第１の推定部２２の処理（ステップＳ１０２の処理）と同等でもよいし、第１の推定部２２の処理とは異なる方法を用いてもよい。例えば、判定部２６は、第１の推定部２２の処理（ステップＳ１０２の処理）と同じ方法を用いる場合、第１の推定情報によって、最終的なカメラ１０Ｂの位置及び姿勢を決定する。

過去の判定処理結果がある場合（ステップＳ１０４，Ｙｅｓ）、第２の推定部２４が、判定部２６による過去の判定処理結果を使用して、上述の第２の推定情報を推定する（ステップＳ１０６）。具体的には、第２の推定部２４は、過去の判定処理結果に含まれる位置及び姿勢を入力として、フィルタリングによって現在の位置及び姿勢を推定する。フィルタリングは、例えばカルマンフィルタリングを用いてもよい。第２の推定部２４は、過去の判定処理結果（例えば第１の推定部２２によって推定された前フレームまでのカメラ１０Ｂの位置及び姿勢が判定部２６により選択されていた場合は、当該位置及び姿勢）に対して、カルマンフィルタリングを適用し、現フレームに対するカメラの位置及び姿勢を線形補間によって算出してよい。

次に、第２の算出部２５が、上述の第２の信頼度を算出する（ステップＳ１０７）。

第２の信頼度の算出方法は、例えば推定したカメラ姿勢の回転成分を４成分で表現したクォータニオンを用いることで実現可能である。このとき、回転成分のうちのｑｗ（＝ｃｏｓ＾２（θ／２）または１－ｑｗの値が高いほど信頼度が高いというように定義してよい。また、推定したカメラ姿勢の並進成分を３変数で表現したトランスレーションを用いてもよく、その場合は、ＴｘとＴｚの移動量から得られる２次の近似式の第１項が大きければ信頼度が高いというように定義してよい。

次に、判定部２６が、第１の信頼度と第２の信頼度とに基づき、第１の推定情報と第２の推定情報とからカメラ１０Ｂの位置及び姿勢を決定する（ステップＳ１０８）。具体的には、判定部２６は、第１の信頼度および第２の信頼度に基づいて、どちらかの推定情報を判定処理結果（最終的な出力結果）としてもよい。判定部２６は、例えば第１の推定部１０２で推定が行われ、かつ、第１の信頼度が閾値（第１の閾値）より大きければ、第１の推定情報を最終的な出力結果にしてもよい。また例えば、判定部２６は、第１の信頼度が閾値以下の場合は第２の信頼度を参照し、第２の信頼度が閾値（第２の閾値）より大きければ、第２の推定情報を最終的な出力結果にしてもよい。また例えば、判定部２６は、第１の信頼度が閾値以下であり、かつ、第２の信頼度が閾値以下である場合、判定処理結果を出力しない。

また例えば、判定部２６は、第１の信頼度が閾値よりも大きく、かつ、第２の信頼度が閾値よりも大きい場合、第１の推定情報と第２の推定情報とを統合することによって得られた新たな位置及び姿勢によって、カメラ１０Ｂの位置及び姿勢を決定し、決定された位置及び姿勢を判定処理結果として出力する。

第１の推定情報と第２の推定情報とを統合する具体的な方法について説明する。例えば、判定部２６は、第１の信頼度と第２の信頼度とに基づく重み付き平均によって、第１の推定情報に含まれるカメラ１０Ｂの並進成分と、第２の推定情報に含まれるカメラ１０Ｂの並進成分とを統合する。また例えば、判定部２６は、第１の信頼度と第２の信頼度とに基づく球面線形補間によって、第１の推定情報に含まれるカメラ１０Ｂの回転成分と、第２の推定情報に含まれるカメラ１０Ｂの回転成分とを統合する。

なお、信頼度に対する閾値（判定閾値）はユーザが任意に定めてよく、例えば０．９９９７等の浮動小数点で表現される定数としてよい。また、第１の信頼度と第２の信頼度とで同一の閾値を定めてもよいし、それぞれ異なる閾値を定めてもよい。例えば第１の信頼度を主の推定方法とする場合は、第１の信頼度に対する閾値を低めに設定してよい。また第１の信頼度が閾値を超えない場合に、第２の推定部による推定情報を必ず出力結果として採用するように、第２の信頼度の閾値を０としてもよい。

以上、説明したように、第１実施形態の推定装置２０では、取得部２１は、カメラ１０Ｂによって撮像された画像を取得する。第１の推定部２２は、画像からカメラ１０Ｂの位置及び姿勢の少なくとも一方を示す第１の推定情報を推定する。第１の算出部２３は、第１の推定情報の信頼度を示す第１の信頼度を算出する。第２の推定部２４は、カメラ１０Ｂの位置及び姿勢の少なくとも一方を示す第２の推定情報を推定する。第２の算出部は、第２の推定情報の信頼度を示す第２の信頼度を算出する。判定部２６は、第１及び第２の信頼度に基づき、第１及び第２の推定情報からカメラ１０Ｂの位置及び姿勢の少なくとも一方を決定し、決定された位置及び姿勢の少なくとも一方を判定処理結果として出力する。第２の推定部２４は、判定部２６による過去の判定処理結果に基づいて第２の推定情報を推定する。

これにより第１実施形態の推定装置２０によれば、カメラ１０Ｂ以外のセンサを用いずに、移動軌跡を途切れさせることなく自己位置を推定することができる。例えば、カメラ１０Ｂが搭載された移動体１０の自己位置を、移動軌跡を途切れさせることなく推定することができる。

（第２実施形態）
次に第２実施形態について説明する。第２実施形態の説明では、第１実施形態と同様の説明については省略し、第１実施形態と異なる箇所について説明する。第２実施形態では、２つの異なる方法で画像から、当該画像を撮像したカメラ１０Ｂの位置及び姿勢を推定する。

［機能構成の例］
図４は第２実施形態の推定装置２０の処理部２０Ａ－２の機能構成の例を示す図である。処理部２０Ａ－２は、取得部２１、第１の推定部２２、第１の算出部２３、第２の推定部２４、第２の算出部２５、判定部２６、第３の推定部２７、及び、第３の算出部２８を備える。すなわち、第２実施形態では、第１実施形態の構成に第３の推定部２７、及び、第３の算出部２８が更に追加されている。

第３の推定部２７は、取得部２１から画像を受け付けると、当該画像から、第１の推定部２２とは異なる方法で、カメラ１０Ｂの位置及び姿勢を推定し、推定された位置及び姿勢を第３の推定情報として出力する。例えば、第１の推定部２２が、複数の画像にＳｆＭを適用することにより、それぞれの画像が撮像されたときのカメラ１０Ｂの位置及び姿勢を推定する場合、第３の推定部２７は、画像にＳＬＡＭを適用することにより、画像が撮像されたときのカメラ１０Ｂの位置及び姿勢を推定する。

第３の算出部２８は、第３の推定情報の信頼度を示す第３の信頼度を算出する。

判定部２６は、第１の信頼度と第２の信頼度と第３の信頼度とに基づき、第１の推定情報と第２の推定情報と第３の推定情報とから、カメラ１０Ｂの位置及び姿勢を決定する。第１乃至第３の信頼度は、カメラ１０Ｂの姿勢の回転成分の値、及び、カメラ１０Ｂの並進成分の移動量の少なくとも一方に基づいて算出される。

［推定方法の例］
図５は第２実施形態の推定方法の例を示すフローチャートである。

ステップＳ２０１～ステップＳ２０３の処理は、第１実施形態のステップＳ１０１～ステップＳ１０３の処理と同じなので説明を省略する。

第３の推定部２７が、ステップＳ２０１の処理により撮像された画像を使用して、当該画像から、第１の推定部２２とは異なる方法で、カメラ１０Ｂの位置及び姿勢を推定し、上述の第３の推定情報を推定する（ステップＳ２０４）。

次に、第３の算出部２８が、上述の第３の信頼度を算出する（ステップＳ２０５）。第３の信頼度の算出方法は、第１の信頼度の算出方法と同様であるので説明を省略する。

次に、判定部２６が、判定部２６による過去の判定処理結果があるか否かを判定する（ステップＳ２０６）。

過去の判定処理結果が存在しない場合（ステップＳ２０６，Ｎｏ）、判定部２６は、ステップＳ２０１の処理により取得された画像から直接、カメラ１０Ｂの位置及び姿勢を決定する（ステップＳ２０７）。カメラ１０Ｂの位置及び姿勢を決定する方法は、例えば第１の推定部２２の処理（ステップＳ２０２の処理）、又は、第３の推定部２７の処理（ステップＳ２０４の処理）と同等でもよいし、第１の推定部２２又は第３の推定部２７の処理とは異なる方法を用いてもよい。例えば、判定部２６は、第１の信頼度と第３の信頼度とを比較し、信頼度がより高い方の処理結果によって、最終的なカメラ１０Ｂの位置及び姿勢を決定してもよい。例えば、判定部２６は、第１の推定部２２の処理（ステップＳ２０２の処理）と同じ方法を用いる場合、第１の推定情報によって、最終的なカメラ１０Ｂの位置及び姿勢を決定する。

過去の判定処理結果がある場合（ステップＳ２０６，Ｙｅｓ）、第２の推定部２４が、判定部２６による過去の判定処理結果を使用して、上述の第２の推定情報を推定する（ステップＳ２０８）。

次に、第２の算出部２５が、上述の第２の信頼度を算出する（ステップＳ２０９）。

次に、判定部２６が、第１の信頼度と第２の信頼度と第３の信頼度とに基づき、第１の推定情報と第２の推定情報と第３の推定情報とからカメラ１０Ｂの位置及び姿勢を決定する（ステップＳ２１０）。具体的には、判定部２６は、第１の信頼度、第２の信頼度および第３の信頼度に基づいて、いずれかの推定情報を最終的な出力結果としてよい。判定部２６は、例えば第１の推定部２０２で推定が行われ、かつ、第１の信頼度が閾値以上であれば第１の推定情報を最終的な出力結果にしてもよい。また例えば、判定部２６は、第１の信頼度が閾値以下の場合は第３の信頼度を参照し、第３の信頼度が閾値以上であれば第３の推定情報を最終的な出力結果にしてもよい。また例えば、判定部２６は、第３の信頼度も閾値以下の場合は、第２の推定情報を最終的な出力結果としてよい。

また例えば、別の統合方法として、判定部２６は、第１の信頼度および第３の信頼度に基づいてそれぞれの推定情報を統合し、新たなカメラ１０Ｂの位置及び姿勢を最終的な出力結果としてもよい。具体的には、判定部２６は、３変数で表現される並進成分の対しては、第１の推定情報と第３の推定情報とを用いて線形重み付き平均によって新たな並進成分を算出してよい。また判定部２６は、クォータニオンによる４成分で表現される回転成分に対しては、それぞれの回転成分の４変数における共通項であるθに着目し、球面線形補間（ＳＬＥＲＰ：Ｓｐｈｅｒｉｃａｌ Ｌｉｎｅａｒ Ｉｎｔｅｒｐｏｌａｔｉｏｎ）によって新たな回転成分を算出してよい。

なお、信頼度に対する閾値はユーザが任意に定めてよく、例えば０．９９９７等の浮動小数点で表現される定数としてよい。また、第１の信頼度と第２の信頼度と第３の信頼度とで同一の閾値を定めてもよいし、それぞれ異なる閾値を定めてもよい。例えば第１の信頼度と第３の信頼度とを主の推定手法とする場合は、第１の信頼度と第３の信頼度とに対する閾値を低めに設定してよい。また第１の信頼度と第３の信頼度とが閾値を超えない場合に、第２の推定部による推定情報を必ず出力結果として採用するように、第２の信頼度の閾値を０としてもよい。

以上、説明したように、第２実施形態の推定装置２０によれば、第３の信頼度を有する第３の推定情報に更に基づいて、移動体１０の位置及び姿勢を推定することができる。

（第３実施形態）
次に第３実施形態について説明する。第３実施形態の説明では、第２実施形態と同様の説明については省略し、第２実施形態と異なる箇所について説明する。第３実施形態では、判定部２６により判定されたカメラ１０Ｂの位置及び姿勢を保存又は棄却する制御について説明する。

［機能構成の例］
図６は第３実施形態の推定装置２０の処理部２０Ａ－３の機能構成の例を示す図である。処理部２０Ａ－３は、取得部２１、第１の推定部２２、第１の算出部２３、第２の推定部２４、第２の算出部２５、判定部２６、第３の推定部２７、第３の算出部２８、及び、記憶制御部２９を備える。すなわち、第３実施形態では、第２実施形態の構成に記憶制御部２９が更に追加されている。

記憶制御部２９は、画像取得時の時系列情報と、判定処理結果とを含む推定結果を記憶部２０Ｂに記憶する。画像取得時の時系列情報は、例えば画像を特定する番号（フレーム番号等）である。判定処理結果は、判定部２６により決定された直近のカメラ１０Ｂの位置及び姿勢を含む。

また、記憶制御部２９は、上述の第１乃至第３の信頼度の全てが閾値以下であり、かつ、カメラ１０Ｂの位置及び姿勢が出力されていない区間が推定可能区間を超えない場合に上述の推定結果を棄却する。記憶制御部２９による記憶制御の詳細、及び、推定可能区間の説明については、図７のフローチャートを参照して後述する。

［推定方法の例］
図７は第３実施形態の推定方法の例を示すフローチャートである。

ステップＳ３０１～ステップＳ３１０の処理は、第１実施形態のステップＳ２０１～ステップＳ２１０の処理と同じなので説明を省略する。

記憶制御部２９が、第１乃至第３の信頼度の全てが閾値以下であるか否かを判定する（ステップＳ３１１）。

第１乃至第３の信頼度の全てが閾値以下でない場合（ステップＳ３１１，Ｎｏ）、記憶制御部２９は、ステップＳ３１０の処理によって決定された位置及び姿勢、画像取得時の時系列情報と、統合処理判定処理結果とを含む推定結果を記憶部２０Ｂに記憶する（ステップＳ３１２）。

第１乃至第３の信頼度の全てが閾値以下である場合（ステップＳ３１１，Ｙｅｓ）、記憶制御部２９は、カメラ１０Ｂの位置及び姿勢の推定可能区間（前回記憶された判定処理結果が有効な有効区間）であるか否かを判定する（ステップＳ３１３）。

推定可能区間とは、推定装置２０がカメラ２０Ｂの位置及び姿勢を推定可能な時系列の長さである。時系列の長さは、例えば画像のフレーム数で表現してもよく、またフレーム番号に対応した時刻（タイムスタンプ）であってもよい。つまり、記憶制御部２９は、推定情報が最後に保存されてからの区間がこの推定可能区間を超えていない場合（ステップＳ３１３，Ｙｅｓ）、推定結果を棄却する（ステップＳ３１４）。推定可能区間を超えている場合（ステップＳ３１３，Ｎｏ）、推定結果を記憶部２０Ｂに記憶する（ステップＳ３１２）。

具体的には、例えば記憶制御部２９が推定を開始してからｎフレーム目に推定結果を記憶部２０Ｂに記憶し、推定装置２０の推定可能区間が５フレームの場合は、ｎ＋１～ｎ＋４フレーム目までは推定結果を棄却してもよい。

一方、記憶制御部２９は、ｎ＋５フレームまで保存されているカメラ１０Ｂの位置及び姿勢が更新されていない場合は、信頼度に依らず推定結果（カメラ１０Ｂの位置及び姿勢）を記憶部２０Ｂに記憶する。その場合のカメラ１０Ｂの位置及び姿勢は、第１の推定部２２、第２の推定部２４、及び、第３の推定部２７によって推定されたカメラ１０Ｂの位置及び姿勢のいずれかでもよいし、それぞれの位置及び姿勢の結果を統合した新たな位置及び姿勢であってもよい。

以上、説明したように、第３実施形態の推定装置２０では、記憶制御部２９が、第１乃至第３の信頼度のいずれかが判定閾値より大きい場合、判定処理結果を記憶し、第１乃至第３の信頼度の全てが判定閾値以下であり、かつ、カメラ１０Ｂの位置及び姿勢が更新されていない区間が、前回記憶された判定処理結果が有効な有効区間内である場合、今回判定された判定処理結果を棄却し、第１乃至第３の信頼度の全てが判定閾値以下であり、かつ、カメラ１０Ｂの位置及び姿勢が更新されていない区間が、有効区間を超えている場合、今回判定された判定処理結果を記憶する。

これにより第３実施形態の推定装置２０によれば、カメラ１０Ｂ以外のセンサを用いずに、移動軌跡を途切れさせることなく、移動体１０の自己位置（位置及び姿勢の少なくとも一方）を推定することができる。

最後に、第１乃至第３実施形態の推定装置２０のハードウェア構成の例について説明する。

［ハードウェア構成の例］
図９は第１乃至第３実施形態の推定装置２０のハードウェア構成の例を示す図である。推定装置２０は、制御装置３０１、主記憶装置３０２、補助記憶装置３０３、表示装置３０４、入力装置３０５及び通信装置３０６を備える。制御装置３０１、主記憶装置３０２、補助記憶装置３０３、表示装置３０４、入力装置３０５及び通信装置３０６は、バス３１０を介して接続されている。

なお、表示装置３０４、入力装置３０５及び通信装置３０６は備えていなくてもよい。例えば、推定装置２０が他の装置と接続される場合、当該他の装置の表示機能、入力機能及び通信機能を利用してもよい。

制御装置３０１は補助記憶装置３０３から主記憶装置３０２に読み出されたプログラムを実行する。制御装置３０１は、例えばＣＰＵ等の１以上のプロセッサである。主記憶装置３０２はＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、及び、ＲＡＭ等のメモリである。補助記憶装置３０３はメモリカード、及び、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）等である。

表示装置３０４は情報を表示する。表示装置３０４は、例えば液晶ディスプレイである。入力装置３０５は、情報の入力を受け付ける。入力装置３０５は、例えばハードウェアキー等である。なお表示装置３０４及び入力装置３０５は、表示機能と入力機能とを兼ねる液晶タッチパネル等でもよい。通信装置３０６は他の装置と通信する。

推定装置２０で実行されるプログラムは、インストール可能な形式又は実行可能な形式のファイルでＣＤ－ＲＯＭ、メモリカード、ＣＤ－Ｒ、及び、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）等のコンピュータで読み取り可能な記憶媒体に記憶されてコンピュータ・プログラム・プロダクトとして提供される。

また推定装置２０で実行されるプログラムを、インターネット等のネットワークに接続されたコンピュータ上に格納し、ネットワーク経由でダウンロードさせることにより提供するように構成してもよい。また推定装置２０が実行するプログラムを、ダウンロードさせずにインターネット等のネットワーク経由で提供するように構成してもよい。

また推定装置２０で実行されるプログラムを、ＲＯＭ等に予め組み込んで提供するように構成してもよい。

推定装置２０で実行されるプログラムは、推定装置２０の機能のうち、プログラムにより実現可能な機能を含むモジュール構成となっている。

プログラムにより実現される機能は、制御装置３０１が補助記憶装置３０３等の記憶媒体からプログラムを読み出して実行することにより、主記憶装置３０２にロードされる。すなわちプログラムにより実現される機能は、主記憶装置３０２上に生成される。

なお推定装置２０の機能の一部を、ＩＣ等のハードウェアにより実現してもよい。ＩＣは、例えば専用の処理を実行するプロセッサである。

また複数のプロセッサを用いて各機能を実現する場合、各プロセッサは、各機能のうち１つを実現してもよいし、各機能のうち２つ以上を実現してもよい。

本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

１０ 移動体
１０Ａ 出力部
１０Ｂ カメラ
１０Ｃ センサ
１０Ｄ 通信部
１０Ｅ ディスプレイ
１０Ｆ スピーカ
１０Ｇ 動力制御部
１０Ｈ 動力部
１０Ｉ バス
２０ 推定装置
２１ 取得部
２２ 第１の推定部
２３ 第１の算出部
２４ 第２の推定部
２５ 第２の算出部
２６ 判定部
２７ 第３の推定部
２８ 第３の算出部
２９ 記憶制御部
３０１ 制御装置
３０２ 主記憶装置
３０３ 補助記憶装置
３０４ 表示装置
３０５ 入力装置
３０６ 通信装置
３１０ バス

Claims (11)

1. カメラによって撮像された画像を取得する取得部と、
   前記画像から前記カメラの位置及び姿勢の少なくとも一方を示す第１の推定情報を推定する第１の推定部と、
   前記第１の推定情報の信頼度を示す第１の信頼度を算出する第１の算出部と、
   前記カメラの位置及び姿勢の少なくとも一方を示す第２の推定情報を推定する第２の推定部と、
   前記第２の推定情報の信頼度を示す第２の信頼度を算出する第２の算出部と、
   前記第１及び第２の信頼度に基づき、前記第１及び第２の推定情報から前記カメラの位置及び姿勢の少なくとも一方を決定し、決定された位置及び姿勢の少なくとも一方を判定処理結果として出力する判定部と、を備え、
   前記第２の推定部は、前記判定部による過去の判定処理結果に基づいて前記第２の推定情報を推定する、
   推定装置。
2. 前記第２の推定部は、前記過去の判定処理結果にカルマンフィルタリングを適用して、現在の前記カメラの位置及び姿勢の少なくとも一方を線形補間することによって推定する、
   請求項１に記載の推定装置。
3. 前記判定部は、
   前記第１の信頼度が第１の閾値よりも大きい場合、前記第１の推定情報によって前記カメラの位置及び姿勢の少なくとも一方を判定し、
   前記第１の信頼度が前記第１の閾値以下であり、かつ、前記第２の信頼度が第２の閾値よりも大きい場合、前記第２の推定情報によって前記カメラの位置及び姿勢の少なくとも一方を判定し、
   前記第１の信頼度が前記第１の閾値以下であり、かつ、前記第２の信頼度が第２の閾値以下である場合、前記判定処理結果を出力しない、
   請求項１に記載の推定装置。
4. 前記判定部は、前記第１の信頼度が第１の閾値よりも大きく、かつ、前記第２の信頼度が第２の閾値よりも大きい場合、前記第１の推定情報と前記第２の推定情報とを統合することによって得られた新たな位置及び姿勢の少なくとも一方によって、前記カメラの位置及び姿勢の少なくとも一方を決定する、
   請求項１に記載の推定装置。
5. 前記判定部は、前記第１の信頼度と前記第２の信頼度とに基づく重み付き平均によって、前記第１の推定情報に含まれる前記カメラの並進成分と、前記第２の推定情報に含まれる前記カメラの並進成分とを統合する、
   請求項４に記載の推定装置。
6. 前記判定部は、前記第１の信頼度と前記第２の信頼度とに基づく球面線形補間によって、前記第１の推定情報に含まれる前記カメラの回転成分と、前記第２の推定情報に含まれる前記カメラの回転成分とを統合する、
   請求項４に記載の推定装置。
7. 前記第２の推定部とは異なる推定方法を用いて、前記画像から前記カメラの位置及び姿勢の少なくとも一方を示す第３の推定情報を推定する第３の推定部と、
   前記第３の推定情報の信頼度を示す第３の信頼度を算出する第３の算出部と、を更に備え、
   前記判定部は、前記第３の信頼度に更に基づき、前記第１乃至第３の推定情報から前記カメラの位置及び姿勢の少なくとも一方を決定する、
   請求項１に記載の推定装置。
8. 前記第１乃至第３の信頼度は、前記カメラの姿勢の回転成分の値、及び、前記カメラの並進成分の移動量の少なくとも一方に基づいて算出される、
   請求項７に記載の推定装置。
9. 前記第１乃至第３の信頼度のいずれかが判定閾値より大きい場合、前記判定処理結果を記憶し、前記第１乃至第３の信頼度の全てが判定閾値以下であり、かつ、前記カメラの位置及び姿勢の少なくとも一方が更新されていない区間が、前回記憶された前記判定処理結果が有効な有効区間内である場合、今回判定された前記判定処理結果を棄却し、前記第１乃至第３の信頼度の全てが判定閾値以下であり、かつ、前記カメラの位置及び姿勢の少なくとも一方が更新されていない区間が、前記有効区間を超えている場合、今回判定された前記判定処理結果を記憶する記憶制御部、
   を更に備える請求項７に記載の推定装置。
10. カメラによって撮像された画像を取得するステップと、
    前記画像から前記カメラの位置及び姿勢の少なくとも一方を示す第１の推定情報を推定するステップと、
    前記第１の推定情報の信頼度を示す第１の信頼度を算出するステップと、
    前記カメラの位置及び姿勢の少なくとも一方を示す第２の推定情報を推定するステップと、
    前記第２の推定情報の信頼度を示す第２の信頼度を算出するステップと、
    前記第１及び第２の信頼度に基づき、前記第１及び第２の推定情報から前記カメラの位置及び姿勢の少なくとも一方を決定し、決定された位置及び姿勢の少なくとも一方を判定処理結果として出力するステップと、を含み、
    前記第２の推定情報を推定するステップは、前記出力するステップによる過去の判定処理結果に基づいて前記第２の推定情報を推定する、
    推定方法。
11. コンピュータを、
    カメラによって撮像された画像を取得する取得部と、
    前記画像から前記カメラの位置及び姿勢の少なくとも一方を示す第１の推定情報を推定する第１の推定部と、
    前記第１の推定情報の信頼度を示す第１の信頼度を算出する第１の算出部と、
    前記カメラの位置及び姿勢の少なくとも一方を示す第２の推定情報を推定する第２の推定部と、
    前記第２の推定情報の信頼度を示す第２の信頼度を算出する第２の算出部と、
    前記第１及び第２の信頼度に基づき、前記第１及び第２の推定情報から前記カメラの位置及び姿勢の少なくとも一方を決定し、決定された位置及び姿勢の少なくとも一方を判定処理結果として出力する判定部、として機能させ、
    前記第２の推定部は、前記判定部による過去の判定処理結果に基づいて前記第２の推定情報を推定する、
    プログラム。

Images