特許 7581016 情報処理装置、情報処理方法、及びプログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-11-01

(45)【発行日】2024-11-12

(54)【発明の名称】情報処理装置、情報処理方法、及びプログラム

(51)【国際特許分類】

   G06T 7/70 20170101AFI20241105BHJP

   G06T 7/00 20170101ALI20241105BHJP

   G01B 11/00 20060101ALI20241105BHJP

【ＦＩ】

G06T7/70 Z

G06T7/00 300F

G01B11/00 H

【請求項の数】 16

(21)【出願番号】P 2020182541

(22)【出願日】2020-10-30

(65)【公開番号】P2022072863

(43)【公開日】2022-05-17

【審査請求日】2023-10-24

(73)【特許権者】

【識別番号】000001007

【氏名又は名称】キヤノン株式会社

(74)【代理人】

【識別番号】100090273

【弁理士】

【氏名又は名称】國分 孝悦

(72)【発明者】

【氏名】冨岡 誠

【審査官】佐田 宏史

(56)【参考文献】

【文献】特開２０１６－０９１０６５（ＪＰ，Ａ）

【文献】特開２０１９－１８５４０４（ＪＰ，Ａ）

【文献】特開平０６－２３１２５２（ＪＰ，Ａ）

【文献】特開２０１７－１３００４２（ＪＰ，Ａ）

【文献】国際公開第２０１９／１３０９４５（ＷＯ，Ａ１）

【文献】工藤 彰、外５名，“光源環境の変化にロバストな自己位置推定のための画像特徴データベースの構築”，情報処理学会研究報告，日本，情報処理学会，2015年10月30日，Vol.2015-CVIM-199, No.16，pp.1-6

【文献】Ximei Xu et al.，"UAV Pose Estimation Based on Prior Information and RANSAC Algorithm"，2018 IEEE 3rd International Conference on Image, Vision and Computing (ICIVC)，米国，IEEE，2018年06月27日，pp.605-608

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０６Ｔ １／００，７／００－７／９０

Ｇ０６Ｖ １０／００－１０／９８

Ｇ０１Ｂ １１／００－１１／３０

(57)【特許請求の範囲】

【請求項1】

撮像装置から時系列的に入力された第一の画像と第二の画像との間で特徴領域の位置の対応付けを行う第一の対応付け手段と、
前記第一の対応付け手段による前記特徴領域の位置の対応付けが失敗であった場合、前記第一の対応付け手段による前記対応付けの方法とは異なる方法で、前記第一の画像と第二の画像との間で前記特徴領域の位置の対応付けを行う第二の対応付け手段と、
前記第一の対応付け手段による前記対応付けが成功した特徴領域の位置、および、前記第二の対応付け手段による前記対応付けが成功した特徴領域の位置に基づいて、前記撮像装置の位置姿勢を取得する取得手段と、
を有することを特徴とする情報処理装置。

【請求項2】

前記特徴領域の位置の三次元座標を表す幾何情報を含む三次元地図情報を保持する保持手段を有し、
前記三次元地図情報が前記特徴領域の情報を保持することを特徴とする請求項１に記載の情報処理装置。

【請求項3】

前記第二の対応付け手段は、前記第一の画像と前記第二の画像の画像全体または局所領域の平均輝度差、中央値の差、ヒストグラムの差、前記撮像装置の露出差、前記撮像装置の感度差の、少なくとも一つが減少するように、前記第一の画像と前記第二の画像の少なくとも一方の画像または当該画像の局所領域の輝度値を補正してから、前記特徴領域の位置の対応付けを行うことを特徴とする請求項１または請求項２に記載の情報処理装置。

【請求項4】

前記第二の対応付け手段は、前記特徴領域の特徴量が減少するようにして前記特徴領域の位置の対応付けを行うことを特徴とする請求項１から請求項３のいずれか１項に記載の情報処理装置。

【請求項5】

前記撮像装置の移動量または前記特徴領域の移動量の、少なくともいずれか一方に基づいて、前記第二の画像について予測した位置を取得する予測手段を有し、
前記第二の対応付け手段は、前記予測した位置を初期位置として前記第二の画像における前記特徴領域の位置の対応付けを行うことを特徴とする請求項１から請求項４のいずれか１項に記載の情報処理装置。

【請求項6】

前記取得手段が取得した前記位置姿勢に基づいて、前記第一の画像の前記特徴領域の特徴量を補正する補正手段を有し、
前記第二の対応付け手段は、前記補正手段が前記特徴量を補正した前記特徴領域と、前記第二の画像の特徴領域とを用いて、前記位置の対応付けを行うことを特徴とする請求項１から請求項５のいずれか１項に記載の情報処理装置。

【請求項7】

前記第二の対応付け手段は、前記特徴領域の位置の対応付けが失敗であった場合、前記対応付けに失敗した際の方法とはさらに異なる方法で、前記第一の画像と第二の画像との間で前記特徴領域の位置を対応付ける処理を行うことを特徴とする請求項１から請求項６のいずれか１項に記載の情報処理装置。

【請求項8】

前記特徴領域の位置の前記対応付けの成否を判定する判定手段を有することを特徴とする請求項１から請求項７のいずれか１項に記載の情報処理装置。

【請求項9】

前記判定手段は、前記撮像装置の移動量または前記特徴領域の移動量に基づいて予測した位置と、前記対応付けされた位置との間の距離が閾値以上である場合に、前記対応付けが失敗したと判定することを特徴とする請求項８に記載の情報処理装置。

【請求項10】

前記判定手段は、前記対応付けされた位置に基づいて、前記第一の画像と前記第二の画像との間の幾何対応関係を取得し、前記幾何対応関係の残差が所定の閾値以上である場合に、前記対応付けが失敗したと判定することを特徴とする請求項８に記載の情報処理装置。

【請求項11】

前記判定手段は、前記第一の画像と前記第二の画像の画像全体または局所領域の平均輝度差、中央値の差、ヒストグラムの差、前記撮像装置の露出差、前記撮像装置の感度差の、少なくとも一つが、対応して設定された閾値以上である場合に、前記対応付けが失敗したと判定することを特徴とする請求項８に記載の情報処理装置。

【請求項12】

前記第二の対応付け手段による前記対応付けの処理時間が所定時間を超える場合には前記対応付けの処理を打ち切る停止手段を有することを特徴とする請求項１から請求項１１のいずれか１項に記載の情報処理装置。

【請求項13】

移動装置の制御値を算出する制御値算出手段を有し、
前記制御値算出手段は、前記第一の対応付け手段による前記対応付けが失敗であった場合、前記移動装置の移動速度または角速度の少なくとも一方を減少させる制御値を算出することを特徴とする請求項１から請求項１２のいずれか１項に記載の情報処理装置。

【請求項14】

請求項１から請求項１３のいずれか１項に記載の情報処理装置と、
前記情報処理装置が取得した撮像装置の位置姿勢に基づいて、移動装置を制御する制御装置と、
を有することを特徴とするシステム。

【請求項15】

一つ以上のプロセッサが実行する情報処理方法であって、
撮像装置から時系列的に入力された第一の画像と第二の画像との間で特徴領域の位置の対応付けを行う第一の対応付け工程と、
前記第一の対応付け工程による前記特徴領域の位置の対応付けが失敗であった場合、前記第一の対応付け工程による前記対応付けの方法とは異なる方法で、前記第一の画像と第二の画像との間で前記特徴領域の位置の対応付けを行う第二の対応付け工程と、
前記第一の対応付け工程による前記対応付けが成功した特徴領域の位置、および、前記第二の対応付け工程による前記対応付けが成功した特徴領域の位置に基づいて、前記撮像装置の位置姿勢を取得する取得工程と、
を有することを特徴とする情報処理方法。

【請求項16】

コンピュータを、請求項１から請求項１４のいずれか１項に記載の情報処理装置が有する各手段として機能させるためのプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、撮像装置の位置および姿勢を取得する情報処理技術に関する。

【背景技術】

【0002】

