JP6503906B2 - 画像処理装置、画像処理方法および画像処理プログラム - Google Patents

Description

本発明は、画像処理装置等に関する。

ＰＣ（Personal Computer）および携帯端末等に取り付けられたカメラの撮影画像を基にして、撮影画像に対するカメラの位置姿勢を求める技術がある。また、カメラの位置姿勢を用いて、ＰＣおよび携帯端末等の画面に表示された撮影画像にコンピュータグラフィックス（ＣＧ）等の付加情報を重畳表示させ、ユーザの作業支援を実現するＡＲ技術がある。

図１１は、ＡＲ技術の一例を示す図である。図１１に示すように、例えば、利用者が携帯端末１０に内蔵されたカメラを用いて、マーカ１１および点検対象１２を撮影すると、携帯端末１０の表示画面１０ａに、マーカ１１に対するオブジェクト情報１３が表示される。

カメラの位置姿勢を求める技術としては、撮影画像に含まれる特徴点を用いる従来技術１が開示されている。特徴点は、着目点の近傍での濃淡変動が大きく、濃淡変動によって着目点の画像上の位置が一意に規定されることを基準として検出される。従来技術１では、予め生成しておいた特徴点の３次元座標の集合を用いる。以下の説明では、特徴点の３次元座標をマップ点と表記し、マップ点の集合を初期マップと適宜表記する。例えば、従来技術１では、現在の撮影画像中に存在する特徴点と、撮影画像上に投影したマップ点とを対応付けることにより、カメラの位置姿勢を推定する。

図１２は、カメラの位置姿勢を求める従来技術１を説明するための図である。図１２に示す例では、マップ点Ｓ_１〜Ｓ_６が存在するものとする。あるマップ点Ｓ_ｉは、世界座標系で、式（１）によって示される。撮影画像２０上には、特徴点ｘ_１〜ｘ_６が存在するものとする。ある特徴点ｘ_ｉは、カメラ座標系で、式（２）によって示される。撮影画像２０上に投影したマップ点を、投影点ｐ_１’〜ｐ_６’とする。ある投影点ｐ_ｉ’は、カメラ座標系で、式（３）によって示される。

Ｓ_ｉ＝（ｘ，ｙ，ｚ）・・・（１）

ｘ_ｉ＝（ｕ，ｖ）・・・（２）

ｐ_ｉ’＝（ｕ’，ｖ’）・・・（３）

例えば、従来技術１では、式（４）によって算出される二乗和Ｅが最小となるようなカメラ位置姿勢行列Ｍを算出することで、カメラの位置姿勢を求める。

続いて、初期マップを生成する従来技術１について説明する。図１３は、初期マップを生成する従来技術１を説明するための図である。例えば、従来技術１では、ステレオ撮影の原理を用いる。従来技術１は、撮影位置を変えた２枚の撮影画像中の特徴点で同じものを対応付けしていく。従来技術１は、対応付けした複数の対応点の各撮影画像中の位置関係から対応点をマップ点とした初期マップを生成する。

図１３に示す例では、復元されるマップ点をＳ_ｉとし、初期のカメラの撮影位置Ｃａおよびマップ点Ｓ_ｉを結ぶ線分と、１枚目の撮影画像２０ａとが交差する点を、特徴点ｘ_ａｉとする。２番目のカメラの撮影位置Ｃｂおよびマップ点Ｓ_ｉを結ぶ線分と、２枚目の撮影画像２０ｂとが交差する点を、特徴点ｘ_ｂｉとする。そうすると対応点は、特徴点ｘ_ａｉおよび特徴点ｘ_ｂｉとなる。

初期マップの３次元座標の原点には、１枚目の撮影画像のカメラ位置・撮影方向を用いることが一般的である。図１４は、カメラの撮影方向の定義の一例を示す図である。図１４に示すように、例えば、カメラＣ１０の位置（Ｔ_ｘ、Ｔ_ｙ、Ｔ_ｚ）、撮影方向（Ｒ_ｘ、Ｒ_ｙ、Ｒ_ｚ）を基準にして、初期マップの３次元座標の原点を定義する。

ここで、初期マップを用いてカメラの位置姿勢を求める従来技術１では、原理的に、下記に説明する適正条件１〜４が当てはまる。適正条件１は、マップ点の３次元座標の精度が高いほどカメラの位置姿勢の推定精度が向上するというものである。このため、ステレオ撮影に使用する２枚の撮影画像を何度も取り直すことがある。

適正条件２は、マップ点とマッチングする特徴点がカメラに近いほどカメラの位置姿勢の推定精度が向上するというものである。これは、撮影対象の特徴点がカメラに近いほど、相対的にカメラの分解能が上がり、撮影画像中の特徴点の位置の精度が向上するためである。

適正条件３は、マップ点が多いほどカメラの位置姿勢の推定精度が向上するというものである。適正条件４は、マップ点の分布の広がりが広いほどカメラの位置姿勢の推定精度が向上するというものである。

原理上は特徴点とマッチングしたマップ点が５点または８点あればカメラの位置姿勢の推定を行えるので、従来技術では、適正条件２を配慮せずに、適正条件３、４を優先してできるだけ多くのマップ点を用いている。

しかし、上述したステレオ撮影によって初期マップを生成すると、原理的にｚ軸方向の精度が悪いと言う問題がある。計算手法の工夫で精度を向上させる試みはあるものの、本質的な解決策とはなっていない。

一方、リアルタイムで対象物の３次元座標を取得できる３Ｄ距離センサが普及してきている。特に、３Ｄ距離センサとカメラの両方をセットし相互に位置関係の補正を行うことで、カメラの撮影画像の各特徴点の３次元座標をリアルタイムに求めることが可能となっている。３Ｄ距離センサを用いると、初期マップ生成の際の１枚目の撮影画像中の特徴点の検出の時点で、マップ点の３次元座標を規定することができ、更にマップ点の位置精度もステレオ撮影によるマップ点の位置精度より高くなる。

特開２０００−２９３６８７号公報 特開２０１２−６８７７２号公報

しかしながら、上述した従来技術では、カメラの位置姿勢の推定精度が劣化するという問題がある。

例えば、３Ｄ距離センサは、測定できるカメラからの距離に制限があり、高精度に３次元座標を測定できる距離が短いという問題がある。また、３Ｄ距離センサは、対象物の色が黒に近い場合や、材質が光りを反射しにくい場合に測定ができないという問題がある。このため、従来技術１の代わりに、３Ｄ距離センサを用いると、対象物によってはマップ点として使用できる点数が少なくなり、適正条件３，４を満たさず、カメラの位置姿勢の推定精度がかえって劣化する場合がある。

