JP6499047B2 - 計測装置、方法及びプログラム - Google Patents

Description

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

従来から、走行する車両などの移動体に設置されたカメラにより時系列で撮像された複数の画像、及び当該移動体の移動量を用いて、３次元計測を行う技術が知られている。

特開２０１４−１４２２４１号公報

しかしながら、上述したような従来技術では、撮像された画像内に他の移動体が存在する場合、当該他の移動体の移動の結果、画像間での当該他の移動体の位置にズレが生じるため、３次元計測の精度が悪くなってしまう。

本発明が解決しようとする課題は、時系列で撮像された複数の画像内に移動体が存在する場合であっても、３次元計測の精度の悪化を抑えることができる計測装置、方法及びプログラムを提供することを目的とする。

実施形態の計測装置は、第１取得部と、第２取得部と、第３取得部と、特定部と、第１推定部と、探索部と、計測部と、を備える。第１取得部は、移動体に搭載された撮像部により時系列で撮像された複数の画像を取得する。第２取得部は、前記移動体の位置を示す第１位置情報、及び前記移動体の向きを示す第１向き情報を取得する。第３取得部は、前記移動体の周囲を移動する他の移動体の位置を示す第２位置情報を含む他移動体情報を取得する。特定部は、前記第１位置情報、前記第１向き情報、及び前記他移動体情報に基づいて、前記複数の画像それぞれに対し、前記他の移動体が存在する移動体領域を特定する。第１推定部は、前記複数の画像に基づいて、前記撮像部の位置及び姿勢を推定する。探索部は、前記複数の画像それぞれの前記移動体領域以外の非移動体領域間で、対応点の組を複数探索する。計測部は、前記撮像部の位置及び姿勢と前記複数の対応点の組とに基づいて３次元計測を行う。

本実施形態の計測装置及び移動体の構成例を示す図。 本実施形態の移動体の例を示す模式図。 本実施形態のモデル情報の例を示す図。 本実施形態の画像の例を示す図。 本実施形態の移動体領域の特定例を示す説明図。 本実施形態の移動体領域の特定例を示す説明図。 本実施形態の移動体領域の特定例を示す説明図。 本実施形態の特徴点の追跡手法の例を示す説明図。 本実施形態の特徴点の追跡手法の例を示す説明図。 本実施形態の対応点の探索手法の例を示す説明図。 本実施形態の３次元点の抽出手法の例を示す説明図。 本実施形態の空間の分割手法の例を示す説明図。 本実施形態の代表点の選択手法の例を示す説明図。 本実施形態の移動面の推定手法の例を示す説明図。 本実施形態の障害物の検出手法例を示す説明図。 本実施形態の処理例を示すフローチャート。 本実施形態の比較例を示す説明図。 本実施形態の効果例を示す説明図。 本実施形態の計測装置のハードウェア構成例を示す図。

以下、添付図面を参照しながら、実施形態を詳細に説明する。

図１は、本実施形態の計測装置１０の構成、及び当該計測装置１０を搭載する移動体１の構成の一例を示す図であり、図２は、本実施形態の移動体１の一例を示す模式図である。図１に示すように、計測装置１０は、撮像部５及び方位センサ６を備える移動体１に搭載され、第１取得部１１と、第２取得部１３と、第３取得部１５と、特定部１７と、第１推定部１８と、探索部１９と、計測部２１と、第２推定部２３と、検出部２５と、出力部２７と、を備える。

本実施形態では、移動体１が、路面を移動面として移動する自動車などの車両である場合を例に取り説明するが、これに限定されるものではない。移動体１は、例えば、水面を移動面として移動する船舶や、床面を移動面として移動するロボットなど、移動面を移動可能な物体であればどのようなものであってもよい。

撮像部５は、例えば、画像センサやカメラなどにより実現できる。撮像部５は、移動体１の周囲（例えば、移動体１の進行方向）を時系列で撮像し、撮像した複数の画像を計測装置１０に出力する。

方位センサ６は、移動体１の向きを検知し、検知した移動体１の向きを示す第１位置情報を計測装置１０に出力する。

第１取得部１１、第２取得部１３、第３取得部１５、特定部１７、第１推定部１８、探索部１９、計測部２１、第２推定部２３、検出部２５、及び出力部２７は、例えば、ＣＰＵ（Central Processing Unit）などの処理装置にプログラムを実行させること、即ち、ソフトウェアにより実現してもよいし、ＩＣ（Integrated Circuit）などのハードウェアにより実現してもよいし、ソフトウェア及びハードウェアを併用して実現してもよい。なお、計測装置１０は、チップ（集積回路）として実現してもよいし、通常のコンピュータとして実現してもよい。