画像情報に基づくカメラの位置及び姿勢の計測は、複合現実感（ＭＲ）／拡張現実感（ＡＲ）における現実空間と仮想物体との位置合わせや、ロボットや自動車の自己位置推定、物体や空間の三次元モデリングなど様々な目的で利用される。特許文献１では、画像から検出した特徴点を時間的に前後する画像間で対応付け、カメラの位置姿勢を算出する方法が開示されている。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０１８－９９１８号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

しかしながら、例えば環境の照明条件やカメラの露出等が変化して画像の輝度が変化する場合、特許文献１の技術では、時間的に前後する画像中の特徴点の対応付けに失敗し易く、位置姿勢の精度や安定性が低下するという問題が生じる。

【0005】

そこで、本発明は、安定して高い精度で位置姿勢を取得可能にすることを目的とする。

【課題を解決するための手段】

【0006】

本発明の情報処理装置は、撮像装置から時系列的に入力された第一の画像と第二の画像との間で特徴領域の位置の対応付けを行う第一の対応付け手段と、前記第一の対応付け手段による前記特徴領域の位置の対応付けが失敗であった場合、前記第一の対応付け手段による前記対応付けの方法とは異なる方法で、前記第一の画像と第二の画像との間で前記特徴領域の位置の対応付けを行う第二の対応付け手段と、前記第一の対応付け手段による前記対応付けが成功した特徴領域の位置、および、前記第二の対応付け手段による前記対応付けが成功した特徴領域の位置に基づいて、前記撮像装置の位置姿勢を取得する取得手段と、を有することを特徴とする。

【発明の効果】

【0007】

本発明によれば、安定して高い精度で位置姿勢を取得可能になる。

【図面の簡単な説明】

【0008】

【図1】第一の実施形態の情報処理装置の機能構成例を示す図である。

【図2】第一の実施形態の情報処理装置のハードウェア構成例を示す図である。

【図3】第一の実施形態に係る情報処理の流れを示すフローチャートである。

【図4】対応付け成否結果と閾値設定等に用いるＧＵＩの一例を示す図である。

【図5】第二の実施形態の情報処理装置の機能構成例を示す図である。

【図6】第二の実施形態に係る情報処理の流れを示すフローチャートである。

【発明を実施するための形態】

【0009】

以下、添付図面を参照して実施形態を詳しく説明する。なお、以下の実施形態は本発明を限定するものではない。実施形態には複数の特徴が記載されているが、これらの複数の特徴の全てが本発明に必須のものとは限らず、また複数の特徴は任意に組み合わせられてもよい。さらに、添付図面においては、同一若しくは同様の構成又は処理には同一の参照番号を付し、重複した説明は省略する。

【0010】

＜第一の実施形態＞
第一の実施形態として、撮像装置（以下カメラとする）から時系列的に入力される画像間で特徴領域をマッチングし、カメラの位置姿勢を取得する情報処理装置について説明する。本実施形態において、情報処理装置によって取得された位置姿勢は、例えば移動装置の制御に用いられるとする。本実施形態では、移動装置の一例として移動ロボットを挙げる。なおカメラの位置姿勢に基づいて移動ロボット等の移動装置を制御する方法については公知であるためその詳細な説明は省略する。

【0011】

本実施形態の情報処理装置は、カメラから時系列的に入力された（カメラで撮像された）時間的前後の画像間で特徴領域を対応付け、それら対応付けられた特徴領域の位置を追跡することで時系列上の時刻毎のカメラの位置姿勢を推定する。本実施形態では、カメラから時系列的に入力された画像のうち、時系列上で第一の時刻に入力された画像を第一の画像とし、その第一の時刻より時系列上で後の第二の時刻に入力された画像を第二の画像と定義する。また本実施形態において、画像の特徴領域とは、画像内に写っている被写体の角部分など幾何学的な構造を有する画像領域などであり、以下の説明では、当該特徴領域を特徴点と呼ぶことにする。また本実施形態において、カメラの位置姿勢の情報は、現実空間中に規定された世界座標におけるカメラの位置を表す３パラメータ、およびカメラの姿勢を表す３パラメータを合わせた６パラメータで表される情報であるとする。例えば、カメラの位置を表す３パラメータはｘｙｚ座標で表され、カメラの姿勢を表す３パラメータはＹａｗ（ヨー）軸、Ｒｏｌｌ（ロール）軸、Ｐｉｔｃｈ（ピッチ）軸の回転角度で表されるとする。

【0012】

本実施形態の情報処理装置は、例えば環境の照明条件やカメラの露出の変化などによって、カメラから入力される画像の輝度が変化して、画像の特徴点の位置の対応付けに失敗した場合、別の対応付け方法を用いて特徴点の対応付けを行うようにする。本実施形態では、まず、予め決めた第一の対応付け処理によって画像の特徴点の対応付けを行うが、当該第一の対応付け処理で特徴点位置の対応付けに失敗した場合、第一の対応付け処理とは異なる方法による第二の対応付け処理を行う。本実施形態の情報処理装置は、このように複数の対応付け方法を相補的に用いて特徴点の対応付けを行うことで、正しく対応付けがなされた特徴点の数（特徴点の対応付け数）を増加させる。これにより、本実施形態の情報処理装置は、入力画像の輝度が変化しても、高い精度で安定的にカメラ位置姿勢を取得可能とする。

【0013】

図１は、第一の実施形態に係る情報処理装置１と、撮像装置１０と移動ロボットを制御するロボット制御装置１００とを有する制御システムの構成を示した機能ブロック図である。
情報処理装置１は、撮像装置１０であるカメラで撮像されて入力された画像に基づいて、当該カメラの位置姿勢を算出し、その位置姿勢の情報を、不図示の移動ロボットに内蔵もしくは接続されているロボット制御装置１００に出力する。ロボット制御装置１００は、その位置姿勢の情報を基に、移動ロボットの制御を行う。

【0014】

撮像装置１０は、ステレオカメラであるとする。本実施形態では、ステレオカメラを構成している左側のカメラを左カメラ、右側のカメラを右カメラと呼ぶことにする。また本実施形態では、特に断りがない限り、情報処理装置１は、ステレオカメラの左カメラと右カメラのうち、例えば左カメラが撮像した画像を基にカメラ（撮像装置１０）の位置姿勢を算出する例を説明する。もちろん情報処理装置１は、右カメラが撮像した画像を基にカメラの位置姿勢が算出してもよい。また本実施形態では、左カメラが撮像した画像を画像Ｌ、右カメラが撮像した画像を画像Ｒとし、左右両方の画像をステレオ画像と総称する。また本実施形態において、画像Ｌを撮像した時刻を特定する場合、例えば時刻ｔに撮像した画像Ｌは画像Ｌ（ｔ）と表記することにする。

【0015】

図１に示すように、情報処理装置１は、画像入力部１１、第一の対応付け部１２、第一の判定部１３、第二の対応付け部１４、および算出部１５を有して構成されている。
画像入力部１１は、撮像装置１０であるカメラと接続されており、そのカメラが例えば毎秒６０フレームで時系列的に撮像したステレオ画像を、その時系列の順に取得して、第一の対応付け部１２および第二の対応付け部１４に出力する。ここでは、撮像装置１０は、モノクロのステレオの画像を取得するとする。

【0016】

第一の対応付け部１２は、画像入力部１１に時系列順に入力された、第一の時刻の第一の画像と、第一の時刻とは異なる第二の時刻の第二の画像とから、それぞれ特徴点を検出し、第一の画像と第二の画像の間で同一の特徴点を対応付ける処理を行う。本実施形態の例の場合、第一の対応付け部１２は、時系列順に入力された時刻ｔの画像Ｌ（ｔ）と時刻ｔ－１の画像Ｌ（ｔ－１）との間で、同一の特徴点を対応付ける第一の対応付け処理を行う。詳細は後述するが、第一の対応付け部１２は、第一の画像中の特徴点の位置に対し、第二の画像上で対応した特徴点の位置を対応付ける処理を行う。そして、第一の対応付け処理の結果は、第一の判定部１３と算出部１５とに出力される。

【0017】