図１５は、３Ｄ距離センサに測定された３Ｄ距離画像の一例を示す図である。図１５において、画像３０ａは、カメラによって撮影された撮影画像である。画像３０ｂは、画像３０ａに対応する３Ｄ距離センサによって測定された３Ｄ距離画像である。画像３０ｂにおいて、黒色の領域は、３Ｄ距離センサで検出できない領域を示す。また、画像３０ｂにおいて、距離が近いほど白く撮影される。例えば、対象物３１は、カメラからの距離が４０ｃｍの対象物であり、対象物３２は、カメラからの距離が１４０ｃｍの対象物である。

１つの側面では、本発明は、カメラの位置姿勢の推定精度を向上させることができる画像処理装置、画像処理方法および画像処理プログラムを提供することを目的とする。

第１の案では、画像処理装置は、記憶部と、マッチング部と、位置姿勢推定部とを有する。記憶部は、距離センサにより測定された特徴点の３次元座標を示す第１マップ点と、複数の撮影画像に基づいて特定された特徴点の３次元座標を示す第２マップ点との情報を含むマップ点情報を記憶する。マッチング部は、第１マップ点を撮像画像上に投影した第１投影点と、撮影画像上の特徴点とを比較して、類似する第１投影点と特徴点とのペアを作成する。マッチング部は、作成したペアの個数に基づいて、第２マップ点を用いたマッチングを実行するか否かを判定する。マッチング部は、第２マップ点を用いたマッチングを実行すると判定した場合には、第２マップ点を撮像画像上に投影した第２投影点と、撮影画像上の特徴点とを比較して、類似する第２投影点と特徴点とのペアを作成する。位置姿勢推定部は、マッチング部のマッチング結果およびマップ点情報を基にして、撮影画像を撮影したカメラの位置および姿勢を推定する。

カメラの位置姿勢の推定精度を向上させることができる。

図１は、本実施例に係る画像処理装置の構成を示す機能ブロック図である。 図２は、本実施例に係る初期マップ生成部の構成を示す機能ブロック図である。 図３は、撮影画像から検出した特徴点の一例を示す図である。 図４は、初期マップ情報のデータ構造の一例を示す図である。 図５は、カメラ位置姿勢推定部の構成を示す機能ブロック図である。 図６は、マップ点の分布状況の判定方法の一例を示す図である。 図７は、本実施例に係る初期マップ生成部の処理手順を示すフローチャートである。 図８は、本実施例に係るカメラ位置姿勢推定部の処理手順を示すフローチャート（１）である。 図９は、本実施例に係るカメラ位置姿勢推定部の処理手順を示すフローチャート（２）である。 図１０は、画像処理プログラムを実行するコンピュータの一例を示す図である。 図１１は、ＡＲ技術の一例を示す図である。 図１２は、カメラの位置姿勢を求める従来技術１を説明するための図である。 図１３は、初期マップを生成する従来技術１を説明するための図である。 図１４は、カメラの撮影方向の定義の一例を示す図である。 図１５は、３Ｄ距離センサに測定された３Ｄ距離画像の一例を示す図である。

以下に、本願の開示する画像処理装置、画像処理方法および画像処理プログラムの実施例を図面に基づいて詳細に説明する。なお、この実施例によりこの発明が限定されるものではない。

図１は、本実施例に係る画像処理装置の構成を示す機能ブロック図である。図１に示すように、この画像処理装置１００は、カメラ５０ａと、３Ｄ距離センサ５０ｂと、初期マップ生成部１００ａと、カメラ位置姿勢推定部１００ｂとを有する。

カメラ５０ａは、画像を撮影する装置である。カメラ５０ａが撮影した画像を撮影画像と表記する。カメラ５０ａは、撮影画像の情報を、初期マップ生成部１００ａ、カメラ位置姿勢推定部１００ｂに出力する。カメラ５０ａは、複数の位置に移動可能であり、例えば、撮影位置Ｃａ、撮影位置Ｃｂで画像を撮影する。利用者が手動でカメラ５０ａの撮影位置を変更してもよいし、画像処理装置１００が自動でカメラ５０ａの撮影位置を変更してもよい。

３Ｄ距離センサ５０ｂは、３Ｄ距離センサ５０ｂが測定できる範囲内の対象物と、３Ｄ距離センサ５０ｂとの距離の情報を含む３Ｄ距離画像を取得するセンサである。３Ｄ距離センサ５０ｂは、３Ｄ距離画像の情報を、初期マップ生成部１００ａに出力する。例えば、３Ｄ距離センサ５０ｂが撮影する３Ｄ距離画像は、カメラ５０ａが撮影位置Ｃａで撮影した撮影画像に対応する。

初期マップ生成部１００ａは、マップ点の集合となる初期マップ情報を生成する処理部である。マップ点は、特徴点の３次元座標に対応するものである。初期マップ生成部１００ａは、カメラ５０ａを用いたステレオ撮影によってマップ点を求める処理と、３Ｄ距離センサ５０ｂによって撮影された３Ｄ距離画像によってマップ点を求める処理とを併用する。初期マップ生成部１００ａは、初期マップ情報を、カメラ位置姿勢推定部１００ｂに出力する。

以下の説明において、適宜、３Ｄ距離センサ５０ｂによって撮影された３Ｄ距離画像によって求めたマップ点を「第１マップ点」と表記し、カメラ５０ａを用いたステレオ撮影によって求めたマップ点を「第２マップ点」と表記する。また、第１マップ点、第２マップ点をとくに区別しない場合には、単にマップ点と表記する。

一般的に、第２マップ点よりも、第１マップ点の方が３次元座標の精度が高い。このため、初期マップ生成部１００ａは、第２マップ点の情報よりも、第１マップ点の情報を優先して、初期マップ情報に保存する。また、初期マップ生成部１００ａは、マップ点の情報を、初期マップ情報に保存する場合には、３Ｄ距離画像によって求めた第１マップ点であるのか、ステレオ撮影によって求めた第２マップ点であるのかを合わせて、初期マップ情報に保存する。

カメラ位置姿勢推定部１００ｂは、第２マップ点よりも、第１マップ点を優先して使用して、第１マップ点を撮影画面上に投影した第１投影点と、撮影画像上の特徴点とを比較し、類似する第１投影点と特徴点とのペアを作成するマッチングを行う。カメラ位置姿勢推定部１００ｂは、マッチング結果および初期マップ情報を基にして、カメラ５０ａの位置および姿勢を推定する。

なお、カメラ位置姿勢推定部１００ｂは、類似する第１投影点と特徴点とのペアの個数が閾値未満である場合には、第２マップ点を撮影画面上に投影した第２投影点と、撮影画像上の特徴点とを比較する。カメラ位置姿勢推定部１００ｂは、類似する第２投影点と特徴点とのペアを作成することで、不足するペアの個数を、第２マップ点を用いて補う。

カメラ位置姿勢推定部１００ｂは、測定精度の高い第１マップ点を優先して使用し、不足する分は、第２マップ点を用いて補うことで、投影点と特徴点とのマッチングを行うため、カメラ５０ａの位置姿勢の推定精度を向上させることができる。