第１取得部１１は、撮像部５から、時系列で撮像された複数の画像を取得する。

第２取得部１３は、移動体１の位置を示す第１位置情報、及び移動体１の向きを示す第１向き情報を逐次取得する。例えば、第２取得部１３は、ＧＰＳ（Global Positioning System）衛星などから第１位置情報を取得し、方位センサ６から第１向き情報を取得する。但し、第１位置情報及び第１向き情報の取得方法は、これに限定されるものではない。

第３取得部１５は、移動体１の周囲を移動する他の移動体の位置を示す第２位置情報を含む他移動体情報を取得する。なお、他移動体情報は、他の移動体の速度を示す速度情報、他の移動体の形状を抽象化したモデルを示すモデル情報、他の移動体の色及び模様の少なくともいずれかを示すテクスチャ情報、及び他の移動体の向きを示す第２向き情報の少なくともいずれかを更に含んでいてもよい。モデル情報は、図３に示すように、少なくとも他の移動体の幅、高さ、及び奥行の長さの少なくともいずれかを示していればよい。この場合、モデル情報は、他の移動体の形状を直方体に抽象化したモデルを示す。但し、モデル情報は、他の移動体の幅、高さ、及び奥行の長さのいずれかの長さを定数倍した立方体のモデルを示すようにしてもよいし、他の移動体の幅、高さ、及び奥行の長さのいずれかの長さから車の一般的な縦横比率により他の辺の長さを推定した直方体のモデルを示すようにしてもよい。

例えば、第３取得部１５は、移動体１の周囲を移動する他の移動体と車車間通信（例えば、ＩＥＥＥ８０２．２１ｐ規格の無線通信）を行うことにより、当該他の移動体から他移動体情報を逐次取得する。この場合、他の移動体は、ＧＰＳ衛星などから第２位置情報を取得して、他移動体情報に含め、移動体１（計測装置１０）に送信する。また、他の移動体は、自身の速度を検出して速度情報を生成し、他移動体情報に含めてもよい。また、他の移動体は、自身のモデル情報やテクスチャ情報を予め保持していれば、これらを他移動体情報に含めてもよい。また、他の移動体は、自身に搭載された方位センサから第２向き情報を取得して、他移動体情報に含めてもよい。

また例えば、第３取得部１５は、移動体１の周囲の路肩に存在する監視装置と路車間通信（例えば、ＩＥＥＥ８０２．２１ｐ規格の無線通信）を行うことにより、当該監視装置から移動体１の周囲を移動する他の移動体の他移動体情報を逐次取得してもよい。監視装置は、例えば、監視カメラなどが挙げられるが、これに限定されるものではない。この場合、監視装置は、移動体１の周囲を移動する他の移動体を撮像し、撮像した画像を用いて、第２位置情報を算出して、他移動体情報に含め、移動体１（計測装置１０）に送信する。なお、監視装置は、第２位置情報の算出にＧＰＳ衛星などから取得した監視装置の位置情報を用いてもよい。また監視装置は、撮像した画像を用いて、モデル情報、テクスチャ情報、及び第２向き情報を算出して、他移動体情報に含めてもよい。

特定部１７は、第２取得部１３により取得された第１位置情報及び第１向き情報、並びに第３取得部１５により取得された他移動体情報に基づいて、第１取得部１１により取得された複数の画像それぞれに対し、他の移動体が存在する移動体領域を特定する。

具体的には、特定部１７は、他移動体情報に含まれる速度情報が示す速度が第１閾値以上であれば、他の移動体が移動中であると判断する。なお、他移動体情報に速度情報が含まれない場合、特定部１７は、今回の他移動体情報に含まれる第２位置情報が示す位置と前回の他移動体情報に含まれる第２位置情報が示す位置との差分、及び他移動体情報の取得間隔から、他移動体情報の速度を算出すればよい。