第一の判定部１３は、第一の対応付け部１２が画像Ｌ（ｔ）と画像Ｌ（ｔ－１）との間で対応付けた特徴点同士が同一の特徴点であるかどうかを判定、つまり、第一の対応付け処理が成功したか又は失敗したかの成否判定を行う。詳細については後述するが、第一の判定部１３は、第一の対応付け部１２が第一の画像と第二の画像との間で特徴点同士を対応付けた位置（対応位置）が正しいか否かを判定する。そして、第一の判定部１３は、第一の対応付け処理の成否判定の結果を示す第一の成否判定結果を、第二の対応付け部１４に出力する。

【0018】

第二の対応付け部１４は、第一の判定部１３から入力された第一の成否判定結果に基づいて、第二の対応付け処理を実行する。例えば、第二の対応付け部１４は、第一の成否判定結果が対応付けの失敗を示す場合、第一の対応付け処理とは異なる方法で第二の対応付け処理を実行して、第一の対応付け処理で失敗した特徴点の対応付けを行う。第二の対応付け部１４で実行される第二の対応付け処理の詳細は後述する。そして、第二の対応付け部１４による第二の対応付け処理の結果は、算出部１５に出力される。

【0019】

算出部１５は、画像Ｌ（ｔ）と画像Ｌ（ｔ－１）との間で、第一の対応付け処理が成功した特徴点の位置関係、または第二の対応付け処理が行われた場合の特徴点の位置関係に基づいて、カメラの位置姿勢を算出する。算出部１５における位置姿勢算出処理の詳細は後述する。

【0020】

図２は、本実施形態に係る情報処理装置１のハードウェア構成例を示す図である。図２に示すように、情報処理装置１は、ハードウェア構成として、ＣＰＵ２１、ＲＯＭ２２、ＲＡＭ２３、外部メモリ２４、入力部２５、表示部２６、通信Ｉ／Ｆ２７、Ｉ／Ｏ２８、およびシステムバス２９を有する。

【0021】

ＣＰＵ２１は、システムバス２９に接続された各種デバイスの制御を行う。
ＲＯＭ２２は、ＢＩＯＳのプログラムやブートプログラムを記憶する。ＲＡＭ２３は、ＣＰＵ２１の主記憶装置として使用される。外部メモリ２４は、情報処理装置１のＣＰＵ２１が実行する本実施形態に係る情報処理プログラムを含む各種プログラムを格納する。

【0022】

入力部２５は、キーボードやマウスなどを介してユーザから入力される情報等を、内部で使用可能な情報に変換する処理等を行う。表示部２６は、ＣＰＵ２１からの指示に従って表示装置に表示する画像等の情報を生成する。なお、表示装置は、液晶表示装置やプロジェクタ、ＬＥＤインジケーターなどであり、種類は問わない。

【0023】

通信Ｉ／Ｆ２７は、ネットワークを介して情報通信を行うインターフェイス部である。インターフェイス規格は、イーサネット（登録商標）などのネットワーク通信規格でもよいし、ＵＳＢなどのシリアル通信、無線通信等など種類は問わない。なお、情報処理装置１が算出した位置姿勢の情報は、通信Ｉ／Ｆ２７を介して、移動ロボットに内蔵もしくは接続されているロボット制御装置１００に出力される。Ｉ／Ｏ２８は、撮像装置１０であるカメラと接続され、当該カメラにおいて撮像されたステレオ画像が入力される。

【0024】

図３は、第一の実施形態に係る情報処理装置１において行われる情報処理の流れを示すフローチャートである。図３のフローチャートに示す各ステップの処理は、例えばＣＰＵ２１が本実施形態に係る情報処理プログラムを実行して、図１の機能ブロックの各機能部として動作することにより実現される。

【0025】

まずステップＳ１１において、ＣＰＵ２１はシステムの初期化を行う。すなわちＣＰＵ２１は、外部メモリ２４から本実施形態に係る情報処理プログラムを読み込み、情報処理装置１を動作可能な状態にする。またＣＰＵ２１は、必要に応じて、外部メモリ２４から各種設定パラメータをＲＡＭ２３に読み込む。各種設定パラメータには、カメラパラメータが含まれる。カメラパラメータは、例えばＺｈａｎｇの方法によって事前に校正されているものとする。Ｚｈａｎｇの方法は、Ｚ． Ｚｈａｎｇ，"Ａ ｆｌｅｘｉｂｌｅ ｎｅｗ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ，Ｔｒａｎｓ． ＰＡＭＩ２０００に開示されている。また本実施形態において、ステレオカメラの左右のカメラ間の相対的な位置姿勢（カメラの外部パラメータ）は、三次元形状が既知のパターンを撮影して、そのパターンを基準とした相対位置を算出することで、事前に校正してあるものとする。

【0026】

次にステップＳ１２において、画像入力部１１は、カメラが時系列的に撮像したステレオ画像を取得する。またこのとき、画像入力部１１は、カメラから時系列的に入力される時刻ごとの各ステレオ画像の各々に対し、カメラパラメータを用いてレンズ歪みの影響を除去した画像に変換する処理をも行う。

【0027】

次にステップＳ１３において、第一の対応付け部１２は、画像入力部１１を介してカメラから時系列的に入力された第一の画像と第二の画像との間で特徴点を対応付ける第一の対応付け処理を行う。例えば第一の対応付け部１２は、カメラから第一の時刻ｔ－１に入力された画像Ｌ（ｔ－１）から検出した特徴点が、第二の時刻ｔに入力された画像Ｌ（ｔ）中でどこの位置になっているかを探索する。そして、第一の対応付け部１２は、画像Ｌ（ｔ－１）の特徴点の位置と、画像Ｌ（ｔ）中の同じ特徴点の位置とを対応付けるような位置対応付け処理を行う。

【0028】

具体的に説明すると、第一の対応付け部１２は、まず、画像入力部１１に入力された第一の時刻ｔ－１の画像Ｌ（ｔ－１）から、特徴点を検出する。特徴点の検出には、例えばＳｈｉらの手法（Ｊ． Ｓｈｉ ａｎｄ Ｃ． Ｔｏｍａｓｉ， Ｇｏｏｄ Ｆｅａｔｕｒｅｓ ｔｏ Ｔｒａｃｋ， Ｐｒｏｃ． ＣＶＰＲ１９９４）などを用いることができる。次に、第一の対応付け部１２は、画像Ｌ（ｔ－１）上で検出した特徴点が、第二の時刻ｔの画像Ｌ（ｔ）のどこに写っているのかを探索する。この場合の特徴点の探索の手法としては、ＴｏｍａｓｉらのＫＬＴ法などが挙げられる。ＴｏｍａｓｉらのＫＬＴ法は、Ｃ． Ｔｏｍａｓｉ ａｎｄ Ｔ． Ｋａｎａｄｅ， "Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｔｒａｃｋｉｎｇ ｏｆ Ｐｏｉｎｔ Ｆｅａｔｕｒｅｓ"， Ｃａｒｎｅｇｉｅ Ｍｅｌｌｏｎ Ｕｎｉｖｅｒｓｉｔｙ Ｔｅｃｈｎｉｃａｌ Ｒｅｐｏｒｔ ＣＭＵ－ＣＳ－９１－１３２， １９９１に開示されている。このように第一の対応付け部１２は、第一の時刻の第一の画像と第二の時刻の第二の画像との間で特徴点の対応付けを行うことで、それら第一の時刻と第二の時刻の二つの画像間における特徴点位置の対応関係を取得する。

【0029】

また本実施形態において、第一の対応付け部１２は、ステレオ画像間で特徴点のマッチング処理も合わせて行い、当該ステレオ画像間でマッチングした特徴点を用い、三角測量の原理によってカメラに対する各特徴点の奥行の情報も算出する。具体的に説明すると、第一の対応付け部１２は、左カメラの画像Ｌ（ｔ－１）上で検出した特徴点周囲の小パッチをリファレンスとして、右カメラ画像Ｒ（ｔ－１）を走査するブロックマッチング法によって、ステレオ画像間で対応した特徴点を探索する。そして、第一の対応付け部１２は、ステレオ画像間で対応した特徴点の位置を、カメラの外部パラメータを基に、カメラ座標系における奥行値に変換し、これを保持する。

【0030】