次に、図１に示した初期マップ生成部の構成について説明する。図２は、本実施例に係る初期マップ生成部の構成を示す機能ブロック図である。図２に示すように、この初期マップ生成部１００ａは、画像取得部１０１と、特徴点検出部１０２と、特徴点マッチング処理部１０３と、特徴点ステレオ処理部１０４とを有する。また、初期マップ生成部１００ａは、３Ｄ距離画像取得部１０５と、特徴点３次元座標取得部１０６と、マップ点生成部１０７と、初期マップ記憶部１０８とを有する。

画像取得部１０１は、カメラ５０ａから撮影画像の情報を取得する処理部である。初期マップ生成部１００ａは、ステレオ撮影の原理を用いて第２マップ点を特定するため、画像取得部１０１は、カメラ５０ａの撮影位置の異なる２枚の撮影画像を取得する。カメラ５０ａの撮影位置は、画像取得部１０１が自動で設定しても良いし、利用者が手動で設定しても良い。画像取得部１０１は、撮影画像の情報を、特徴点検出部１０２および３Ｄ距離画像取得部１０５に出力する。

特徴点検出部１０２は、撮影画像から特徴点を検出する処理部である。例えば、特徴点検出部１０２は、着目点の近傍での濃淡変動が大きく、濃淡変動によって着目点の画像上の位置が一意に規定されることを基準として、特徴点を検出する。特徴点は、対象物の角に対応する場合が多い。図３は、撮影画像から検出した特徴点の一例を示す図である。図３に示す例では、丸印が、特徴点を示している。

特徴点検出部１０２は、特徴点の情報を、特徴点マッチング処理部１０３、特徴点３次元座標取得部１０６に出力する。特徴点の情報は、例えば、撮影画像上の特徴点を識別する情報と、特徴点の座標と、特徴点周辺の画素の情報を対応付けた情報となる。特徴点検出部１０２は、撮影位置Ｃａで撮影された撮影画像から検出した第１の特徴点の情報と、撮影位置Ｃｂで撮影された撮影画像から検出した第２の特徴点の情報を、特徴点マッチング処理部１０３に出力する。

特徴点マッチング処理部１０３は、第１の特徴点の情報と、第２の特徴点の情報とを比較して、対象の同じ場所を検出していて、類似している特徴点同士をマッチングして特徴点のペアを作成する処理部である。特徴点マッチング処理部１０３は、マッチングした特徴点のペアの情報を、特徴点ステレオ処理部１０４に出力する。例えば、特徴点のペアの情報は、ペアとなる特徴点を識別する情報と、ペアとなる特徴点の座標を含む。

例えば、特徴点マッチング処理部１０３は、第１の特徴点と第２の特徴点との座標の差分が閾値未満となり、かつ、第１の特徴点周辺の画素値と第２の特徴点周辺の画素値との差分が閾値未満となる場合に、第１の特徴点と第２の特徴点を、特徴点のペアとする。

特徴点ステレオ処理部１０４は、特徴点のペアの情報と、ステレオ撮影の原理に基づいて、特徴点の３次元座標を算出する処理部である。特徴点ステレオ処理部１０４が算出した特徴点の３次元座標は、上述した第２マップ点に対応する。

特徴点ステレオ処理部１０４は、第２マップ点の情報を、マップ点生成部１０７に出力する。第２マップ点の情報は、第２マップ点の３次元座標、特徴量、画像パターンテンプレートを含む。ここで、特徴量は、第２マップ点に対応する撮影画像上の特徴点の周辺の画素値を所定の順番で並べたものである。画像パターンテンプレートは、第２マップ点に対応する撮影画像上の特徴点を含む所定範囲の画像である。

３Ｄ距離画像取得部１０５は、３Ｄ距離センサ５０ｂで測定した３Ｄ距離画像を取得する処理部である。また、３Ｄ距離画像取得部１０５は、カメラ５０ａの撮影位置Ｃａで撮影された撮影画像を取得する。３Ｄ距離画像取得部１０５は、３Ｄ距離画像と、撮影画像とを特徴点３次元座標取得部１０６に出力する。

特徴点３次元座標取得部１０６は、特徴点の情報と、撮影画像と、３Ｄ距離画像とを基にして、特徴点の３次元座標を特定する処理部である。例えば、特徴点３次元座標取得部１０６は、撮影画像上の特徴点の位置を、特徴点の情報を基にして特定し、特定した特徴点の位置と、３Ｄ距離画像との関係から、特徴点の３次元座標を特定する。特徴点３次元座標取得部１０６が特定した特徴点の３次元座標は、上述した第１マップ点に対応する。

特徴点３次元座標取得部１０６は、第１マップ点の情報を、マップ点生成部１０７に出力する。第１マップ点の情報は、第１マップ点の３次元座標、特徴量、画像パターンテンプレートを含む。ここで、特徴量は、第１マップ点に対応する撮影画像上の特徴点の周辺の画素値を所定の順番で並べたものである。画像パターンテンプレートは、第１マップ点に対応する撮影画像上の特徴点を含む所定範囲の画像である。

マップ点生成部１０７は、第１マップ点の情報および第２マップ点の情報を基にして、初期マップ情報を生成する処理部である。マップ点生成部１０７は、初期マップ情報を、初期マップ記憶部１０８に格納する。

マップ点生成部１０７は、初期マップ情報を生成する場合には、第１マップ点の情報を、第２マップ点の情報よりも優先する。例えば、マップ点生成部１０７は、第１マップ点の３次元座標と、第２マップ点の３次元座標とが類似する場合には、第１マップ点の３次元座標の情報を、初期マップ情報に登録する。第１マップ点の３次元座標と、第２マップ点の３次元座標とが類似するとは、例えば、第１マップ点の３次元座標と、第２マップ点の３次元座標との差分が閾値未満であることを示す。

図４は、初期マップ情報のデータ構造の一例を示す図である。図４に示すように、この初期マップ情報は、識別番号と、３次元座標と、種類と、特徴量と、画像パターンテンプレートとを対応付ける。識別番号は、マップ点を一意に識別する番号である。種類は、マップ点が、第１マップ点であるのか、第２マップ点であるのかを示す情報である。マップ点が第１マップ点である場合には、種類が「３Ｄ距離センサ」となる。マップ点が第２マップ点である場合には、種類が「ステレオ」となる。特徴量、画像パターンテンプレートに関する説明は、上記の特徴量、画像パターンテンプレートに関する説明と同様である。

初期マップ記憶部１０８は、初期マップ情報を記憶する記憶部である。初期マップ生成部１００ａは、初期マップ記憶部１０８に格納された初期マップ情報を、カメラ位置姿勢推定部１００ｂに出力する。