そして特定部１７は、他の移動体が移動中であると判断した場合、第２取得部１３により取得された第１位置情報及び第１向き情報、並びに第３取得部１５により取得された他移動体情報の第２位置情報に基づいて、第１取得部１１により取得された画像上での他の移動体の位置を特定する。なお、特定部１７は、移動体１の位置及び向き、並びに他の移動体の位置が分かるため、移動体１の撮像部５により撮像された画像上での移動体１と他の移動体との位置関係を特定でき、当該画像上での他の移動体の位置を特定できる。また、第１位置情報、第１向き情報、他移動体情報、及び画像は、略同一時刻に取得されているものとする。

更に特定部１７は、画像上で特定した他の移動体の位置に基づいて、当該画像上で当該他の移動体が存在する移動体領域を特定する。

具体的には、特定部１７は、特定した他の移動体の位置に基づく予め定められた大きさの領域を移動体領域に特定する。例えば、特定部１７は、特定した他の移動体の位置を中心とした予め定められた画素数の領域を移動体領域に特定する。なお、移動体領域の形状は、矩形や円などどのような形状であってもよい。

また、特定部１７は、特定した他の移動体の位置に基づく当該他の移動体と移動体１との距離に応じた大きさの領域を移動体領域に特定してもよい。例えば、特定部１７は、図４に示すように、他の移動体３１、３２の位置（中心位置）を特定した場合、図５に示すように、他の移動体３１の位置を中心とした移動体領域４１、他の移動体３２の位置を中心とした移動体領域４２を特定してもよい。図４及び図５に示す例では、他の移動体３１よりも他の移動体３２の方が、移動体１との距離が長い（遠い）ため、移動体領域４１よりも移動体領域４２の方が、大きさ（画素数）が小さくなっている。なお、移動体１と他の移動体との距離は、第１位置情報及び第２位置情報から求められる。

また、特定部１７は、特定した他の移動体の位置に基づく当該他の移動体のモデルに応じた領域を移動体領域に特定してもよいし、更に、他の移動体の向きに応じた領域を移動体領域に特定してもよいし、更に、他の移動体と移動体１との距離に応じた領域を移動体領域に特定してもよい。例えば、特定部１７は、図４に示すように、他の移動体３１、３２の位置（中心位置）を特定した場合、図６に示すように、他の移動体３１の位置を中心とした移動体領域５１、他の移動体３２の位置を中心とした移動体領域５２を特定してもよい。

図６に示す例では、移動体領域５１は、他の移動体３１のモデル、向き、及び移動体１との距離に応じた領域となっており、具体的には、３次元空間上で他の移動体３１のモデル及び向きを設定して他の移動体３１の位置が中心となるように投影された領域となっている。同様に、移動体領域５２は、他の移動体３２のモデル、向き、及び移動体１との距離に応じた領域となっており、具体的には、３次元空間上で他の移動体３２のモデル及び向きを設定して他の移動体３２の位置が中心となるように投影された領域となっている。なお、他の移動体のモデルはモデル情報、他の移動体の向きは第２向き情報から求められる。

また、移動体領域を、他の移動体３１のモデル、及び移動体１との距離に応じた領域とする場合、モデルの奥行きを画像上に投影した長さを矩形の水平方向の長さ、モデルの高さを画像上に投影した矩形の高さ方向の長さとして投影すればよい。

また、特定部１７は、特定した他の移動体の位置に基づく当該他の移動体のテクスチャ情報に応じた領域を移動体領域に特定してもよい。例えば、特定部１７は、図４に示すように、他の移動体３１、３２の位置（中心位置）を特定した場合、図７に示すように、他の移動体３１の移動体領域６１、他の移動体３２の移動体領域６２を特定してもよい。

図７に示す例では、移動体領域６１は、他の移動体３１のテクスチャ情報に応じた領域となっており、具体的には、他の移動体３１の位置付近でテクスチャ情報が示す色と同一又は類似する色を持つ領域となっている。同様に、移動体領域６２は、他の移動体３２のテクスチャ情報に応じた領域となっており、具体的には、他の移動体３２の位置付近でテクスチャ情報が示す色と同一又は類似する色を持つ領域となっている。

第１推定部１８は、第１取得部１１により取得された複数の画像に基づいて、撮像部５の位置及び姿勢を推定する。具体的には、第１推定部１８は、複数の画像それぞれから特徴点を抽出し、当該複数の画像間で当該特徴点を追跡して、撮像部５の位置及び姿勢を推定する。但し、本実施形態では、第１推定部１８は、複数の画像それぞれの移動体領域以外の非移動体領域から特徴点を抽出し、当該複数の画像間で当該特徴点を追跡して、撮像部５の位置及び姿勢を推定する。