次にステップＳ１４において、第一の判定部１３は、第一の対応付け部１２による第一の対応付け処理の結果について成否を判定する。例えば第一の判定部１３は、算出済みのカメラの位置姿勢の履歴に基づいて算出部１５が算出した特徴点の予測位置と、ステップＳ１３で対応付けられた特徴点位置との間の距離を基に、第一の対応付け部１２による第一の対応付け処理の成否を判定する。

【0031】

具体的に説明すると、第一の判定部１３は、まず、ステップＳ１３で対応付けた特徴点位置と、奥行値とを基に、時刻ｔ－１におけるカメラに対する特徴点の三次元位置を算出する。次に、第一の判定部１３は、時刻ｔ－１について算出した特徴点の三次元位置に対し、時刻ｔ－２と時刻ｔ－１の画像を基に算出されている位置姿勢（位置姿勢の履歴）を外挿し、時刻ｔにおけるカメラの位置姿勢を予測する。さらに、第一の判定部１３は、その予測した時刻ｔにおいて左カメラが撮像するはずの画像Ｌ（ｔ）上に特徴点を投影し、特徴点の予測位置を算出する。そして、第一の判定部１３は、算出した予測位置と、ステップＳ１３の第一の対応付け処理で算出された特徴点位置との間のユークリッド距離を算出し、その距離が事前に定めた閾値以上であれば、第一の対応付け処理は失敗であると判定する。また本実施形態では、第一の判定部１３は、対応付けが成功であると判定した特徴点の個数が、予め定めた所定の閾値個数以下であれば、第一の画像Ｌ（ｔ－１）と第二の画像Ｌ（ｔ）との間における特徴点位置の対応付け処理は失敗したと判定する。本実施形態では、第一の判定部１３において対応付け処理が成功したと判定された場合、情報処理装置１の処理は、算出部１５によるステップＳ１６に進み、一方、失敗したと判定された場合、第二の対応付け部１４によるステップＳ１５に進む。

【0032】

ステップＳ１５に進んだ場合、第二の対応付け部１４は、ステップＳ１４で対応付け処理が失敗したと判定された特徴点を、ステップＳ１３での第一の対応付け処理とは異なる方法による第二の対応付け処理を用いて対応付ける。本実施形態の場合、時刻が異なる画像間で輝度変化があっても特徴点の対応付けができるように、第二の対応付け部１４は、第二の対応付け処理において、画像パッチの平均輝度を減算した上でテンプレートマッチングを行うようにする。

【0033】

具体的には、第二の対応付け部１４は、まず時刻ｔ－１の画像Ｌ（ｔ－１）から検出された特徴点の位置を初期値とし、時刻ｔの画像Ｌ（ｔ）において、当該初期値の位置を含む所定の範囲を、探索範囲として設定する。次に第二の対応付け部１４は、時刻ｔ－１の画像Ｌ（ｔ－１）から検出された特徴点周りの小パッチの輝度値平均を算出し、その輝度値平均を小パッチの各画素値から減算して、参照パッチを作成する。次に第二の対応付け部１４は、画像Ｌ（ｔ）上で前述のように設定した探索範囲を走査するように小パッチを順次生成し、それら順次生成した小パッチ毎に平均輝度を減算した探索パッチを生成する。さらに第二の対応付け部１４は、参照パッチと探索パッチとの差の二乗和（Ｚｅｒｏ－ｍｅａｎ Ｓｕｍ ｏｆ Ｓｑｕａｒｅｄ Ｄｉｓｔａｎｃｅ： ＺＳＳＤ値）を計算し、それを当該探索パッチの誤差値とする。そして第二の対応付け部１４は、探索範囲の中で誤差値が最小となる座標を、対応した特徴点の位置とする。このステップＳ１５の処理後、情報処理装置１の処理はステップＳ１７に進む。

【0034】

またステップＳ１６に進んだ場合、算出部１５は、時刻ｔ－１と時刻ｔの画像の特徴点のうち、ステップＳ１３またはステップＳ１５で対応付けられた特徴点の対応関係と奥行値とに基づいて、カメラの位置姿勢を算出する。すなわち算出部１５は、ステップＳ１３で対応付けられてステップＳ１４で対応付け成功と判定された特徴点、またはステップＳ１５で対応付けられた特徴点の対応関係と、それらの奥行値とに基づいて、カメラの位置姿勢を算出する。本実施形態の場合、算出部１５は、時刻ｔ－１の画像Ｌ（ｔ－１）における特徴点の三次元位置を、時刻ｔの画像Ｌ（ｔ）に投影したときに誤差が最小となるように、カメラの位置姿勢を算出する。具体的には、算出部１５は、いわゆるＰＮＰ問題（Ｐｅｒｓｐｅｃｔｉｖｅ－ｎ－Ｐｏｎｔ Ｐｒｏｂｌｅｍ）を解くことで、カメラの位置姿勢を算出する。そして、当該算出部１５によって算出されたカメラの位置姿勢の情報は、ロボット制御装置１００に送られる。このステップＳ１６の処理後、情報処理装置１の処理は、ステップＳ１７に進む。

【0035】

ステップＳ１７に進むと、ＣＰＵ２１は、処理を終了するか否かの判断を行う。本実施形態においては、不図示の移動ロボットが目的地に到着あるいは目的の姿勢等になった場合、ロボット制御装置１００はシステムを終了するコマンドを情報処理装置１に送る。終了コマンドが入力された場合、情報処理装置１のＣＰＵ２１は、図３のフローチャートの処理を終了し、そうでなければステップＳ１２に処理を戻して、位置姿勢を取得する処理を続行する。

【0036】

＜第一の実施形態に係る効果＞
以上説明したように、第一の実施形態においては、第一の対応付け処理の成否を判定し、対応付けに失敗したと判定した特徴点を、第一の対応付け処理とは異なる第二の対応付け処理によって対応付ける。これにより、第二の実施形態によれば、画像の輝度が変化する場合であっても、特徴点の対応点数を増加させることができ、安定して高い精度でカメラの位置姿勢を算出することが可能となる。

【0037】

＜第一の実施形態の変形例＞
本実施形態において、撮像装置１０はモノクロ画像を取得するステレオカメラとしたが、カラー画像を取得するステレオカメラであってもよい。また、撮像装置１０は、ステレオカメラに限定されず、単眼カメラ、奥行カメラなど、任意のカメラであってもよい。なお、撮像装置１０として単眼カメラを用いる場合、位置姿勢の算出のためには奥行値が必要となる。この場合、第一の対応付け部１２や第二の対応付け部１４は、単眼カメラの動きから奥行きを算出する。単眼カメラの動きから奥行きを算出する手法としては、Ｋｌｅｉｎらの手法が挙げられる。Ｋｌｅｉｎらの手法は、Ｇ． Ｋｌｅｉｎ ａｎｄ Ｄ． Ｍｕｒｒａｙ， "Ｐａｒａｌｌｅｌ ｔｒａｃｋｉｎｇ ａｎｄ ｍａｐｐｉｎｇ ｆｏｒ ｓｍａｌｌ ＡＲ ｗｏｒｋｓｐａｃｅｓ，" Ｐｒｏｃ． ＩＳＭＡＲ， ２００７に開示されている。

【0038】

