JP5109294B2

JP5109294B2 - ３次元位置補正装置

Info

Publication number: JP5109294B2
Application number: JP2006168786A
Authority: JP
Inventors: 昭二田中
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2006-06-19
Filing date: 2006-06-19
Publication date: 2012-12-26
Anticipated expiration: 2026-06-19
Also published as: JP2007333679A

Description

本発明は、実写映像に映された対象物画像に合わせて、補足情報を正確に重畳表示する３次元位置補正装置に関する。

本発明は、例えば、実写映像や実写画像に映された建物や道路に合わせて、補足情報を正確に重畳表示する場合に、カメラと対象物までの距離や方向を測定する処理の演算量を抑えて、効率的に補足情報の重畳処理を行なうためになされたものである。

従来、カメラを用いて、対象物の位置および姿勢を認識するものとして、特許文献１のものがある。この技術は、配電作業ロボットの自動制御を目的に開発されたものであり、円柱形上の物体を想定し、画像から抽出される縦方向の輪郭エッジ（線分）と横方向の帯エッジを３次元モデルに当てはめて円柱の位置と姿勢を求めていた。当てはめ処理においては、上記縦方向エッジと３次元モデルから取得される線分が適合するように３次元モデルの位置姿勢パラメータを調整し、再度３次元モデルを投影したときの適合誤差を求める。さらに、横方向のエッジについても同様の処理を行なう。この当てはめ処理を当てはめ候補となる全ての組み合わせに対して行ない、誤差が最小となるものを選択することにより対象物の位置と姿勢を推定していた。

別の従来技術では、ＧＰＳ（ＧｌｏｂａｌＰｏｓｉｔｉｏｎｉｎｇＳｙｓｔｅｍ）やジャイロなど、カーナビに搭載されているセンサーと画像処理をハイブリッドに組み合わせることにより実写に情報を重畳する際のカメラ姿勢情報を推定していた。例えば、非特許文献１では、地図データに格納されている道路データ（リンク情報、道路幅、レーン数）から、道路の３次元モデルデータを構築し、実写から抽出した白線とマッチングを行なうことにより、ＧＰＳおよびジャイロの検出位置ずれ誤りであるドリフトの問題を解消していた。

さらに別の従来技術である非特許文献２では、画像から抽出される特徴点やエッジと３次元の点および線分モデルと対応付けることによりカメラの姿勢情報を推定していた。

特開平１１−５１６１１号公報（第１８頁、図１） Zhencheng Hu and Keiichi Uchimura, "Solution of Camera Registration Problem via 3D-2D Parameterized Model Matching for On-Road Navigation, " International Journal of Image and Graphics, Vol. 4, No. 1, pp. 1-18, 2004. Behzad Kamagar-Parsi and Behrooz Kamgar-Parsi, "Algorithms for Matching 3D Line Sets," IEEE Transaction on Pattern Analysis and Machine Intelligence, Vol. 26, No. 5, pp. 582-593, 2004.

上記の特許文献１で開示された従来技術では、円柱のような左右対称の物体にしか用いることができない問題があった。また、当てはめ処理において縦方向、横方向のエッジそれぞれを分けて処理する必要があり、その度に３次元モデルを２次元画像へ投影する処理を行なわなければならないため、演算量が大きくなるという問題があった。

また、上記の非特許文献１で開示された従来技術では、道路データから推定した道路の３次元モデルが実際と異なる場合が考えられ、また、道路が混雑している場合にマッチングに必要な白線が抽出できないなどの問題があった。

また、上記の非特許文献２で開示された従来技術では、画像から抽出される全ての２次元特徴と３次元モデルの特徴との対応を検査する必要があり、抽出される２次元特徴の数が多くなると演算量が大きくなるという問題があった。

本発明は上記課題を解決するためになされたものであり、任意形状の物体を撮影したときの物体の位置および姿勢、あるいは画像を撮影したカメラの位置および姿勢を画像から抽出した特徴と、対象とする物体の３次元モデルから抽出した特徴との対応を取る際に、上記２次元特徴の候補を絞り込むことにより、全体の演算量を抑えることを目的にするものである。