例えば、図８に示す時刻ｔ−１の時点で撮像された画像及び図９に示す時刻ｔの時点で撮像された画像の例の場合、第１推定部１８は、移動体領域７４以外の非移動体領域から特徴点を抽出し、図８に示す画像から抽出した特徴点を図９に示す画像上で追跡する。この場合、第１推定部１８は、図８に示す画像から抽出した特徴点７１、７２、７３それぞれを、図９に示す画像から抽出した特徴点８１、８２、８３として追跡し、画像上での各特徴点の時系列での対応を求める。そして第１推定部１８は、求めた各特徴点の時系列での対応（姿勢回転Ｒ及び並進移動Ｔ）から、前回の撮像部５の位置及び姿勢からの相対的な撮像部５の位置及び姿勢（現在の撮像部５の位置及び姿勢）をエピポーラ幾何で求める。

なお、特徴点は、画像中の輝度差の大きい点（例えば、横方向での輝度差が大きく、かつ縦方向での輝度差が大きい点）とすることができ、例えば、Ｈａｒｒｉｓコーナーディテクタなどの公知の抽出手法で抽出できる。

なお本実施形態では、特徴点の追跡の一例として、特徴点同士を対応付ける手法を記載しているが、特徴点をある画素周囲の領域ととらえ、領域と領域とで画素を対応付けたり、領域と特徴点とで画素を対応付けることも特徴点の追跡に含まれる。

探索部１９は、第１取得部１１により取得された複数の画像それぞれの非移動体領域間で、対応点の組を複数探索する。具体的には、探索部１９は、前回撮像された画像上の非移動体領域に探索点を配置し、今回撮像された画像上で当該探索点に対応する対応点を探索し、対応点の組とする。

なお、前回撮像された画像上の非移動体領域に対する探索点の配置は、非移動体領域のエッジ部分全体に配置するようにしてもよいし、非移動体領域全体に均等に配置するようにしてもよい。また、探索部１９は、第１推定部１８により推定された撮像部５の位置及び姿勢から、今回撮像された画像上で探索点が観測されうる範囲を特定できるため、当該特定した範囲を探索して、探索点の対応点を探索する。具体的には、探索点は、図１０に示すように、エピポーラ線（図１０では、矢印で図示）上で観測されうるため、探索部１９は、エピポーラ線上で探索点に対応する対応点を探索する。

本実施形態では、前回撮像された画像上に探索点を配置し、今回撮像された画像上で当該探索点に対応する対応点を探索する例について説明したが、今回撮像された画像上に探索点を配置し、前回撮像された画像上で当該探索点に対応する対応点を探索するようにしてもよい。

計測部２１は、第１推定部１８により推定された撮像部５の位置及び姿勢と探索部１９により探索された複数の対応点の組とに基づいて３次元計測を行い、３次元点を得る。具体的には、計測部２１は、第１推定部１８により推定された撮像部５の位置及び姿勢と探索部１９により探索された複数の対応点の組とを用いて、三角測量の原理などを利用した３次元計測を行うことで、対応点それぞれの奥行を復元し、３次元点を得る。

第２推定部２３は、計測部２１により得られた３次元点群に基づいて、移動体１が移動する移動面を推定する。移動面の推定には、例えば、公知技術であるＲＡＮＳＡＣによる平面検出手法などを用いればよい。具体的には、計測部２１により得られた３次元点群のうち第２閾値以下の高さにある３点を１組としてランダムで複数回取得し、３点で構成される平面と第３閾値距離内に存在する他の３次元点の個数が最も多い平面を移動面として推定すればよい。

なお、以下の手法を用いれば高精度に移動面を推定することが可能である。

第２推定部２３は、第１推定部１８により推定された撮像部５の姿勢（移動体１の姿勢）の時系列変化に基づいて、第４閾値を設定する。具体的には、第２推定部２３は、撮像部５の姿勢の時系列変化が大きくなるほど、値が小さくなるように第４閾値を設定する。詳細には、第２推定部２３は、撮像部５の姿勢の時系列変化を示す値を単調減少関数に適用して求めた値を第４閾値に設定する。