本実施形態では、第一の対応付け部１２における対応付けの手法としてＫＬＴ法を挙げ、第二の対応付け部１４における対応付けの手法としてＺＳＳＤ値を用いたテンプレートマッチング法を挙げた。ここで、第二の対応付け部１４が実施する第二の対応付け処理の手法は、異なる二枚の画像間で同一の特徴点を対応付けることができるのであれば、他の手法であってもよい。例えば、画像の小パッチを切り出してテンプレートマッチングで探索を行う手法が用いられてもよいし、ＳＩＦＴ特徴量やＯＲＢ特徴量といった特徴量を用いた特徴点マッチングを行う手法が用いられてもよい。また第二の対応付け処理の際には、例えば、Ｂｅｒｇｍａｎｎらのカメラの露出を推定して輝度補正をするＧａｉｎ ｒｏｂｕｓｔ ＫＬＴ ｆｅａｔｕｒｅ Ｔｒａｃｋｓの手法が利用されてもよい。Ｇａｉｎ ｒｏｂｕｓｔ ＫＬＴ ｆｅａｔｕｒｅ Ｔｒａｃｋｓの手法は、Ｐ． Ｂｅｒｇｍａｎｎ． ｅｔ．ａｌ， Ｏｎｌｉｎｅ Ｐｈｏｔｏｍｅｔｒｉｃ Ｃａｌｉｂｒａｔｉｏｎ ｏｆ Ａｕｔｏ Ｅｘｐｏｓｕｒｅ Ｖｉｄｅｏ ｆｏｒ Ｒｅａｌｔｉｍｅ Ｖｉｓｕａｌ Ｏｄｏｍｅｔｒｙ ａｎｄ ＳＬＡＭ， Ｉｎ Ｐｒｏｃ． ＲＡ－Ｌ２０１８に開示されている。また例えば、カメラが露出やゲイン値を取得できる構成、あるいはＢｅｒｇｍａｎｎらの方法によりカメラの露出を推定できる構成であれば、予め画像が同一の露出やゲインとなるように補正する前処理を行ってから第二の対応付け処理が実施されてもよい。また露出やゲインを取得できない場合には、前処理として、画像全体や局所領域（小領域）毎に、輝度の平均値や中央値を減算したり、ヒストグラムを平坦化したりする補正を行い、画像の輝度差を低減してから第二の対応付け処理が実施されてもよい。すなわち第二の対応付け処理は、画像全体又は小領域の平均輝度差、中央値の差、ヒストグラムの差、撮像装置の露出差や感度差の、少なくとも一つが減少するように、画像全体や小領域の輝度値を補正してから実施されてもよい。本実施形態の場合、これら画像全体や小領域における輝度値の補正は、第一の対応付け処理が失敗したときの第二の対応付け処理の際に行われる。なおこれら第二の対応付け処理として適用可能な各手法は、第一の対応付け処理の際に利用されてもよいが、この場合も、第一の対応付け処理の手法と第二の対応付け処理の手法とでは異なる手法が用いられる。

【0039】

また情報処理装置は、第二の対応付け処理が失敗した特徴点を第三の対応付け処理で対応付けし、さらに第三の対応付け処理に失敗した特徴点を第四の点対応付け処理で対応つけるというように、複数の対応付け処理を行う構成であってもよい。この場合、情報処理装置は、第三の対応付け処理を行う第三の対応付け部、第四の点対応付け処理を行う第四の対応付け部など、複数の対応付け部を有する構成となる。または第二の対応付け部は、第二の対応付け処理が失敗した場合に、第三の対応付け処理、第四の対応付け処理のように順に対応付け処理を変更していくような構成が用いられてもよい。このように、複数の対応付け処理を実行可能な構成にすることで、より安定して特徴点の対応付けができるようになる。

【0040】

また本実施形態の場合、第一の判定部１３は、カメラの位置姿勢の履歴から予測したカメラ位置の画像に三次元的な特徴点位置を投影した特徴点の予測位置に基づいて第一の対応付け処理の成否を判定したが、成否判定手法はこの例に限定されない。すなわち第一の判定部１３における対応付けの成否判定処理は、特徴点の対応付けが失敗したか否か（成功したか否か）を検知可能であれば、他の方法が用いられてもよい。

【0041】

例えば第一の判定部１３は、時刻ｔ－２と時刻ｔ－１の画像上の特徴点位置を履歴として保持し、時刻ｔ－２から時刻ｔ－１の画像上での特徴点の移動速度を算出し、その移動速度を基に、時刻ｔの画像上における特徴点位置を予測してもよい。また例えば、第一の判定部１３は、画像上の特徴点の位置の履歴を保持し、それら保持した位置を基に特徴点の平均移動ベクトルを算出し、その平均移動ベクトルと特徴点の移動量との差が所定量以上であれば対応付けに失敗したと判定してもよい。また第一の判定部１３は、例えば、特徴点位置の対応関係からＦ行列のような画像間の幾何対応関係を算出し、その幾何対応関係の残差が所定の閾値以上となる、整合しない特徴点は対応付けに失敗したと判定してもよい。また第一の判定部１３は、例えば、特徴点の対応関係からカメラの相対位置姿勢を一度算出し、再投影誤差の値が所定値より大きい特徴点は対応付けが失敗したと判定してもよい。また第一の判定部１３は、例えば、特徴点の分布の変動量が所定量以上であれば、対応付けに失敗したと判定してもよい。例えば、第一の判定部１３は、対応付けた特徴点数が、二つの時刻の画像間で所定数以上の個数減少または増大した場合や、対応付けた特徴点の平均位置が二つの時刻の画像間で所定距離以上変化した場合に、対応付けに失敗したと判定する。

【0042】

また、第一の判定部１３は、画像の輝度変化を観測し、当該輝度変化に基づいて特徴点位置の対応付け処理の成否を推定する構成であってもよい。例えば、第一の判定部１３は、画像全体や一部の局所領域の平均輝度変化量が所定量以上、またはヒストグラムの変化量が所定量以上であれば、特徴点位置の対応付けに失敗すると推定（判定）する。また第一の判定部１３は、例えば、カメラの露出やゲイン値を取得し、それらの値の変動量が所定量より大きければ、特徴点の対応付けに失敗すると推定（判定）することもできる。

【0043】

第一の実施形態の情報処理装置は、時刻ｔ－１の画像で特徴点を検出し、時刻ｔの画像から対応した特徴点を探索したが、特徴点はある一時刻の画像から検出しなければならないものではない。情報処理装置は、例えば、各時刻で対応付けた特徴点位置を時系列的に追跡する構成でもよい。すなわち情報処理装置は、時刻ｔの画像から検出した特徴点を、時刻ｔ＋１の画像、時刻ｔ＋２の画像、時刻ｔ＋３の画像、・・・というように、順次対応付けていくような追跡を行う構成であってもよい。このような構成にすると、すべての画像で特徴点を検出しなくてよいので、処理速度が向上する。

【0044】

また本実施形態では、第一の対応付け部１２と第二の対応付け部１４とは共に、第一の時刻と第二の時刻の画像間で特徴点の対応付けを行う例を挙げたが、第一、第二の対応付け部は、異なる時刻の画像の特徴点を対応付ける構成でもよい。つまり、第一の対応付け部１２は時刻ｔ－２と時刻ｔの画像間で特徴点の対応付けを行い、第二の対応付け部１４は時刻ｔ－１と時刻ｔの画像間で特徴点の対応付けを行うような構成でもよい。このような構成によれば、複数の時刻の画像間で特徴点の対応付けが行われることになり、対応付けされる特徴点数が増加し、安定して高い精度で位置姿勢を算出することが可能になる。

【0045】

また情報処理装置は、画像から検出した特徴点の三次元座標を表す三次元幾何情報を含む三次元地図情報を保持するようにし、その三次元地図中の特徴点と入力画像の特徴点とを対応付けて保持する構成でもよい。このようにすることで、前後の画像の特徴点の対応関係による位置姿勢算出の場合と比較し、位置姿勢算出の蓄積誤差を低減することができる。

【0046】

また情報処理装置は、例えば地図中に保持する特徴点同士の対応付けに、本実施形態における特徴点の対応付け方法を適用することもできる。このようにすることで、地図内での特徴点の対応付け数が増加し、Ｋｌｅｉｎらの手法で説明されている地図最適化実施時の地図精度が向上し、位置姿勢算出の精度を向上させることができる。

【0047】

本実施形態では、算出部１５が算出した位置姿勢を基に、移動装置の一例である移動ロボットの移動や姿勢等を制御する例を想定している。移動ロボットには、例えば無人搬送車（ＡＧＶ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ））、自律移動ロボット（ＡＭＲ（Ａｕｔｏｎｏｍｏｕｓ Ｍｏｂｉｌｅ Ｒｏｂｏｔ））、ドローンや自動運転車などが挙げられる。また移動ロボットは、自律的に移動するものに限らず、本実施形態の情報処理装置を載せた移動機構の付いた移動装置の位置姿勢を計測するような計測装置として構成されてもよい。

【0048】