このような問題を解決するため、本発明に係る３次元位置補正装置は、カメラ画像を入力する画像入力手段と、入力された前記カメラ画像を、白から黒までの明暗の濃淡だけで表示されたグレースケール画像に変換するグレースケール変換手段と、現在のカメラ位置および姿勢を測定する測位手段と、複数種類の、ポリゴンから構成される３次元モデルを格納する３次元モデルＤＢと、前記３次元モデルＤＢから、前記測位手段により測定された前記現在のカメラ位置および姿勢で見える範囲の複数種類の前記３次元モデルのモデル特徴を抽出するモデル特徴抽出手段と、予め設定された画像特徴抽出条件に基づいて、前記グレースケール画像から、前記モデル特徴とのマッチングに必要な画像特徴を抽出する画像特徴抽出手段と、予め設定された特徴対応抽出条件に基づいて、前記モデル特徴と前記画像特徴とを２次元平面上でマッチングさせて、前記モデル特徴と前記画像特徴との対応関係である特徴対応情報を取得する２次元マッチング手段と、前記特徴対応情報に基づいて、前記測位手段で測定した前記現在のカメラ位置および姿勢を補正するために、前記特徴対応情報から、カメラあるいは前記モデル特徴に対応する物体の３次元の位置および姿勢のパラメータを、３次元姿勢パラメータとして推定する３次元姿勢パラメータ推定手段とを備えることとしたものである。

本発明によれば、画像特徴抽出手段により、グレースケール画像から、モデル特徴とのマッチングに必要な画像特徴を所定の画像特徴制約条件で絞り込んで抽出し、また、２次元マッチング手段により、モデル特徴と画像特徴との対応関係である特徴対応情報を、所定の特徴対応制約条件で絞り込んで取得するように構成したので、全体の演算量を抑えることができるようになる。

実施の形態１．
本発明の実施の形態１を、図１〜図９を用いて説明する。図１は、本発明の実施の形態１の３次元位置補正装置の構成図である。図２は、本発明の実施の形態１における、実写映像中の物体上に適切に情報を重畳する例の概念図である。図３は、本発明の実施の形態１における、３次元位置補正装置の基本処理動作を示すフローチャートである。図４は、本発明の実施の形態１における、画像特徴抽出手段における処理結果の説明図である。図５は、本発明の実施の形態１における、画像特徴抽出手段の処理動作を示すフローチャートである。図６は、本発明の実施の形態１における、線分情報抽出手段で抽出された線分情報を格納するテーブルを示す図である。図７は、本発明の実施の形態１における、コーナー検出手段で検出したコーナー情報を格納するテーブルを示す図である。図８は、本発明の実施の形態１における、コーナーＩＤを格納する２次元配列を示す図である。図９は、本発明の実施の形態１における、複数の線分が異なる角度で交わっている重要な線分情報の例を示す図である。以下、図１〜図９を適宜参照しながら本発明の実施の形態１を詳細に説明する。

図１において、１０１は３次元位置補正装置、１０２はカメラからの画像を入力する画像入力手段、１０３は画像入力手段１０２からカラー画像が入力されたときに画像を白から黒までの明暗の濃淡だけで表示されたグレースケールに変換するグレースケール変換手段、１０４はグレースケール画像からモデル特徴とのマッチングに必要な２次元画像特徴を所定の画像特徴制約条件で絞り込んで抽出する画像特徴抽出手段、１０５はカメラの初期位置および姿勢情報をＧＰＳやジャイロセンサーなどから獲得する測位手段、１０６は２次元画像特徴に対応する３次元モデル特徴を抽出するモデル特徴抽出手段、１０７は３次元モデル特徴と２次元画像特徴とを２次元上でマッチングするための２次元マッチング手段、１０８は２次元マッチング手段で対応の取れた２次元画像特徴と３次元モデル特徴から、対象となる物体あるいはカメラの３次元姿勢パラメータを推定する３次元姿勢パラメータ推定手段、１０９は例えば、図２に示すように、実写映像中の物体上に適切に情報を重畳する場合に、３次元位置補正装置１０１で獲得したカメラの位置および姿勢情報を用いて情報を重畳する情報重畳手段である。

また、画像特徴抽出手段１０４は、画像からエッジを抽出するエッジ抽出手段１１０と、画像から曲率の高いコーナー情報を検出するコーナー検出手段１１１と、抽出されたエッジを直線近似する線分情報抽出手段１１２と、抽出された線分情報とコーナー情報に基づいて対応線分候補を絞り込む対応線分候補抽出手段１１３とにより構成されている。