次に、図１に示したカメラ位置姿勢推定部１００ｂの構成について説明する。図５は、カメラ位置姿勢推定部の構成を示す機能ブロック図である。図５に示すように、このカメラ位置姿勢推定部１００ｂは、画像取得部２１０と、初期マップ記憶部２２０と、マッチング部２３０と、位置姿勢推定部２４０とを有する。

画像取得部２１０は、カメラ５０ａから撮影画像の情報を取得する処理部である。画像取得部２１０は、撮影画像の情報を、マッチング部２３０に出力する。

初期マップ記憶部２２０は、図４で説明した初期マップ情報を記憶する記憶部である。カメラ位置姿勢推定部１００ｂは、初期マップ生成部１００ａから初期マップ情報を取得し、取得した初期マップ情報を、初期マップ記憶部２２０に格納する。

マッチング部２３０は、第１マップ点を撮像画像上に投影した第１投影点と、撮影画像上の特徴点とを比較して、類似する第１投影点と特徴点とのペアを作成する。マッチング部２３０は、作成したペアの個数に基づいて、第２マップ点を用いたマッチングを実行するか否かを判定する。マッチング部２３０は、第２マップ点を用いたマッチングを実行すると判定した場合には、第２マップ点を撮像画像上に投影した第２投影点と、撮影画像上の特徴点とを比較して、類似する第２投影点と特徴点とのペアを作成する。マッチング部２３０は、マッチング結果を、位置姿勢推定部２４０に出力する。

マッチング部２３０は、特徴点検出部２３１と、マップ点投影部２３２と、マップ点使用条件設定部２３３と、マッチング処理部２３４と、マッチング結果判定部２３５とを有する。

特徴点検出部２３１は、撮影画像から特徴点を検出する処理部である。特徴点検出部２３１が特徴点を検出する処理は、上述した特徴点検出部１０２が特徴点を検出する処理と同様である。特徴点検出部２３１は、特徴点の情報および撮影画像の情報を、マップ点投影部２３２に出力する。

マップ点投影部２３２は、初期マップ記憶部２２０に格納された初期マップ情報を参照し、第１マップ点を撮影画像中に射影変換して投影した第１投影点を求める処理部である。マップ点投影部２３２は、複数の第１投影点のうち、撮影画像の撮影範囲に含まれる第１投影点の情報を、マップ点使用条件設定部２３３に出力する。例えば、第１投影点の情報は、第１投影点の２次元座標、第１投影点に対応する第１マップ点の３次元座標、第１マップ点の特徴量、特徴点の情報、撮影画像の情報を含む。

また、マップ点投影部２３２は、マップ点使用条件設定部２３３からの要求により、第２投影点の情報を生成する。マップ点投影部２３２は、初期マップ情報を参照し、第２マップ点を撮影画像中に射影変換して投影した第２投影点を求める。マップ点投影部２３２は、複数の第２投影点のうち、撮影画像の撮影範囲に含まれる第２投影点の情報を、マップ点使用条件設定部２３３に出力する。例えば、第２投影点の情報は、第２投影点の２次元座標、第２投影点に対応する第２マップ点の３次元座標、第２マップ点の特徴量、特徴点の情報、撮影画像の情報を含む。

マップ点使用条件設定部２３３は、選択条件に基づいて、マッチング処理部２３４で使用するマップ点（投影点）を選択する処理部である。例えば、マップ点使用条件設定部２３３は、後述する第１選択条件、第２選択条件または第３選択条件を用いる。マップ点使用条件設定部２３３は、はじめは、第１選択条件を用いて動作し、後述するマッチング結果判定部２３５の判定結果に基づいて、第１選択条件を、第２選択条件または第３選択条件に切り替えて動作する。

マップ点使用条件設定部２３３は、マッチング結果判定部２３５からマッチング結果「否」を受け付けた場合に、第２選択条件または第３選択条件を選択する。例えば、マップ点使用条件設定部２３３は、第１選択条件選択中に、マッチング結果「否」を受け付けると、第２選択条件を選択して動作する。マップ点使用条件設定部２３３は、第２選択条件選択中に、マッチング結果「否」を受け付けると、第３選択条件を選択して動作する。以下において、第１選択条件、第２選択条件、第３選択条件の具体例について説明する。

第１選択条件は、第１マップ点を、第２マップ点よりも優先して選択するという条件である。第１選択条件に基づくマップ点使用条件設定部２３３は、第１マップ点の３次元座標、第１投影点の２次元座標、特徴点の情報、撮影画像の情報を、マッチング処理部２３４に出力する。

第２選択条件は、不足するマッチング成功数を補うべく、第２マップ点を選択する場合に、複数の第２マップ点のうち、カメラ５０ａにより近い第２マップ点を優先して選択するという条件である。マップ点使用条件設定部２３３は、マップ点投影部２３２に、第２投影点の情報を要求する。

第２選択条件に基づくマップ点使用条件設定部２３３は、選択した第２マップ点の３次元座標、第２投影点の２次元座標、特徴点の情報、撮影画像の情報を、マッチング処理部２３４に出力する。また、マップ点使用条件設定部２３３は、上記第２マップ点の情報に加えて、第１マップ点の３次元座標、第１投影点の２次元座標を、マッチング処理部２３４に出力する。

一般に、３Ｄ距離センサ、ステレオ撮影の原理で特定したマップ点の３次元座標の精度は、撮影時のカメラ５０ａにより近いマップ点ほど精度が高くなる。このため、第２マップ点を選択する場合には、撮影時のカメラ５０ａにより近い第２マップ点を選択することで、カメラの位置姿勢の推定精度を向上させる。

マップ点の３次元座標は、初期マップ情報を作成した時のカメラ５０ａの撮影位置Ｃａを原点にして定義されている。撮影位置Ｃａからマップ点までの距離を求めるためには、まず、マップ点の各３次元座標値Ｐ_ｉ（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）を、撮影時のカメラ５０ａの位置を原点とした座標値ｐ_ｉ（ｘ_ｉ、ｙ_ｉ，ｚ_ｉ）に移動させて変換する。この変換には、カメラ５０ａの並進成分と回転成分とを用いる。並進成分Ｔは、式（５）によって表される。回転成分Ｒは、式（６）によって表される。式（５）と式（６）とを組み合わせたものが、カメラ位置姿勢行列Ｍに対応する。

並進成分Ｔ＝（Ｔ_Ｘ，Ｔ_Ｙ，Ｔ_Ｚ）・・・（５）

座標値Ｐ_ｉと、座標値ｐ_ｉとの関係は、並進成分Ｔおよび回転成分Ｒを用いて、式（７）に示すものとなる。このため、各マップ点の撮影時のカメラまでの距離ｄ_ｉは、式（８）に示すものとなる。

ｐ_ｉ＝Ｒ・Ｐ_ｉ＋Ｔ・・・（７）

マップ点使用条件設定部２３３は、式（８）に基づいて、各第２マップ点と撮影時のカメラ５０ａまでの距離ｄ_ｉを算出し、距離ｄ_ｉの小さい第２マップ点を優先して選択する。

