(57)【特許請求の範囲】
【請求項１】
処理対象の撮像画像上の特徴点と予め保持されているマップ点とをマッチングし、マッチングに成功した前記特徴点と対応する前記マップ点を前記撮像画像上に投影した投影点との誤差に基づいて、処理対象の前記撮像画像を撮像したカメラの位置と姿勢を推定する位置・姿勢推定装置であって、
前記マップ点の周辺形状を特定し、特定した前記周辺形状が平面であるか否かを判定する判定手段と、
前記周辺形状が平面ではない前記マップ点の周辺形状の歪みを算出する第１算出手段と、
算出された前記周辺形状の歪みが小さいほど、前記周辺形状が平面ではない前記マップ点の優先度を大きく算出する第２算出手段と、
前記周辺形状が平面である前記マップ点に対応する前記誤差に付加する重みと比較して、前記周辺形状が平面ではない前記マップ点に対応する前記誤差に付加する重みを小さくすることによって、前記カメラの位置と姿勢の推定における前記周辺形状が平面である前記マップ点の寄与率と比較して、前記周辺形状が平面ではない前記マップ点の寄与率が小さくなるように調整する調整手段であって、前記優先度が小さいほど、前記周辺形状が平面ではない前記マップ点に対応する前記誤差に付加する重みを小さくする該調整手段と、
を備え、
前記マップ点は、撮像対象における特徴点に対応する世界座標系の三次元点である、
ことを特徴とする位置・姿勢推定装置。
【請求項２】
前記調整手段は、処理対象の撮像画像上の前記特徴点とのマッチング対象から前記周辺形状が平面ではない前記マップ点を除外することで、前記周辺形状が平面ではない前記マップ点の寄与率をゼロにする、
ことを特徴とする請求項１に記載の位置・姿勢推定装置。
【請求項３】
前記調整手段は、処理対象の撮像画像上の前記特徴点とのマッチングに成功した前記マップ点から、前記周辺形状が平面ではない前記マップ点の比率が所定値以下となるように前記周辺形状が平面ではない前記マップ点を除外することで、前記カメラの位置と姿勢の前記推定における前記周辺形状が平面ではない前記マップ点の寄与率を小さくする、
ことを特徴とする請求項１に記載の位置・姿勢推定装置。
【請求項４】
処理対象の撮像画像上の特徴点と予め保持されているマップ点とをマッチングし、マッチングに成功した前記特徴点と対応する前記マップ点を前記撮像画像上に投影した投影点との誤差に基づいて、処理対象の前記撮像画像を撮像したカメラの位置と姿勢を推定する位置・姿勢推定装置であって、
前記マップ点の周辺形状を特定し、特定した前記周辺形状が平面であるか否かを判定する判定手段と、
前記カメラの位置と姿勢の推定における前記周辺形状が平面である前記マップ点の寄与率と比較して、前記周辺形状が平面ではない前記マップ点の寄与率が小さくなるように調整して、処理対象の撮像画像上の前記特徴点と前記周辺形状が平面である前記マップ点とを最初にマッチングさせ、マッチングに成功した前記特徴点と前記マップ点との組数が所定の閾値に満たない場合に、マッチングの際の条件を緩和し、緩和した前記条件にしたがって、再マッチングを行わせる調整手段と、
を備え、
前記マップ点は、撮像対象における特徴点に対応する世界座標系の三次元点である、
ことを特徴とする位置・姿勢推定装置。
【請求項５】
前記調整手段は、マッチングに成功した前記組数が前記閾値に到達するまで、マッチングの際の前記条件を緩和し、再マッチングを繰り返し行わせる、
ことを特徴とする請求項４に記載の位置・姿勢推定装置。
【請求項６】
マッチングの際の前記条件の緩和は、前記周辺形状が平面ではない前記マップ点の一部をマッチング対象にすることを含んでいる、
ことを特徴とする請求項４又は５に記載の位置・姿勢推定装置。
【請求項７】
前記周辺形状が平面ではない前記マップ点の周辺形状の歪みを算出する第１算出手段と、
算出された前記周辺形状の歪みが小さいほど、前記周辺形状が平面ではない前記マップ点の優先度を大きく算出する第２算出手段と、
を、更に備え、
前記調整手段は、処理対象の撮像画像上の前記特徴点と前記周辺形状が平面である前記マップ点とを最初にマッチングさせ、マッチングに成功した前記組数が前記閾値に満たない場合に、前記優先度が高い順に前記周辺形状が平面ではない前記マップ点をマッチング対象とする、
ことを特徴とする請求項６に記載の位置・姿勢推定装置。
【請求項８】
前記マップ点は、前記カメラで前記撮像対象を撮像した２枚の撮像画像上の前記特徴点をステレオマッチングすることで算出される、
ことを特徴とする請求項１乃至７のいずれか一に記載の位置・姿勢推定装置。
【請求項９】
前記マップ点は、前記カメラで前記撮像対象を撮像した撮像画像上の前記特徴点を、３次元距離センサにより検出された前記撮像対象の３次元座標と対応付けることで算出される、
ことを特徴とする請求項１乃至８のいずれか一に記載の位置・姿勢推定装置。
【請求項１０】
処理対象の撮像画像上の特徴点と予め保持されているマップ点とをマッチングし、マッチングに成功した前記特徴点と対応する前記マップ点を前記撮像画像上に投影した投影点との誤差に基づいて、処理対象の前記撮像画像を撮像したカメラの位置と姿勢を推定する位置・姿勢推定装置の位置・姿勢推定方法であって、
前記マップ点の周辺形状を特定し、
特定した前記周辺形状が平面であるか否かを判定し、
前記周辺形状が平面ではない前記マップ点の周辺形状の歪みを算出し、
算出された前記周辺形状の歪みが小さいほど、前記周辺形状が平面ではない前記マップ点の優先度を大きく算出し、
前記周辺形状が平面である前記マップ点に対応する前記誤差に付加する重みと比較して、前記周辺形状が平面ではない前記マップ点に対応する前記誤差に付加する重みを小さくすることによって、前記カメラの位置と姿勢の推定における前記周辺形状が平面である前記マップ点の寄与率と比較して、前記周辺形状が平面ではない前記マップ点の寄与率が小さくなるように調整し、
前記マップ点は、撮像対象における特徴点に対応する世界座標系の三次元点であり、
前記調整では、前記優先度が小さいほど、前記周辺形状が平面ではない前記マップ点に対応する前記誤差に付加する重みを小さくする、
ことを特徴とする位置・姿勢推定方法。
【請求項１１】
処理対象の撮像画像上の特徴点と予め保持されているマップ点とをマッチングし、マッチングに成功した前記特徴点と対応する前記マップ点を前記撮像画像上に投影した投影点との誤差に基づいて、処理対象の前記撮像画像を撮像したカメラの位置と姿勢を推定する位置・姿勢推定装置の位置・姿勢推定方法であって、
前記マップ点の周辺形状を特定し、
特定した前記周辺形状が平面であるか否かを判定し、
前記カメラの位置と姿勢の推定における前記周辺形状が平面である前記マップ点の寄与率と比較して、前記周辺形状が平面ではない前記マップ点の寄与率が小さくなるように調整して、処理対象の撮像画像上の前記特徴点と前記周辺形状が平面である前記マップ点とを最初にマッチングさせ、マッチングに成功した前記特徴点と前記マップ点との組数が所定の閾値に満たない場合に、マッチングの際の条件を緩和し、緩和した前記条件にしたがって、再マッチングを行わせ、
前記マップ点は、撮像対象における特徴点に対応する世界座標系の三次元点である、
ことを特徴とする位置・姿勢推定方法。
【請求項１２】
処理対象の撮像画像上の特徴点と予め保持されているマップ点とをマッチングし、マッチングに成功した前記特徴点と対応する前記マップ点を前記撮像画像上に投影した投影点との誤差に基づいて、処理対象の前記撮像画像を撮像したカメラの位置と姿勢を推定する位置・姿勢推定装置のコンピュータに、
前記マップ点の周辺形状を特定し、
特定した前記周辺形状が平面であるか否かを判定し、
前記周辺形状が平面ではない前記マップ点の周辺形状の歪みを算出し、
算出された前記周辺形状の歪みが小さいほど、前記周辺形状が平面ではない前記マップ点の優先度を大きく算出し、
前記周辺形状が平面である前記マップ点に対応する前記誤差に付加する重みと比較して、前記周辺形状が平面ではない前記マップ点に対応する前記誤差に付加する重みを小さくすることによって、前記カメラの位置と姿勢の推定における前記周辺形状が平面である前記マップ点の寄与率と比較して、前記周辺形状が平面ではない前記マップ点の寄与率が小さくなるように調整する、
処理を実行させ、
前記マップ点は、撮像対象における特徴点に対応する世界座標系の三次元点であり、
前記調整する処理では、前記優先度が小さいほど、前記周辺形状が平面ではない前記マップ点に対応する前記誤差に付加する重みを小さくする、
ことを特徴とするプログラム。
【請求項１３】
処理対象の撮像画像上の特徴点と予め保持されているマップ点とをマッチングし、マッチングに成功した前記特徴点と対応する前記マップ点を前記撮像画像上に投影した投影点との誤差に基づいて、処理対象の前記撮像画像を撮像したカメラの位置と姿勢を推定する位置・姿勢推定装置のコンピュータに、
前記マップ点の周辺形状を特定し、
特定した前記周辺形状が平面であるか否かを判定し、
前記カメラの位置と姿勢の推定における前記周辺形状が平面である前記マップ点の寄与率と比較して、前記周辺形状が平面ではない前記マップ点の寄与率が小さくなるように調整して、処理対象の撮像画像上の前記特徴点と前記周辺形状が平面である前記マップ点とを最初にマッチングさせ、マッチングに成功した前記特徴点と前記マップ点との組数が所定の閾値に満たない場合に、マッチングの際の条件を緩和し、緩和した前記条件にしたがって、再マッチングを行わせる、
処理を実行させ、
前記マップ点は、撮像対象における特徴点に対応する世界座標系の三次元点である、
ことを特徴とするプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、位置・姿勢推定装置、位置・姿勢推定方法、及び、プログラムに関する。
【背景技術】
【０００２】
近年、拡張現実感（Ａｕｇｍｅｎｔｅｄ  Ｒｅａｌｉｔｙ：ＡＲ）に関する技術の開発が盛んに行われている。ＡＲは、人間が知覚する現実環境をコンピュータにより拡張する技術の総称である。
【０００３】
ＡＲの一例として、携帯型のパーソナルコンピュータやタブレットや高機能携帯電話などの携帯情報端末装置に接続されているカメラで撮像した画像（以下、撮像画像という）を、携帯情報端末装置の表示画面上に表示する際に、コンピュータグラフィックス等で付加情報を重畳表示し、ユーザの作業支援を実現する技術が知られている（例えば、非特許文献１を参照）。
【０００４】
付加情報を適した位置に重畳表示させるためには、カメラの位置と姿勢を推定する必要があり、その推定精度が非常に重要となる。カメラの位置と姿勢の推定方法としては、撮像画像に基づいてカメラの位置と姿勢を推定する技術が知られている。
【０００５】
撮像画像に基づいてカメラの位置と姿勢を推定する技術の一つとして、撮像画像に含まれる特徴的な点（以下、特徴点という）を用いる方法が知られている（例えば、特許文献１を参照）。特徴点は、通常、注目点の近傍領域での濃淡変動が大きく、且つ濃淡変動によって注目点の位置が一意に規定されることを基準として検出される点である。
【０００６】
特徴点を用いてカメラの位置と姿勢を推定する方法は、予め算出された特徴点の３次元座標（以下、マップ点という）を用い、処理対象の撮像画像中の特徴点とマップ点とをマッチング処理により対応付けることで、カメラの位置と姿勢を推定するものである。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００５−３２６２７４号公報
【非特許文献】
【０００８】
【非特許文献１】石井裕剛ら，  拡張現実感を利用した原子力発電プラントの解体支援手法の提案と評価，  日本バーチャルリアリティ学会論文誌，  Ｖｏｌ．１３，  Ｎｏ．２，  ｐｐ．２８９−３００，  ２００８年６月
【発明の概要】
【発明が解決しようとする課題】
【０００９】
しかしながら、特徴点の中には、カメラの動きに対する特徴点の近傍領域における濃淡変動の特徴の変化が小さい特徴点と、濃淡変動の特徴の変化が大きい特徴点がある。カメラの動きに対する濃淡変動の特徴の変化が小さい特徴点は、マッチング処理において誤マッチングが生じる確率が低い。また、カメラの動きに対する濃淡変動の特徴の変化が小さい特徴点は、検出し易く検出位置の精度も高い。
【００１０】
これに対し、カメラの動きに対する濃淡変動の特徴の変化が大きい特徴点は、マッチング処理において誤マッチングが生じる確率が高く、特徴点として検出された場合の検出位置の精度も低い。このように、特徴点の中には、誤マッチングが生じる確率が高い特徴点がある。
【００１１】
特許文献１に開示されている方法では、撮像画像において検出された全ての特徴点を同等に扱ってカメラの位置と姿勢を推定している。そのため、誤マッチングが原因となり、カメラの位置と姿勢の推定精度が劣化してしまうという問題がある。
【００１２】
一つの側面では、本発明は、誤マッチングの影響を低減することで、カメラの位置と姿勢の推定精度を向上させることを可能とする位置・姿勢推定装置、位置・姿勢推定方法、及び、プログラムを提供することを課題とする。
【課題を解決するための手段】
【００１３】
一態様における位置・姿勢推定装置は、処理対象の撮像画像上の特徴点と予め保持されているマップ点とをマッチングし、マッチングに成功した特徴点と対応するマップ点を撮像画像上に投影した投影点との誤差に基づいて、処理対象の撮像画像を撮像したカメラの位置と姿勢を推定する位置・姿勢推定装置であって、マップ点の周辺形状を特定し、特定した周辺形状が平面であるか否かを判定する判定手段と、周辺形状が平面ではないマップ点の周辺形状の歪みを算出する第１算出手段と、算出された周辺形状の歪みが小さいほど、周辺形状が平面ではないマップ点の優先度を大きく算出する第２算出手段と、周辺形状が平面であるマップ点に対応する誤差に付加する重みと比較して、周辺形状が平面ではないマップ点に対応する誤差に付加する重みを小さくすることによって、カメラの位置と姿勢の推定における周辺形状が平面であるマップ点の寄与率と比較して、周辺形状が平面ではないマップ点の寄与率が小さくなるように調整する調整手段であって、優先度が小さいほど、周辺形状が平面ではないマップ点に対応する誤差に付加する重みを小さくする調整手段と、を備え、マップ点は、撮像対象における特徴点に対応する世界座標系の三次元点である、ことを特徴としている。
【００１４】
一態様における位置・姿勢推定方法は、処理対象の撮像画像上の特徴点と予め保持されているマップ点とをマッチングし、マッチングに成功した特徴点と対応するマップ点を撮像画像上に投影した投影点との誤差に基づいて、処理対象の撮像画像を撮像したカメラの位置と姿勢を推定する位置・姿勢推定装置の位置・姿勢推定方法であって、マップ点の周辺形状を特定し、特定した周辺形状が平面であるか否かを判定し、周辺形状が平面ではないマップ点の周辺形状の歪みを算出し、算出された周辺形状の歪みが小さいほど、周辺形状が平面ではないマップ点の優先度を大きく算出し、周辺形状が平面であるマップ点に対応する誤差に付加する重みと比較して、周辺形状が平面ではないマップ点に対応する誤差に付加する重みを小さくすることによって、カメラの位置と姿勢の推定における周辺形状が平面であるマップ点の寄与率と比較して、周辺形状が平面ではないマップ点の寄与率が小さくなるように調整し、マップ点は、撮像対象における特徴点に対応する世界座標系の三次元点であり、前記調整では、優先度が小さいほど、周辺形状が平面ではないマップ点に対応する誤差に付加する重みを小さくする、ことを特徴としている。
【００１５】
一態様におけるプログラムは、処理対象の撮像画像上の特徴点と予め保持されているマップ点とをマッチングし、マッチングに成功した特徴点と対応するマップ点を撮像画像上に投影した投影点との誤差に基づいて、処理対象の撮像画像を撮像したカメラの位置と姿勢を推定する位置・姿勢推定装置のコンピュータに、マップ点の周辺形状を特定し、特定した周辺形状が平面であるか否かを判定し、周辺形状が平面ではないマップ点の周辺形状の歪みを算出し、算出された周辺形状の歪みが小さいほど、周辺形状が平面ではないマップ点の優先度を大きく算出し、周辺形状が平面であるマップ点に対応する誤差に付加する重みと比較して、周辺形状が平面ではないマップ点に対応する誤差に付加する重みを小さくすることによって、カメラの位置と姿勢の推定における周辺形状が平面であるマップ点の寄与率と比較して、周辺形状が平面ではないマップ点の寄与率が小さくなるように調整する、処理を実行させ、マップ点は、撮像対象における特徴点に対応する世界座標系の三次元点であり、前記調整する処理では、優先度が小さいほど、周辺形状が平面ではないマップ点に対応する誤差に付加する重みを小さくする、ことを特徴としている。
【発明の効果】
【００１６】
一つの側面では、誤マッチングの影響を低減することが可能となる。これにより、カメラの位置と姿勢の推定精度を向上させることが可能となる。
【図面の簡単な説明】
【００１７】
【図１】実施形態１における情報端末装置の構成例を示す機能ブロック図である。
【図２】実施形態１におけるマップ点情報記憶部の構成例を示す図である。
【図３】特徴点の種別について説明するための図である。
【図４】ＡとＢは、いずれも、各種別の特徴について説明するための図である。
【図５】実施形態１における種別特定の方法の例を説明するための図である。
【図６】Ａは、カメラ座標系と画像平面座標系について説明するための図であり、Ｂは、投影点について説明するための図である。
【図７】実施形態１におけるカメラの位置・姿勢の推定方法の例について説明するための図である。
【図８】実施形態１におけるマップ点情報設定処理のフローを説明するためのフローチャートの例である。
【図９】実施形態１におけるカメラ位置・姿勢推定処理のフローを説明するためのフローチャートの例である。
【図１０】実施形態２における情報端末装置の構成例を示す機能ブロック図である。
【図１１】実施形態２におけるマップ点情報記憶部の構成例を示す図である。
【図１２】実施形態２におけるマッチング条件記憶部の構成例を示す図である。
【図１３】実施形態２における優先度の算出方法の例を説明するための図である。
【図１４】実施形態２における優先度を算出する関数の例を表したグラフである。
【図１５】実施形態２におけるマップ点情報設定処理のフローを説明するためのフローチャートの例である。
【図１６】実施形態２におけるカメラ位置・姿勢推定処理のフローを説明するためのフローチャートの例の一部である。
【図１７】実施形態２におけるカメラ位置・姿勢推定処理のフローを説明するためのフローチャートの例の他の一部である。
【図１８】実施形態３における情報端末装置の構成例を示す機能ブロック図である。
【図１９】実施形態３における重み情報記憶部の構成例を示す図である。
【図２０】実施形態３における重み情報記憶部の別の構成例を示す図である。
【図２１】実施形態４における情報端末装置の構成例を示す機能ブロック図である。
【図２２】実施形態４における種別特定の方法の例を説明するための図である。
【図２３】実施形態４におけるマップ点情報設定処理のフローを説明するためのフローチャートの例である。
【図２４】実施形態における情報端末装置のハードウェア構成の例を示す図である。
【発明を実施するための形態】
【００１８】
以下に本発明の実施の形態について図面を参照しながら詳細に説明する。
【００１９】
（実施形態１）
図１は、本実施形態１における情報端末装置１の構成例を示す機能ブロック図である。本実施形態１における情報端末装置１は、処理対象の撮像画像中の特徴点と予め保持しているマップ点とをマッチング処理により対応付けることで、カメラ（不図示）の位置と姿勢を推定する位置・姿勢推定装置である。本実施形態１における情報端末装置１は、図１に示すように、画像取得部１０と、記憶部２０と、出力部３０と、制御部４０と、を備えている。
【００２０】
画像取得部１０は、例えば、機器インターフェース、あるいは、通信モジュールなどを備えており、接続されている撮像装置（カメラ）により撮像された撮像画像を順次取得する。そして、画像取得部１０は、取得した撮像画像を、制御部４０に出力する。この際、画像取得部１０は、取得した撮像画像をフレームメモリ（不図示）に一時的に格納し、制御部４０が処理のタイミングに合わせて撮像画像をフレームメモリから順次取得するようにしてもよい。
【００２１】
記憶部２０は、例えば、Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ（ＲＡＭ）、Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ（ＲＯＭ）、Ｈａｒｄ  Ｄｉｓｋ  Ｄｒｉｖｅ（ＨＤＤ）などを備えている。記憶部２０は、制御部４０が備える、例えば、Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ（ＣＰＵ）のワークエリア、情報端末装置１全体を制御するための動作プログラムなどの各種プログラムを格納するプログラムエリア、各種データを格納するデータエリアとして機能する。
【００２２】
また、記憶部２０は、図１に示すように、マップ点情報記憶部２１として機能する。マップ点情報記憶部２１は、マップ点ごとにそのマップ点に関する情報（以下、マップ点情報という）を対応付けて格納している。なお、マップ点は、上述したように、特徴点の３次元点であり、マッチング処理の際に撮像画像上の特徴点とのマッチング対象候補となる点である。
【００２３】
図２は、本実施形態１におけるマップ点情報記憶部２１の構成例を示す図である。マップ点情報記憶部２１は、例えば、図２に示すように、複数のマップ点とそれらのマップ点情報をテーブル化して格納してる。本実施形態１におけるマップ点情報記憶部２１は、「マップ点識別子」ごとに、「３次元座標」と、「種別」と、「特徴量」と、「画像パターンテンプレート」と、を対応付けて格納している。なお、マップ点情報記憶部２１に格納されている各種のマップ点情報は、後述するように撮像画像から抽出されるが、例えば、撮像対象のＣＡＤデータがある場合には、ＣＡＤデータからこれらのマップ点情報を抽出し、抽出したマップ点情報を予め格納するようにしてもよい。
【００２４】
「マップ点識別子」は、マップ点を一意に識別可能な識別子である。「３次元座標」は、対応するマップ点の世界座標系における３次元座標であり、マップ点情報設定部４２（詳しくは後述）により算出される。「種別」は、対応するマップ点の種別であり、マップ点情報設定部４２により特定される。
【００２５】
「特徴量」は、対応するマップ点の近傍領域の濃淡変動の特徴を数値ベクトル量化したものであり、マッチング処理の際に用いられる。「画像パターンテンプレート」は、対応するマップ点の近傍領域の画像データであり、マッチング処理の際に用いることで、相関性を利用して特徴点とマップ点とのマッチングを効率的に行うことが可能となる。
【００２６】
マップ点（特徴点）の種別としては、本実施形態１においては、「平面模様点」と「物理端点」の２種類がある。「平面模様点」は、縁を除く平面上に存在する特徴点であり、「物理端点」は、「平面模様点」以外の特徴点、つまり、物体の角など平面の縁に存在する特徴点である。
【００２７】
図３は、特徴点の種別について説明するための図である。図３に示すように、特徴点ＭＰ１〜４は、縁を除く平面上に存在することから平面模様点である。これに対し、特徴点ＭＰ５とＭＰ６は、物体（キューブ）の角の点であることから物理端点である。
【００２８】
図４Ａと図４Ｂは、いずれも、各種別のマップ点（特徴点）の特徴について説明するための図であり、カメラの位置と姿勢を変化させた場合の、特徴点ＭＰ３とＭＰ５の各近傍領域における濃淡変動の特徴の変化について説明するための図である。
【００２９】
カメラの位置と姿勢が変化して、撮像画像が図４Ａに示す画像から図４Ｂに示す画像に変化したとすると、平面模様点である特徴点ＭＰ３の近傍領域における濃淡変動の特徴の変化は、図４に示すように、非常に小さい。このように、平面模様点である特徴点は、カメラの動きに対する近傍領域における濃淡変動の特徴の変化が小さい。これに対し、物理端点である特徴点ＭＰ５の近傍領域における濃淡変動の特徴の変化は、図４に示すように、非常に大きい。これは、カメラの位置と姿勢が変化して、物理的な遮蔽（オクルージョン）が発生し、物体（キューブ）の右側面が前面の影に入ってしまい、更に、近傍領域に背部の壁が写り込んでしまっているからである。このように、物理端点である特徴点は、カメラの動きに対する近傍領域における濃淡変動の特徴の変化が大きい。
【００３０】
図１に戻り、出力部３０は、例えば、機器インターフェース、あるいは、通信モジュールなどを備えており、出力情報生成部４６（詳しくは後述）により生成された出力情報を、接続されている表示装置（不図示）に出力する。
【００３１】
制御部４０は、例えば、ＣＰＵなどを備えており、記憶部２０のプログラムエリアに格納されている動作プログラムを実行して、図１に示すように、特徴点検出部４１と、マップ点情報設定部４２と、マッチング条件設定部４３と、マッチング処理部４４と、推定部４５と、出力情報生成部４６としての機能を実現する。また、制御部４０は、動作プログラムを実行して、情報端末装置１全体を制御する制御処理や詳しくは後述のマップ点情報設定処理などの処理を実行する。
【００３２】
特徴点検出部４１は、入力された撮像画像を分析し、注目点の近傍領域における濃淡変動が大きく、濃淡変動によって注目点の画像上の位置が一意に規定される点を特徴点として検出する。特徴点の検出は、ＨａｒｒｉｓオペレータやＦｅａｔｕｒｅ  ｆｒｏｍ  Ａｃｃｅｌｅｒａｔｅｄ  Ｓｅｇｍｅｎｔ  Ｔｅｓｔ（ＦＡＳＴ）特徴検出など任意の技術を用いることが可能である。なお、以下において、特徴点検出部４１により検出された撮像画像上の特徴点を、検出点とも称する。
【００３３】
マップ点情報設定部４２は、マップ点情報記憶部２１の設定を行う。より具体的には、マップ点情報設定部４２は、例えば、逐次残差法や面積相関法といったステレオマッチング法を用いて、特徴点検出部４１により検出された、２枚の撮像画像における特徴点をマッチングする。そして、マップ点情報設定部４２は、マッチングに成功した特徴点の３次元座標をステレオ処理で算出する。
【００３４】
そして、マップ点情報設定部４２は、マッチングに成功した各特徴点において、該特徴点（注目特徴点）と隣接する特徴点（以下、隣接特徴点という）との位置関係を検出する。この際、マップ点情報設定部４２は、予め定められた所定個数（例えば、３以上）の隣接特徴点を、例えば、距離が近い順に選択する。そして、マップ点情報設定部４２は、マッチングに成功した各特徴点の種別を特定する。つまり、マップ点情報設定部４２は、注目特徴点と選択した隣接特徴点が同一平面上に分布している場合には、その注目特徴点の種別は「平面模様点」であると特定し、一方、注目特徴点と選択した隣接特徴点が同一平面上に分布していない場合には、その注目特徴点の種別は「物理端点」であると特定する。
【００３５】
更に、マップ点情報設定部４２は、マッチングに成功した各特徴点の特徴量を算出すると共に、マッチングに成功した各特徴点の画像パターンテンプレートを生成する。そして、マップ点情報設定部４２は、マップ点情報（３次元座標、種別、特徴量、画像パターンテンプレート）をマップ点識別子ごとに対応つけてマップ点情報記憶部２１に格納する。
【００３６】
ここで、図５を参照して、具体例にしたがって、本実施形態１におけるマップ点（特徴点）の種別特定の方法について説明する。図５は、本実施形態１における種別特定の方法の例を説明するための図である。
【００３７】
注目特徴点ＭＰ７の隣接特徴点として特徴点ＭＰ８〜１１がマップ点情報設定部４２により選択されたとすると、図５に示すように、注目特徴点ＭＰ７と隣接特徴点８〜１１の全てが同一平面上に分布していない。したがって、この場合、マップ点情報設定部４２は、注目特徴点ＭＰ７の種別を「物理端点」であると特定する。
【００３８】
これに対して、注目特徴点ＭＰ１２の隣接特徴点として特徴点ＭＰ１３〜１６がマップ点情報設定部４２により選択されたとすると、図５に示すように、注目特徴点ＭＰ１２と隣接特徴点１３〜１６の全てが同一平面上に分布している。したがって、この場合、マップ点情報設定部４２は、注目特徴点ＭＰ１２の種別を「平面模様点」であると特定する。
【００３９】
図１に戻り、マッチング条件設定部４３は、特徴点検出部４１により検出された撮像画像上の特徴点（検出点）とマッチングさせるマッチング対象となるマップ点の選択条件などのマッチング条件を設定する。本実施形態１においては、マッチング条件設定部４３は、「平面模様点」のマップ点を選択対象とする選択条件をマッチング条件として設定する。これにより、カメラの動きにより近傍領域における濃淡変動の特徴の変化が大きい「物理端点」を除外してマッチングを行うことができ、誤マッチングの発生を抑制することが可能となる。
【００４０】
マッチング処理部４４は、マッチング条件設定部４３により設定されたマッチング条件（本実施形態１においては選択条件）に従ってマップ点を選択し、特徴点検出部４１により検出された撮像画像上の特徴点（検出点）と選択したマップ点とをマッチングする。より具体的には、マッチング処理部４４は、マップ点情報記憶部２１に格納されている特徴量及び／又は画像パターンテンプレートに基づいて、特徴点（検出点）とマップ点とをマッチングする。マッチング方法としては、例えば、Ｆｒａｍｅ  ｔｏ  Ｆｒａｍｅ  Ｔｒａｃｋｉｎｇ法やＤｅｔｅｃｔｉｏｎ法や両方法をミックスした方法など各種の方法を用いることができる。
【００４１】
本実施形態１においては、マッチング条件設定部４３は、「平面模様点」のマップ点をマッチング対象とする選択条件をマッチング条件として設定することから、マッチング処理部４４は、マップ点情報記憶部２１に格納されているマップ点の中から種別が「平面模様点」のマップ点を選択し、選択したマップ点と検出点とのマッチングを行う。これにより、カメラの動きにより近傍領域における濃淡変動の特徴の変化が大きい「物理端点」を除外してマッチングを行うことができ、誤マッチングの発生を抑制することが可能となる。
【００４２】
ここで、図６を参照して、カメラ座標系などの座標系についてと、実世界（世界座標系）における撮像対象上の特徴点（以下、三次元点という）と対応する三次元点を撮像画像上に投影した点（以下、投影点という）との関係について簡単に説明する。図６Ａは、カメラ座標系と画像平面座標系について説明するための図であり、図６Ｂは、投影点について説明するための図である。
【００４３】
図６Ａを参照して、画像平面座標系の原点Ｏｉは光軸と撮像画像を表す画像平面ＩＰとの交点にとり、画像平面ＩＰの水平方向をＸｉ軸、垂直方向をＹｉ軸にとる。また、カメラの光点をカメラ座標系の原点Ｏｃとし、カメラ座標系のＺｃ軸は光軸の反対方向、Ｘｃ軸は画像平面ＩＰの水平方向と平行に、Ｙｃ軸は画像平面ＩＰの垂直方向と平行にとる。
【００４４】
次に、図６Ｂを参照して、カメラ座標系の原点Ｏｃ（カメラの光点）と三次元点Ｓｐｉ＝（ｘｉ，ｙｉ，ｚｉ）とを結ぶ直線と画像平面ＩＰとの交点が、三次元点Ｓｐｉに対応する投影点Ｘｐ’ｉ＝（ｕ’ｉ，ｖ’ｉ）となる。つまり、カメラ座標系の原点Ｏｃと画像平面座標系の投影点Ｘｐ’ｉと世界座標系の三次元点Ｓｐｉは共線条件を満たす関係にある。
【００４５】
図１に戻り、推定部４５は、マッチング処理部４４によるマッチングに成功した特徴点（検出点）とマップ点との組に基づいて、カメラの位置と姿勢を推定する。より具体的には、推定部４５は、特徴点（検出点）と対応するマップ点を撮像画像上に投影した投影点との距離の２乗和、つまり、特徴点（検出点）と対応するマップ点の投影点との２乗誤差和Ｅが最小となるカメラ姿勢行列Ｍを算出することで、従来技術に従って、カメラの位置と姿勢を推定する。なお、２乗誤差和以外に、平均誤差や誤差和などを用いることも可能である。
【００４６】
マッチングに成功した特徴点（検出点）の座標をＸｐｉ＝（ｕｉ，ｖｉ）とし、対応するマップ点の投影点の座標をＸｐ’ｉ＝（ｕ’ｉ，ｖ’ｉ）とすると、マッチングに成功した特徴点（検出点）と対応するマップ点の投影点との２乗誤差和Ｅは、以下の式１で表すことができる。
Ｅ＝Σ｜Ｘｐｉ−Ｘｐ’ｉ｜２      ・・・・・（式１）
【００４７】
ここで、図７は、本実施形態１におけるカメラの位置・姿勢の推定方法の例について説明するための図である。図７に示す例は、マップ点である三次元点Ｓｐ１〜Ｓｐ６とそれらの投影点Ｘｐ’１〜Ｘｐ’６と三次元点Ｓｐ１〜Ｓｐ６とのマッチングに成功した撮像画像上の特徴点（検出点）Ｘｐ１〜Ｘｐ６との関係を模式的に示したものである。
【００４８】
図１に戻り、出力情報生成部４６は、推定部４５により推定されたカメラの位置と姿勢とに基づいて、表示装置（不図示）の表示画面上に表示されている撮像画像に重畳表示させる内容などの出力情報を生成する。
【００４９】
次に、図８を参照して、本実施形態１におけるマップ点情報設定処理の流れについて説明する。図８は、本実施形態１におけるマップ点情報設定処理のフローを説明するためのフローチャートの例である。本マップ点情報設定処理は、例えば、ユーザによる操作部（不図示）を介した所定の操作（特定のアプリケーションの起動など）をトリガとして開始される。なお、本マップ点情報設定処理は、撮像画像が取得される度、あるいは、所定の間隔で、繰り返し実行されてもよい。
【００５０】
画像取得部１０は、１枚目の撮像画像を取得し、取得した撮像画像を制御部４０に出力する（ステップＳ００１）。特徴点検出部４１は、入力された１枚目の撮像画像の特徴点を検出する（ステップＳ００２）。
【００５１】
そして、画像取得部１０は、２枚目の撮像画像を取得し、取得した撮像画像を制御部４０に出力する（ステップＳ００３）。特徴点検出部４１は、入力された２枚目の撮像画像の特徴点を検出する（ステップＳ００４）。
【００５２】
そして、マップ点情報設定部４２は、１枚目と２枚目の撮像画像の特徴点をステレオマッチングすると共に（ステップＳ００５）、マッチングに成功した各特徴点の３次元座標を算出する（ステップＳ００６）。そして、マップ点情報設定部４２は、マッチングに成功した特徴点ごとに、所定個数の隣接特徴点を選択し（ステップＳ００７）、該特徴点とその隣接特徴点との位置関係を検出する（ステップＳ００８）。
【００５３】
そして、マップ点情報設定部４２は、マッチングに成功した特徴点ごとに種別を特定する（ステップＳ００９）。そして、マップ点情報設定部４２は、マッチングに成功した各特徴点の特徴量を算出すると共に、画像パターンテンプレートを生成する（ステップＳ０１０）。
【００５４】
そして、マップ点情報設定部４２は、マップ点情報（３次元座標、種別、特徴量、画像パターンテンプレート）をマップ点識別子ごとに対応つけてマップ点情報記憶部２１に格納する（ステップＳ０１１）。そして、本処理は終了する。
【００５５】
次に、図９を参照して、本実施形態１におけるカメラ位置・姿勢推定処理の流れについて説明する。図９は、本実施形態１におけるカメラ位置・姿勢推定処理のフローを説明するためのフローチャートの例である。本カメラ位置・姿勢推定処理は、例えば、マップ点情報設定処理が実行されマップ点情報がマップ点情報記憶部２１に格納されたことをトリガとして開始される。
【００５６】
画像取得部１０は、撮像画像を取得し、取得した撮像画像を制御部４０に出力する（ステップＳ２０１）。そして、特徴点検出部４１は、入力された撮像画像の特徴点を検出する（ステップＳ２０２）。そして、マッチング条件設定部４３は、マッチング対象のマップ点の種別を平面模様点とするマッチング条件を設定する（ステップＳ２０３）。
【００５７】
マッチング処理部４４は、種別が平面模様点であるマップ点を選択し（ステップＳ２０４）、検出された特徴点（検出点）と選択したマップ点とをマッチングする（ステップＳ２０５）。そして、推定部４５は、マッチングに成功したマップ点と特徴点（検出点）との組みに基づいて、カメラの位置と姿勢を推定する（ステップＳ２０６）。
【００５８】
そして、制御部４０は、処理の終了が指示されたか否かを判定する（ステップＳ２０７）。制御部４０は、例えば、ユーザが操作部（不図示）を介して所定の操作（例えば、アプリケーションの終了）がなされた否かを判定することで、処理の終了が指示されたか否かを判定する。制御部４０により終了の指示はまだ無いと判定された場合には（ステップＳ２０７；ＮＯ）、処理はステップＳ２０１に戻り、前述の処理を繰り返す。一方、制御部４０により、終了が指示されたと判定された場合には（ステップＳ２０７；ＹＥＳ）、本処理は終了する。
【００５９】
上記実施形態１によれば、情報端末装置１は、撮像画像上の特徴点（検出点）とのマッチング対象候補となる各マップ点の種別を特定し、マップ点の内で種別が平面模様点のマップ点を選択する。そして、情報端末装置１は、特徴点（検出点）と選択したマップ点とをマッチングし、マッチングに成功した特徴点（検出点）とマップ点の投影点との誤差に基づいてカメラの位置と姿勢を推定する。すなわち、誤マッチングの可能性がより高い物理端点をマッチング対象から除外し、カメラの位置と姿勢の推定における物理端点の寄与率をゼロにする。こうすることで、誤マッチングの発生を抑制することが可能となり、カメラの位置と姿勢の推定精度を向上させることが可能となる。
【００６０】
また、上記実施形態１によれば、情報端末装置１は、２枚の撮像画像をステレオマッチングして検出した特徴点の３次元座標を算出する。そして、情報端末装置１は、所定個数の隣接特徴点との位置関係に基づいて、各特徴点の種別を特定する。
【００６１】
（実施形態２）
実施形態１においては、撮像画像上に検出された特徴点（検出点）とマップ点とのマッチング処理時に種別が平面模様点のマップ点のみをマッチング対象とした。本実施形態２においては、マッチングに成功したマップ点と特徴点（検出点）との組数が所定の閾値Ｚ（詳しくは後述）に満たない場合には、マッチングに成功した組数が所定の閾値Ｚ以上となるように、種別が物理端点のマップ点もマッチング対象に含めてマッチング条件を調整する。
【００６２】
こうすることで、推定時に用いるマッチングに成功した特徴点（検出点）とマップ点との組数を所定数以上に維持することが可能となり、平面模様点の数が少なくカメラの位置と姿勢の推定精度が劣化してしまうことが予測される場合であっても、推定精度をより高くすることが可能となる。
【００６３】
図１０は、本実施形態２における情報端末装置１の構成例を示す機能ブロック図である。本実施形態２における情報端末装置１の基本的な構成は、実施形態１の場合と同じである。但し、図１０に示すように、記憶部２０がマッチング条件記憶部２２として更に機能する点と、制御部４０が優先度算出部４７として更に機能する点と、で実施形態１の場合と異なっている。また、マップ点情報記憶部２１が優先度の情報を更に格納する点で実施形態１の場合と異なっている。更に、マップ点情報設定部４２とマッチング条件設定部４３とマッチング処理４４がそれぞれ果たす機能が、実施形態１の場合と若干異なっている。
【００６４】
ここで、図１１を参照して、本実施形態２におけるマップ点情報記憶部２１の構成について説明する。図１１は、本実施形態２におけるマップ点情報記憶部２１の構成例を示す図である。本実施形態２におけるマップ点情報記憶部２１では、図１１に示すように、種別が物理端点のマップ点に対して「優先度」が更に対応付けられて格納されている。こうすることで、マッチング対象として種別が物理端点のマップ点を選択させる際に、優先度に基づいて物理端点のマップ点を選択させることが可能となる。
【００６５】
「優先度」は、マッチング処理部４４が物理端点のマップ点を選択する場合の一つの尺度であり、優先度算出部４７により算出される。なお、本実施形態２においては、優先度の値が高い程、優先度が高いことを示している。優先度については更に後述する。
【００６６】
次に、図１２を参照して、マッチング条件記憶部２２の構成について説明する。図１２は、本実施形態２におけるマッチング条件記憶部２２の構成例を示す図である。マッチング条件記憶部２２は、例えば、図１２に示すように、情報をテーブル化して格納している。マッチング条件記憶部２２は、マッチング条件設定部４３がマップ点の選択条件などのマッチング条件を設定する際に参照され、図１２に示すように、「設定回数」ごとに「マッチング条件」を対応付けて格納している。
【００６７】
「設定回数」は、マッチング条件の設定の回数を示しており、マッチング条件設定部４３は、例えば、マッチング条件の設定が２回目の場合には、設定回数“２”に対応するマッチング条件を設定する。なお、最初の設定されるマッチング条件（設定回数“１”に対応するマッチング条件）は、本実施形態２においては、種別が「平面模様点」のマップ点をマッチング対象とする選択条件である。
【００６８】
「マッチング条件」は、例えば、マッチング対象とする物理端点のマップ点の選択条件などのマッチング処理に関する条件である。マッチング条件は、例えば、優先度が高い順に所定個数の物理端点を選択させるような選択条件であってもよい。また、マッチング条件は、例えば、優先度に関係なく無作為に所定個数の物理端点を選択させるような選択条件であってもよい。また、マッチング条件は、例えば、優先度が所定値以上の物理端点を選択させるような選択条件であってもよい。また、マッチング条件は、例えば、マッチング処理の際にマッチングに成功したと判定する、特徴点（検出点）とマップ点との類似度の閾値を低くさせるような条件であってもよい。所定個数は常に同じであってもよいし、設定回数に従って可変（例えば、少なく）してもよい。但し、カメラの動きによる物理端点の近傍領域における濃淡変動の特徴の変化が小さい程優先度が高くなるようにしていることから、優先度が高い順に選択することが好ましい。また、設定回数が増えるにしたがって、マッチング条件は条件が緩和されるように設定される。
【００６９】
図１０に戻り、制御部４０は、例えば、ＣＰＵなどを備えており、記憶部２０のプログラムエリアに格納されている動作プログラムを実行して、図１０に示すように、特徴点検出部４１と、マップ点情報設定部４２と、マッチング条件設定部４３と、マッチング処理部４４と、推定部４５と、出力情報生成部４６と、優先度算出部４７としての機能を実現する。また、制御部４０は、動作プログラムを実行して、情報端末装置１全体を制御する制御処理や詳しくは後述のマップ点情報設定処理などの処理を実行する。
【００７０】
優先度算出部４７は、種別が物理端点のマップ点（特徴点）の周辺形状の歪み（反り）を算出し、算出した周辺形状（反り）の歪みに基づいて、種別が物理端点のマップ点（特徴点）の優先度を算出する。より具体的には、優先度算出部４７は、物理端点ごとに、例えば、該物理端点（注目物理端点）から所定の距離範囲に存在するマップ点（特徴点）を抽出し、抽出したマップ点（特徴点）の内で所定個数（例えば、３個）のマップ点（特徴点）の組み合わせを任意に所定組数選択する。
【００７１】
そして、優先度算出部４７は、選択したマップ点（特徴点）の組み合わせごとに、注目物理端点との位置関係を検出する。そして、優先度算出部４７は、注目物理端点を含む他のマップ点（特徴点）が分布する平面と、その平面に分布していないマップ点（特徴点）と注目物理端点とを結ぶ線分と、がなす角度θをそれぞれ算出し、更に、それらの平均角度を算出する。つまり、優先度算出部４７は、注目物理端点の周辺形状の歪み（反り）を算出する。
【００７２】
例えば、図１３を参照して、優先度算出部４７が、注目物理端点ＭＰ１７から所定の距離範囲に存在するマップ点（特徴点）の組み合わせとしてマップ点（特徴点）ＭＰ１８〜ＭＰ２０から成る組を選択したとする。マップ点（特徴点）ＭＰ２０は、図１３に示すように、注目物理端点ＭＰ１７とマップ点（特徴点）ＭＰ１８とＭＰ１９とが分布する平面上に分布していない。したがって、この場合、優先度算出部４７は、注目物理端点ＭＰ１７とマップ点（特徴点）ＭＰ１８とＭＰ１９とが分布する平面と、注目物理端点ＭＰ１７とマップ点（特徴点）ＭＰ２０とを結ぶ線分と、がなす角度θを算出する。ここで、図１３は、本実施形態２における優先度の算出方法の例を説明するための図である。
【００７３】
そして、優先度算出部４７は、例えば、図１４に示すように、周辺形状の歪み（反り）が小さいほど優先度が高くなるように定義された関数Ｆにしたがって、算出した平均角度（周辺形状の歪み）に基づいて優先度を算出する。つまり、優先度算出部４７は、物理端点の周辺形状が平面に近いほど、その物理端点における優先度を高くする。ここで、図１４は、本実施形態２における優先度を算出する関数Ｆの例を表したグラフである。
【００７４】
なお、周辺形状の歪みは、例えば、注目物理端点から所定の距離範囲に存在するマップ点（特徴点）と注目物理端点とを平面近似して、例えば、近似した平面（以下、近似平面という）からの平均誤差や誤差の総和や２乗誤差の総和などを算出することで求めることもできる。
【００７５】
近似平面が、例えば、ａＸ＋ｂＹ＋ｃＺ＝ｄだとすると、注目物理端点から所定の距離範囲に存在するマップ点（特徴点）と注目物理端点の近似平面からの２乗誤差の総和（周辺形状の歪み）Ｄは以下の式２に従って求めることができる。
Ｄ＝Σ｜ｄ−ａＸ−ｂＹ−ｃＺ｜２      ・・・・・（式２）
【００７６】
図１０に戻り、マップ点情報設定部４２は、マップ点情報記憶部２１の設定を行う。より具体的には、マップ点情報設定部４２は、例えば、逐次残差法や面積相関法といったステレオマッチング法を用いて、特徴点検出部４１により検出された、２枚の撮像画像における特徴点をマッチングする。そして、マップ点情報設定部４２は、マッチングに成功した特徴点の３次元座標をステレオ処理で算出する。
【００７７】
そして、マップ点情報設定部４２は、マッチングに成功した各特徴点において、該特徴点（注目特徴点）と隣接特徴点との位置関係を検出する。この際、マップ点情報設定部４２は、予め定められた所定個数（例えば、３以上）の隣接特徴点を、例えば、距離が近い順に選択する。そして、マップ点情報設定部４２は、マッチングに成功した各特徴点の種別を特定する。つまり、マップ点情報設定部４２は、注目特徴点と選択した隣接特徴点が同一平面上に分布している場合には、その注目特徴点の種別は「平面模様点」であると特定し、一方、注目特徴点と選択した隣接特徴点が同一平面上に分布していない場合には、その注目特徴点の種別は「物理端点」であると特定する。
【００７８】
更に、マップ点情報設定部４２は、マッチングに成功した各特徴点の特徴量を算出すると共に、マッチングに成功した各特徴点の画像パターンテンプレートを生成する。そして、マップ点情報設定部４２は、種別が物理端点であるマップ点に関しては優先度算出部４７により算出された優先度を含むマップ点情報をマップ点識別子ごとに対応つけてマップ点情報記憶部２１に格納する。
【００７９】
マッチング条件設定部４３は、マッチング条件記憶部２２を参照して、特徴点検出部４１により検出された撮像画像上の特徴点（検出点）とマッチングさせるマッチング対象となるマップ点の選択条件などのマッチング条件を設定する。より具体的には、マッチング条件設定部４３は、マッチング条件記憶部２２を参照して、マッチング条件の設定回数に基づいてマッチング条件を特定し、特定したマッチング条件を設定する。なお、本実施形態２においては、マッチング条件設定部４３は、最初のマッチング条件として、種別が「平面模様点」のマップ点をマッチング対象とする選択条件を設定する。
【００８０】
そして、マッチング処理部４４によるマッチングの結果、マッチングに成功した特徴点（検出点）とマップ点との組数が所定の閾値Ｚ以上とならない場合には、マッチング条件設定部４３は、マッチング条件記憶部２２を参照し、別のマッチング条件を設定して、マッチング条件を緩和する。マッチング条件設定部４３は、この処理を、マッチングに成功した組数が所定の閾値Ｚ以上となる迄、あるいは、未選択の物理端点が無くなる迄、繰り返す。
【００８１】
ここで、所定の閾値Ｚは、カメラの位置と姿勢の推定を行う上で、推定精度の揺らぎを解消するために最低限必要となる特徴点（検出点）とマップ点との組数を実験的に求めた値である。
【００８２】
マッチング処理４４は、マッチング条件設定部４３により設定されたマッチング条件がマップ点の選択条件である場合には、その選択条件に従って、マップ点を選択する。そして、マッチング処理４４は、特徴点検出部４１により検出された撮像画像上の特徴点（検出点）と選択したマップ点とを再マッチングする。マッチング条件が変更され、その変更がマップ点の選択条件の変更を伴う場合には、マッチング処理４４は、変更後の選択条件に従ってマップ点を選択し、変更後のマッチング条件に従って、選択したマップ点と撮像画像上の特徴点（検出点）とを再マッチングする。
【００８３】
これに対し、変更後のマッチング条件がマップ点の選択条件の変更を伴うものではない場合には、マッチング処理部４４は、変更後のマッチング条件に従って、特徴点（検出点）と以前に選択したマップ点とをマッチングする。マップ点の選択条件の変更を伴わないマッチング条件としては、例えば、マッチング処理の際にマッチングに成功したと判定する、特徴点（検出点）とマップ点との類似度の閾値を低くするマッチング条件などである。
【００８４】
次に、図１５を参照して、本実施形態２におけるマップ点情報設定処理の流れについて説明する。図１５は、本実施形態２におけるマップ点情報設定処理のフローを説明するためのフローチャートの例である。本マップ点情報設定処理は、例えば、ユーザによる操作部（不図示）を介した所定の操作（特定のアプリケーションの起動など）をトリガとして開始される。なお、本マップ点情報設定処理は、撮像画像が取得される度、あるいは、所定の間隔で、繰り返し実行されてもよい。
【００８５】
画像取得部１０は、１枚目の撮像画像を取得し、取得した撮像画像を制御部４０に出力する（ステップＳ００１）。特徴点検出部４１は、入力された１枚目の撮像画像の特徴点を検出する（ステップＳ００２）。
【００８６】
そして、画像取得部１０は、２枚目の撮像画像を取得し、取得した撮像画像を制御部４０に出力する（ステップＳ００３）。特徴点検出部４１は、入力された２枚目の撮像画像の特徴点を検出する（ステップＳ００４）。
【００８７】
そして、マップ点情報設定部４２は、１枚目と２枚目の撮像画像の特徴点をステレオマッチングすると共に（ステップＳ００５）、マッチングに成功した各特徴点の３次元座標を算出する（ステップＳ００６）。そして、マップ点情報設定部４２は、マッチングに成功した特徴点ごとに、所定個数の隣接特徴点を選択し（ステップＳ００７）、該特徴点とその隣接特徴点との位置関係を検出する（ステップＳ００８）。
【００８８】
そして、マップ点情報設定部４２は、マッチングに成功した特徴点ごとに種別を特定する（ステップＳ００９）。そして、マップ点情報設定部４２は、マッチングに成功した各特徴点の特徴量を算出すると共に、画像パターンテンプレートを生成する（ステップＳ０１０）。
【００８９】
そして、優先度算出部４７は、種別が物理端点のマップ点（特徴点）の周辺形状の歪みを算出し（ステップＳ３０１）、算出した周辺形状の歪みに基づいて、種別が物理端点のマップ点（特徴点）の優先度を算出する（ステップＳ３０２）。
【００９０】
そして、マップ点情報設定部４２は、種別が物理端点であるマップ点に関しては優先度算出部４７により算出された優先度を含むマップ点情報をマップ点識別子ごとに対応つけてマップ点情報記憶部２１に格納する（ステップＳ０１１Ａ）。そして、本処理は終了する。
【００９１】
次に、図１６と図１７を参照して、本実施形態２におけるカメラ位置・姿勢推定処理の流れについて説明する。図１６と図１７は、それぞれ、本実施形態２におけるカメラ位置・姿勢推定処理のフローを説明するためのフローチャートの例の一部と、他の一部である。本カメラ位置・姿勢推定処理は、例えば、マップ点情報設定処理が実行されマップ点情報がマップ点情報記憶部２１に格納されたことをトリガとして開始される。
【００９２】
画像取得部１０は、撮像画像を取得し、取得した撮像画像を制御部４０に出力する（ステップＳ２０１）。そして、特徴点検出部４１は、入力された撮像画像の特徴点を検出する（ステップＳ２０２）。そして、マッチング条件設定部４３は、マッチング対象のマップ点の種別を平面模様点とするマッチング条件を設定する（ステップＳ２０３）。
【００９３】
マッチング処理部４４は、種別が平面模様点であるマップ点を選択し（ステップＳ２０４）、検出された特徴点（検出点）と選択したマップ点とをマッチングする（ステップＳ２０５）。そして、マッチング条件設定部４３は、マッチングに成功した特徴点（検出点）とマップ点との組数が所定の閾値Ｚ以上であるか否かを判定する（ステップＳ４０１）。
【００９４】
マッチング条件設定部４３により、マッチングに成功した組数が所定の閾値Ｚ以上であると判定された場合には（ステップＳ４０１；ＹＥＳ）、処理はステップＳ２０６の処理へと進む。一方、マッチングに成功した組数が所定の閾値Ｚ未満であると判定した場合には（ステップＳ４０１；ＮＯ）、マッチング条件設定部４３は、マッチング条件記憶部２２を参照して、マッチング条件を変更（緩和）する（ステップＳ４０２）。
【００９５】
そして、マッチング処理部４４は、変更後のマッチング条件がマップ点の選択条件の変更を含むか否かを判定する（ステップＳ４０３）。マップ点の選択条件の変更を含むと判定した場合には（ステップＳ４０３；ＹＥＳ）、マッチング処理部４４は、更に、未選択の物理端点が有るか否かを判定する（ステップＳ４０４）。
【００９６】
未選択の物理端点が有ると判定した場合には（ステップＳ４０４；ＹＥＳ）、マッチング処理部４４は、変更後のマッチング条件に従って物理端点を選択する（ステップＳ４０５）。そして、マッチング処理部４４は、変更後のマッチング条件に従って、選択した物理端点と特徴点（検出点）とを再マッチングする（ステップＳ４０６）。そして、処理は後述のステップＳ４０７の処理へと進む。一方、マッチング処理部４４により、未選択の物理端点が無いと判定された場合には（ステップＳ４０４；ＮＯ）、処理はステップＳ２０６の処理へと進む。
【００９７】
ステップＳ４０３の処理において、マップ点の選択条件の変更を含まないと判定した場合には（ステップＳ４０３；ＮＯ）、マッチング処理部４４は、変更後のマッチング条件に従って、前回選択した物理端点と特徴点（検出点）とを再マッチングする（ステップＳ４０６）。
【００９８】
そして、マッチング条件設定部４３は、マッチングに成功した特徴点（検出点）とマップ点との組数が所定の閾値Ｚ以上であるか否かを再度判定する（ステップＳ４０７）。マッチング条件設定部４３により、マッチングに成功した組数が所定の閾値Ｚ未満であると判定された場合には（ステップＳ４０７；ＮＯ）、処理はステップＳ４０２の処理へと戻り、前述の処理を繰り返す。
【００９９】
一方、マッチング条件設定部４３により、マッチングに成功した組数が所定の閾値Ｚ以上であると判定された場合には（ステップＳ４０７；ＹＥＳ）、推定部４５は、マッチングに成功したマップ点と特徴点（検出点）との組みに基づいて、カメラの位置と姿勢を推定する（ステップＳ２０６）。
【０１００】
そして、制御部４０は、処理の終了が指示されたか否かを判定する（ステップＳ２０７）。制御部４０は、例えば、ユーザが操作部（不図示）を介して所定の操作（例えば、アプリケーションの終了）がなされた否かを判定することで、処理の終了が指示されたか否かを判定する。制御部４０により終了の指示はまだ無いと判定された場合には（ステップＳ２０７；ＮＯ）、処理はステップＳ２０１に戻り、前述の処理を繰り返す。一方、制御部４０により、終了が指示されたと判定された場合には（ステップＳ２０７；ＹＥＳ）、本処理は終了する。
【０１０１】
上記実施形態２によれば、情報端末装置１は、撮像画像上の特徴点（検出点）とのマッチング対象候補となる各マップ点の種別を特定し、カメラの位置と姿勢の推定における平面模様点の寄与率と比較して、カメラの位置と姿勢の推定における物理端点の寄与率を小さくする。こうすることで、誤マッチングの影響を低減し、カメラの位置と姿勢の推定精度を向上させることが可能となる。
【０１０２】
また、上記実施形態２によれば、情報端末装置１は、撮像画像上の特徴点（検出点）とのマッチング対象候補となる各マップ点の種別を特定し、平面模様点のマップ点をマッチング対象として最初に選択する。こうすることで、カメラの位置と姿勢の推定における平面模様点の寄与率と比較して、カメラの位置と姿勢の推定における物理端点の寄与率を小さくする。これにより、誤マッチングの影響を低減し、カメラの位置と姿勢の推定精度を向上させることが可能となる。
【０１０３】
また、上記実施形態２によれば、情報端末装置１は、平面模様点のみをマッチング対象とした際のマッチングに成功した特徴点（検出点）とマップ点との組数が所定の閾値Ｚに満たない場合に、マッチング条件を変更（緩和）し、変更後のマッチング条件に従って、特徴点（検出点）とマップ点との再マッチングを行う。こうすることで、マッチングに成功した組数を増やすことができることから、カメラの位置と姿勢の推定精度の揺らぎを解消し、カメラの位置と姿勢の推定精度を向上させることが可能となる。
【０１０４】
また、上記実施形態２によれば、情報端末装置１は、マッチングに成功した特徴点（検出点）とマップ点との組数が所定の閾値Ｚに満たない場合に、マッチングに成功した組数が所定の閾値Ｚに到達するまで、マッチング条件の緩和し、特徴点（検出点）とマップ点との再マッチングを繰り返し行う。こうすることで、カメラの位置と姿勢の推定精度の揺らぎを解消するために必要な組数を維持することが可能となり、カメラの位置と姿勢の推定精度をさらに向上させることが可能となる。
【０１０５】
また、上記実施形態２によれば、情報端末装置１は、マッチングに成功した特徴点（検出点）とマップ点との組数が所定の閾値Ｚに満たない場合に、物理端点を任意に選択し、選択した物理端点を含めて特徴点（検出点）との再マッチングを行う。こうすることで、マッチングに成功した組数を増やすことができることから、カメラの位置と姿勢の推定の揺らぎを解消し、カメラの位置と姿勢の推定精度を向上させることが可能となる。
【０１０６】
また、上記実施形態２によれば、情報端末装置１は、物理端点に対して優先度を算出する。そして、情報端末装置１は、マッチングに成功した特徴点（検出点）とマップ点との組数が所定の閾値Ｚに満たない場合に、算出した優先度に基づいて物理端点を選択する。そして、情報端末装置１は、選択した物理端点を含めて特徴点（検出点）との再マッチングを行う。優先度は、物理端点の周辺形状の歪みが小さいほど高くする。こうすることで、誤マッチングを発生させる可能性がより低い物理端点から優先的にマッチング対象とすることが可能となり、誤マッチングの発生を抑制することが可能となる。これにより、カメラの位置と姿勢の推定精度を向上させることが可能となる。
【０１０７】
（実施形態３）
本実施形態３においては、物理端点をマッチング対象とした場合に、カメラの位置と姿勢を推定する際の物理端点の寄与率を小さくすることで、誤マッチングが生じた場合の影響を低減させる。これにより、カメラの位置と姿勢の推定精度を向上させる。なお、本実施形態３の構成は、実施形態１と２のいずれに対しても適用することが可能であるが、ここでは、実施形態１に適用した場合の例について説明する。
【０１０８】
図１８は、本実施形態３における情報端末装置１の構成例を示す機能ブロック図である。本実施形態３における情報端末装置１の基本的な構成は、実施形態１の場合と同じである。但し、図１８に示すように、記憶部２０が、重み情報記憶部２３として更に機能する点で、実施形態１の場合と異なっている。また、マッチング条件設定部４３とマッチング処理部４４と推定部４５とがそれぞれ果たす機能が、実施形態１の場合と若干異なっている。
【０１０９】
図１９は、本実施形態３における重み情報記憶部２３の構成例を示す図である。本実施形態３における重み情報記憶部２３は、図１９に示すように、マップ点の「種別」ごとに「重み」を対応付けて格納している。
【０１１０】
「重み」は、カメラの位置と姿勢の推定時に、マッチングに成功した特徴点（検出点）と対応するマップ点の投影点との誤差（例えば、２疊誤差）に付加する重みである。本実施形態３においては、平面模様点の重みＷ１は物理端点の重みＷ２より大きく、例えば、Ｗ１＝１であり、これに対して、物理端点の重みＷ２は、例えば、Ｗ２＝０．６である。つまり、本実施形態３においては、全ての物理端点に対して同一の重みＷ２が適用される。
【０１１１】
マッチング条件設定部４３は、特徴点検出部４１により検出された撮像画像上の特徴点（検出点）とマッチングさせるマッチング対象となるマップ点の選択条件などのマッチング条件を設定する。本実施形態３においては、マッチング条件設定部４３は、全てのマップ点をマッチング対象とする選択条件をマッチング条件として設定する。
【０１１２】
マッチング処理部４４は、マッチング条件設定部４３により設定されたマッチング条件に従ってマップ点を選択し、特徴点検出部４１により検出された撮像画像上の特徴点（検出点）と選択したマップ点とをマッチングする。
【０１１３】
本実施形態３においては、マッチング条件設定部４３は、全てのマップ点をマッチング対象とする選択条件をマッチング条件として設定することから、マッチング処理部４４は、マップ点情報記憶部２１に格納されている全てのマップ点を選択し、選択したマップ点と特徴点（検出点）とのマッチングを行う。
【０１１４】
推定部４５は、マッチング処理部４４によるマッチングに成功した特徴点（検出点）とマップ点との組に基づいて、カメラの位置と姿勢を推定する。この際、推定部４５は、カメラの位置と姿勢の推定における平面模様点の寄与率と比較して、カメラの位置と姿勢の推定における物理端点の寄与率を小さくする。こうすることで、物理端点をマッチング対象としても、誤マッチング発生の影響を低減することが可能となり、カメラの位置と姿勢の推定精度を向上させることができる。
【０１１５】
例えば、推定部４５は、特徴点（検出点）と対応するマップ点の投影点との２乗誤差に対応するマップ点に応じた重みＷｐｉを付加した重み付き２乗誤算Ｗｐｉ｜Ｘｐｉ−Ｘｐ’ｉ｜２の総和Ｅ’が最小となるカメラ姿勢行列Ｍを算出することで、従来技術に従って、カメラの位置と姿勢を推定する。推定部４５は、重み情報記憶部２３を参照して、マップ点の種別に基づいてマップ点に応じた重みＷｐｉを特定する。
【０１１６】
マッチングに成功した特徴点（検出点）と対応するマップ点の投影点との重み付き２乗誤差の総和Ｅ’は、以下の式３で表すことができる。
Ｅ’＝ΣＷｐｉ｜Ｘｐｉ−Ｘｐ’ｉ｜２      ・・・・・（式３）
【０１１７】
なお、本実施形態３の構成を実施形態２に適用した場合において、マップ点が物理端点である場合に、設定されている優先度に応じて付加する重みＷｐｉを可変してよい。この場合、優先度が小さい程、付加する重みＷｐｉを小さくすればよい。こうすることで、誤マッチングの可能性が高くなる周辺形状の歪み（反り）が大きな物理端点ほど付加する重みＷｐｉを小さくすることができる。これにより、誤マッチングが生じた場合の影響をさらに低減させることができる。
【０１１８】
この場合の重み情報記憶部２３の構成は、例えば、図２０に示すようにすればよい。つまり、優先度の大きさに応じた重みが対応付けられているようにすればよい。ここで、図２０は、本実施形態３における重み情報記憶部２３の別の構成例を示す図である。
【０１１９】
なお、上述した方法は、カメラの位置と姿勢の推定における平面模様点の寄与率と比較して、カメラの位置と姿勢の推定における物理端点の寄与率を小さくする方法の一例である。例えば、マッチングの際に全ての物理端点を含めてマッチングを行い、推定部４５は、マッチングに成功した物理端点の比率が所定値（例えば、１０％）以下となるように物理端点を間引く（除外する）ことで物理端点の推定における寄与率を小さくしてもよい。
【０１２０】
上記実施形態３によれば、情報端末装置１は、マッチングに成功した特徴点（検出点）とマップ点との組の内で、マップ点の種別が物理端点の比率が所定値以下となるように、マップ点の種別が物理端点の組を除外して、カメラの位置と姿勢の推定を行う。こうすることで、カメラの位置と姿勢の推定における物理端点の寄与率を小さくすることが可能となる。これにより、誤マッチングの影響を低減させ、カメラの位置と姿勢の推定精度を向上させることが可能となる。
【０１２１】
また、上記実施形態３によれば、情報端末装置１は、マッチングに成功した特徴点（検出点）とマップ点の投影点との誤差に基づいて、カメラの位置と姿勢の推定を行う際に、マップ点の種別に応じた重みを誤差に付加して推定を行う。誤差に付加する重みは、平面模様点より物理端点の方を小さくする。こうすることで、カメラの位置と姿勢の推定における物理端点の寄与率を小さくすることが可能となる。これにより、誤マッチングの影響を低減させ、カメラの位置と姿勢の推定精度を向上させることが可能となる。
【０１２２】
また、上記実施形態３によれば、物理端点に対応する誤差に付加する重みは、その物理端点に設定されている優先度に応じて可変させる。つまり、物理端点に対応する誤差に付加する重みは、優先度が小さくなる程小さくする。こうすることで、誤マッチングを発生させる可能性がより高い物理端点ほど、カメラの位置と姿勢の推定における寄与率を小さくすることができる。
【０１２３】
（実施形態４）
本実施形態１においては、マップ点の３次元座標を算出する際に、ステレオマッチング法を用いる例について説明した。
【０１２４】
本実施形態４においては、３Ｄ距離センサ（３次元距離センサ）（不図示）を用いてマップ点の３次元座標を取得する場合のマップ点情報設定処理について説明する。３Ｄ距離センサを用いれば、カメラとの位置関係を補正することで、撮像画像上の各画素の３次元座標をリアルタイムに取得することが可能である。なお、本実施形態４の構成は、上述の各実施形態に対して適用することが可能であるが、ここでは、実施形態２に適用した場合の例について説明する。
【０１２５】
図２１は、本実施形態４における情報端末装置１の構成例を示す機能ブロック図である。本実施形態４における情報端末装置１の基本的な構成は、実施形態２の場合と同じである。但し、図２１に示すように、情報端末装置１が位置情報取得部５０を更に備える点で、実施形態２の場合と異なっている。また、マップ点情報設定部４２と優先度算出部４７とがそれぞれ果たす機能が、実施形態２の場合と若干異なっている。
【０１２６】
位置情報取得部５０は、例えば、機器インターフェースや通信モジュールなどを備えており、接続されている３Ｄ距離センサから撮像対象の各点における３次元座標を示す情報（以下、３Ｄ位置情報（３次元位置情報）という）を取得する。そして、位置情報取得部５０は、取得した３Ｄ位置情報を制御部４０に出力する。
【０１２７】
マップ点情報設定部４２は、マップ点情報記憶部２１の設定を行う。より具体的には、本実施形態４におけるマップ点情報設定部４２は、入力された３Ｄ位置情報に基づいて、特徴点検出部４１により検出された各特徴点の３次元座標を特定する。
【０１２８】
そして、マップ点情報設定部４２は、検出された各特徴点において、該特徴点（注目特徴点）と隣接特徴点との位置関係を検出する。この際、マップ点情報設定部４２は、予め定められた所定個数（例えば、３以上）の隣接特徴点を、例えば、距離が近い順に選択する。そして、マップ点情報設定部４２は、検出された各特徴点の種別を特定する。つまり、マップ点情報設定部４２は、注目特徴点と選択した隣接特徴点が同一平面上に分布している場合には、その注目特徴点の種別は「平面模様点」であると特定し、一方、注目特徴点と選択した隣接特徴点が同一平面上に分布していない場合には、その注目特徴点の種別は「物理端点」であると特定する。
【０１２９】
更に、マップ点情報設定部４２は、検出された各特徴点の特徴量を算出すると共に、検出された各特徴点の画像パターンテンプレートを生成する。そして、マップ点情報設定部４２は、種別が物理端点であるマップ点に関しては優先度算出部４７により算出された優先度を含むマップ点情報をマップ点識別子ごとに対応つけてマップ点情報記憶部２１に格納する。
【０１３０】
なお、３Ｄ距離センサの３Ｄ位置情報を用いることで、マップ点情報設定部４２は、注目特徴点から所定の距離範囲に存在する所定個数（例えば、３以上）の画素を任意に選択し、選択した画素と注目特徴点との位置関係に基づいて、検出された各特徴点の種別を特定することも可能である。
【０１３１】
ここで、図２２を参照して、選択した画素と注目特徴点との位置関係に基づく特徴点の種別の特定方法について説明する。図２２は、本実施形態４における種別特定の方法の例を説明するための図である。
【０１３２】
３Ｄ距離センサの画像は、３Ｄ距離センサからの距離に応じて各画素の明るさ（輝度）を変化させた画像である。つまり、３Ｄ距離センサの画像は、距離が遠いほど画素の明るさを暗く（輝度を低く）した画像である。この３Ｄ距離センサの画像における特徴点ＭＰ２１の近傍領域の画像に注目すると、明るい画素の領域と暗い画素の領域が混在していることが分かる。
【０１３３】
これに対し、３Ｄ距離センサの画像における特徴点ＭＰ２２の近傍領域の画像に注目すると、その領域内の各画素の明るさ（輝度）はほぼ同じであることが分かる。このように、３Ｄ距離センサを用いれば、撮像画像上の各画素の３次元座標を正確に得ることが可能である。したがって、マップ点情報設定部４２は、撮像画像上の注目特徴点から所定の距離範囲（所定画素数範囲）に存在する所定個数（例えば、３以上）の画素を任意に選択し、３Ｄ位置情報に基づいて、選択した複数の画素の３次元座標を特定することで、検出された各特徴点の種別を特定することができる。
【０１３４】
図２１に戻り、優先度算出部４７は、種別が物理端点のマップ点（特徴点）の周辺形状の歪み（反り）を算出し、算出した周辺形状（反り）の歪みに基づいて、種別が物理端点のマップ点（特徴点）の優先度を算出する。優先度は、実施形態２で説明した方法を用いることで算出することが可能である。より具体的には、優先度算出部４７は、物理端点ごとに、例えば、該物理端点（注目物理端点）から所定の距離範囲（所定画素数範囲）に存在する複数の画素を任意に選択し、選択した画素の内で所定個数（例えば、３個）の特徴点（マップ点）の組み合わせを任意に所定組数選択する。選択する画素は、もちろん、マップ点（特徴点）に対応する画素であってもよい。
【０１３５】
そして、優先度算出部４７は、選択した画素の組み合わせごとに、３Ｄ位置情報に基づいて、注目物理端点との位置関係を検出する。そして、優先度算出部４７は、注目物理端点と少なくとも一部の画素（世界座標系における対応する点）が分布する平面と、その平面に分布していない画素（世界座標系における対応する点）と注目物理端点とを結ぶ線分と、がなす角度θをそれぞれ算出し、更に、それらの平均角度を算出する。つまり、優先度算出部４７は、注目物理端点の周辺形状の歪み（反り）を算出する。
【０１３６】
そして、優先度算出部４７は、例えば、図１４に例示した関数Ｆにしたがって、算出した平均角度（周辺形状の歪み）に基づいて優先度を算出する。つまり、優先度算出部４７は、物理端点の周辺の形状が平面に近いほど、その物理端点における優先度を高くする。
【０１３７】
なお、周辺形状の歪みは、例えば、注目物理端点から所定の距離範囲（所定画素数範囲）に存在する画素（世界座標系における対応する点）と注目物理端点とを平面近似して、例えば、近似した平面（以下、近似平面という）からの平均誤差や誤差の総和や２乗誤差の総和などを算出することで求めることもできる。
【０１３８】
次に、図２３を参照して、本実施形態４におけるマップ点情報設定処理の流れについて説明する。図２３は、本実施形態４におけるマップ点情報設定処理のフローを説明するためのフローチャートの例である。本マップ点情報設定処理は、例えば、ユーザによる操作部（不図示）を介した所定の操作（特定のアプリケーションの起動など）をトリガとして開始される。なお、本マップ点情報設定処理は、撮像画像が取得される度、あるいは、所定の間隔で、繰り返し実行されてもよい。
【０１３９】
画像取得部１０は、撮像画像を取得し、取得した撮像画像を制御部４０に出力する（ステップＳ５０１）。特徴点検出部４１は、入力された撮像画像の特徴点を検出する（ステップＳ５０２）。そして、位置情報取得部５０は、３Ｄ位置情報を取得し、取得した３Ｄ位置情報を制御部４０に出力する（ステップＳ５０３）。
【０１４０】
そして、マップ点情報設定部４２は、入力された３Ｄ位置情報に基づいて、検出された各特徴点の３次元座標を特定する（ステップＳ５０４）。そして、マップ点情報設定部４２は、検出された特徴点ごとに、所定個数の隣接特徴点を選択し（ステップＳ５０５）、３Ｄ位置情報に基づいて、該特徴点（注目特徴点）とその隣接特徴点との位置関係を検出する（ステップＳ５０６）。
【０１４１】
そして、マップ点情報設定部４２は、検出された特徴点ごとに種別を特定する（ステップＳ５０７）。そして、マップ点情報設定部４２は、検出された各特徴点の特徴量を算出すると共に、画像パターンテンプレートを生成する（ステップＳ５０８）。
【０１４２】
そして、優先度算出部４７は、種別が物理端点のマップ点（特徴点）の周辺形状の歪みを算出し（ステップＳ５０９）、算出した周辺形状の歪みに基づいて、種別が物理端点のマップ点（特徴点）の優先度を算出する（ステップＳ５１０）。
【０１４３】
そして、マップ点情報設定部４２は、種別が物理端点であるマップ点に関しては優先度算出部４７により算出された優先度を含むマップ点情報をマップ点識別子ごとに対応つけてマップ点情報記憶部２１に格納する（ステップＳ５１１）。そして、本処理は終了する。
【０１４４】
上記実施形態４によれば、情報端末装置１は、撮像画像上の特徴点を検出し、３Ｄ距離センサによる３Ｄ位置情報に基づいて、検出した各特徴点の３次元座標を特定することで、マップ点を設定する。こうすることで、リアルタイムにマップ点情報を設定することが可能となる。
【０１４５】
また、上記実施形態４によれば、情報端末装置１は、検出した特徴点（検出点）の近傍に存在する複数の画素を任意に選択し、選択した画素（世界座標系における対応する点）と特徴点（検出点）との位置関係に基づいて、各特徴点（検出点）の種別を特定する。
【０１４６】
図２４は、実施形態１乃至４における情報端末装置１のハードウェア構成の例を示す図である。図１などに示す情報端末装置１は、例えば、図２４に示す各種ハードウェアにより実現されてもよい。図２４の例では、情報端末装置１は、ＣＰＵ２０１、ＲＡＭ２０２、ＲＯＭ２０３、ＨＤＤ２０４、カメラや表示装置や３Ｄ距離センサなどを接続するための機器インターフェース２０５、通信モジュール２０６、読取装置２０７を備え、これらのハードウェアはバス２０８を介して接続されている。
【０１４７】
ＣＰＵ２０１は、例えば、ＨＤＤ２０４に格納されている動作プログラムをＲＡＭ２０２にロードし、ＲＡＭ２０２をワーキングメモリとして使いながら各種処理を実行する。ＣＰＵ２０１は、動作プログラムを実行することで、図１などに示す制御部４０の各機能部を実現することができる。
【０１４８】
なお、上記動作を実行するための動作プログラムを、フレキシブルディスク、Ｃｏｍｐａｃｔ  Ｄｉｓｋ−Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ（ＣＤ−ＲＯＭ）、Ｄｉｇｉｔａｌ  Ｖｅｒｓａｔｉｌｅ  Ｄｉｓｋ（ＤＶＤ）、Ｍａｇｎｅｔｏ  Ｏｐｔｉｃａｌ  ｄｉｓｋ（ＭＯ）などのコンピュータで読み取り可能な記録媒体２０９に記憶して配布し、これを情報端末装置１の読取装置２０７で読み取ってコンピュータにインストールすることにより、上述の処理を実行するように構成してもよい。さらに、インターネット上のサーバ装置が有するディスク装置等に動作プログラムを記憶しておき、通信モジュール２０６を介して、情報端末装置１のコンピュータに動作プログラムをダウンロード等するものとしてもよい。
【０１４９】
なお、実施形態に応じて、ＲＡＭ２０２、ＲＯＭ２０３、ＨＤＤ２０４以外の他の種類の記憶装置が利用されてもよい。例えば、情報端末装置１は、Ｃｏｎｔｅｎｔ  Ａｄｄｒｅｓｓａｂｌｅ  Ｍｅｍｏｒｙ（ＣＡＭ）、Ｓｔａｔｉｃ  Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ（ＳＲＡＭ）、Ｓｙｎｃｈｒｏｎｏｕｓ  Ｄｙｎａｍｉｃ  Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ（ＳＤＲＡＭ）などの記憶装置を有してもよい。
【０１５０】
なお、実施形態に応じて、情報端末装置１のハードウェア構成は図２４とは異なっていてもよく、図２４に例示した規格・種類以外のその他のハードウェアを情報端末装置１に適用することもできる。
【０１５１】
例えば、図１などに示す情報端末装置１の制御部４０の各機能部は、ハードウェア回路により実現されてもよい。具体的には、ＣＰＵ２０１の代わりに、Ｆｉｅｌｄ  Ｐｒｏｇｒａｍｍａｂｌｅ  Ｇａｔｅ  Ａｒｒａｙ（ＦＰＧＡ）などのリコンフィギュラブル回路や、Ａｐｐｌｉｃａｔｉｏｎ  Ｓｐｅｃｉｆｉｃ  Ｉｎｔｅｇｒａｔｅｄ  Ｃｉｒｃｕｉｔ（ＡＳＩＣ）などにより、図１などに示す制御部４０の各機能部が実現されてもよい。もちろん、ＣＰＵ２０１とハードウェア回路の双方により、これらの機能部が実現されてもよい。
【０１５２】
以上において、いくつかの実施形態及びその変形例について説明した。しかしながら、実施形態は上記の実施形態に限定されるものではなく、上述の実施形態の各種変形形態及び代替形態を包含するものとして理解されるべきである。例えば、各種実施形態は、その趣旨及び範囲を逸脱しない範囲で構成要素を変形して具体化できることが理解されよう。また、前述した実施形態に開示されている複数の構成要素を適宜組み合わせることにより、種々の実施形態を成すことができることが理解されよう。更には、実施形態に示される全構成要素からいくつかの構成要素を削除して又は置換して、或いは実施形態に示される構成要素にいくつかの構成要素を追加して種々の実施形態が実施され得ることが当業者には理解されよう。
【０１５３】
以上の実施形態１乃至４を含む実施形態に関し、さらに以下の付記を開示する。
（付記１）
処理対象の撮像画像上の特徴点と予め保持されているマップ点とをマッチングし、マッチングに成功した前記特徴点と対応する前記マップ点を前記撮像画像上に投影した投影点との誤差に基づいて、処理対象の前記撮像画像を撮像したカメラの位置と姿勢を推定する位置・姿勢推定装置であって、
前記マップ点の周辺形状を特定し、特定した前記周辺形状が平面であるか否かを判定する判定手段と、
前記カメラの位置と姿勢の推定における前記周辺形状が平面である前記マップ点の寄与率と比較して、前記周辺形状が平面ではない前記マップ点の寄与率が小さくなるように調整する調整手段と、
を備え、
前記マップ点は、撮像対象における特徴点に対応する世界座標系の三次元点である、
ことを特徴とする位置・姿勢推定装置。
（付記２）
前記調整手段は、前記周辺形状が平面である前記マップ点に対応する前記誤差に付加する重みと比較して、前記周辺形状が平面ではない前記マップ点に対応する前記誤差に付加する重みを小さくすることで、前記カメラの位置と姿勢の前記推定における前記周辺形状が平面ではない前記マップ点の寄与率を小さくする、
ことを特徴とする付記１に記載の位置・姿勢推定装置。
（付記３）
前記周辺形状が平面ではない前記マップ点の周辺形状の歪みを算出する第１算出手段と、
算出された前記周辺形状の歪みが小さいほど、前記周辺形状が平面ではない前記マップ点の優先度を大きく算出する第２算出手段と、
を、更に備え、
前記調整手段は、前記優先度が小さいほど、前記周辺形状が平面ではない前記マップ点に対応する前記誤差に付加する重みを小さくする、
ことを特徴とする付記２に記載の位置・姿勢推定装置。
（付記４）
前記調整手段は、処理対象の撮像画像上の前記特徴点とのマッチング対象から前記周辺形状が平面ではない前記マップ点を除外することで、前記周辺形状が平面ではない前記マップ点の寄与率をゼロにする、
ことを特徴とする付記１に記載の位置・姿勢推定装置。
（付記５）
前記調整手段は、処理対象の撮像画像上の前記特徴点とのマッチングに成功した前記マップ点から、前記周辺形状が平面ではない前記マップ点の比率が所定値以下のとなるように前記周辺形状が平面ではない前記マップ点を除外することで、前記カメラの位置と姿勢の前記推定における前記周辺形状が平面ではない前記マップ点の寄与率を小さくする、
ことを特徴とする付記１に記載の位置・姿勢推定装置。
（付記６）
前記調整手段は、処理対象の撮像画像上の前記特徴点と前記周辺形状が平面である前記マップ点とを最初にマッチングさせ、マッチングに成功した前記特徴点と前記マップ点との組数が所定の閾値に満たない場合に、マッチングの際の条件を緩和し、緩和した前記条件にしたがって、再マッチングを行わせる、
ことを特徴とする付記１に記載の位置・姿勢推定装置。
（付記７）
前記調整手段は、マッチングに成功した前記組数が前記閾値に到達するまで、マッチングの際の前記条件を緩和し、再マッチングを繰り返し行わせる、
ことを特徴とする付記６に記載の位置・姿勢推定装置。
（付記８）
マッチングの際の前記条件の緩和は、前記周辺形状が平面ではない前記マップ点の一部をマッチング対象にすることを含んでいる、
ことを特徴とする付記６又は７に記載の位置・姿勢推定装置。
（付記９）
前記周辺形状が平面ではない前記マップ点の周辺形状の歪みを算出する第１算出手段と、
算出された前記周辺形状の歪みが小さいほど、前記周辺形状が平面ではない前記マップ点の優先度を大きく算出する第２算出手段と、
を、更に備え、
前記調整手段は、処理対象の撮像画像上の前記特徴点と前記周辺形状が平面である前記マップ点とを最初にマッチングさせ、マッチングに成功した前記組数が前記閾値に満たない場合に、前記優先度が高い順に前記周辺形状が平面ではない前記マップ点をマッチング対象とする、
ことを特徴とする付記８に記載の位置・姿勢推定装置。
（付記１０）
前記マップ点は、前記カメラで前記撮像対象を撮像した２枚の撮像画像上の前記特徴点をステレオマッチングすることで算出される、
ことを特徴とする付記１乃至９のいずれか一に記載の位置・姿勢推定装置。
（付記１１）
前記マップ点は、前記カメラで前記撮像対象を撮像した撮像画像上の前記特徴点を、３次元距離センサにより検出された前記撮像対象の３次元座標と対応付けることで算出される（実施形態４）、
ことを特徴とする付記１乃至９のいずれか一に記載の位置・姿勢推定装置。
（付記１２）
処理対象の撮像画像上の特徴点と予め保持されているマップ点とをマッチングし、マッチングに成功した前記特徴点と対応する前記マップ点を前記撮像画像上に投影した投影点との誤差に基づいて、処理対象の前記撮像画像を撮像したカメラの位置と姿勢を推定する位置・姿勢推定装置の位置・姿勢推定方法であって、
前記マップ点の周辺形状を特定し、
特定した前記周辺形状が平面であるか否かを判定し、
前記カメラの位置と姿勢の推定における前記周辺形状が平面である前記マップ点の寄与率と比較して、前記周辺形状が平面ではない前記マップ点の寄与率が小さくなるように調整し、
前記マップ点は、撮像対象における特徴点に対応する世界座標系の三次元点である、
ことを特徴とする位置・姿勢推定方法。
（付記１３）
処理対象の撮像画像上の特徴点と予め保持されているマップ点とをマッチングし、マッチングに成功した前記特徴点と対応する前記マップ点を前記撮像画像上に投影した投影点との誤差に基づいて、処理対象の前記撮像画像を撮像したカメラの位置と姿勢を推定する位置・姿勢推定装置のコンピュータに、
前記マップ点の周辺形状を特定し、
特定した前記周辺形状が平面であるか否かを判定し、
前記カメラの位置と姿勢の推定における前記周辺形状が平面である前記マップ点の寄与率と比較して、前記周辺形状が平面ではない前記マップ点の寄与率が小さくなるように調整する、
処理を実行させ、
前記マップ点は、撮像対象における特徴点に対応する世界座標系の三次元点である、
ことを特徴とするプログラム。
（付記１４）
処理対象の撮像画像上の特徴点と予め保持されているマップ点とをマッチングし、マッチングに成功した前記特徴点と対応する前記マップ点を前記撮像画像上に投影した投影点との誤差に基づいて、処理対象の前記撮像画像を撮像したカメラの位置と姿勢を推定する位置・姿勢推定装置のコンピュータに、
前記マップ点の周辺形状を特定し、
特定した前記周辺形状が平面であるか否かを判定し、
前記カメラの位置と姿勢の推定における前記周辺形状が平面である前記マップ点の寄与率と比較して、前記周辺形状が平面ではない前記マップ点の寄与率が小さくなるように調整する、
処理を実行させ、
前記マップ点は、撮像対象における特徴点に対応する世界座標系の三次元点である、
ことを特徴とするプログラムを記憶した記録媒体。
【符号の説明】
【０１５４】
１      情報端末装置
１０    画像取得部
２０    記憶部
２１    マップ点情報記憶部
２２    マッチング条件記憶部
２３    重み情報記憶部
３０    出力部
４０    制御部
４１    特徴点検出部
４２    マップ点情報設定部
４３    マッチング条件設定部
４４    マッチング処理部
４５    推定部
４６    出力情報生成部
４７    優先度算出部
５０    位置情報取得部
ＭＰ１〜２２  特徴点（あるいはマップ点）
ＩＰ  画像平面
θ  角度
Ｆ  関数
２０１  ＣＰＵ
２０２  ＲＡＭ
２０３  ＲＯＭ
２０４  ＨＤＤ
２０５  機器インターフェース
２０６  通信モジュール
２０７  読取装置
２０８  バス
２０９  記録媒体
【図１】
0006405891.tif 000002
【図２】
0006405891.tif 000003
【図３】
0006405891.tif 000004
【図４】
0006405891.tif 000005
【図５】
0006405891.tif 000006
【図６】
0006405891.tif 000007
【図７】
0006405891.tif 000008
【図８】
0006405891.tif 000009
【図９】
0006405891.tif 000010
【図１０】
0006405891.tif 000011
【図１１】
0006405891.tif 000012
【図１２】
0006405891.tif 000013
【図１３】
0006405891.tif 000014
【図１４】
0006405891.tif 000015
【図１５】
0006405891.tif 000016
【図１６】
0006405891.tif 000017
【図１７】
0006405891.tif 000018
【図１８】
0006405891.tif 000019
【図１９】
0006405891.tif 000020
【図２０】
0006405891.tif 000021
【図２１】
0006405891.tif 000022
【図２２】
0006405891.tif 000023
【図２３】
0006405891.tif 000024
【図２４】
0006405891.tif 000025