例えば、単調減少関数がｙ＝−ａｘ＋ｂ（ａ、ｂは、任意の変数とする）であるとする。この場合、第２推定部２３は、現時刻ｔの移動体１の撮像部５の姿勢を示す値と、現時刻ｔからＰ時間前の撮像部５の姿勢の算出時刻ｔ−Ｐの移動体１の撮像部５の姿勢を示す値と、の差分の絶対値を算出し、算出した差分の絶対値を単調減少関数のｘに代入して求めたｙの値を第４閾値に設定する。

なお、算出時刻ｔ−Ｐは、前回の撮像部５の姿勢の算出時刻であることが好ましいが、これに限定されず、前回以前の撮像部５の姿勢の算出時刻であればよい。また、差分の絶対値としては、例えば、Ｒｏｌｌ，Ｐｉｔｃｈ，Ｙａｗそれぞれの差分の絶対値の合計値などが挙げられるが、これに限定されるものではない。

第２推定部２３は、計測部２１により得られた３次元点群のうち移動体１の移動方向での移動体１からの距離が、設定した第４閾値以下の複数の３次元点を抽出する。

図１１は、本実施形態の３次元点の抽出手法の一例を示す説明図であり、計測部２１により得られた３次元点群１０１をｙｚ平面で表している。なお、移動体１の移動方向は、ｚ軸方向（詳細には、＋ｚ方向）となる。図１１に示す例の場合、第２推定部２３は、３次元点群１０１のうち、ｚ座標の値が設定した第４閾値Ｔ以下の複数の３次元点１０２を抽出する。

第２推定部２３は、抽出した複数の３次元点が位置する空間を、移動体１の移動方向で複数の分割空間に分割する。

図１２は、本実施形態の空間の分割手法の一例を示す説明図であり、３次元点群１０１及び複数の３次元点１０２をｙｚ平面で表している。図１２に示す例の場合、第２推定部２３は、抽出した複数の３次元点１０２が取り得るｚ座標の最小値Ｌ及び最大値Ｕを特定し、ｚ座標の値がＬ以上Ｕ以下となる１つの空間をｚ軸方向でｋ（ｋ≧２）等分し、ｋ個のブロック空間に分割する。なお、図１２では、Ｕ＝Ｔである場合を例示しているが、Ｕの値はＵ≦Ｔの関係が成り立てばよい。

第２推定部２３は、分割した分割空間毎に、当該分割空間に含まれる３次元点の中から代表点を選択する。具体的には、第２推定部２３は、分割空間に含まれる３次元点のうち鉛直方向で最下点にある３次元点を、代表点に選択する。

図１３は、本実施形態の代表点の選択手法の一例を示す説明図であり、複数の３次元点１０２及び分割されたブロック空間をｙｚ平面で表している。図１３に示す例の場合、第２推定部２３は、各ブロック空間において、ｙ座標の値が最大となる３次元点を代表点に選択しており、第１ブロック空間においては、３次元点１０３−１を代表点に選択しており、第ｋブロック空間においては、３次元点１０３−ｋを代表点に選択している。

第２推定部２３は、選択した複数の代表点を近似する面を、移動体１が移動する移動面に推定する。

図１４は、本実施形態の移動面の推定手法の一例を示す説明図であり、分割されたブロック空間及び選択したｋ個の代表点１０３をｙｚ平面で表している。図１４に示す例の場合、第２推定部２３は、選択したｋ個の代表点１０３を近似する面１０４（但し、図１４に示すｙｚ平面上では、直線で表される）を、移動体１が移動する移動面に推定する。

検出部２５は、計測部２１により得られた３次元点群と第２推定部２３により推定された移動面とに基づいて、障害物を検出する。具体的には、検出部２５は、計測部２１により得られた３次元点群のうち移動面上に存在しない３次元点を障害物として検出する。

図１５は、本実施形態の検出部２５の障害物の検出手法の一例を示す説明図であり、計測部２１により得られた３次元点群１０１及び第２推定部２３により推定された移動面１０４をｙｚ平面で表している。図１５に示す例の場合、検出部２５は、３次元点群１０１に含まれる３次元点毎に、移動面１０４とのｙ軸方向での距離ｄを算出し、距離ｄが誤差以上となる３次元点を、障害物を構成する３次元点として検出する。ここで、誤差とは、計測部２１による３次元計測の計測誤差である。

出力部２７は、検出部２５の検出結果に基づく出力を行う。例えば、検出した障害物の位置を移動体１に備えられたスピーカ（図示省略）から音声出力させたり、検出した障害物の位置を第１取得部１１により取得された画像上に示して、移動体１に備えられたディスプレイで表示出力させたりする。