第３選択条件は、不足するマッチング成功数を補うべく、第２マップ点を選択する場合に、選択する複数の第２マップ点の分布が広くなるように、第２マップ点を選択する。マップ点使用条件設定部２３３は、第２投影点の情報を、マップ点投影部２３２に要求する。

第３選択条件に基づくマップ点使用条件設定部２３３は、選択した第２マップ点の３次元座標、第２投影点の２次元座標、特徴点の情報、撮影画像の情報を、マッチング処理部２３４に出力する。また、マップ点使用条件設定部２３３は、上記第２マップ点の情報に加えて、第１マップ点の３次元座標、第１投影点の２次元座標を、マッチング処理部２３４に出力する。

上述したように、カメラ５０ａの位置姿勢の推定精度は、マップ点の分布が広いほど向上する。この分布は、マップ点の３次元座標値の３次元空間上の分布ではなく、３次元座標値をカメラの撮影画像の画面上に投影した投影点の２次元座標値ｐ_ｉ’（ｕ_ｉ’、ｖ_ｉ’）となる。３次元座標値から２次元座標値への変換は、式（９）によって表すことができる。式（９）において、Ａは、予め設定される３×３の透視変換行列である。

マップ点の２次元座標値ｐ_ｉ’が撮影画像の全体に均一に広がっている方が、均一に広がっていない場合と比較して、カメラ５０ａの位置姿勢の推定精度が向上する。ここで、２次元座標値の分布状態を評価する方法はいろいろあり、どのような方法を選択しても良い。例えば、２次元座標値の重心点（ｕｇ，ｖｇ）を求め、この重心点の画像内の位置と、重心点から各２次元座標値の分散ＳＤｄを用いる方法がある。マップ点使用条件設定部２３３は、式（１０）に基づいて重心点（ｕｇ，ｖｇ）を算出する。マップ点使用条件設定部２３３は、式（１１）に基づいて距離の分散ＳＤｄを算出する。式（１０）および式（１１）において、Ｎは、撮影画像内に位置するマップ点の２次元座標の個数を示す。

マップ点使用条件設定部２３３は、所定数の第２マップを選択し、選択した第２マップ点の２次元座標値と、式（１０）、式（１１）に基づいて、距離の分散ＳＤｄを算出する。マップ点使用条件設定部２３３は、距離の分散ＳＤｄが、基準値ＳＤ_ｍｉｎ以上である場合には、選択した第２マップ点の情報を、マッチング処理部２３４に出力する。

一方、マップ点使用条件設定部２３３は、距離の分散ＳＤｄが、基準値ＳＤ_ｍｉｎ未満である場合には、マップ点の２次元座標から重心点までの距離Ｄ_ｉが遠い第２マップ点を優先して選択する。マップ点使用条件設定部２３３は、式（１２）に基づいて、距離を算出する。マップ点使用条件設定部２３３は、マップ点を選択した後に、上記処理を繰り返し実行する。

ところで、マップ点使用条件設定部２３３は、マップ点の分布状況をより正確に把握する場合には、撮影画像を縦横複数のブロックに分割し、それぞれのブロック個々に対して、マップ点の個数と分布の判定を行い、必要に応じて、マップ点を追加する処理を行っても良い。

図６は、マップ点の分布状況の判定方法の一例を示す図である。図６に示すように、撮影画像４０のブロック４１，４２，４３には、所定の個数のマップ点が存在していない。このため、マップ点使用条件設定部２３３は、初期マップ情報を参照し、ブロック４１，４２，４３に含まれる第２マップ点（第２投影点）を選択する。また、マップ点使用条件設定部２３３は、その他のブロックについても、ブロック単位で、距離の分散ＳＤｄが、基準値ＳＤ_ｍｉｎ未満であるか否かを判定し、ブロック単位で、第２マップ点を選択する処理を行っても良い。

マッチング処理部２３４は、撮影画像中の特徴点と、マップ点の投影点とを比較して、類似している特徴点と、投影点とをマッチングすることで、投影点と特徴点とのペアを作成する処理部である。マッチング処理部２３４は、投影点と特徴点とのペアの情報を、マッチング結果判定部２３５に出力する。投影点と特徴点とのペアの情報は、ペアとなった特徴点の座標と、投影点の座標との組と、投影点に対応するマップ点の情報を含む。

例えば、マッチング処理部２３４は、座標が最も近い投影点と特徴点とを選択し、選択した投影点に対応するマップ点の特徴量と、特徴点の周囲の画素値に基づく特徴量とを比較し、特徴量の差分が閾値未満である場合に、選択した投影点と特徴点とをペアとする。

また、マッチング処理部２３４は、マップ点に第１マップ点と、第２マップ点とが存在する場合には、第１マップ点の投影点を第２マップ点の投影点よりも優先して、特徴点とのペアとして選択する。例えば、ある特徴点と第１投影点との距離と、ある特徴点と第２投影点との距離が同じ場合には、ある特徴点と第１投影点とをペアとしても良い。

マッチング結果判定部２３５は、マッチング処理部２３４から投影点と特徴点とのペアの情報を取得した場合に、マッチング結果の良否を判定する処理部である。マッチング結果判定部２３５は、マッチング結果が「良」であると判定した場合には、投影点と特徴点とのペアの情報を、位置姿勢推定部２４０に出力する。一方、マッチング結果判定部２３５は、マッチング結果が「否」であると判定した場合には、マッチング結果「否」を、マップ点使用条件設定部２３３に出力する。

マッチング結果判定部２３５の処理の一例について説明する。マッチング結果判定部２３５は、投影点と特徴点とのペアの情報を取得した場合に、ペアの個数が所定数Ｎ_ｍｉｎ未満である場合に、マッチング結果が「否」であると判定する。

更に、マッチング結果判定部２３５は、上述した式（１１）に基づいて、距離の分散ＳＤｄを算出してもよい。マッチング結果判定部２３５は、距離の分散ＳＤｄが、基準値ＳＤ_ｍｉｎ未満である場合に、マッチング結果が「否」であると判定する。

位置姿勢推定部２４０は、マッチング結果判定部２３５から投影点と特徴点とのペアの情報を取得した場合に、投影点と特徴点とのペアの情報および初期マップ情報を基にして、カメラ５０ａの位置姿勢を推定する処理部である。

例えば、位置姿勢推定部２４０は、上述した式（４）によって算出される二乗和Ｅが最小となるようなカメラ位置姿勢行列Ｍを算出することで、カメラ５０ａの位置姿勢を算出する。位置姿勢推定部２４０は、算出したカメラ５０ａの位置姿勢の情報を出力する。