また、モデル特徴抽出手段１０６は、現在のカメラの位置および姿勢情報とカメラパラメータを用いて３次元モデルを投影したときの線分情報を獲得するモデル線分獲得手段１１４と、３次元モデルを格納する３次元モデルＤＢ１１５と、カメラパラメータを格納するカメラパラメータＤＢ１１６とにより構成されている。

次に、図３を用いて、本発明の実施の形態１における３次元位置補正装置の基本処理動作について説明する。まず、画像入力手段１０２においてカメラで撮影されたカラー画像、あるいは、白黒画像を入力する（ステップＳ１０１）。ここで、入力された画像がカラー画像である場合、画像特徴の抽出精度を高めるために、グレースケール変換手段１０３でカラー画像をグレースケール画像に変換する（ステップＳ１０２）。カラー画像からグレースケール画像に変換するためには、例えば次の計算式（１）を用いて行なう。

ここで、Gray(x、y)とは、座標(x、y)におけるグレースケール画像の画素値、R(x、y)は座標(x、y)におけるカラー画像の赤色成分の画素値、G(x、y)は座標(x、y)におけるカラー画像の緑色成分の画素値、B(x、y)は座標(x、y)におけるカラー画像の青色成分の画素値である。

次に、画像特徴抽出手段１０４において、マッチングに必要な画像特徴を抽出する。まず、エッジ抽出手段１１０で画像からエッジを抽出する（ステップＳ１０３）。エッジを抽出するためには、例えば下記の文献１に記載されたＳｏｂｅｌオペレータとよばれる画像フィルタを用いる。

エッジ抽出手段１１０で抽出したエッジを、線分情報抽出手段１１２において始点と終点からなる直線情報に変換する（ステップＳ１０４）。この結果、エッジ抽出結果は、例えば、図４の中央のようになる。ここで、画像から抽出したエッジを上記線分情報として抽出する手法としては、例えば、下記の文献２に示された手法を用いる。

上記線分情報を抽出する一方で、コーナー検出手段１１１において画像からコーナー情報を検出する（ステップＳ１０５）。この結果、コーナー検出結果は、例えば、図４の左のようになる。なお、画像からコーナー情報を検出する方法としては、例えば、下記の文献３に示された方法を用いる。

・文献１：「画像解析ハンドブック」、東京大学出版会
・文献２： Tsuda, K., Minoh, M., Ikeda, K., “Extracting Straight-Lines by Sequential Fuzzy Clustering, ” PRL(17), No. 6, May 15 1996, pp. 643-649.
・文献３： Chris Harris and Mike Stephens, “A Combined Corner and Edge Detector, ” Proceedings of The Fourth Alvey Vision Conference, Manchester, pp 147-151. 1988

以上で求めた線分情報とコーナー情報に基づき、３次元モデル特徴と対応付ける線分情報の候補を対応線分候補として、対応線分候補抽出手段１１３で抽出する（ステップＳ１０６）。

ステップＳ１０６までの処理と平行して、３次元モデル特徴を、測位手段１０５およびモデル特徴抽出手段１０６で抽出する。まず、３次元位置補正装置１０１に装備されたＧＰＳおよびジャイロセンサーからの情報を測位手段１０５で取得する（ステップＳ１０７）。次に、取得した３次元位置補正装置１０１の位置および方向に基づき、モデル線分獲得手段１１４において、カメラの視野に入る物体の３次元モデルを、３次元モデルＤＢ１１５から取得する。なお、取得する３次元モデルはワイヤーフレームモデルとして表現されているものとする。そして、前記位置および方向をカメラの位置および姿勢とし、さらに、カメラの焦点距離などのパラメータを、カメラパラメータＤＢ１１６から取得し、前記取得した３次元モデルを２次元平面に投影する（ステップＳ１０８）。このとき、３次元モデルのワイヤーフレーム（線分情報）を画像化するのではなく、２次元平面上のどの位置に線分の始点、終点が配置されるかを演算する。また、上記で３次元モデルを投影するとき、モデル内の各面およびモデル間の前後関係から見えなくなる線分情報も存在するので、投影処理時に陰線消去処理を同時に行なう。

以上で求めた２組の２次元線分（画像から抽出した対応線分候補と、３次元モデルを投影した２次元のモデル線分）に対し、２次元マッチング手段１０７においてマッチング処理を行ない（ステップＳ１０９）、求めた線分対応から３次元姿勢パラメータ推定手段１０８において、カメラの正確な位置および姿勢情報を推定する（ステップＳ１１０）。