図１６は、本実施形態の処理の手順の流れの一例を示すフローチャートである。

まず、第１取得部１１は、撮像部５から、時系列で撮像された画像を逐次取得し、第２取得部１３は、第１位置情報、及び第１向き情報を逐次取得する（ステップＳ１０１）。

続いて、第３取得部１５は、移動体１の周囲を移動する他の移動体の他移動体情報を取得する（ステップＳ１０３）。

続いて、特定部１７は、第２取得部１３により取得された第１位置情報及び第１向き情報、並びに第３取得部１５により取得された他移動体情報に基づいて、第１取得部１１により取得された画像に対し、他の移動体が存在する移動体領域を特定する（ステップＳ１０５）。

続いて、第１推定部１８は、撮像部５の位置及び姿勢を推定するため、第１取得部１１により取得された複数の画像それぞれの非移動体領域から特徴点を抽出し、十分な特徴点を抽出できていれば（ステップＳ１０７でＹｅｓ）、複数の画像間で当該特徴点を追跡（トラッキング）して（ステップＳ１０９）、撮像部５の位置及び姿勢を推定する（ステップＳ１１１）。

なお、十分な特徴点を抽出できていなかったり（ステップＳ１０７でＮｏ）、撮像部５の位置及び姿勢の推定に失敗した場合（ステップＳ１１３でＮｏ）、処理は終了となる。

続いて、撮像部５の位置及び姿勢の推定に成功した場合（ステップＳ１１３でＹｅｓ）、探索部１９は、第１取得部１１により取得された複数の画像それぞれの非移動体領域間で、対応点の組を複数探索する（ステップＳ１１４）。

続いて、計測部２１は、第１推定部１８により推定された撮像部５の位置及び姿勢と探索部１９により探索された複数の対応点の組とに基づいて３次元計測を行い、３次元点を得る（ステップＳ１１５）。

続いて、第２推定部２３は、計測部２１により得られた３次元点群に基づいて、移動体１が移動する移動面を推定する（ステップＳ１１７）。

続いて、検出部２５は、計測部２１により得られた３次元点群と第２推定部２３により推定された移動面とに基づいて、障害物を検出する（ステップＳ１１９）。

続いて、出力部２７は、検出部２５の検出結果に基づく出力を行う（ステップＳ１２１）。

図１７に示すように、移動中の他の移動体２００に対して、画像上で対応点の組を探索してしまうと、前回撮像された時点での他の移動体２００上の探索点２０１の位置と今回撮像された時点での他の移動体２００の対応点２０２の位置とがズレてしまう。このため、探索点２０１と対応点２０２との組を対応点の組として３次元計測を行ってしまうと、三角測量の原理上、３次元点（詳細には、奥行き）を精度よく求めることはできない。

一方、図１８に示すように、物体などの移動しない非移動体２１０に対して、画像上で対応点の組を探索しても、前回撮像された時点での非移動体２１０上の探索点２１１の位置と今回撮像された時点での非移動体２１０の対応点２１１の位置とにズレはほぼ生じない。このため、探索点２１１と対応点２１１との組を対応点の組として３次元計測を行った場合、三角測量の原理上、３次元点（詳細には、奥行き）を精度よく求めることができる。

本実施形態では、複数の画像間での対応点の組の探索を移動体領域以外の非移動体領域で行うため、移動体（他の移動体）を除外して対応点の組を探索することができるので、３次元計測の精度の悪化を抑えることができる。

このように本実施形態によれば、３次元計測の精度の悪化を抑えることができるため、このようにして得られた３次元点を用いて障害物検出を行えば、障害物の検出精度を向上させることができ、例えば、高さが１０ｃｍ程度の微小な障害物であっても正しく検出することが可能となる。

また本実施形態によれば、移動体の姿勢の変化が大きいほど閾値の値を小さくし、移動体の姿勢の変化が大きくなってもその影響により３次元計測精度が悪くなりにくい、移動体から近い部分の３次元点を用いて移動体の移動面を推定するようにしているため、移動面の推定精度を向上させることもできる。

このように本実施形態によれば、移動面の推定精度も向上させることができるため、このようにして推定された移動面を用いて障害物検出を行えば、障害物の検出精度をより向上させることができる。

また本実施形態によれば、撮像部５の位置及び姿勢を推定する際にも非移動体領域から特徴点を抽出するため、追跡する特徴点の位置にズレはほぼ生じず、撮像部５の位置及び姿勢の推定精度の悪化も抑えることができる。