次に、本実施例に係る初期マップ生成部１００ａの処理手順について説明する。図７は、本実施例に係る初期マップ生成部の処理手順を示すフローチャートである。図７に示すように、初期マップ生成部１００ａの画像取得部１０１は、１枚目の撮影画像を取得する（ステップＳ１０１）。例えば、１枚目の撮影画像は、撮影位置Ｃａでカメラ５０ａに撮影された撮影画像に対応する。

初期マップ生成部１００ａの特徴点検出部１０２は、１枚目の撮影画像から特徴点を検出する（ステップＳ１０２）。初期マップ生成部１００ａの３Ｄ距離画像取得部１０５は、３次元距離画像を取得する（ステップＳ１０３）。

画像取得部１０１は、２枚目の撮影画像を取得する（ステップＳ１０４）。例えば、２枚目の撮影画像は、撮影位置Ｃｂでカメラ５０ａに撮影された撮影画像に対応する。特徴点検出部１０２は、２枚目の撮影画像から特徴点を検出する（ステップＳ１０５）。

初期マップ生成部１００ａの特徴点マッチング処理部１０３は、２枚の撮影画像の特徴点をマッチングする（ステップＳ１０６）。初期マップ生成部１００ａの特徴点ステレオ処理部１０４は、ステレオ処理でのマップ点（第２マップ点）の３次元座標を算出する（ステップＳ１０７）。

初期マップ生成部１００ａの特徴点３次元座標取得部１０６は、３Ｄ距離画像からマップ点（第１マップ点）の３次元座標を算出する（ステップＳ１０８）。初期マップ生成部１００ａのマップ点生成部１０７は、マップ点の選択および整理を行い（ステップＳ１０９）、初期マップ情報を初期マップ記憶部１０８に保存する（ステップＳ１１０）。例えば、マップ点生成部１０７は、ステップＳ１０９において、第１マップ点を、第２マップ点よりも優先して、初期マップ情報を生成する。

次に、本実施例に係るカメラ位置姿勢推定部１００ｂの処理手順について説明する。図８および図９は、本実施例に係るカメラ位置姿勢推定部の処理手順を示すフローチャートである。図８に示すように、カメラ位置姿勢推定部１００ｂの画像取得部２１０は、撮影画像を取得する（ステップＳ２０１）。カメラ位置指定推定部１００ｂの特徴点検出部２３１は、特徴点を検出する（ステップＳ２０２）。

カメラ位置姿勢推定部１００ｂのマップ点投影部２３２は、初期マップ情報からマップ点の情報を読み出し（ステップＳ２０３）、マップ点を撮影画像に投影する（ステップＳ２０４）。マップ点投影部２３２は、撮影画像内に収まるマップ点を抽出する（ステップＳ２０５）。

カメラ位置姿勢推定部１００ｂのマップ点使用条件設定部２３３は、抽出したマップ点のカメラからの距離を算出する（ステップＳ２０６）。マップ点使用条件設定部２３３は、抽出したマップ点から種類［３Ｄ距離センサ］の第１マップ点を選択する（ステップＳ２０７）。カメラ位置姿勢推定部１００ｂのマッチング処理部２３４はが特徴点とマップ点（投影点）とのマッチング処理を行う。また、カメラ位置姿勢推定部１００ｂのマッチング結果判定部２３５は、マッチング成功点数を特定し（ステップＳ２０８）、図９のステップＳ２０９に移行する。

図９に説明移行する。マッチング結果判定部２３５は、マッチング成功点数がＮ_ｍｉｎ以上であるか否かを判定する（ステップＳ２０９）。マッチング結果判定部２３５は、マッチング成功点数がＮ_ｍｉｎ以上である場合には（ステップＳ２０９，Ｙｅｓ）、ステップＳ２１４に移行する。一方、マッチング結果判定部２３５は、マッチング成功点数がＮ_ｍｉｎ以上でない場合には（ステップＳ２０９，Ｎｏ）、ステップＳ２１０に移行する。

マップ点使用条件設定部２３３は、種類［ステレオ］のマップ点の選択条件を設定する（ステップＳ２１０）。ステップ２１０において、例えば、マップ点使用条件設定部２３３は、第２選択条件または第３選択条件を設定する。マップ点使用条件設定部２３３は、種類［ステレオ］の第２マップ点を選択する（ステップＳ２１１）。

マッチング処理部２３４は、特徴点とマップ点とのマッチング処理を行い、マッチング結果判定部２３５は、マッチング成功点数を特定する（ステップＳ２１２）。

マッチング結果判定部２３５は、マッチング成功点数がＮ_ｍｉｎ以上であるか否かを判定する（ステップＳ２１３）。マッチング結果判定部２３５は、マッチング成功点数がＮ_ｍｉｎ以上である場合には（ステップＳ２１３，Ｙｅｓ）、ステップＳ２１４に移行する。一方、マッチング結果判定部２３５は、マッチング成功点数がＮ_ｍｉｎ以上でない場合には（ステップＳ２１３，Ｎｏ）、ステップＳ２１０に移行する。

マッチング結果判定部２３５は、距離の分散ＳＤｄがＳＤ_ｍｉｎ以上であるか否かを判定する（ステップＳ２１４）。マッチング結果判定部２３５は、距離の分散ＳＤｄがＳＤ_ｍｉｎ以上である場合には（ステップＳ２１４，Ｙｅｓ）、ステップＳ２１５に移行する。一方、マッチング結果判定部２３５は、距離の分散ＳＤｄがＳＤ_ｍｉｎ以上でない場合には（ステップＳ２１４，Ｎｏ）、ステップＳ２１０に移行する。

カメラ位置姿勢推定部１００ｂの位置姿勢推定部２４０は、カメラの位置姿勢を推定する（ステップＳ２１５）。カメラ位置姿勢推定部１００ｂは、処理を終了するか否かを判定する（ステップＳ２１６）。カメラ位置姿勢推定部１００ｂは、処理を終了する場合には（ステップＳ２１６，Ｙｅｓ）、処理を終了する。一方、カメラ位置姿勢推定部１００ｂは、処理を終了しない場合には（ステップＳ２１６，Ｎｏ）、図８のステップＳ２０１に移行する。

次に、本実施例に係る画像処理装置１００の効果について説明する。画像処理装置１００は、３Ｄ距離センサ５０ｂにより取得した第１マップ点と撮影画像の特徴点とのマッチング処理を行う。画像処理装置１００は、マッチング成功数が不足する場合には、ステレオ撮影の原理により求めた第２マップ点を追加して、不足するマッチング成功数を補い、カメラ５０ａの位置姿勢を推定する。一般的に、第２マップ点よりも、第１マップ点の方が３次元座標の精度が高いため、第２マップ点の情報よりも、第１マップ点の情報を優先して選択することで、カメラの位置姿勢の推定精度を向上させることができる。

画像処理装置１００は、初期マップ生成部１００ａが初期マップ情報に、第１マップ点の情報を第２マップ点の情報よりも優先して保持する。一般的に、第２マップ点よりも、第１マップ点の方が３次元座標の精度が高いため、かかる初期マップ情報を用いてカメラの位置姿勢を推定することで、精度を向上させることができる。