以下、画像特徴抽出手段１０４において、所定の画像特徴制約条件で対応線分候補を抽出する処理について、図５を用いて詳細に説明する。線分情報抽出手段１１２で抽出された線分情報は図６に示すテーブル形式で格納する。図６の通り、線分の始点（x、y）、終点（x、y）と、線分の直線パラメータとして次式（２）のパラメータを格納する。

また、コーナー検出手段１１１で検出したコーナー情報は、図７に示すテーブル形式で格納する。このテーブルでは、各コーナーＩＤに対する座標（x、y）と、コーナーを構成する線分の数、および各線分のＩＤが格納されている。

対応線分候補抽出手段１１３は、まず、図８に示すような入力画像と同じサイズの２次元配列を用意する（ステップＳ２０１）。次に、コーナー検出手段１１１で検出した全てのコーナーに対して、コーナーの位置から一定距離の範囲の配列要素にコーナーＩＤを格納する（ステップＳ２０２、図７）。次に、線分情報抽出手段１１２で抽出した図６に示す全ての線分情報に対して、各線分の始点位置、終点位置に上記２次元配列要素に格納したコーナーＩＤが存在するか否かをチェックする（ステップＳ２０３）。もし、始点位置および終点位置両方にコーナーＩＤが存在しない場合（ステップＳ２０４）は、その線分情報を図６に示すテーブルから削除する（ステップＳ２０６）。始点位置あるいは終点位置のいずれか、あるいは両方にコーナーＩＤが存在する場合は、図７に示すテーブルの対応するコーナーＩＤの情報を求め、線分数を１つ増やし、かつ、線分ＩＤを記憶する（ステップＳ２０５）。

図６に示す全ての線分情報に対して処理した後、図７に示すコーナー情報から、各コーナーに対応する線分数をチェックし（ステップＳ２０７）、Ｎ個以上存在するコーナーを求めて、Ｎ個を下回るコーナーを図７のテーブルから削除する（ステップＳ２１０）。このとき、例えば、Ｎを３として設定する。

次に、図７に残されたコーナーに対応する線分について、線分同士の角度がθ以上あるかをチェックする（ステップＳ２０９）。これは、後で３次元位置および姿勢パラメータを推定する際、抽出した線分が平行なものを対象とすると、パラメータが適切に求まらないためである。つまり、図９に示すように、平行な線分よりも複数の線分が異なる角度で交わっている部分のほうが、情報量が多く、重要であるとする。線分同士の角度は、図６に示す線分情報の直線パラメータを用いて求めることができる。ここで、直線パラメータのa、bは直線の傾きを表すパラメータであるため、２つの直線のなす角度は、直線１の傾きパラメータをa1、b1、直線2の傾きパラメータをa2、b2とすると、次式（３）で求めることができる。

ここで、角度がθ以上ということは、式（３）の左辺の絶対値が、cosθ以下であることを意味する。この関係を用いて、ステップＳ２１１で線分同士の角度がθ以上か否かを求める。以上のようにして求めた線分とコーナーを、以降の処理の対象とする（ステップＳ２１２）。例えば、絞り込まれた結果の線分とコーナーは、図４の右のようになる。

次に、図１０から図１６を用いてモデル特徴抽出手段１０６の動作を説明する。まず、３次元モデルＤＢ１１５には、各モデルの情報として、図１０から図１３に示す情報が格納されているとする。図１０は、３次元モデルを構成する頂点座標の情報であり、各頂点には頂点ＩＤがふられている。図１１は、３次元モデルを構成するポリゴンの情報であり、ポリゴンＩＤとともに、ポリゴンを構成する頂点数、頂点ＩＤ、世界座標系におけるポリゴンの法線ベクトルから構成されている。図１２は、各頂点を結ぶ線分の情報であり、始点および終点に対応する頂点ＩＤと、その線分ＩＤから構成されている。図１３は、各頂点が始点あるいは終点となっている線分情報であり、各頂点に属する線分数と線分ＩＤから構成されている。カメラパラメータＤＢ１１６には、カメラ固有のパラメータとしてカメラ内部変数（焦点距離、歪係数等）が格納されている。