また本実施形態では、算出部１５が算出した位置姿勢を移動装置の制御に適用する例を挙げたが、位置姿勢を用いる装置であれば、位置姿勢の適用先は移動装置に限定されるものではない。例えば本実施形態の情報処理装置は、複合現実感（ＭＲ）や拡張現実感（ＡＲ）といった仮想物体を現実画像に合成して提示する際の位置姿勢推定装置としても利用できる。また情報処理装置は、バーチャル空間を移動する人工現実感（ＶＲ）における現在位置姿勢を、現実空間の位置姿勢を基に入力する装置としても利用できる。その他にも、情報処理装置は、カメラを搭載したロボットハンドの先端位置を計測する装置として用いることもできる。

【0049】

さらに本実施形態の情報処理装置は、例えば画像入力部１１を介した入力画像の表示機能や、特徴点の対応付け結果の表示機能、対応付けの成否判定に用いる閾値を調整するための、入力表示部を備えた構成であってもよい。
図４は、入力表示部の機能を、タッチパネルを有するディスプレイ上に表示するＧＵＩ（Ｇｒａｐｈｉｃａｌ Ｕｓｅｒ Ｉｎｔｅｒｆａｃｅ）として構成した例を示した図である。

【0050】

図４において、ウィンドウ４０は、入力表示部の機能を実現するＧＵＩが表示されたウィンドウ画面例を示している。カメラ画像４１は、画像入力部１１を介して入力されたカメラの撮像画像例である。カメラ画像４１内の丸印４２は、画像から検出した特徴点のうち、第一の対応付け部１２が対応付けに成功した特徴点を表している。カメラ画像４１内の三角印４３は、画像から検出した特徴点のうち、第二の対応付け部１４が対応付けした特徴点を表している。カメラ画像４１内のバツ印４４は、画像から検出した特徴点のうち、対応付け処理に失敗した特徴点を表している。カメラ画像４１内の文字列４５は、第一の判定部１３が画像の輝度変化を検知したことを表示するメッセージである。

【0051】

ウィンドウ４０内の入力フォーム４６は、第一の判定部１３が予測した特徴点位置と、第一の対応付け部１２が対応付けた特徴点位置との誤差に対する閾値が入力されるエリアである。予測された特徴点位置と対応付けられた特徴点位置との誤差が、入力フォーム４６に入力された閾値以上になる場合、第一の判定部１３は、特徴点の対応付けに失敗したと判定する。キーボードやマウスなどから図２の入力部２５を介して入力フォーム４６に数値が入力されると、情報処理装置はその数値に応じた閾値を設定する。このようなＵＩが用いられることで、ユーザは、対応付けの成否を判定する閾値を調整することができるようになる。

【0052】

ウィンドウ４０内の入力フォーム４７は、カメラの露出変動値に対する閾値が入力されるエリアである。カメラの露出変動値が、入力フォーム４７に入力された閾値以上になる場合、第一の判定部１３は、画像の輝度変化が生じたと判定する。キーボードやマウスなどから図２の入力部２５を介して入力フォーム４７に数値が入力されると、情報処理装置はその数値に応じた閾値を設定する。このようなＵＩが用いられることで、ユーザは、輝度変化が生じたことを認識することができるようになるとともに、輝度変化の検知の閾値を調整することができるようになる。

【0053】

＜第二の実施形態＞
第一の実施形態では、第二の対応付け処理時に、まず、対応付けの初期値を予測し、また、対応付け処理時の位置姿勢に基づいて画像の見えを予測して、特徴量を算出する。第二の実施形態では、さらに、第二の対応付け処理を所定の時間で打ち切り、第二の対応付け処理で対応付けを誤った特徴点を除去する。これにより、第二の実施形態では、第一の実施形態と比較して、短時間で高い精度による特徴点の対応付け処理を実現する。以下、第二の実施形態に係る情報処理について説明する。

【0054】

第二の実施形態においても、第一の実施形態と同様に、撮像装置１０として左カメラと右カメラからなるステレオカメラが用いられ、特に断りがない限り左カメラの位置姿勢を撮像装置の位置姿勢とする。また第一の実施形態と同様に、左カメラが撮像した画像Ｌと右カメラが撮像した画像Ｒの左右両方の画像をステレオ画像と呼ぶ。また第一の実施形態と同様に、例えば時刻ｔで撮像された画像を画像Ｌ（ｔ）と表記する。

【0055】

図５は、第二の実施形態に係る情報処理装置５と、撮像装置１０と、ロボット制御装置１００とを有する制御システムの機能構成を示す図である。第二の実施形態の情報処理装置５は、第一の実施形態で説明した構成に加えて、予測部５１、補正部５２、処理停止部５３、および第二の判定部５４を有している。第二実施形態の場合、第一の判定部１３による第一の成否判定結果は、予測部５１と補正部５２にも入力される。

【0056】

予測部５１は、第一の成否判定結果が失敗を示す場合、カメラの移動量または特徴点の移動量の少なくともいずれか一方に基づいて、第二の画像について予測した特徴点の位置を取得する。例えば第二の画像が時刻ｔの画像Ｌ（ｔ）である場合、予測部５１は、第一の成否判定結果が失敗と判定されていれば、まず、算出部１５が算出した位置姿勢の履歴に基づいて、時刻ｔにおけるカメラの位置姿勢を予測する。さらに、予測部５１は、時刻ｔにおいて予測したカメラの位置姿勢を基に、時刻ｔの画像Ｌ（ｔ）における特徴点の位置を予測する。そして、予測部５１による予測結果は、補正部５２に出力される。予測部５１にて行われる予測処理の詳細は後述する。

【0057】

補正部５２は、第一の成否判定結果が失敗を示す場合、算出部１５が算出した位置姿勢に基づいて、第一の画像の特徴点における特徴量が減少するように補正する。第二の画像が時刻ｔの画像Ｌ（ｔ）である場合、補正部５２は、まず、算出部１５が算出した位置姿勢の履歴に基づいて、時刻ｔにおけるカメラの位置姿勢を予測する。さらに、補正部５２は、時刻ｔにおいて予測したカメラの位置姿勢を基に、時刻ｔに撮影する画像Ｌ（ｔ）上で特徴点が観測されるであろう特徴点周りの小パッチを画像Ｌ（ｔ－１）から生成する。そして、補正部５２は、その生成した小パッチを、第二の対応付け部１４に出力する。

【0058】

処理停止部５３は、第二の対応付け部１４の処理時間を計測し、その処理時間が、算出部１５における位置姿勢算出時の処理レート（例えば６０分の１秒）に相当する所定時間を超過する場合には処理停止信号を第二の対応付け部１４に出力する。

【0059】

第二の実施形態に係る第二の対応付け部１４は、予測部５１が予測した予測位置を初期位置とし、その初期位置に対して、時系列上の第二の画像上から対応した位置を算出する。また第二の実施形態に係る第二の対応付け部１４は、補正部５２が特徴点について補正した特徴量に基づいて、その特徴点に対して第二の画像上で対応した位置を算出する。第一の画像Ｌ（ｔ－１）と第二の画像Ｌ（ｔ）を例に挙げると、第二の対応付け部１４は、予測部５１が予測した特徴点位置と補正部５２が作成した小パッチとを基に、それら画像Ｌ（ｔ）と画像Ｌ（ｔ－１）とを用いた第二の対応付け処理を行う。そして第二の対応付け部１４は、第二の対応付け処理により同一であるとした特徴点を対応付け、その対応付け処理の結果を第二の判定部５４に出力する。第二の実施形態において、第二の対応付け部１４で行われる対応付け処理の詳細は後述する。また第二の実施形態において、第二の対応付け部１４は、処理停止部５３から処理停止信号が入力された場合には、その入力に従い、すべての特徴点の対応付け処理が終わっていない場合でも対応付け処理を打ち切る。

【0060】

第二の判定部５４は、第二の対応付け部１４において実行された画像間の特徴点の対応付け処理の成否を判定、つまり第二の対応付け処理により対応付けられた特徴点が同一の特徴点か否かを判定する。そして、第二の判定部５４は、第二の対応付け処理が成功したと判定した場合、つまり画像間の特徴点が同一であると判定した場合、その特徴点の対応関係を算出部１５に出力する。

【0061】