画像処理装置１００は、マップ点を選択する場合に、カメラ５０ａにより近いマップ点を優先して選択し、選択したマップ点を撮像画像上に投影した投影点と、特徴点とのマッチングを実行する。撮影時のカメラ５０ａにより近いマップ点は検出精度が高いと言えるため、カメラ５０ａにより近いマップ点を優先して選択することで、カメラの位置姿勢の推定精度を向上させることができる。

画像処理装置１００は、撮像画像上に投影したマップ点の分布状況が閾値よりも大きくなるという条件に基づき、マップ点を選択し、選択したマップ点を撮像画像上に投影した投影点と特徴点とのマッチングを実行する。マップ点の分布の広がりが広いほどカメラの位置姿勢の推定精度が向上するため、マップ点の分布状況が閾値よりも大きくなるという条件を利用することで、位置姿勢の推定精度を向上させることができる。

ところで、３Ｄ距離センサ５０ｂで３次元座標値を求めたマップ点の個数が多すぎる場合は、その中にカメラからの距離が比較的遠く大きな誤差を含むマップ点を含んでいて、カメラ位置・姿勢推定の精度が比較的低下してしまう可能性が有る。この場合には使用するマップ点の個数を絞り込むこともできるが、カメラ位置姿勢推定部１００ｂは、この際に小さな誤差を含むマップ点を極力選択するように、撮影時のカメラから遠いマップ点を優先的に除外していくことも良い。

次に、上記実施例に示した画像処理装置１００と同様の機能を実現する画像処理プログラムを実行するコンピュータの一例について説明する。図１０は、画像処理プログラムを実行するコンピュータの一例を示す図である。

図１０に示すように、コンピュータ３００は、各種演算処理を実行するＣＰＵ３０１と、ユーザからのデータの入力を受け付ける入力装置３０２と、ディスプレイ３０３とを有する。また、コンピュータ３００は、記憶媒体からプログラム等を読み取る読み取り装置３０４と、ネットワークを介して他のコンピュータとの間でデータの授受を行うインターフェース装置３０５とを有する。また、コンピュータ３００は、各種情報を一時記憶するＲＡＭ３０６と、ハードディスク装置３０７とを有する。そして、各装置３０１〜３０７は、バス３０８に接続される。

ハードディスク装置３０７は、マッチングプログラム３０７ａ、位置姿勢推定プログラム３０７ｂを有する。ＣＰＵ３０１は、マッチングプログラム３０７ａ、位置姿勢推定プログラム３０７ｂを読み出してＲＡＭ３０６に展開する。マッチングプログラム３０７ａは、マッチングプロセス３０６ａとして機能する。位置姿勢推定プログラム３０７ｂは、位置姿勢推定プロセス３０６ｂとして機能する。

なお、マッチングプログラム３０７ａ、位置姿勢推定プログラム３０７ｂについては、必ずしも最初からハードディスク装置３０７に記憶させておかなくても良い。例えば、コンピュータ３００に挿入されるフレキシブルディスク（ＦＤ）、ＣＤ−ＲＯＭ、ＤＶＤディスク、光磁気ディスク、ＩＣカードなどの「可搬用の物理媒体」に各プログラムを記憶させておく。そして、コンピュータ３００がマッチングプログラム３０７ａ、位置姿勢推定プログラム３０７ｂを読み出して実行するようにしてもよい。

以上の各実施例を含む実施形態に関し、さらに以下の付記を開示する。

（付記１）距離センサにより測定された特徴点の３次元座標を示す第１マップ点と、複数の撮影画像に基づいて特定された特徴点の３次元座標を示す第２マップ点との情報を含むマップ点情報を記憶する記憶部と、
前記第１マップ点を撮像画像上に投影した第１投影点と、前記撮影画像上の特徴点とを比較して、類似する第１投影点と特徴点とのペアを作成し、
作成したペアの個数に基づいて、前記第２マップ点を用いたマッチングを実行するか否かを判定し、前記第２マップ点を用いたマッチングを実行すると判定した場合には、前記第２マップ点を撮像画像上に投影した第２投影点と、前記撮影画像上の特徴点とを比較して、類似する第２投影点と特徴点とのペアを作成するマッチング部と、
前記マッチング部のマッチング結果および前記マップ点情報を基にして、前記撮影画像を撮影したカメラの位置および姿勢を推定する位置姿勢推定部と
を有することを特徴とする画像処理装置。

（付記２）前記マップ点情報は、前記第１マップ点の情報を第２マップ点の情報よりも優先して保持することを特徴とする付記１に記載の画像処理装置。

（付記３）前記マッチング部は、類似する第１投影点と特徴点とのペアの個数が閾値未満である場合に、前記第２投影点と、前記撮影画像上の特徴点とを比較して、類似する第２投影点と特徴点とのペアを作成することを特徴とする付記１または２に記載の画像処理装置。

（付記４）前記マッチング部は、前記第２マップ点を用いたマッチングを実行すると判定した場合には、前記カメラにより近い第２マップ点を優先して選択し、選択した第２マップ点を撮像画像上に投影した第２投影点と、前記特徴点とのマッチングを実行することを特徴とする付記１、２または３に記載の画像処理装置。

（付記５）前記マッチング部は、前記撮像画像上に投影した前記第２マップ点の分布状況が閾値よりも大きくなるという条件に基づき、前記第２マップ点を選択し、選択した第２マップ点を撮像画像上に投影した第２投影点と特徴点とのマッチングを実行することを特徴とする付記１〜４の何れか一つに記載の画像処理装置。

（付記６）コンピュータが実行する画像処理方法であって、
距離センサにより測定された特徴点の３次元座標を示す第１マップ点と、複数の撮影画像に基づいて特定された特徴点の３次元座標を示す第２マップ点との情報を含むマップ点情報を記憶する記憶装置を参照し、
前記第１マップ点を撮像画像上に投影した第１投影点と、前記撮影画像上の特徴点とを比較して、類似する第１投影点と特徴点とのペアを作成し、
作成したペアの個数に基づいて、前記第２マップ点を用いたマッチングを実行するか否かを判定し、
前記第２マップ点を用いたマッチングを実行すると判定した場合には、前記第２マップ点を撮像画像上に投影した第２投影点と、前記撮影画像上の特徴点とを比較して、類似する第２投影点と特徴点とのペアを作成する
処理を実行することを特徴とする画像処理方法。

（付記７）前記マップ点情報は、前記第１マップ点の情報を第２マップ点の情報よりも優先して保持することを特徴とする付記６に記載の画像処理方法。

（付記８）前記ペアを作成する処理は、類似する第１投影点と特徴点とのペアの個数が閾値未満である場合に、前記第２投影点と、前記撮影画像上の特徴点とを比較して、類似する第２投影点と特徴点とのペアを作成することを特徴とする付記６または７に記載の画像処理方法。