図１４に、モデル特徴抽出手段１０６の動作のフローチャートを示す。モデル線分獲得手段１１４では、まず、測位手段１０５で獲得した現在のカメラ位置および方向に基づき、対象となるモデル情報（図１０〜図１３）を３次元モデルＤＢ１１５から獲得する（ステップＳ３０１）。次に、獲得したモデルの全てのポリゴンをカメラ座標系に投影し（ステップＳ３０２）、ポリゴンの法線ベクトルがカメラと反対方向を向いているかをチェックし、反対方向を向いているポリゴン、つまり、裏がえしになっているポリゴンを削除する（ステップＳ３０３）。これは、例えば、カメラ座標系のｚ軸が、原点から遠ざかる方向が正の方向だとしたら、法線ベクトルのｚ値が正のものを削除すればよい。

次に、透視投影変換により全てのポリゴンを投影し（ステップＳ３０４）、ポリゴンの各頂点の奥行き（ｚ座標）の最大値でポリゴンを並べ替える（ステップＳ３０５）。ステップＳ３０４においては、カメラパラメータＤＢ１１６に格納されているカメラパラメータを用いる。ポリゴンを並べ替えた後、カメラに対して一番手前にあるポリゴンから、他のポリゴンとの干渉をチェックする（ステップＳ３０６）。

ここで、ポリゴンを構成する線分がポリゴンと干渉する、あるいは、干渉しないケースは、図１５に示すとおり、以下の４通りがある。
Case1）投影面において干渉しない。
Case2）線がポリゴンを覆っている。
Case3）ポリゴンが線を完全に覆っている。
Case4）ポリゴンが線の一部を覆っている。
Case1およびCase2の場合は、線はポリゴンによって遮蔽されない。Case3およびCase４の場合は、線は完全あるいは一部ポリゴンによって遮蔽される。なお、本発明が対象としているモデルでは、ポリゴンが別のポリゴンを貫くことはないとする。

各線がポリゴンによって遮蔽されるか否かを判定するためには、まず、ポリゴンを無限平面としたときに、線とその平面との関係を求めればよい。ここで、線の始点をPs = (xs、 ys、 zs)、終点をPe = (xe、 ye、 ze)とする。このとき、z座標に関する条件を、以下の式（４）とする。

無限平面の法線ベクトルをＮとしたとき、ＮとPsおよびPeの内積が次の式（５）の条件を満たすとき、始点と終点は無限平面の表側に存在するため、線分はポリゴンによって遮蔽されない。

次に、ＮとPsおよびPeの内積が次の式（６）の条件を満たすとき、線分はポリゴンに一部遮蔽される可能性がある。

さらに、ＮとPsおよびPeの内積が次の式（７）の条件を満たすとき、線分はポリゴンに完全あるいは一部遮蔽される可能性がある。

線分がポリゴンに遮蔽される可能性がある場合、次に、投影面上での線分とポリゴンの各線分との交点を求める。線分間の交点は、各線分の方程式を以下の式（８）の通り定義すると、交点は、式（９）で求めることができる。

このとき、Ａの逆行列が存在するか否かを判定するためには、Ａの行列式が０でないことを調べればよい。そして、求めた交点が、線分の始点、終点の間に存在するか否かを調べ、始点と終点の間に交点が存在する場合は、線分とポリゴンが干渉することになる。ただし、交点がポリゴンの頂点と一致する場合は交点としない。

さらに、始点および終点について、ポリゴンの無限平面の裏面に存在するものが、投影面においてポリゴンに内包されるか否かをチェックする。２次元平面において、ある点が線分で囲まれた矩形に内包されているか否かは、次のようにして判定できる。すなわち、いま、直線をax + by + c = 0、任意の点p = (x'、y')とすると、全ての線分に対して次式（１０）のrを求めたときに全ての符号が一致する場合、点pはポリゴンに内包される。

次に、各交点の奥行きを求め、線分上の交点の奥行きがポリゴン上の奥行きよりも大きいものを求める。そして、始点、終点、交点全てに対してポリゴンの裏面に存在する点を投影面のx軸に沿って左から並べたとき、次式（１１）の２点Ｐi、Ｐi+1を結ぶ部分が遮蔽される。