（変形例）
なお、上記実施形態では、移動面との距離（高さ方向での距離）が誤差以上となる３次元点を、障害物を構成する３次元点として検出する例について説明したが、移動面との距離（高さ方向での距離）が誤差以上かつ第５閾値以下となる３次元点を、障害物を構成する３次元点として検出するようにしてもよい。この場合、第５閾値を、移動体１の車高程度の高さ（距離）とすれば、信号機や歩道橋などが障害物として誤検出されてしまうことを防止できる。

（ハードウェア構成）
図１９は、実施形態及び変形例の計測装置のハードウェア構成の一例を示す図である。図１９に示すように、実施形態及び変形例の計測装置は、ＣＰＵなどの制御装置９０１と、ＲＯＭやＲＡＭなどの主記憶装置９０２と、ＨＤＤやＳＳＤなどの補助記憶装置９０３と、ディスプレイなどの表示装置９０４と、キーボードやマウスなどの入力装置９０５と、通信インタフェースなどの通信装置９０６と、を備えており、通常のコンピュータを利用したハードウェア構成となっている。

実施形態及び変形例の計測装置で実行されるプログラムは、インストール可能な形式又は実行可能な形式のファイルでＣＤ−ＲＯＭ、ＣＤ−Ｒ、メモリカード、ＤＶＤ（Digital Versatile Disk）、フレキシブルディスク（ＦＤ）等のコンピュータで読み取り可能な記憶媒体に記憶されて提供される。

また、実施形態及び変形例の計測装置で実行されるプログラムを、インターネット等のネットワークに接続されたコンピュータ上に格納し、ネットワーク経由でダウンロードさせることにより提供するようにしてもよい。また、実施形態及び変形例の計測装置で実行されるプログラムを、インターネット等のネットワーク経由で提供または配布するようにしてもよい。また、実施形態及び変形例の計測装置で実行されるプログラムを、ＲＯＭ等に予め組み込んで提供するようにしてもよい。

実施形態及び変形例の計測装置で実行されるプログラムは、上述した各部をコンピュータ上で実現させるためのモジュール構成となっている。実際のハードウェアとしては、ＣＰＵがＲＯＭやＨＤＤなどからプログラムをＲＡＭ上に読み出して実行することにより、上記各部がコンピュータ上で実現されるようになっている。

本発明は、上記実施形態及び変形例そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化することができる。また、上記実施形態及び変形例に開示されている複数の構成要素の適宜な組み合わせにより、種々の発明を形成することができる。例えば、上記実施形態及び変形例に示される全構成要素からいくつかの構成要素を削除してもよい。さらに、異なる実施形態及び変形例にわたる構成要素を適宜組み合わせても良い。

例えば、実施形態のフローチャートにおける各ステップを、その性質に反しない限り、実行順序を変更し、複数同時に実施し、あるいは実施毎に異なった順序で実施してもよい。

以上のように、上記実施形態及び変形例によれば、時系列で撮像された複数の画像内に移動体が存在する場合であっても、３次元計測の精度の悪化を抑えることができる。

１ 移動体
５ 撮像部
６ 方位センサ
１０ 計測装置
１１ 第１取得部
１３ 第２取得部
１５ 第３取得部
１７ 特定部
１８ 第１推定部
１９ 探索部
２１ 計測部
２３ 第２推定部
２５ 検出部
２７ 出力部

Claims (14)

1. 移動体に搭載された撮像部により時系列で撮像された複数の画像を取得する第１取得部と、
   前記移動体の位置を示す第１位置情報、及び前記移動体の向きを示す第１向き情報を取得する第２取得部と、
   前記移動体の周囲を移動する他の移動体の位置を示す第２位置情報を含む他移動体情報を取得する第３取得部と、
   前記第１位置情報、前記第１向き情報、及び前記他移動体情報に基づいて、前記複数の画像それぞれに対し、前記他の移動体が存在する移動体領域を特定する特定部と、
   前記複数の画像に基づいて、前記撮像部の位置及び姿勢を推定する第１推定部と、
   前記複数の画像それぞれの前記移動体領域以外の非移動体領域間で、対応点の組を複数探索する探索部と、
   前記撮像部の位置及び姿勢と前記複数の対応点の組とに基づいて３次元計測を行う計測部と、
   を備える計測装置。