（付記９）前記ペアを作成する処理は、前記第２マップ点を用いたマッチングを実行すると判定した場合には、カメラにより近い第２マップ点を優先して選択し、選択した第２マップ点を撮像画像上に投影した第２投影点と、前記特徴点とのマッチングを実行することを特徴とする付記６、７または８に記載の画像処理方法。

（付記１０）前記ペアを作成する処理は、前記撮像画像上に投影した前記第２マップ点の分布状況が閾値よりも大きくなるという条件に基づき、前記第２マップ点を選択し、選択した第２マップ点を撮像画像上に投影した第２投影点と特徴点とのマッチングを実行することを特徴とする付記６〜９の何れか一つに記載の画像処理方法。

（付記１１）コンピュータに、
距離センサにより測定された特徴点の３次元座標を示す第１マップ点と、複数の撮影画像に基づいて特定された特徴点の３次元座標を示す第２マップ点との情報を含むマップ点情報を記憶する記憶装置を参照し、
前記第１マップ点を撮像画像上に投影した第１投影点と、前記撮影画像上の特徴点とを比較して、類似する第１投影点と特徴点とのペアを作成し、
作成したペアの個数に基づいて、前記第２マップ点を用いたマッチングを実行するか否かを判定し、
前記第２マップ点を用いたマッチングを実行すると判定した場合には、前記第２マップ点を撮像画像上に投影した第２投影点と、前記撮影画像上の特徴点とを比較して、類似する第２投影点と特徴点とのペアを作成する
処理を実行させることを特徴とする画像処理プログラム。

（付記１２）前記マップ点情報は、前記第１マップ点の情報を第２マップ点の情報よりも優先して保持することを特徴とする付記１１に記載の画像処理プログラム。

（付記１３）前記ペアを作成する処理は、類似する第１投影点と特徴点とのペアの個数が閾値未満である場合に、前記第２投影点と、前記撮影画像上の特徴点とを比較して、類似する第２投影点と特徴点とのペアを作成することを特徴とする付記１１または１２に記載の画像処理プログラム。

（付記１４）前記ペアを作成する処理は、前記第２マップ点を用いたマッチングを実行すると判定した場合には、カメラにより近い第２マップ点を優先して選択し、選択した第２マップ点を撮像画像上に投影した第２投影点と、前記特徴点とのマッチングを実行することを特徴とする付記１１、１２または１３に記載の画像処理プログラム。

（付記１５）前記ペアを作成する処理は、前記撮像画像上に投影した前記第２マップ点の分布状況が閾値よりも大きくなるという条件に基づき、前記第２マップ点を選択し、選択した第２マップ点を撮像画像上に投影した第２投影点と特徴点とのマッチングを実行することを特徴とする付記１１〜１４の何れか一つに記載の画像処理プログラム。

５０ａ カメラ
５０ｂ ３Ｄ距離センサ
１００ 画像処理装置
１００ａ 初期マップ生成部
１００ｂ カメラ位置姿勢推定部

Claims (7)

1. 距離センサにより測定された特徴点の３次元座標を示す第１マップ点と、複数の撮影画像に基づいて特定された特徴点の３次元座標を示す第２マップ点との情報を含むマップ点情報を記憶する記憶部と、
   前記第１マップ点を撮像画像上に投影した第１投影点と、前記撮影画像上の特徴点とを比較して、類似する第１投影点と特徴点とのペアを作成し、
   作成したペアの個数に基づいて、前記第２マップ点を用いたマッチングを実行するか否かを判定し、前記第２マップ点を用いたマッチングを実行すると判定した場合には、前記第２マップ点を撮像画像上に投影した第２投影点と、前記撮影画像上の特徴点とを比較して、類似する第２投影点と特徴点とのペアを作成するマッチング部と、
   前記マッチング部のマッチング結果および前記マップ点情報を基にして、前記撮影画像を撮影したカメラの位置および姿勢を推定する位置姿勢推定部と
   を有することを特徴とする画像処理装置。
2. 前記マップ点情報は、前記第１マップ点の情報を第２マップ点の情報よりも優先して保持することを特徴とする請求項１に記載の画像処理装置。
3. 前記マッチング部は、類似する第１投影点と特徴点とのペアの個数が閾値未満である場合に、前記第２投影点と、前記撮影画像上の特徴点とを比較して、類似する第２投影点と特徴点とのペアを作成することを特徴とする請求項１または２に記載の画像処理装置。
4. 前記マッチング部は、前記第２マップ点を用いたマッチングを実行すると判定した場合には、前記カメラにより近い第２マップ点を優先して選択し、選択した第２マップ点を撮像画像上に投影した第２投影点と、前記特徴点とのマッチングを実行することを特徴とする請求項１、２または３に記載の画像処理装置。
5. 前記マッチング部は、前記撮像画像上に投影した前記第２マップ点の分布状況が閾値よりも大きくなるという条件に基づき、前記第２マップ点を選択し、選択した第２マップ点を撮像画像上に投影した第２投影点と特徴点とのマッチングを実行することを特徴とする請求項１〜４の何れか一つに記載の画像処理装置。
6. コンピュータが実行する画像処理方法であって、
   距離センサにより測定された特徴点の３次元座標を示す第１マップ点と、複数の撮影画像に基づいて特定された特徴点の３次元座標を示す第２マップ点との情報を含むマップ点情報を記憶する記憶装置を参照し、
   前記第１マップ点を撮像画像上に投影した第１投影点と、前記撮影画像上の特徴点とを比較して、類似する第１投影点と特徴点とのペアを作成し、
   作成したペアの個数に基づいて、前記第２マップ点を用いたマッチングを実行するか否かを判定し、
   前記第２マップ点を用いたマッチングを実行すると判定した場合には、前記第２マップ点を撮像画像上に投影した第２投影点と、前記撮影画像上の特徴点とを比較して、類似する第２投影点と特徴点とのペアを作成する
   処理を実行することを特徴とする画像処理方法。
7. コンピュータに、
   距離センサにより測定された特徴点の３次元座標を示す第１マップ点と、複数の撮影画像に基づいて特定された特徴点の３次元座標を示す第２マップ点との情報を含むマップ点情報を記憶する記憶装置を参照し、
   前記第１マップ点を撮像画像上に投影した第１投影点と、前記撮影画像上の特徴点とを比較して、類似する第１投影点と特徴点とのペアを作成し、
   作成したペアの個数に基づいて、前記第２マップ点を用いたマッチングを実行するか否かを判定し、
   前記第２マップ点を用いたマッチングを実行すると判定した場合には、前記第２マップ点を撮像画像上に投影した第２投影点と、前記撮影画像上の特徴点とを比較して、類似する第２投影点と特徴点とのペアを作成する
   処理を実行させることを特徴とする画像処理プログラム。

Images