以上で求めた遮蔽部分を除いて線分を分割し、分割された線分情報を基に次のポリゴンとの干渉チェックを行なう。上記処理により、他のポリゴンにより遮蔽される頂点と線分が除去される（ステップＳ３０7）。ステップＳ３０7で、残された頂点と線分の情報は図１６に示すとおり記憶する。なお、線分の始点、終点の座標、および直線パラメータは２次元に投影後のものである。このように、本発明では、３次元モデルを２次元に投影するとき、その結果を画像化するのではなく、パラメータ化して記憶する。

次に図１７を用いて２次元マッチング手段１０７の処理を詳細に説明する。まず、画像から抽出されたコーナーの特徴と類似したモデルコーナーを求める（ステップＳ４０１）。ここで、類似とは、例えば、各コーナーに属する線分数が一致しており、かつ、対応する線分間の角度がθ以内の場合に、類似と判定する。次に、求めたモデルコーナーを、対応する上記画像上のコーナーと一致するようにモデルを平行移動する。そして、モデルの各線分と指定範囲内の画像線分との距離および角度を求める（ステップＳ４０２）。以上で求めた距離と角度から、それぞれが閾値以内の線分対応を求める（ステップＳ４０３）。例えば、図１８に示すようなモデルと画像から抽出した線分が存在した場合、図１８の左図のモデルにおいて丸で囲まれたコーナーが、右図の丸で囲まれた箇所の線分と対応が取れているとする。

このとき、モデルを画像から抽出したコーナーに合わせて平行移動したとき、モデルの各線分Ａ、Ｂ、Ｃ、Ｄに対応する画像の線分は例えば以下の通りとなる。
Ａ：８、９
Ｂ：１、２、３
Ｃ：４
Ｄ：７

以上で求めた線分対応の候補に対して、最適な線分対応を求める（ステップＳ４０４）。最適な線分対応を求めるためのコスト関数として、下記の文献４に示されたIntegrated Squared Perpendicular Distance（ＩＳＰＤ）とOmission Errorを組み合わせたものを用いる。

・文献４： How Easy is Matching 2D Line Models Using Local Search? J. Ross Beveridge, and Edward M. Riseman, IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol 19, No 6, June 1997

Integrated Squared Perpendicular Distanceは、図１９に示すように、データセグメント上の各点からモデルセグメントに対して垂線をおろしたときの距離をあらわすものであり、距離関数をv(t)、データセグメントの長さをlとしたとき、ＩＳＰＤは次式（１２）で定義できる。

ＩＳＰＤは、モデルセグメントとデータセグメントの位置ずれの程度をあらわすものである。ここで、上記定義では、モデルやデータセグメントの大きさにより値にばらつきが出てしまうため、正規化する必要がある。そこでモデルセグメントの長さをLmとしたとき、ＩＳＰＤをLmで正規化した値を新たにFittingエラー（Ｅfit）として、次式（１３）のように定義する。

次にOmission Errorに関して説明する。Omission Errorは、図２０に示すとおり、モデルセグメントと対応付けられたデータセグメントをモデルセグメントに投影したときに、データセグメントがカバーしていない部分がどの程度存在するかを示すものである。データセグメントによってカバーされない部分のモデルセグメントの長さに対する比率（Omission Rateと呼ぶ）をpとすると、pは次式（１４）で定義できる。

ここで、lmはモデルセグメントの長さ、ldは対応付けられたデータセグメントをモデルセグメントに投影したときの長さである。以上で求めた各モデルセグメントのOmission Rateを下記の式（１５）のように統合する。

以上で定義したエラー関数に基づき、ＩＳＰＤのFittingエラー、および、Omission Errorの合計が最小となる組み合わせを求める。以上の処理を、各オブジェクト単位に全てのコーナーに対して処理し（ステップＳ４０５）、最もマッチング誤差が小さい線分対応を求める（ステップＳ４０６）。

次に、２次元マッチング手段１０７が求めた線分対応から、カメラの３次元姿勢パラメータを推定する処理（図３、ステップＳ１１０）について詳細に説明する。３次元のモデルセグメントをカメラ投影面に投影した２次元のモデルセグメントと、実写から抽出したデータセグメントとの関係は、図２１に示すとおりである。３次元のモデルセグメントを無限線としてカメラ投影面に投影したとき、カメラ姿勢情報が正しければ２つの線分は一致する。よって、モデルセグメントに対応するデータセグメントの各端点から下ろした垂線の長さをＥとすると、下記の式（１６）が成り立つ。