2. 前記第１推定部は、前記複数の画像それぞれから特徴点を抽出し、前記複数の画像間で当該特徴点を追跡して、前記撮像部の位置及び姿勢を推定する請求項１に記載の計測装置。
3. 前記第１推定部は、前記複数の画像それぞれの前記非移動体領域から特徴点を抽出し、前記複数の画像間で当該特徴点を追跡して、前記撮像部の位置及び姿勢を推定する請求項２に記載の計測装置。
4. 前記移動体領域は、前記他の移動体の位置に基づく予め定められた大きさの領域である請求項１〜３のいずれか１つに記載の計測装置。
5. 前記移動体領域は、前記他の移動体の位置に基づく前記他の移動体と前記移動体との距離に応じた大きさの領域である請求項１〜３のいずれか１つに記載の計測装置。
6. 前記他移動体情報は、前記他の移動体の形状を抽象化したモデルを示すモデル情報を更に含み、
   前記移動体領域は、前記他の移動体の位置に基づく前記他の移動体のモデルに応じた領域である請求項１〜５のいずれか１つに記載の計測装置。
7. 前記他移動体情報は、前記他の移動体の向きを示す第２向き情報を更に含み、
   前記移動体領域は、前記他の移動体の位置に基づく前記他の移動体の向きに更に応じた領域である請求項６に記載の計測装置。
8. 前記他移動体情報は、前記他の移動体の色及び模様の少なくともいずれかを示すテクスチャ情報を更に含み、
   前記移動体領域は、前記他の移動体の位置に基づく前記他の移動体の前記テクスチャ情報に応じた領域である請求項１〜５のいずれか１つに記載の計測装置。
9. 前記計測部は、前記３次元計測を行って、３次元点群を得、
   前記３次元点群に基づいて、前記移動体が移動する移動面を推定する第２推定部と、
   前記３次元点群と前記移動面とに基づいて、障害物を検出する検出部と、を更に備える請求項１〜８のいずれか１つに記載の計測装置。
10. 前記検出部は、前記３次元点群のうち前記移動面上に存在しない３次元点を前記障害物として検出する請求項９に記載の計測装置。
11. 前記第３取得部は、前記他の移動体から前記他移動体情報を取得する請求項１〜１０のいずれか１つに記載の計測装置。
12. 前記第３取得部は、前記移動体及び前記他の移動体を監視する監視装置から前記他移動体情報を取得する請求項１〜１０のいずれか１つに記載の計測装置。
13. 移動体に搭載された撮像部により時系列で撮像された複数の画像を取得する第１取得ステップと、
    前記移動体の位置を示す第１位置情報、及び前記移動体の向きを示す第１向き情報を取得する第２取得ステップと、
    前記移動体の周囲を移動する他の移動体の位置を示す第２位置情報を含む他移動体情報を取得する第３取得ステップと、
    前記第１位置情報、前記第１向き情報、及び前記他移動体情報に基づいて、前記複数の画像それぞれに対し、前記他の移動体が存在する移動体領域を特定する特定ステップと、
    前記複数の画像に基づいて、前記撮像部の位置及び姿勢を推定する推定ステップと、
    前記複数の画像それぞれの前記移動体領域以外の非移動体領域間で、対応点の組を複数探索する探索ステップと、
    前記撮像部の位置及び姿勢と前記複数の対応点の組とに基づいて３次元計測を行う計測ステップと、
    を含む計測方法。
14. 移動体に搭載された撮像部により時系列で撮像された複数の画像を取得する第１取得ステップと、
    前記移動体の位置を示す第１位置情報、及び前記移動体の向きを示す第１向き情報を取得する第２取得ステップと、
    前記移動体の周囲を移動する他の移動体の位置を示す第２位置情報を含む他移動体情報を取得する第３取得ステップと、
    前記第１位置情報、前記第１向き情報、及び前記他移動体情報に基づいて、前記複数の画像それぞれに対し、前記他の移動体が存在する移動体領域を特定する特定ステップと、
    前記複数の画像に基づいて、前記撮像部の位置及び姿勢を推定する推定ステップと、
    前記複数の画像それぞれの前記移動体領域以外の非移動体領域間で、対応点の組を複数探索する探索ステップと、
    前記撮像部の位置及び姿勢と前記複数の対応点の組とに基づいて３次元計測を行う計測ステップと、
    をコンピュータに実行させるためのプログラム。