(57)【要約】【課題】三次元情報取得装置において、使用環境の制約なしに、また、三次元情報取得装置の設置状態によらず、汎用的な三次元情報を生成可能な技術を提供する。【解決手段】三次元情報取得装置に搭載された姿勢検出部で検出した姿勢情報を用い、三次元取得装置で取得した三次元取得装置固有の座標系の座標値を、予め定めた三次元情報取得装置の姿勢によらない世界座標系の座標値に変換する。姿勢検出部は、姿勢情報として、世界座標系の各軸に対する回転角を検出する。【選択図】図１
2014074615.tif 000002
【特許請求の範囲】
【請求項１】
各画素の画素値が撮影領域内の物体までの距離値である距離画像を取得する距離画像センサを備える三次元情報生成装置であって、
前記距離画像センサの姿勢情報を検出する姿勢検出部と、
前記距離画像の各画素値および前記姿勢情報から、前記撮影領域の、前記距離画像センサの姿勢に依存しない世界座標系の三次元情報を算出する世界座標生成部と、を備えること
を特徴とする三次元情報生成装置。
【請求項２】
請求項１記載の三次元情報生成装置であって、
前記姿勢検出部は、重力センサを備え、前記姿勢情報として、前記距離画像センサの、前記世界座標系における重力方向の軸以外の２軸の周りの回転角を検出すること
を特徴とする三次元情報生成装置。
【請求項３】
請求項２記載の三次元情報生成装置であって、
前記姿勢検出部は、さらに、地磁気センサを備え、前記姿勢情報として、前記距離画像センサの、前記世界座標系における前記重力軸周りの回転角をさらに検出すること
を特徴とする三次元情報生成装置。
【請求項４】
請求項１から３いずれか１項記載の三次元情報生成装置であって、
前記距離画像センサは受光レンズを備え、
前記世界座標生成部は、前記距離画像の画素情報、前記受光レンズの結像情報、および、前記姿勢情報をもとに、前記世界座標系の三次元情報算出時に用いる数式の要素のテーブルを予め作成し、当該テーブルの値を用いて、前記三次元情報を算出すること
を特徴とする三次元情報生成装置。
【請求項５】
距離画像センサにより、各画素の画素値が撮影領域内の物体までの距離値である距離画像を取得する距離画像取得ステップと、
前記距離画像取得時の前記距離画像センサの姿勢情報を用い、前記撮影領域の、前記距離画像センサの姿勢に依存しない世界座標系の三次元情報を算出する世界座標算出ステップと、を含むこと
を特徴とする三次元情報生成方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、三次元情報取得技術に関し、特に、光飛行時間型距離画像センサを用いる、三次元情報取得技術に関する。
【背景技術】
【０００２】
レーザ距離計つきのトランジット（計測器）を用い、空間内の所定の位置にある物体の三次元座標値（三次元情報）を取得する技術がある。一般に、レーザ距離計で求まる三次元座標値は、計測器を原点とした、計測器座標系における三次元情報である。物体の三次元情報は、レーザ距離計により計測した距離に基づいて算出されるため、計測器の姿勢の影響を受ける。従って、撮像範囲が同一空間内の所定の位置にある物体であっても、計測器の姿勢によって、得られる三次元情報は一定ではない。
【０００３】
これを解決するものとして、ターゲットマーカを用いる技術がある（例えば、特許文献１参照）。特許文献１に開示の技術では、撮影位置からみたマーカ形状の歪みを用いてカメラ姿勢を検出し、これを用いて、計測器座標系で得られた同一空間内の物体位置を、世界座標系の座標値に変換する。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平１０−２２１０７４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかし、特許文献１に開示の技術では、予め、ターゲットマーカの設置が必須であり、計測器の使用環境および撮像範囲が限定される。例えば、ターゲットマーカが設置されていない空間では使用できない。また、計測器の姿勢が大きく変化し、ターゲットマーカが撮像範囲外となった場合は、ターゲットマーカを設置し直す必要がある。また、計測器座標系から世界座標系への座標変換は、ターゲットマーカと計測器との相対位置関係に依存する。従って、ターゲットマーカに、高い設置精度が要求される。
【０００６】
本発明は、上記事情に鑑みてなされたもので、使用環境の制約なしに、また、三次元情報取得装置の設置状態によらず、汎用的な三次元情報を生成可能な技術を提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明は、三次元情報取得装置に搭載された姿勢検出部で検出した姿勢情報を用い、三次元取得装置で取得した三次元取得装置固有の座標系の座標値を、三次元情報取得装置の姿勢によらない予め定めた世界座標系の座標値に変換する。姿勢検出部は、姿勢情報として、世界座標系の各軸に対する回転角を検出する。
【０００８】
具体的には、各画素の画素値が撮影領域内の物体までの距離値である距離画像を取得する距離画像センサを備える三次元情報生成装置であって、前記距離画像センサの姿勢情報を検出する姿勢検出部と、前記距離画像の各画素値および前記姿勢情報から、前記撮影領域の、前記距離画像センサの姿勢に依存しない世界座標系の三次元情報を算出する世界座標生成部と、を備えることを特徴とする三次元情報生成装置を提供する。
【０００９】
また、距離画像センサにより、各画素の画素値が撮影領域内の物体までの距離値である距離画像を取得する距離画像取得ステップと、前記距離画像取得時の前記距離画像センサの姿勢情報を用い、前記撮影領域の、前記距離画像センサの姿勢に依存しない世界座標系の三次元情報を算出する世界座標算出ステップと、を含むことを特徴とする三次元情報生成方法を提供する。
【発明の効果】
【００１０】
本発明によれば、使用環境の制約なしに、また、三次元情報取得装置の設置状態によらず、汎用的な三次元情報を生成できる。
【図面の簡単な説明】
【００１１】
【図１】（ａ）は、第一の実施形態の三次元情報生成装置のブロック図であり、（ｂ）は、第一の実施形態の撮像素子のブロック図である。
【図２】（ａ）は、第一の実施形態のセンサ座標系を、（ｂ）は、第一の実施形態の世界座標系を、それぞれ説明するための説明図である。
【図３】第一の実施形態の、センサ座標系と世界座標系との関係を説明するための説明図である。
【図４】（ａ）は、第一の実施形態のｘ軸方向の画素数Ｍが奇数の場合の撮像範囲の算出方法を、（ｂ）は、第一の実施形態のｘ軸方向の画素数Ｍが偶数の場合の撮像範囲の算出方法を説明するための説明図である。
【図５】（ａ）は、第一の実施形態のｙ軸方向の画素数Ｎが奇数の場合の撮像範囲の算出方法を、（ｂ）は、第一の実施形態のｙ軸方向の画素数Ｎが偶数の場合の撮像範囲の算出方法を説明するための説明図である。
【図６】第一の実施形態の各画素の撮像範囲の三次元情報算出手法を説明するための説明図である。
【図７】第一の実施形態のＺ軸周りの回転を考慮した世界座標値算出手法を説明するための説明図である。
【図８】（ａ）〜（ｃ）は、第一の実施形態の世界座標値算出手法を説明するための説明図である。
【図９】（ａ）は、第一の実施形態の画素のアドレスを説明するための説明図である。（ｂ）は、第一の実施形態の画素数Ｍが奇数の場合の、計算に用いる画素位置と画素のアドレスとの関係を説明するための説明図であり、（ｃ）は、第一の実施形態の画素数Ｍが偶数の場合の、計算に用いる画素位置と画素のアドレスとの関係を説明するための説明図である。
【図１０】（ａ）は、第一の実施形態の画素数Ｍが奇数の場合の、計算に用いる画素位置と画素のアドレスとの関係を説明するための説明図であり、（ｂ）は、第一の実施形態の画素数Ｍが偶数の場合の、計算に用いる画素位置と画素のアドレスとの関係を説明するための説明図である。
【図１１】（ａ）および（ｂ）は、第一の実施形態の世界座標算出式内の要素のテーブルの例を説明するための説明図である。
【図１２】第一の実施形態の世界座標算出式内の要素のテーブルの例を説明するための説明図である。
【図１３】（ａ）および（ｂ）は、第一の実施形態の世界座標算出式内の要素のテーブルの例を説明するための説明図である。
【図１４】第一の実施形態の世界座標算出処理のフローチャートである。
【図１５】（ａ）〜（ｄ）は、第二の実施形態の世界座標値算出手法を説明するための説明図である。
【発明を実施するための形態】
【００１２】
＜＜第一の実施形態＞＞
以下、本発明を適用する第一の実施形態について説明する。以下、本発明の実施形態を説明するための全図において、同一機能を有するものは同一符号を付し、その繰り返しの説明は省略する。
【００１３】
本実施形態の三次元情報生成装置は、距離画像センサで取得した距離値から、撮像範囲の世界座標系での三次元情報を生成する。ここで、本実施形態での世界座標は、距離値を取得する距離画像センサの姿勢によらない座標系をいう。これに対し、距離画像センサを基準とした座標系を、センサ座標系と呼ぶ。
【００１４】
まず、本実施形態の三次元情報生成装置の構成を説明する。図１（ａ）は、本実施形態の三次元情報生成装置１００のブロック図である。本図に示すように、本実施形態の三次元情報生成装置１００は、制御部１１０と、距離画像センサ１２０と、姿勢検出部１６０と、世界座標生成部１７０と、を備える。
【００１５】
距離画像センサ１２０は、撮像空間に光を照射する光源部１３０と、反射光を受光して電荷に変換する受光部１４０と、受光部１４０が変換した電荷から距離画像を生成する距離画像生成部１５０とを備える、電荷振り分け式の光飛行時間型の距離画像取得装置である。光源部１３０から、対象物体１９０へ照射した光の反射光を受光部１４０で受光することにより、光飛行時間計測法（ＴＯＦ：Ｔｉｍｅ  ｏｆ  ｆｌｉｇｈｔ）を用いて、光飛行時間を測定し、当該光飛行時間に基づいて、対象物体１９０までの距離を得、距離画像生成部１５０において、距離画像を生成する。
【００１６】
受光部１４０は、入射光を受光する撮像素子１４１を備える。撮像素子１４１は、距離画像生成部１５０で生成される画像の画素毎に設けられる。各撮像素子１４１は、図１（ｂ）に示すように、入射光を受光し、電荷に変換する光電変換部１４２と、光電変換部１４２で変換した電荷を振り分けて蓄積する電荷振分部１４３と、を備える。
【００１７】
電荷振分部１４３は、複数の電荷蓄積部１４４を備え、予め定めたタイミングで、それぞれの電荷蓄積部１４４に変換後の電荷を蓄積する。本実施形態では、電荷振分部１４３は、制御部１１０が光源部１３０に出力する変調信号に同期して、電荷を各電荷蓄積部１４４に振り分ける。
【００１８】
なお、本実施形態では、受光部１４０の前方にはレンズ（受光レンズ）が取り付けられ、所定の画角を構成する。
【００１９】
距離画像生成部１５０は、電荷振分部１４３が振り分け、各電荷蓄積部１４４に蓄積した電荷から、画素毎の距離値を算出し、距離画像を生成する。ここで、生成される三次元情報は、距離画像センサ１２０を基準としたセンサ座標系における座標値である。
【００２０】
例えば、電荷振分部１４３が４つの電荷蓄積部１４４を備える場合、光源１３０の変調周期Ｔｓを４等分した時間ずつずらしたゲート信号に従って、電荷振分部１４３は、各電荷蓄積部１４４に電荷を振り分ける。そして、予め定めた時間間隔、例えば、Δｔ毎に、各電荷蓄積部１４４に蓄積された電荷（Ｃ１、Ｃ２、Ｃ３、Ｃ４）を、距離画像生成部１５０に出力する。
【００２１】
距離画像生成部１５０は、電荷蓄積期間Δｔ毎に、受け取った各電荷量（Ｃ１、Ｃ２、Ｃ３、Ｃ４）を用いて、距離Ｄを以下の算出式（１）に従って、算出する。なお、ｃは光速である。
【数１】
2014074615.tif 000003
ここで、Ｔｄは、遅延時間、すなわち、位相差φであり、上述のように、変調周期Ｔｓを４等分して蓄積した各各電荷量（Ｃ１、Ｃ２、Ｃ３、Ｃ４）を用いて、以下の式（２）で表される。
【数２】
2014074615.tif 000004
【００２２】
姿勢検出部１６０は、距離画像センサ１２０の姿勢を検出し、姿勢情報として世界座標生成部１７０に出力する。検出は、所定の時間間隔で継続的に行われる。
【００２３】
世界座標生成部１７０は、距離画像生成部１５０が生成した距離画像から、予め定めた世界座標系の三次元情報を算出する。算出には、距離画像の画素位置、画素値、受光レンズの結像情報、および、姿勢検出部１６０が検出した姿勢情報を用いる。
【００２４】
ここで、本実施形態のセンサ座標系と、世界座標系とについて簡単に説明する。
【００２５】
センサ座標系２００は、図２（ａ）に示すように、距離画像センサ１２０の撮像素子１４１の配列方向を、それぞれ、ｘ軸方向およびｙ軸方向とし、撮像素子１４１の受光面に直交する方向をｚ軸方向とする右手座標系である。センサ座標系２００は、撮像素子１４１の配列の中心を通るｚ軸上の、受光レンズの焦点位置を原点とする。
【００２６】
世界座標系３００は、距離画像センサ１２０の姿勢によらない座標系である。本実施形態では、図２（ｂ）に示すように、Ｙ軸方向を重力（ｇ）方向に平行な方向（ここでは、重力方向と逆方向）とする右手座標系である。Ｘ軸方向およびＺ軸方向は、Ｙ軸方向に直交する平面（水平面）上で、それぞれ、互いに直交する方向が定められる。
【００２７】
センサ座標系２００と世界座標系３００との関係を、図３に示す。本図に示すように、センサ座標系２００と世界座標系３００とは、原点は共通とする。ここで、θｒは、センサ座標系２００（すなわち、距離画像センサ１２０；三次元情報生成装置１００）の、世界座標系３００のＺ軸周りの回転角（ロール角）、θｐは、センサ座標系２００の、世界座標系３００のＸ軸周りの回転角（ピッチ角）、θｙは、センサ座標系２００の、世界座標系３００のＹ軸周りの回転角（ヨー角）である。すなわち、θｒ、θｐ、θｙは、それぞれ、センサ座標系２００のｘｚ平面と世界座標系３００のＸＺ平面、同ｙｚ平面とＹＺ平面、同ｘｙ平面とＸＹ平面が、それぞれなす角度である。以下、本明細書では、各回転角について、反時計回りを正方向とする。
【００２８】
本実施形態の姿勢検出部１６０は、例えば、重力センサ等で構成され、重力加速度を計測して、距離画像センサ１２０の鉛直方向に対する傾きを検出する。すなわち、本実施形態の姿勢検出部１６０が検出する姿勢情報は、距離画像センサ１２０のロール角θｒとピッチ角θｐとである。
【００２９】
次に、世界座標生成部１７０が、この姿勢情報を用い、距離画像生成部１５０が生成した距離画像から世界座標を生成する原理を説明する。
【００３０】
まず、距離画像からの、センサ座標系２００における三次元情報（座標値）の算出について説明する。ここでは、撮像素子１４１は、センサ座標系２００のｘ軸方向にＭ個、ｙ軸方向にＮ個、配列されるものとする。ここで、ＭおよびＮは、１以上の整数とする。
【００３１】
図４（ａ）および（ｂ）を用い、距離画像の各画素に対応する撮像素子１４１の、ｘ軸方向の撮像範囲を説明する。受光部１４０に取り付けられたレンズの結像情報のうち、水平方向の画角をθｈとする。このとき、Ｍ個の撮像素子１４１それぞれに割り当てられる、ｘ軸方向の撮像範囲φｈは、以下の式（３）で表される。
【数３】
2014074615.tif 000005
【００３２】
ここで、ｘ軸方向の中心の撮像素子１４１から、−ｘ軸方向のｍ番目の撮像素子１４１について、その撮像素子１４１の撮像範囲の中央線２１１がｚ軸となす角度θ（ｍ）を求める。ここでは、Ｍが奇数の場合と、偶数の場合とに分けて考える。なお、ｍは整数である。
【００３３】
Ｍが奇数の場合、図４（ａ）に示すように、ｍの取り得る値の範囲は、以下の式（４）のとおりである。
【数４】
2014074615.tif 000006
ｍ番目の撮像素子１４１の撮像範囲φｈ（ｍ）は、上記φｈとｍとを用いて、以下の式（５）で表される。
【数５】
2014074615.tif 000007
従って、ｍ番目の撮像素子１４１の撮像範囲φｈ（ｍ）の中央線２１１が、ｚ軸となす角度θ（ｍ）は、以下の式（６）で表される。
【数６】
2014074615.tif 000008
【００３４】
一方、Ｍが偶数の場合、図４（ｂ）に示すように、ｍの取り得る値の範囲は、以下の式（７）のとおりである。
【数７】
2014074615.tif 000009
なお、ｍ≠０とする。また、ｍ番目の撮像素子１４１の撮像範囲φｈ（ｍ）は、上記φｈとｍとを用いて、以下の式（８）で表される。
【数８】
2014074615.tif 000010
従って、ｍ番目の撮像素子１４１の撮像範囲φｈ（ｍ）の中央線２１１が、ｚ軸となす角度θ（ｍ）は、以下の式（９）で表される。
【数９】
2014074615.tif 000011
【００３５】
以下、ｍ番目の撮像素子１４１のｘ軸方向の撮像範囲の中央線２１１とｚ軸とがなす角度θ（ｍ）を、当該撮像素子１４１のｘ軸方向の画素角と呼ぶ。
【００３６】
次に、図５（ａ）および（ｂ）を用い、各撮像素子１４１の、ｙ軸方向の撮像範囲の算出方法を示す。受光部１４０に取り付けられたレンズの結像情報のうち、垂直方向の画角をθｖとする。このとき、Ｎ個の撮像素子１４１それぞれに割り当てられる、ｙ軸方向の撮像範囲φｖは、以下の式（１０）で表される。
【数１０】
2014074615.tif 000012
【００３７】
ここで、ｙ軸方向の中心の撮像素子１４１から、−ｙ軸方向のｎ番目の撮像素子１４１について、その撮像素子１４１と撮像範囲の中央線２２１とがなす角度θ（ｎ）を求める。ここでは、ｘ軸方向の場合同様、Ｎが奇数の場合と偶数の場合とに分けて考える。なお、ｎは整数である。
【００３８】
Ｎが奇数の場合、図５（ａ）に示すように、ｎの取り得る値の範囲は、以下の式（１１）のとおりである。
【数１１】
2014074615.tif 000013
ｎ番目の撮像素子１４１の撮像範囲φｈ（ｎ）は、上記φｈとｎとを用いて、以下の式（１２）で表される。
【数１２】
2014074615.tif 000014
従って、ｎ番目の撮像素子１４１の撮像範囲φｈ（ｎ）の中央線２２１が、ｚ軸となす角度θ（ｎ）は、以下の式（１３）で表される。
【数１３】
2014074615.tif 000015
【００３９】
一方、Ｎが偶数の場合、図５（ｂ）に示すように、ｎの取り得る値の範囲は、以下の式（１４）のとおりである。
【数１４】
2014074615.tif 000016
なお、ｎ≠０とする。ｎ番目の撮像素子１４１の撮像範囲φｈ（ｎ）は、上記φｈとｎとを用いて、以下の式（１５）で表される。
【数１５】
2014074615.tif 000017
従って、ｎ番目の撮像素子１４１の撮像範囲φｈ（ｎ）の中央線２２１が、ｚ軸となす角度θ（ｎ）は、以下の式（９）で表される。
【数１６】
2014074615.tif 000018
【００４０】
以下、ｎ番目の撮像素子１４１のｙ軸方向の撮像範囲の中央線２１１とｚ軸とがなす角度θ（ｎ）を、当該撮像素子１４１のｙ軸方向の画素角と呼ぶ。
【００４１】
本実施形態の世界座標生成部１７０は、位置が（ｍ，ｎ）で特定される各撮像素子１４１の画素角θ（ｍ）およびθ（ｎ）と、対応する画素の画素値である距離値ｄとから、センサ座標系２００における三次元情報（座標値）を算出する。その算出原理を、図６を用いて説明する。
【００４２】
−ｘ軸方向のｍ番目、−ｙ軸方向のｎ番目の撮像素子１４１（以下、撮像素子（ｍ，ｎ）と呼ぶ。）に対応する画素の距離値をｄとする。この撮像素子（ｍ，ｎ）の撮像範囲（以下、点Ｐとする。）のセンサ座標系２００における座標値（センサ座標値）を（ａ，ｂ，ｃ）とする。
【００４３】
撮像素子（ｍ，ｎ）に対応する画素の画素値がｄであるため、センサ座標系２００の原点から、点Ｐ（上記撮像範囲の物体）までの距離はｄである。点Ｐのｘｚ平面への射影点（ａ，０，ｃ）の動径とｚ軸とがなす角度は、前述の式（６）または式（９）で求めたｘ軸方向の画素角θ（ｍ）である。点Ｐのｙｚ平面への射影点（０，ｂ，ｃ）の動径とｚ軸のなす角度は、前述の式（１３）または式（１６）で求めたｙ軸方向の画素角θ（ｎ）である。
【００４４】
従って、センサ座標系２００における点Ｐのセンサ座標値（ａ．ｂ，ｃ）は、点Ｐまでの距離ｄと、ｘｚ平面，ｙｚ平面への射影の動径とｚ軸とのなす角度θ（ｍ）およびθ（ｎ）と、を用いて、以下の式（１７−１）、式（１７−２）、式（１７−３）で表される。
【数１７】
2014074615.tif 000019
すなわち、撮像素子（ｍ，ｎ）の撮像範囲のセンサ座標値（ａ，ｂ，ｃ）は、当該撮像素子（ｍ，ｎ）に対応する画素の画素値ｄと、ｘ軸方向およびｙ軸方向の画素角θ（ｍ）およびθ（ｎ）とにより、上記式（１７−１）、式（１７−２）、式（１７−３）のように算出される。各撮像素子１４１について、上記式（１７−１）、式（１７−２）、式（１７−３）を算出することにより、各撮像素子１４１の撮像範囲の物体の、センサ座標系２００における三次元情報を求めることができる。これは、各撮像素子１４１に対応する画素の、三次元情報である。
【００４５】
しかし、上記式により算出した三次元情報は距離画像センサ１２０を原点としたセンサ座標系２００における三次元情報である。そのため、得られる三次元情報は、距離画像センサ１２０の姿勢に依存する。このため、同一空間内の、同一位置にある物体であっても、撮像時間が異なり、その間に距離画像センサ１２０の姿勢が変化すると、異なる三次元情報が得られる。本実施形態の世界座標生成部１７０は、センサ座標値を、世界座標系３００の三次元情報（以下、世界座標値と呼ぶ。）に変換する。
【００４６】
次に、本実施形態の世界座標生成部１７０による、センサ座標系２００のセンサ座標値から、世界座標系３００の世界座標値を算出する座標変換処理を説明する。ここでは、上記式（１７−１）、式（１７−２）、および式（１７−３）で求めたセンサ座標値から、世界座標値を算出する場合を例にあげて説明する。
【００４７】
図７を用い、まず、Ｚ軸周りの回転（ロール角）を考慮した世界座標値の算出処理を説明する。センサ座標系２００（ｘｙｚ座標系）は、世界座標系３００（ＸＹＺ座標系）を、Ｚ軸周りに角度θｒだけ回転させた座標系とする。また、センサ座標系２００における点Ｐの座標を（ａ，ｂ，ｃ）とし、世界座標系３００における点Ｐの座標を（Ａ，Ｂ，Ｃ）とする。
【００４８】
まず、点Ｐの、世界座標系３００におけるＸの値Ａを、センサ座標系２００の座標値（ａ，ｂ）およびロール角θｒを用いて算出する。ここでは、ｃ＝Ｃ＝０とし、ＸＹ平面上で考える。
【００４９】
点Ｐの、ｘ軸に垂線４０１を下ろしたときの交点の、センサ座標系２００のｘｙ座標値は（ａ，０）である。点Ｐの、世界座標系３００のＸ軸方向の座標値（Ｘの値）Ａは、座標（ａ，０）の点から、Ｘ軸に垂線４０２を下ろしたときの交点Ａ１のＸの値から、点ＰからＸ軸へ垂線４０３を下ろしたときの交点Ａと交点Ａ１間の長さｋｘを引いたもの（Ａ１−ｋｘ）である。Ｘ軸とｘ軸とがなす角度がθｒであるため、Ａ１の長さおよびｋｘの長さは、以下の式（１８−１）、式（１８−２）で表される。
【数１８】
2014074615.tif 000020
よって、世界座標系３００におけるＸの値Ａは、センサ座標系２００の座標値（ａ，ｂ）およびロール角θｒを用いて、以下の式（１９）で算出される。
【数１９】
2014074615.tif 000021
【００５０】
次に、点Ｐの、世界座標系３００におけるＹの値Ｂを、センサ座標系２００の座標（ａ，ｂ）およびロール角θｒを用いて算出する。
【００５１】
点Ｐからｙ軸に垂線４１１を下ろしたときの交点の、センサ座標系２００の座標は（ｂ、０）である。点Ｐの、世界座標系３００のＹ軸方向の座標値（Ｙの値）Ｂは、座標（ｂ、０）の交点からＹ軸に垂線４１２を下ろしたときの交点Ｂ１のＹの値に、Ｂ１からＢまでの距離ｋｙを加算したもの（Ｂ１＋ｋｙ）である。Ｂは、点ＰからＹ軸に垂線４１３を下ろしたときの交点である。Ｙ軸とｙ軸とがなす角度がθｒであるため、Ｂ１およびｋｙは、以下の式（２０−１）および式（２０−２）で表される。
【数２０】
2014074615.tif 000022
よって、世界座標系３００におけるＹの値Ｂは、センサ座標系２００の座標値（ａ，ｂ）およびロール角θｒを用いて、以下の式（２１）で算出される。
【数２１】
2014074615.tif 000023
【００５２】
以上より、センサ座標系２００における座標が（ａ，ｂ，ｃ）である点Ｐの、ロール角を考慮した際の、世界座標系３００における世界座標（Ａ，Ｂ，Ｃ）は、以下の式（２２−１）、式（２２−２）、式（２２−３）で表される。
【数２２】
2014074615.tif 000024
【００５３】
Ｘ軸周りの回転（ピッチ角）を考慮した世界座標値も、上記同様に算出できる。センサ座標系２００（ｘｙｚ座標系）は、世界座標系３００（ＸＹＺ座標系）を、Ｘ軸周りに角度θｐだけ回転させた座標系とする。このとき、センサ座標系２００における座標が（ａ，ｂ，ｃ）である点Ｐの、ピッチ角θｐを考慮した際の、世界座標系３００における世界座標（Ａ，Ｂ，Ｃ）は、以下の式（２３−１）、式（２３−２）、式（２３−３）で表される。
【数２３】
2014074615.tif 000025
【００５４】
本実施形態の世界座標生成部１７０は、得られたセンサ座標系２００の座標値を、世界座標系３００の座標値に座標変換する場合、図８（ａ）〜（ｃ）に示すように、まず、ロール角を考慮した座標変換により第一座標系３０１への変換を行い、次に、ピッチ角を考慮した座標変換を行い、世界座標系３００へ変換する。
【００５５】
Ｚ軸周りの回転角であるロール角をθｒ、Ｘ軸周りの回転角であるピッチ角をθｐとする。センサ座標系２００の点Ｐの座標を（ａ，ｂ，ｃ）とする。また、Ｚ軸周りの回転を考慮した（ロール角に基づいた）座標変換後の第一座標系３０１における座標を（ａ１，ｂ１，ｃ１）、Ｘ軸周りの回転を考慮した（ピッチ角に基づいた）座標変換後の世界座標系３００における座標を（Ａ，Ｂ，Ｃ）とする。
【００５６】
式（２２−１）、式（２２−２）、式（２２−３）より、センサ座標系２００のロール角がθｒの場合、座標変換後の座標（ａ１，ｂ１，ｃ１）は、以下の式（２４−１）、式（２４−２）、式（２４−３）で表される。
【数２４】
2014074615.tif 000026
【００５７】
そして、第一座標系３０１における座標値をピッチ角θｐに基づいた座標変換を行った後の座標は、式（２３−１）、式（２３−２）、式（２３−３）より、以下の式（２５−１）、式（２５−２）、式（２５−３）で表される。
【数２５】
2014074615.tif 000027
【００５８】
ここで、式（２５−１）、式（２５−２）、式（２５−３）に、上記式（１７−１）、式（１７−２）、式（１７−３）を代入し、世界座標系３００における座標値（Ａ，Ｂ，Ｃ）を、距離画像の画素値である距離ｄと、各画素に対応する撮像素子（ｍ，ｎ）のそれぞれの軸方向の画素角θ（ｍ）およびθ（ｎ）とを用いて表すと、以下の式（２６−１）、式（２６−２）、式（２６−３）となる。
【数２６】
2014074615.tif 000028
【００５９】
これらの式（２６−１）、式（２６−２）、式（２６−３）により、本実施形態の世界座標生成部１７０は、距離画像生成部１５０が生成する距離画像から、姿勢検出部１６０が検出した距離画像センサの姿勢情報（ロール角θｒおよびピッチ角θｐ）を用い、世界座標系３００における三次元情報（Ａ，Ｂ，Ｃ）を生成する。
【００６０】
なお、ここでは、センサ座標系２００から世界座標系３００への変換を、Ｚ軸周りの回転（ロール角）を考慮した変換、Ｘ軸周りの回転（ピッチ角）を考慮した変換の順に行う場合を例にあげているが、これらの変換の順は可換である。また、三角関数を用いた逐次変換に限らず、行列を用いて変換を行ってもよい。
【００６１】
また、世界座標生成部１７０は、上記式（２６−１）、式（２６−２）、式（２６−３）を構成するいくつかの要素を、予め算出し、テーブルとして保持するよう構成してもよい。この場合、世界座標生成部１７０は、世界座標系３００における三次元情報を生成する際、当該テーブルから算出に必要な値を読み出す。テーブルは、三次元情報生成装置１００が備える記憶装置に登録する。
【００６２】
例えば、式（２６−１）、式（２６−２）、式（２６−３）を、以下の３つの式（２７−１）、式（２７−２）、式（２７−３）の組み合わせと考える。
【数２７】
2014074615.tif 000029
本実施形態の世界座標生成部１７０は、これらの３式により得られる値を、距離画像の各画素のアドレスに対応づけたテーブルとして記憶装置に記憶する。
【００６３】
上述のように、上記各式内で変数に用いられる（ｍ，ｎ）は、−ｘ軸方向のｍ番目、−ｙ軸方向のｎ番目の撮像素子１４１を示す。各撮像素子１４１は、距離画像１４０ｉの各画素１４１ｉに対応する。一般に、画像の画素を特定するアドレス（画素アドレス）は、画像の一角を原点とした座標が用いられる。ここでは、図９（ａ）に示すように、距離画像１４０ｉの左上を原点とした座標（Ｋ，Ｌ）で表す。本実施形態では、各撮像素子１４１が、距離画像の各画素に対応するため、距離画像のｘ軸方向の画素数はＭ、ｙ軸方向の画素数はＮである。従って，Ｋは、１≦Ｋ≦Ｍを、Ｌは、１≦Ｌ≦Ｎをそれぞれ満たす整数である。
【００６４】
上記撮像素子１４１を特定するアドレス（ｍ，ｎ）と、画素アドレス（Ｋ，Ｌ）との対応は、画素数Ｍ、Ｎが偶数および奇数によって異なる。ｘ軸方向の対応（ｍとＫとの対応）について、Ｍが奇数の場合を図９（ｂ）に、Ｍが偶数の場合を、図９（ｃ）に示す。また、図１０（ａ）および図１０（ｂ）は、それぞれ、Ｍが奇数の場合およびＭが偶数の場合の、Ｋとｍとの対応を示す表６０１、６０２である。
【００６５】
これらの図に示すように、Ｋ＝１のとき、ｍは最大値となり、Ｋ＝Ｍのとき、ｍは最小値となる。すなわち、Ｍが奇数の場合、Ｋ＝１には、ｍ＝−（Ｍ−１）／２が対応し、Ｋ＝Ｍには、ｍ＝（Ｍ−１）／２が対応する。また、Ｍが偶数の場合、Ｋ＝１には、ｍ＝−Ｍ／２が、Ｋ＝Ｍには、ｍ＝Ｍ／２が対応する。ｙ軸方向も同様に、Ｌ＝１のとき、ｎは最大値となり、Ｌ＝Ｎのとき、ｎは最小値となる。
【００６６】
例えば、上記式（２７−１）、式（２７−２）、式（２７−３）の値Ｔ１、Ｔ２、Ｔ３は、撮像素子１４１のアドレス（ｍ，ｎ）で特定される。各撮像素子１４１のアドレスは、上述のように、画素の水平および垂直アドレスであるＫとＬとに対応する。従って、上記式（２７−１）、式（２７−２）、式（２７−３）の値Ｔ１、Ｔ２、Ｔ３は、画素アドレス（Ｋ，Ｌ）を変数として、以下の式（２８−１）、式（２８−２）、式（２８−３）で表される。
【数２８】
2014074615.tif 000030
【００６７】
ここで、ｘ軸方向の画素角θ（ｍ）は、既知である水平画素数Ｍ、水平画角θｈ、および画素の水平アドレスＫに対応するｍ、により決定する。また、ｙ軸方向の画素角θ（ｎ）は、垂直画素数Ｎ、垂直画角θｖ、および画素の垂直アドレスＬに対応するｎにより決定する。本実施形態の世界座標生成部１７０は、画素アドレス（Ｋ，Ｌ）毎の画素角θ（ｍ）および画素角θ（ｎ）の値Ｕ１（Ｋ）、Ｕ２（Ｌ）を、Ｍ行１列、Ｎ行１列のテーブル５０１、５０２として作成し、記憶装置に保持しておく。
【００６８】
テーブル５０１およびテーブル５０２の一例を、図１１（ａ）および図１１（ｂ）に示す。これらの図に示すように、テーブル５０１およびテーブル５０２は、画素アドレス（Ｋ，Ｌ）毎に、画素角θ（ｍ）またはθ（ｎ）の値を保持する。テーブル５０１およびテーブル５０２は、予め作成してもよいし、三次元情報生成装置１００の起動直後に作成してもよい。
【００６９】
また、上記Ｔ１（Ｋ，Ｌ）は、画素角θ（ｍ）およびθ（ｎ）により得られる。従って、世界座標生成部１７０は、テーブル５０１およびテーブル５０２を利用し、Ｔ１のテーブル５１１を作成する。テーブル５１１は、画素の水平および垂直アドレスであるＫとＬとをインデックスとしたＮ行×Ｍ列のテーブルである。テーブル５１１の一例を図１２に示す。なお、テーブル５１１は、テーブル５０１および５０２の作成に応じ、予め作成してもよいし、三次元情報生成装置１００の起動直後に作成してもよい。
【００７０】
同様に、世界座標生成部１７０は、テーブル５０１およびテーブル５０２を利用し、Ｔ２およびＴ３のテーブル５１２、５１３をそれぞれ作成する。これらのテーブルも、画素の水平および垂直アドレスであるＫとＬとをインデックスとしたＮ行×Ｍ列のテーブルである。テーブル５１２、５１３の一例を、それぞれ、図１３（ａ）および図１３（ｂ）に示す。テーブル５１２およびテーブル５１３は、上述のように、ロール角θｒが更新される毎に作成される。
【００７１】
なお、作成したテーブル５１２、５１３は、ロール角θｒに対応づけて記憶するよう構成してもよい。この場合、ロール角θｒが更新された場合、当該θｒに対応づけてテーブル５１２、５１３が記憶されているか否か判別後、記憶されていない場合のみ作成する。
【００７２】
上記Ｔ１（Ｋ，Ｌ）、Ｔ２（Ｋ，Ｌ）、Ｔ３（Ｋ，Ｌ）を用いると、上記式（２６−１）、式（２６−２）、式（２６−３）は、以下の式（２９−１）、式（２９−２）、式（２９−３）と表される。
Ａ＝ｄ×Ｔ１(Ｋ，Ｌ)×Ｔ２(Ｋ，Ｌ)              ・・・（２９−１）
Ｂ＝ｄ×Ｔ１(Ｋ，Ｌ)×(Ｔ３(Ｋ，Ｌ)cosθp−sinθp)・・・（２９−２）
Ｃ＝ｄ×Ｔ１(Ｋ，Ｌ)×(Ｔ３(Ｋ，Ｌ)sinθp＋cosθp)・・・（２９−３）
従って、世界座標生成部１７０は、上記式（２９−１）、式（２９−２）および式（２９−３）により、距離画像生成部１５０が生成した距離画像から、世界座標系３００の座標値（Ａ，Ｂ，Ｃ）を得る。
【００７３】
さらに、ピッチ角θｐが更新される毎に、ｃｏｓθｐおよびｓｉｎθｐを計算し、保持しておくようにすると、式（２９−１）、式（２９−２）および式（２９−３）は、基本的に乗算と加算とにより構成されることとなる。この場合、三角関数の計算、ルートの計算、割り算といった処理に膨大な時間が必要となる計算がなくなり、世界座標生成時に高速な演算が可能となる。従って、フレームレートの低下なしに三次元情報を得ることができる。また、これらの計算を実現する演算回路は、３つの乗算器と１つの加算器とで構成できる。従って、これにより、世界座標生成部１７０の演算回路を非常に小規模かつ簡易な構成とすることができる。
【００７４】
以上の各部による、本実施形態の世界座標生成部１７０による世界座標系３００における三次元情報算出処理（世界座標算出処理）の流れを説明する。図１４は、本実施形態の世界座標算出処理のフローチャートである。この世界座標算出処理は、距離画像生成部１５０から距離画像を受信したことを契機に開始し、距離画像生成部１５０から距離画像を受信する毎に行われる。
【００７５】
ここでは、予め、テーブル５０１、５０２、５１１、５１２、５１３を作成し、式（２９−１）、式（２９−２）、式（２９−３）により世界座標を算出する場合を例にあげて説明する。従って、世界座標生成部１７０は、世界座標算出処理開始に先立ち、テーブル５０１、５０２、５１１を作成し、記憶装置等に記憶しておく。また、ロール角θｒが０の場合のテーブル５１２、５１３を作成し、記憶装置等に記憶しておく。
【００７６】
世界座標生成部１７０は、距離画像を受け取ると、まず、姿勢検出部１６０からその時点の姿勢情報（ここでは、ロール角θｒおよびピッチ角θｐ）を受け取る（ステップＳ１１０１）。受け取ったロール角θｒおよびピッチ角θｐは、例えば、受け取った時刻に対応づけてハードウェア等の記憶装置に保持しておく。
【００７７】
世界座標生成部１７０は、受け取った姿勢情報を、１回前に受け取った姿勢情報と比較し、変化しているか否かを判別する（ステップＳ１１０２）。ここでは、ロール角θｒおよびピッチ角θｐそれぞれについて、１回前に取得した値との差を計算し、得られた差が、所定の閾値以内であれば、変化無し、と判別し、その他は、変化有りと判別する。なお、１回目の処理時は、比較無しに変化有りと判別する。
【００７８】
ステップＳ１１０２において、ロール角θｒおよびピッチ角θｐの少なくとも一方に変化ありと判別された場合、ステップＳ１１０２で受け取った、変化有りと判別された姿勢情報を用い、テーブル５１２、５１３の中の、変化ありと判別された姿勢情報を用いて算出するテーブルを更新する（ステップＳ１１０３）。一方、ステップＳ１１０２において、変化無し、と判別された場合は、テーブル５１２、５１３はそのままとする。
【００７９】
その後、世界座標生成部１７０は、距離画像の全画素（座標（Ｋ,Ｌ）；距離値ｄ）について、上記テーブル５１１、５１２、５１３および式（２９−１）、式（２９−２）、式（２９−３）を用いて、世界座標系３００における座標値（Ａ，Ｂ，Ｃ）を算出する。ここでは、まず、各画素について、テーブル５１１、５１２、５１３から当該画素位置（Ｋ，Ｌ）に対応づけて記憶されているデータを読み出す（ステップＳ１１０６）。そして、読み出した値を用いて式（２９−１）、式（２９−２）、式（２９−３）により、世界座標値（Ａ，Ｂ，Ｃ）を算出する（ステップＳ１１０７）。以上の処理を、全ての画素について繰り返す（ステップＳ１１０４、Ｓ１１０５、Ｓ１１０８、Ｓ１１０９、Ｓ１１１０、Ｓ１１１１）。全ての画素について世界座標値を得ると、処理を終了する。
【００８０】
以上説明したように、本実施形態によれば、姿勢検出部１６０において、三次元情報生成装置１００の姿勢を姿勢情報として検出する。そして、検出した姿勢情報を用い、距離画像から、撮像範囲の三次元情報を、世界座標系３００の座標値として算出する。すなわち、本実施形態によれば、撮像範囲の三次元情報を、三次元情報生成装置１００の姿勢によらない、世界座標系３００の座標値として三次元情報を得ることができる。このため、三次元情報生成装置１００の姿勢によらず、撮像範囲の三次元情報を、高精度に安定的に得ることができる。
【００８１】
また、本実施形態によれば、距離画像の各画素の世界座標系３００における座標値の算出を、予め算出したテーブルを用いて行う。従って、複雑な演算処理を繰り返す必要がない。このため、高速に、高い精度で世界座標系３００での三次元情報を得ることができる。
【００８２】
本実施形態によれば、三次元情報生成装置１００の姿勢・設置状態によらず、精度良く世界座標系での撮像範囲の三次元情報を算出することができる。このとき、キャリブレーション用の基準物が必要なく、装置単体で世界座標系の三次元情報を算出することができる。本実施形態によれば、基準物が必要ないため、使用環境の制限もなく、かつ、三次元情報生成装置１００の状態変化に対しリアルタイムで世界座標系の三次元情報を得ることができる。
【００８３】
特に、本実施形態の三次元情報生成装置１００が、監視カメラや車載カメラとして用いられる場合、装置の設置位置からの俯瞰的な撮影となる。このような場合、カメラ基準のセンサ座標系２００による距離画像は、実際に人が見るものと異なり、撮影対象物の位置関係や動きが把握しにくい。このような場合であっても、本実施形態のように重力を基準とした世界座標系に変換することにより、見やすく、動きの把握しやすい画像をユーザに提供することができる。
【００８４】
なお、制御部１１０、距離画像生成部１５０、世界座標生成部１７０の各部の処理は、三次元情報生成装置１００が備える記憶装置に予め保持されたプログラムを、同装置１００が備えるＣＰＵがメモリにロードして実行することにより実現する。
【００８５】
また、本実施形態では、距離画像から世界座標系３００における三次元情報を算出するものとして記載したが、これに限られない。三次元情報生成装置１００において、センサ座標系２００における三次元情報（ａ，ｂ，ｃ）が得られる場合、この三次元情報（ａ，ｂ，ｃ）から、上記式（２５−１）、式（２５−２）、式（２５−３）を用い、座標変換することにより、世界座標系３００における三次元情報（Ａ，Ｂ，Ｃ）を得るよう構成してもよい。
【００８６】
＜＜第二の実施形態＞＞
次に、本発明を適用する第二の実施形態を説明する。第一の実施形態では、姿勢情報として、三次元情報生成装置１００の、鉛直方向に対する傾き、すなわち、ロール角およびピッチ角を検出し、その傾きを加味し、距離画像から、撮像範囲の世界座標系における三次元情報を算出している。本実施形態では、姿勢情報として、さらに、Ｙ軸周りの回転角であるヨー角も加味する。
【００８７】
本実施形態の三次元情報生成装置１００は、基本的に第一の実施形態と同様の構成を有する。ただし、上記のように、本実施形態では、撮像範囲の、世界座標系３００における三次元情報の算出に、ヨー角も加味する。このため、姿勢検出部１６０および世界座標生成部１７０が異なる。以下、本実施形態について、第一の実施形態と異なる構成に主眼をおいて説明する。
【００８８】
本実施形態の姿勢検出部１６０は、第一の実施形態同様、距離画像センサ１２０の姿勢を検出し、姿勢情報として、世界座標生成部１７０に出力する。このとき、本実施形態では、上述のように、ロール角θｒおよびピッチ角θｐに加え、ヨー角θｙも検出する。このため、本実施形態の姿勢検出部１６０は、これらの情報を検出可能なセンサが用いられる。例えば、姿勢検出部１６０は、重力センサと、地磁気センサとにより構成される。なお、本実施形態の姿勢情報検出部１６０が検出する姿勢情報は、距離画像センサ１２０のロール角θｒと、ピッチ角θｐと、ヨー角θｙとである。
【００８９】
世界座標生成部１７０は、距離画像生成部１５０が生成した距離画像から、撮像範囲の、世界座標系３００における三次元情報を算出する。算出には、距離画像の各画素に対応する撮像素子１４１の位置を示すアドレス、距離画像の画素値および姿勢検出部１６０が検出した姿勢情報を用いる。
【００９０】
本実施形態においても、世界座標系３００は、鉛直方向をＹ軸とし、Ｙ軸に直交する水平面上にＸ軸およびＺ軸方向を配した座標系とする。
【００９１】
次に、本実施形態の世界座標生成部１７０による、世界座標系３００による三次元情報算出処理を説明する。図１５（ａ）〜（ｄ）に示すように、ロール角θｒ、ピッチ角θｐ、および、ヨー角θｙを考慮した世界座標系の算出は、センサ座標系で得た三次元情報に対し、これらの各角度を考慮した座標変換を順に行うことにより実現する。ここでは、一例として、図１５（ａ）〜（ｄ）に示すように、ロール角θｒ、ピッチ角θｐ、ヨー角θｙの順に変換を行うものとして説明する。
【００９２】
センサ座標系２００の点Ｑの座標を（ａ，ｂ，ｃ）とし、センサ座標系２００の、Ｚ軸周りの回転量（ロール角θｒ）を考慮した座標変換後の第一座標系３０１における同点の座標を（ａ１，ｂ１，ｃ１）、同Ｘ軸周りの回転量（ピッチ角θｐ）を考慮した座標変換後の第二座標系３０２における同点の座標を（ａ２，ｂ２，ｃ２）とする。最後に、同Ｙ軸周りの回転量（ヨー角θｙ）を考慮した座標変換を行い、得られる世界座標系３００における座標を（Ａ，Ｂ，Ｃ）とする。
【００９３】
ここで、センサ座標系２００における座標が（ａ，ｂ，ｃ）である点Ｑの、ロール角θｒおよびピッチ角θｐをそれぞれ考慮した座標変換後の座標は、上記第一の実施形態の式（２２−１）、式（２２−２）、式（２２−３）、式（２３−１）、式（２３−２）、式（２３−３）で表される。また、同点Ｑの、ヨー角を考慮した際の、世界座標系３００における世界座標（Ａ，Ｂ，Ｃ）は、以下の式（３０−１）、（３０−２）、（３０−３）で表される。
【数３０】
2014074615.tif 000031
【００９４】
（ａ１，ｂ１，ｃ１）は、第一の実施形態の式（２４−１）、式（２４−２）、式（２４−３）により算出される。また、（ａ１，ｂ２，ｃ２）は、第一の実施形態の式（２５−１）、式（２５−２）、式（２５−３）と同様に、以下の式（３１−１）、式（３１−２）、式（３１−３）で算出される。
【数３１】
2014074615.tif 000032
【００９５】
そして、第二座標系３０２における座標値（ａ１，ｂ２，ｃ２）を、ヨー角θｙを考慮した座標変換を行った後の座標、すなわち、世界座標系３００における座標（Ａ，Ｂ，Ｃ）は、以下の式（３２−１）、式（３２−２）、式（３２−３）で算出される。
【数３２】
2014074615.tif 000033
これらの式（３２−１）、式（３２−２）、式（３２−３）を変形すると、以下の式（３３−１）、式（３３−２）、式（３３−３）となる。
【数３３】
2014074615.tif 000034
【００９６】
この式（３３−１）、式（３３−２）、式（３３−３）に、第一の実施形態の式（１７−１）、式（１７−２）、式（１７−３）を代入し、距離画像１４０ｉの各画素１４１ｉのアドレス（Ｋ，Ｌ）および距離画像１４０の画素値（距離値）ｄを用いて表すと、以下の式（３４−１）、式（３４−２）、式（３４−３）となる。なお、本実施形態においても、画素アドレス（Ｋ，Ｌ）は、撮像素子１４１のアドレス（ｍ，ｎ）に対応する。
Ａ＝ｄ×Ｓ１(K,L)×(Ｓ２(K)+Ｓ３(L)+cosθpsinθy)・・・（３４−１）
Ｂ＝ｄ×Ｓ１(K,L)×(Ｓ４(K,L)-sinθp)            ・・・（３４−２）
Ｃ＝ｄ×Ｓ１(K,L)×(Ｓ５(K)+Ｓ６(L)+cosθpcosθy)・・・（３４−３）
【００９７】
ここで、Ｓ１（Ｋ，Ｌ）、Ｓ２（Ｋ）、Ｓ３（Ｌ）、Ｓ４（Ｋ，Ｌ）、Ｓ５（Ｋ）、Ｓ６（Ｌ）は、対応する各撮像素子１４１のアドレス（ｍ、ｎ）を用い、それぞれ、以下のように表される。
【数３５】
2014074615.tif 000035
【００９８】
これらの式（３４−１）、式（３４−２）、式（３４−３）により、本実施形態の世界座標生成部１７０は、距離画像生成部１５０が生成する距離画像から、姿勢検出部１６０が検出した距離画像センサの姿勢情報（ロール角θ、ピッチ角θｐおよびヨー角θｙ）を用い、世界座標系３００における三次元情報（Ａ，Ｂ，Ｃ）を生成する。
【００９９】
なお、ここでは、センサ座標系２００から世界座標系３００への変換を、Ｚ軸周りの回転（ロール角）を考慮した変換、Ｘ軸周りの回転（ピッチ角）を考慮した変換、Ｙ軸周りの回転（ヨー角）を考慮した変換の順に行う場合を例にあげているが、座標変換の順は可換である。また、三角関数を用いた逐次変換に限らず、行列を用いて変換を行ってもよい。
【０１００】
また、本実施形態においても、第一の実施形態同様、世界座標生成部１７０は、上記式式（３４−１）、式（３４−２）、式（３４−３）を構成する要素Ｓ１、Ｓ２、Ｓ３、Ｓ４、Ｓ５、Ｓ６を、予め算出し、テーブルとして保持するよう構成してもよい。この場合、世界座標生成部１７０は、座標変換を行う際、当該テーブルから算出に必要な値を読み出す。テーブルは、三次元情報生成装置１００が備える記憶装置に登録する。
【０１０１】
このとき、第一の実施形態同様、各撮像素子１４１の画素角θ（ｍ）およびθ（ｎ）の値Ｕ１（Ｋ）、Ｕ２（Ｌ）を、Ｍ行１列、Ｎ行１列のテーブル５０１、５０２として作成し、記憶装置に保持しておいてもよい。
【０１０２】
Ｓ１（Ｋ，Ｌ）の値は、各撮像素子１４１の画素角θ（ｍ）およびθ（ｎ）により得られる。従って、世界座標生成部１７０は、テーブル５０１およびテーブル５０２を利用し、Ｓ１のテーブルを作成する。このテーブルは、画素の水平および垂直アドレスであるＫとＬとをインデックスとしたＮ行×Ｍ列のテーブルである。なお、このテーブルは、テーブル５０１および５０２の作成に応じ、予め作成してもよいし、三次元情報生成装置１００の起動直後に作成してもよい。
【０１０３】
また、世界座標生成部１７０は、テーブル５０１およびテーブル５０２を利用し、Ｓ２〜Ｓ６のテーブルをそれぞれ作成する。Ｓ２およびＳ５のテーブルは、画素の水平アドレスＫをインデックスとしたＭ行×１列のテーブルである。Ｓ３およびＳ６のテーブルは、画素の垂直アドレスＬをインデックスとしたＮ行×１列のテーブルである。これらのテーブルは、ロール角θｒ、ピッチ角θｐ、ヨー角θｙが更新される毎に作成される。
【０１０４】
なお、Ｓ４のテーブルは、画素の水平および垂直アドレスであるＫとＬとをインデックスとしたＮ行×Ｍ列のテーブルである。このテーブルは、ロール角θｒおよびピッチ角θｐが更新される毎に作成される。
【０１０５】
なお、Ｓ２〜Ｓ６のテーブルは、更新された姿勢情報に対応づけて記憶してもよい。この場合、更新後の姿勢情報に対応づけてテーブルが記憶されているか否か判別後、記憶されていない場合のみ作成する。
【０１０６】
本実施形態では、上述のように、ピッチ角およびヨー角が更新される毎に、ｃｏｓθｐ、ｓｉｎθｐ、ｃｏｓθｙ、ｓｉｎθｙを計算し、それを用いて上記Ｓ２〜Ｓ６のテーブルを作成する。このとき、これらのｃｏｓθｐ、ｓｉｎθｐ、ｃｏｓθｙ、ｓｉｎθｙの計算結果も保持しておく。
【０１０７】
このようにすることにより、世界座標生成部１７０は、上記式（３４−１）、式（３４−２）および式（３４−３）の計算を、乗算と加算とのみにより行うことができる。
【０１０８】
これにより、三角関数の計算、ルートの計算、割り算といった処理に膨大な時間が必要となる計算がなくなり、高速な演算が可能となる。従って、フレームレートの低下なしに三次元情報を得ることが可能となる。また、演算回路は、３つの乗算器と２つの加算器とで構成できる。従って、演算回路を非常に小規模かつ簡易な構成とすることができる。
【０１０９】
本実施形態の世界座標生成部１７０による、世界座標算出処理の流れは、基本的に第一の実施形態と同じである。ただし、更新、読出す対象のテーブルは、本実施形態の式（３５−１）、式（３５−２）、式（３５−３）、式（３５−４）、式（３５−５）、式（３５−６）により算出し、作成するテーブルである。そして、ステップＳ１１０７の世界座標値の算出は、式（３４−１）、式（３４−２）、式（３４−３）を用いて行う。
【０１１０】
以上説明したように、本実施形態によれば、第一の実施形態同様、撮像範囲の三次元情報を、三次元情報生成装置１００の姿勢によらない、世界座標系３００の座標値として三次元情報を得ることができる。このため、三次元情報生成装置１００の姿勢によらず、撮像範囲の三次元情報を、高精度に安定的に得ることができる。
【０１１１】
また、本実施形態によれば、距離画像の各画素の世界座標系３００における座標値の算出を、予め算出したテーブルを用いて行う。従って、複雑な演算処理を繰り返す必要がない。このため、高速に、高い精度で世界座標系３００での三次元情報を得ることができる。
【０１１２】
本実施形態によれば、三次元情報生成装置１００の姿勢・設置態によらず、精度良く世界座標系での撮像範囲の三次元情報を算出することができる。このとき、キャリブレーション用の基準物が必要なく、装置単体で世界座標系の三次元情報を算出することができる。本実施形態によれば、基準物が必要ないため、使用環境の制限もなく、かつ、三次元情報生成装置１００の状態変化に対しリアルタイムで世界座標系の三次元情報を得ることができる。監視カメラや車載カメラとして用いられる場合も、第一の実施形態と同様の効果が得られる。
【０１１３】
なお、本実施形態においても、第一の実施形態同様、制御部１１０、距離画像生成部１５０、世界座標生成部１７０の各部の処理は、三次元情報生成装置１００が備える記憶装置に予め保持されたプログラムを、同装置１００が備えるＣＰＵがメモリにロードして実行することにより実現する。
【０１１４】
また、上記実施形態において、距離画像から世界座標系３００における三次元情報を算出するものとして記載したが、これに限られない。三次元情報生成装置１００において、センサ座標系２００における三次元情報（ａ，ｂ，ｃ）が得られる場合、この三次元情報（ａ，ｂ，ｃ）から、上記式（３３−１）、式（３３−２）、式（３３−３）を用い、座標変換することにより、世界座標系３００における三次元情報（Ａ，Ｂ，Ｃ）を得るよう構成してもよい。
【符号の説明】
【０１１５】
１００：三次元情報生成装置、１１０：制御部、１２０：距離画像センサ、１３０：光源部、１４０：受光部、１４０ｉ：距離画像、１４１：撮像素子、１４１ｉ：画素、１４２：光電変換部、１４３：電荷振分部、１４４：電荷蓄積部、１５０：距離画像生成部、１６０：姿勢検出部、１７０：世界座標生成部、１９０：対象物体、２００：センサ座標系、２１１：中央線、２２１：中央線、３００：世界座標系、３０１：第一座標系、４０１：垂線、４０２：垂線、４０３：垂線、４１１：垂線、４１２：垂線、４１３：垂線、５０１：テーブル、５０２：テーブル、５１１：テーブル、５１２：テーブル、５１３：テーブル
【図１】
2014074615.tif 000036
【図２】
2014074615.tif 000037
【図３】
2014074615.tif 000038
【図４】
2014074615.tif 000039
【図５】
2014074615.tif 000040
【図６】
2014074615.tif 000041
【図７】
2014074615.tif 000042
【図８】
2014074615.tif 000043
【図９】
2014074615.tif 000044
【図１０】
2014074615.tif 000045
【図１１】
2014074615.tif 000046
【図１２】
2014074615.tif 000047
【図１３】
2014074615.tif 000048
【図１４】
2014074615.tif 000049
【図１５】
2014074615.tif 000050