上記の制約条件に基づき、Ｅが最小となるカメラ姿勢を求めればよい。Ｅが最小となるということは、データセグメントの端点がモデルセグメント上にあるということであるから、３次元モデルセグメントとカメラ座標系原点でできる平面の法線ベクトルをＮとし、データセグメントの端点とカメラ座標系原点とを結ぶベクトルをＶとしたとき、下記の式（１７）が成り立つ。

ここで、３次元モデルセグメントの端点を、ｐ１、ｐ２とし、カメラの世界座標系での位置をＴｗ、カメラの回転をＲとすると、ｐ１、ｐ２とカメラ座標系原点でできる平面の法線ベクトルは次式（１８）となる。

一方、投影面のデータセグメントの端点と、カメラ座標系原点とでできるベクトルは、次式で定義できる。

よって、式（１６）は、次式（２０）の通り再定義することができる。

式（２０）を最小化するＲ、および、Ｔを求めるため、Ｅをコスト関数とし、例えばLevenberg Marquardt法などの最適化手法を用いる。なお、最適化過程において、ΔＲを計算しやすくするため、回転行列をRodorigue方程式で表現する。Rodorigue方程式は、回転を３次元ベクトルとして表現したものであり、ベクトルの方向が回転軸、ベクトルのノルムが回転角を表す。よって、ΔＴと合わせて６つのパラメータを、上記最適化アルゴリズムで求める。

上記の通り、本発明は、任意形状の物体を撮影したときの物体の位置および姿勢、あるいは画像を撮影したカメラの位置および姿勢を画像から抽出した特徴と、対象物体の３次元モデルから抽出した特徴との対応を取る際に、上記２次元特徴の候補を絞り込むようにしたので、全体の演算量を抑えることができるようになる。

また、本発明では、３次元モデルＤＢ１１５を備えたことにより、円柱のような左右対称の物体に限定されない物体を対象にでき、かつ、縦方向、横方向のエッジそれぞれを分けて処理する必要がないため、３次元モデルを２次元画像へ投影する処理の繰り返しがなく、全体の演算量を抑えることができるようになる。

また、本発明では、現在のカメラ位置および姿勢で見える範囲の３次元モデルからモデル特徴を抽出するモデル特徴抽出手段１０６を備えているため、道路の３次元モデルを、地図データに格納されている道路データ（リンク情報、道路幅、レーン数）から推定する必要がなく、さらに、白線に依存しないマッチングを行なうため、道路が混雑している場合にも実際の道路を正確に抽出できるようになる。

また、本発明では、上記２次元特徴の候補を絞り込むようにしたので、画像から抽出される全ての２次元特徴と３次元モデルの特徴との対応を検査する必要がないため、全体の演算量を抑えることができるようになる。

本発明の実施の形態１の３次元位置補正装置の構成図である。本発明の実施の形態１における、実写映像中の物体上に適切に情報を重畳する例の概念図である。本発明の実施の形態１における、３次元位置補正装置の基本処理動作を示すフローチャートである。本発明の実施の形態１における、画像特徴抽出手段における処理結果の説明図である。本発明の実施の形態１における、画像特徴抽出手段の処理動作のフローチャートである。本発明の実施の形態１における、線分情報抽出手段で抽出された線分情報を格納するテーブルを示す図である。本発明の実施の形態１における、コーナー検出手段で検出したコーナー情報を格納するテーブルを示す図である。本発明の実施の形態１における、コーナーＩＤを格納する２次元配列を示す図である。本発明の実施の形態１における、複数の線分が異なる角度で交わっている重要な線分情報の例を示す図である。本発明の実施の形態１における、３次元モデルを構成する頂点座標情報の格納例を示す図である。本発明の実施の形態１における、３次元モデルを構成するポリゴン情報の格納例を示す図である。本発明の実施の形態１における、各頂点を結ぶ線分情報の格納例を示す図である。本発明の実施の形態１における、各頂点が始点あるいは終点となっている線分情報の格納例を示す図である。本発明の実施の形態１における、モデル特徴抽出手段の動作のフローチャートである。本発明の実施の形態１における、ポリゴンを構成する線分がポリゴンと干渉する／しないケースの説明図である。本発明の実施の形態１における、対応線分候補の絞り込み処理で残された頂点と線分の情報の格納例を示す図である。本発明の実施の形態１における、２次元マッチング手段の処理のフローチャートである。本発明の実施の形態１における、モデルコーナーに対応する画像上の線分の説明図である。本発明の実施の形態１における、Integrated Squared Perpendicular Distance（ＩＳＰＤ）の説明図である。本発明の実施の形態１における、Omission Errorの説明図である。本発明の実施の形態１における、カメラの３次元姿勢パラメータを推定する処理の説明図である。