第二の実施形態における算出部１５は、第一の対応付け部１２での対応付けが成功した特徴点と、第二の対応付け部１４での対応付けが成功した特徴点とを用いて、カメラの位置姿勢を算出する。算出部１５によるカメラの位置姿勢の算出処理は、第一の実施形態における例と同様であるためその説明は省略する。

【0062】

図６は、第二の実施形態の情報処理装置５における処理の流れを示すフローチャートである。前述の図３のフローチャートと同様、図６のフローチャートに示す各ステップの処理は、例えばＣＰＵ２１が第二の実施形態に係る情報処理プログラムを実行して、図１の機能ブロックの各機能部として動作することにより実現される。図６に示したフローチャートでは、第一の実施形態で説明した処理ステップに加えて、ステップＳ２１、ステップＳ２２、ステップＳ２３、およびステップＳ２４を有する。ステップＳ２１は予測部５１が特徴点位置を予測する処理であり、ステップＳ２２は補正部５２が特徴量を補正する処理である。また、ステップＳ２３は処理停止部５３が処理を停止させるか判定する処理であり、そしてステップＳ２４は第二の判定部５４が第二の対応付け処理の成否判定を行う処理である。

【0063】

第二の実施形態の場合、ステップＳ１４において第一の対応付け処理が失敗と判定された場合、ステップＳ２１の処理に進む。
ステップＳ２１において、予測部５１は、算出部１５が算出した位置姿勢の履歴に基づいてカメラの位置姿勢を予測し、ステップＳ１４で失敗したと判定された特徴点の画像上における座標を予測する。具体例を挙げて説明すると、予測部５１は、まず、ステップＳ１３で検出された特徴点位置と奥行値を基に、時刻ｔ－１の時のカメラに対する特徴点の三次元位置を算出する。次に、予測部５１は、算出部１５が時刻ｔ－２と時刻ｔ－１において算出した位置姿勢（つまり位置姿勢の履歴）に基づき、時刻ｔにおけるカメラの位置姿勢を予測する。そして、予測部５１は、予測した時刻ｔにおけるカメラの位置姿勢の画像Ｌ（ｔ）に特徴点を投影して、その特徴点の予測位置を算出する。この算出された予測位置は、ステップＳ１５における第二の対応付け処理の際の初期値として用いられる。

【0064】

次にステップＳ２２において、補正部５２は、算出部１５が算出した位置姿勢の履歴に基づいて、ステップＳ１４で失敗したと判定された特徴点が画像Ｌ（ｔ）で観測されるであろう特徴点周りの小パッチを作成する。具体的には、補正部５２は、まず時刻ｔ－２と時刻ｔ－１について算出部１５が算出したカメラの位置姿勢（位置姿勢の履歴）を基に、時刻ｔにおけるカメラの位置姿勢を予測する。次に、補正部５２は、時刻ｔ－１における位置姿勢と時刻ｔにおける位置姿勢との間の相対位置姿勢を算出する。そして補正部５２は、算出した相対位置姿勢に基づいて、時刻ｔ－１の画像Ｌ（ｔ－１）における小パッチをホモグラフィー変換して、特徴点が画像Ｌ（ｔ）で観測されるであろう特徴点周りの小パッチを作成する。

【0065】

次に第二の実施形態の場合、ステップＳ１５において、第二の対応付け部１４は、ステップＳ２１で算出された特徴点の予測位置を初期値とし、ステップＳ２２で算出された特徴点周りの小パッチを参照して画像Ｌ（ｔ）上での特徴点位置を探索する。具体的には、第二の対応付け部１４は、ステップＳ２１で算出した特徴点の予測位置の周囲の所定領域を探索ウィンドウ範囲とし、ステップＳ２２で算出された特徴点周りの小パッチを参照パッチとしてテンプレートマッチングを行う。なおこの時の第二の対応付け部１４は、画像Ｌ（ｔ－１）と画像Ｌ（ｔ）との間の輝度の変化に対応するために、第一の実施形態で述べたように小パッチの平均輝度を減算して、テンプレートマッチングを行うとする。そして、第二の対応付け部１４は、探索ウィンドウ範囲内でＺＳＳＤ値が最小となる座標を、特徴点の対応位置とする。

【0066】

次にステップＳ２３において、処理停止部５３は、ステップＳ１５における第二の対応付け処理の処理時間が所定時間を超過しないように処理の停止（打ち切り）を行う。具体的には、処理停止部５３は、事前に指定された位置姿勢算出レートを基に設定された所定時間（例えば８ｍｓ）を第二の対応付け処理の処理時間が超える場合には、それ以降の特徴点の対応付け処理を打ち切る。なお、情報処理装置５の処理は、第二の対応付け処理の処理時間が所定時間を超えていない場合にはステップＳ１５に戻り、処理時間が所定時間を超える場合には次のステップＳ２４に移行する。

【0067】

ステップＳ２４に進むと、第二の判定部５４は、ステップＳ１５で算出された特徴点位置が妥当か否かを判定、つまり第二の対応付け処理の成否判定を行う。具体的には、第二の判定部５４は、ステップＳ２１で予測された特徴点の座標と、ステップＳ１５で算出された特徴点位置とのユークリッド距離を算出する。そして、第二の判定部５４は、算出したユークリッド距離が閾値の距離以上であれば、第二の対応付け処理が失敗であると判定する。一方、算出したユークリッド距離が閾値の距離未満であり、第二の対応付け処理が成功であると判定した場合、第二の判定部５４は、その成功と判定された特徴点の座標を算出部１５に送る。これにより、算出部１５では、ステップＳ１６において、その特徴点の座標を基にカメラの位置姿勢を算出する。

【0068】

＜第二の実施形態に係る効果＞
前述したように、第二の実施形態では、第二の対応付け処理の実施前に、カメラの位置姿勢を予測して特徴点の位置を予測するとともに、その予測した特徴点の特徴量を、予測したカメラ位置姿勢に合わせて補正する。また、第二の実施形態では、第二の対応付け処理を所定の時間で打ち切るとともに、第二の対応付け処理に失敗した誤った特徴点の対応関係を除去する。これにより、第二の実施形態によれば、例えば画像の輝度変化によって特徴点の対応付けに失敗するような場合でも、特徴点の対応付け数を増加させることができ、安定して高い精度で、また短時間で位置姿勢を算出することが可能となる。

【0069】

＜第二の実施形態の変形例＞
第二の実施形態の場合、特徴点位置の予測処理では、カメラの位置姿勢の履歴から予測したカメラ位置からの画像に、三次元的な特徴点位置を投影することで、特徴点の予測位置を算出しているが、この例には限定されない。例えば、情報処理装置は、画像上の特徴点の位置の履歴を保持しておくことで、時刻ｔ－２の画像上の特徴点から時刻ｔ－１の画像上の特徴点への移動速度を算出して、時刻ｔの画像上での特徴点位置を予測してもよい。

【0070】

また本実施形態において、特徴量の補正処理では、カメラの位置姿勢の予測値に基づくホモグラフィー変換によって特徴点の周囲のパッチを算出しているが、この例には限定されない。例えば、カメラの位置姿勢の変化による画像の特徴量の変化を補正できるのであれば、ホモグラフィー変換に限らず、Ａｆｆｉｎｅ変換やスキュー変換、回転する変換等が用いられてもよい。また情報処理装置は、加速度センサなどを搭載し、そのセンサ出力を基に重力軸が一致するよう画像を回転させてからパッチを切り出してもよい。

【0071】

また第二の実施形態における第二の判定部５４の成否判定処理には、前述した第一の実施形態の第一の判定部１３について変形例として述べた成否判定方法が適用されてもよい。
また第二の実施形態では、第二の対応付け処理の処理時間が所定時間を超える場合に第二の対応付け処理を停止する（打ち切る）としたが、この例には限定されない。例えば、位置姿勢算出の出力間隔が所定値を維持できる方法であればよく、所定の特徴点数を処理したならば第二の対応付け処理が打ち切られる構成であってもよい。この場合、情報処理装置は、例えばステップＳ２３に代えて、例えばステップＳ２４の後に所定の特徴点数を処理したかを判定する処理を行い、所定個数の特徴点の対応付け処理に成功した場合、第二の対応付け処理を停止する構成を有すればよい。