符号の説明

１０１３次元位置補正装置、１０２画像入力手段、１０３グレースケール変換手段、１０４画像特徴抽出手段、１０５測位手段、１０６モデル特徴抽出手段、１０７２次元マッチング手段、１０８３次元姿勢パラメータ推定手段、１０９情報重畳手段、１１０エッジ抽出手段、１１１コーナー検出手段、１１２線分情報抽出手段、１１３対応線分候補抽出手段、１１４モデル線分獲得手段、１１５３次元モデルＤＢ、１１６カメラパラメータＤＢ。

Claims

カメラ画像を入力する画像入力手段と、
入力された前記カメラ画像を、白から黒までの明暗の濃淡だけで表示されたグレースケール画像に変換するグレースケール変換手段と、
現在のカメラ位置および姿勢を測定する測位手段と、
複数種類の、ポリゴンから構成される３次元モデルを格納する３次元モデルＤＢと、
前記３次元モデルＤＢから、前記測位手段により測定された前記現在のカメラ位置および姿勢で見える範囲の複数種類の前記３次元モデルのモデル特徴を抽出するモデル特徴抽出手段と、
前記グレースケール画像の特徴を抽出する画像特徴抽出条件を有し、この画像特徴抽出条件に基づいて、前記グレースケール画像から、前記モデル特徴とのマッチングに必要な画像特徴を抽出する画像特徴抽出手段と、
前記モデル特徴と前記画像特徴との対応関係を抽出する特徴対応抽出条件を有し、この特徴対応抽出条件に基づいて、前記モデル特徴と前記画像特徴とを２次元平面上でマッチングさせて、前記モデル特徴と前記画像特徴との対応関係である特徴対応情報を取得する２次元マッチング手段と、
前記２次元マッチング手段により取得された前記特徴対応情報に基づいて、カメラの３次元の位置および姿勢、あるいは前記モデル特徴に対応する物体の３次元の位置および姿勢を補正する３次元姿勢パラメータを推定する３次元姿勢パラメータ推定手段と
を備えたことを特徴とする３次元位置補正装置。
前記画像特徴抽出手段は、前記明暗の濃淡が変化する境界となるエッジ情報を前記グレースケール画像から抽出するエッジ抽出手段と、抽出した前記エッジ情報から線分情報を抽出する線分情報抽出手段と、前記グレースケール画像から、コーナーの位置とコーナーを構成する線分をコーナー情報として検出するコーナー検出手段と、前記画像特徴抽出条件と前記線分情報と前記コーナー情報とに基づいて、前記モデル特徴と対応する前記線分情報の候補を対応線分候補として抽出する対応線分候補抽出手段とを備えたことを特徴とする請求項１記載の３次元位置補正装置。
前記画像特徴抽出手段は、前記画像特徴抽出条件に基づいて、前記コーナー検出手段により検出された前記コーナーに近接していない前記線分情報を排除し、前記コーナーに近接した線分の数が閾値以下の前記コーナーを排除することを特徴とする請求項２記載の３次元位置補正装置。
前記画像特徴抽出手段は、前記画像特徴抽出条件に基づいて、前記コーナーに近接した線分の角度が閾値以下の前記コーナーを排除すること特徴とする請求項２または請求項３に記載の３次元位置補正装置。
前記２次元マッチング手段は、前記特徴対応抽出条件に基づいて、前記コーナー検出手段により検出された前記コーナー情報に類似した前記３次元モデルのコーナー情報を求めることを特徴とする請求項１から請求項４のいずれかに記載の３次元位置補正装置。
前記２次元マッチング手段は、前記特徴対応抽出条件に基づいて、近接する線分の数が類似している前記３次元モデルの前記コーナー情報を求めることを特徴とする請求項５記載の３次元位置補正装置。
前記２次元マッチング手段は、前記特徴対応抽出条件に基づいて、近接する線分の角度が類似している前記３次元モデルの前記コーナー情報を求めることを特徴とする請求項５または請求項６に記載の３次元位置補正装置。