【0072】

また第二の実施形態では、予め全ての特徴点に対して特徴点位置予測および特徴量補正を実施した後、一つの特徴点について第二の対応付け処理を行う毎に処理停止を実施する例を述べたが、この例には限定されない。例えば、情報処理装置は、ステップＳ２１の特徴点位置の予測処理、ステップＳ２２の特徴量補正処理、ステップＳ１５の第二の対応付け処理を、特徴点毎に実施し、処理の停止判定を行う構成であってもよい。この構成の場合、処理が停止された特徴点に対する特徴点位置の予測処理、特徴量の補正処理は実施する必要が無くなり、所定の時間内で、より多くの特徴点に対して第二の対応付け処理を実施することが可能できるようになる。

【0073】

第二の実施形態の情報処理装置は、予測部５１、補正部５２、処理停止部５３、および第二の判定部５４を有する例を示したが、これらの全てを有する必要はなく、一部のみを有する構成でもよい。また、情報処理装置は、予測部５１から第二の判定部５４までのすべてを実装している場合であっても、例えばそれらの一部のみを選択的に利用する構成になされていてもよい。例えば、情報処理装置は、カメラの移動量が所定量より大きい場合のみ、特徴点の位置予測や特徴量の補正を行う構成であってもよい。

【0074】

また情報処理装置は、第一の実施形態の変形例で述べたＧＵＩを提示する構成となされていてもよい。第二の実施形態においてＧＵＩを提示する場合、情報処理装置は、予測部５１、補正部５２、処理停止部５３、および第二の判定部５４が算出した各情報の全て若しくは一部を提示する構成であってもよい。
また情報処理装置は、ＧＵＩを介して第一の実施形態で述べたようなパラメータを調整可能にしてもよく、この場合、輝度変化が生じても安定して、高い精度でカメラの位置姿勢を算出することができるようになる。

【0075】

また情報処理装置は、予測部５１が予測した特徴点の予測位置と、第二の対応付け部１４が算出した特徴点の位置とを結ぶ線分等を表示してもよい。このような表示を行った場合、ユーザは、特徴点毎の予測位置との誤差を容易に確認することができるようになる。
また情報処理装置は、補正部５２が補正した特徴量、すなわち本実施形態では特徴点周りの小パッチを表示してもよい。この場合、ユーザは、正しい特徴点と対応付けられたかどうかを容易に確認することができるようになる。
また情報処理装置は、処理停止部５３が処理を停止させる（打ち切る）までに実行した特徴点数を表示してもよい。この場合、ユーザは、処理停止までに実行された特徴点数を把握することができる。
また情報処理装置は、第二の判定部５４において対応付け処理が失敗したと判定した特徴点を表示してもよい。この場合、ユーザは、画像中のどの点が位置姿勢算出に使用されなかったのか把握することができる。

【0076】

＜第三の実施形態＞
第三の実施形態では、環境の照明条件やカメラの露出が変化して画像の輝度が変化する場合に、移動ロボットの移動速度あるいは姿勢変更の動作速度を低下させて、安定した制御を実現する構成について説明する。第三の実施形態の情報処理装置の場合、算出部１５は、移動ロボットの移動速度あるいは姿勢変更の動作速度等を制御するロボット制御値を算出する制御値算出部としての機能も有する。なお、第三の実施形態では、第一の実施形態で用いた図１の機能ブロック図と図３のフローチャートを流用して説明する。

【0077】

第三の実施形態の場合、算出部１５には、第一の判定部１３の成否判定結果も入力される。第三の実施形態の算出部１５は、前述したカメラの位置姿勢算出処理に加え、第一の判定部１３からの成否判定結果を基に、移動ロボットの移動速度あるいは姿勢変更の動作速度等を制御するロボット制御値を算出する処理をも行う。特に本実施形態では、環境の照明条件やカメラの露出が変化して画像の輝度が変化することで成否判定結果が失敗を示すような場合、算出部１５は、移動ロボットの移動速度や姿勢変更の動作速度を低下させるロボット制御値を算出して出力する。

【0078】

第三の実施形態の場合、算出部１５から出力されたロボット制御値は、移動ロボットに内蔵もしくは接続されているロボット制御装置１００に送られる。ロボット制御装置１００は、算出部１５からのロボット制御値に基づいて、不図示の移動ロボットのモーターの駆動制御を行う。本実施形態の場合、ロボット制御値に基づいて制御されるモーターは、移動ロボットを移動させるモーターや移動ロボットの姿勢を変化させるモーターであるとする。ロボット制御装置１００は、ロボット制御値に基づいて当該モーターの回転速度を制御する。これにより、移動ロボットの移動速度や姿勢変更の動作速度等が制御される。すなわち本実施形態では、照明条件や露出の変化で画像の輝度が変化して成否判定結果が失敗であった場合、移動ロボットの移動速度が低下、もしくは姿勢変更の動作速度が低下するように制御されることになる。

【0079】

第三の実施形態において、成否判定結果に基づいた算出部１５によるロボット制御値の算出および出力処理は、例えば図３に示したステップＳ１６の位置姿勢算出処理が行われた後に実行される。ここで、算出部１５は、カメラの位置姿勢を用いて既知の方法により移動ロボットを制御するロボット制御値を算出する。このとき、算出部１５は、第一の判定部１３の成否判定結果が対応付け処理の失敗を示す場合には、カメラの位置姿勢を基に算出したロボット制御値に対し、所定の係数（例えば０．５）を掛けたロボット制御値を算出する。そして、算出部１５は、そのロボット制御値をロボット制御装置１００に出力する。これにより、ロボット制御装置１００では、０．５の係数が掛けられたロボット制御値に基づく移動ロボットの制御、つまり位置姿勢のみに基づく制御よりも、移動ロボットの移動速度や姿勢変更の動作速度が低下した制御が行われる。

【0080】

＜第三の実施形態に係る効果＞
第三の実施形態によれば、照明条件や露出の変化により画像の輝度が変化する場合に、移動ロボットの移動速度や姿勢変化の速度を低下させることで、位置姿勢が安定した高い精度で移動ロボットを制御することができる。

【0081】

＜第三の実施形態の変形例＞
第三の実施形態におけるロボット制御値は、移動ロボットの速度や角速度を変化させることが可能な信号であればよく、前述したモーターの回転速度を制御する信号に限定されるものではない。例えば、ロボット制御値は、電圧値やＰＷＭ（パルス幅変調）の周期でもよく、任意である。また例えば、移動ロボットがモーター駆動ではなく、空圧アクチュエータにより駆動されるような場合、ロボット制御値はその空気圧を制御する値となる。また例えば、電磁気を用いたリニア駆動によって移動ロボットが駆動される場合、ロボット制御値は電流の周波数を制御する値となる。

【0082】

またロボット制御値の調整方法についても、照明条件や露出の変化で画像の輝度が変化する場合に移動ロボットの速度や角速度を低下させることができれば、任意の方法を用いることができる。例えば、情報処理装置は、第二の対応付け部１４が対応付けた特徴点数が減少するほど、移動ロボットの速度や角速度を低下させるように構成されていてもよい。例えば特徴点の対応付け数が３０点以上の場合は、位置姿勢に基づくロボット制御値に掛ける係数を１とし、例えば３０点未満になればその対応付け数と１／３０との積算値を係数として、位置姿勢に基づくロボット制御値に掛けるようにする。あるいは例えば、対応付けされた特徴点数に反比例した係数を、位置姿勢に基づくロボット制御値に掛けるようにしてもよい。

【0083】

本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける一つ以上のプロセッサがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
上述の実施形態は、何れも本発明を実施するにあたっての具体化の例を示したものに過ぎず、これらによって本発明の技術的範囲が限定的に解釈されてはならないものである。すなわち、本発明は、その技術思想、又はその主要な特徴から逸脱することなく、様々な形で実施することができる。

【符号の説明】

【0084】

１：情報処理装置、１１：画像入力部、１２：第一の対応付け部、１３：第一の判定部、１４：第二の対応付け部、１５：算出部、５１：予測部、５２：補正部、５３：処理停止部、５４：第二の判定部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】