JP6262949B2 - キャリブレーション装置およびキャリブレーションプログラム - Google Patents

Description

本発明は、カメラパラメータを推定するキャリブレーション装置およびキャリブレーションプログラムに関する。

一般に、カメラで撮像した画像と、他のカメラで撮像した画像とを合成するなどして、それぞれのカメラで得られた情報を統合する場合、それぞれのカメラのカメラパラメータが用いられる。ここで、カメラパラメータとは、当該カメラおよび他のカメラにとって基準となる共通の世界座標におけるカメラの姿勢や位置等のことである。この場合、当該カメラで撮像された画像に基づいて、カメラパラメータを、未知パラメータとして推定することが一般的である。
このように、カメラの姿勢や位置等を、未知のパラメータとして推定することをカメラキャリブレーションという。

カメラキャリブレーションの１つの方法は、当該カメラおよび他のカメラに共通の座標である世界座標系における座標が既知である特徴点を有する被写体を撮像し、撮像された画像内の特徴点を検出し、検出した特徴点のそれぞれのカメラを基準としたカメラ座標を求めることによりカメラパラメータを算出するものである。例えば、２次元または３次元のパターン（キャリブレーションパターンと呼ばれる）が被写体として撮像される。

非特許文献１では、カメラパラメータを算出する手法が開示されている。非特許文献１では、キャリブレーションパターンとして、複数の正方形が配置された平面板を用い、当該正方形の角を上記の特徴点として扱い、カメラパラメータを推定している。

Roger Y. TSAI, "A Versatile Camera Calibration Technique for High-Accuracy 3D Machine Vision Metrology Using Off-the-Shelf TV Cameras and Lenses", IEEE Journal of Robotics and Automation, Vol.RA-3, No4, August.pp.323-344.

非特許文献１に記載のキャリブレーションパターンを用いて、カラーカメラのカメラパラメータを推定する場合、カラーカメラがキャリブレーションパターンを撮像して取得したカラー画像に基づいて、上記の正方形の角を特徴点として検出し、さらに、カラー画像の画素値に基づいて、検出した特徴点のカメラ座標を取得する。ここで、カラーカメラとは、例えばＣＣＤ（Charge Coupled Device）やＣＭＯＳ（Complementary Metal-Oxide Semiconductor）素子を用いたカメラである。そして、カラー画像は、その撮像画面上に２次元に配列された画素ごとに、画素値を示すものである。

距離カメラは、レーザーや可視光、赤外線などの電磁波を被写体に照射して、被写体の計測点からの反射に基づいて、距離マップを取得するものである。ここで、距離マップは、距離カメラの撮像画面上に２次元に配列された画素ごとに、光学主点から、当該光学主点と当該画素を結ぶ直線上の計測点へ伸びるベクトルと、撮像画面の単位法線ベクトルとの内積値を奥行き情報として示すものである。特に、Time of Flight方式の距離カメラは、被写体に照射した光の飛行時間（Time of Flight）に基づいて、上記の奥行き情報を取得する距離カメラである。このTime of Flight方式の距離カメラは、さらに反射光量に基づいて、その撮像画面上に２次元に配列された画素ごとに輝度を画素値として取得する。しかし、Time of Flight方式の距離カメラの場合、例えば、画素数は水平１６０画素、垂直１２０画素であり、カラーカメラに比べて解像度が低い。したがって、距離カメラを用いて取得される計測点のカメラ座標の精度は、カラーカメラの場合よりも低い。一方、距離カメラを用いた場合、距離マップに基づいて取得される計測点のカメラ座標は、画素値に基づいて取得される計測点のカメラ座標よりも、精度が高い。

非特許文献１に記載のキャリブレーションパターンを用いて、距離カメラのカメラパラメータを推定する場合、距離カメラはカラーカメラに比べて解像度が低いため、画素値に基づいて、上記の正方形の角を特徴点として、正確に検出することができない。よって、距離カメラのカメラパラメータを高精度に求めることができない。これにより、複数の距離カメラで得られた精度の高い距離マップ等の情報を統合することができない。また、距離カメラで得られた距離マップ等の情報と、他のカラーカメラで得られた情報を統合することができないという問題がある。

そこで、本発明は、以上のような問題を解決するためになされたものであり、共通の座標において、距離カメラと、他のカメラのカメラパラメータを高精度に取得するキャリブレーション装置、およびキャリブレーションプログラムを提供することを課題とする。

本発明は、前記課題を解決するために創案されたものであり、本発明では、平面板に対して予め定められた４以上のキャリブレーションを行うための特徴点のそれぞれに対して、前記特徴点を中心点とする半径が予め定められた球形状半径である球面の全体またはその一部である球形状が配置されるキャリブレーションパターンを撮像する距離カメラが取得した計測点の距離マップに基づいて、距離カメラパラメータを推定するキャリブレーション装置であって、特徴点座標取得手段と、距離カメラパラメータ推定手段と、を備える構成とした。

かかる構成において、キャリブレーション装置は、特徴点座標取得手段によって、前記球形状の立体表面におけるそれぞれの計測点からの距離が前記球形状半径であるカメラ座標系の座標を、前記特徴点のカメラ座標系における座標であるカメラ特徴点座標として取得する。このように、特徴点座標取得手段は、高精度の距離マップを用いて、前記球形状の立体表面におけるそれぞれの計測点からの距離が前記球形状半径であるカメラ座標系の座標を求め、高精度にカメラ特徴点座標を取得することができる。そして、キャリブレーション装置は、距離カメラパラメータ推定手段によって、前記特徴点座標取得手段により取得したカメラ特徴点座標に基づいて、距離カメラパラメータを推定する。

また、本発明では、平面板に対して予め定められた４以上のキャリブレーションを行うための特徴点のそれぞれに対して、前記特徴点を中心点とする半径が予め定められた球形状半径である球面の全体またはその一部である球形状が配置され、さらに、前記平面板に対して予め定められた７以上のキャリブレーションを行うため小形状体中心のそれぞれに対して、前記小形状体中心を中心点とする半径が前記球形状半径よりも小さい球の全体またはその一部であり、所定色を有し、前記球形状とは異なる小形状体が配置されるキャリブレーションパターンを撮像する距離カメラが取得した計測点の距離マップに基づいて、距離カメラパラメータを推定し、さらに、前記キャリブレーションパターンを撮像するカラーカメラが取得したカラー画像に基づいて、カラーカメラパラメータを推定するキャリブレーション装置であって、特徴点座標取得手段と、距離カメラパラメータ推定手段と、カラーカメラ座標取得手段と、カラーカメラパラメータ推定手段と、を備える構成とした。

かかる構成において、キャリブレーション装置は、特徴点座標取得手段によって、前記球形状の立体表面におけるそれぞれの計測点からの距離が前記球形状半径であるカメラ座標系の座標を、前記特徴点のカメラ座標系における座標であるカメラ特徴点座標として取得する。このように、特徴点座標取得手段は、高精度の距離マップを用いて、前記球形状の立体表面におけるそれぞれの計測点からの距離が前記球形状半径であるカメラ座標系の座標を求め、高精度にカメラ特徴点座標を取得することができる。そして、キャリブレーション装置は、距離カメラパラメータ推定手段によって、前記特徴点座標取得手段により取得したカメラ特徴点座標に基づいて、距離カメラパラメータを推定する。

さらに、キャリブレーション装置は、カラーカメラ座標取得手段によって、前記カラー画像に基づいて、前記小形状体の所定色を検出することにより、前記小形状体中心のカラーカメラ座標系における座標を、カラーカメラ座標として取得する。このように、カラーカメラ座標取得手段は、高精度のカラー画像を用いて、前記小形状体の所定色を検出し、高精度にカラーカメラ座標を取得することができる。そして、キャリブレーション装置は、カラーカメラパラメータ推定手段によって、前記カラーカメラ座標取得手段が取得したカラーカメラ座標に基づいて、カラーカメラパラメータを推定する。

また、本発明に係るキャリブレーション装置は、ＣＰＵ（Central Processing Unit）、記憶手段（例えば、メモリ、ハードディスク）等のハードウェア資源を備えるコンピュータを、前記した各手段として協調動作させるための、それぞれキャリブレーションプログラムによって実現することもできる。このプログラムは、通信回線を介して配布してもよく、光ディスクや磁気ディスク、フラッシュメモリ等の記録媒体に書き込んで配布してもよい。

本発明によれば、キャリブレーション装置は、特徴点座標取得手段によって、高精度にカメラ特徴点座標を取得することができるため、距離カメラのカメラパラメータを高精度に取得することができる。これにより、共通の座標である世界座標において、距離カメラと、他の距離カメラのカメラパラメータを高精度に取得することができる。

また、本発明によれば、キャリブレーション装置は、さらに、カラーカメラ座標取得手段によって、高精度にカラーカメラ座標を取得することができるため、カラーカメラのカメラパラメータを高精度に取得することができる。これにより、共通の座標である世界座標において、距離カメラと、他のカラーカメラのカメラパラメータを高精度に取得することができる。

本発明の第１の実施形態の概要を説明するための説明図である。 本発明の第１の実施形態におけるキャリブレーションパターンの一例を示す図である。（ａ）は、正面図である。（ｂ）は、側面図である。 本発明の第１の実施形態における世界座標系の一例を示す図である。（ａ）は、正面図である。（ｂ）は、側面図である。 本発明の第１の実施形態におけるカメラ座標系の一例を示す図である。 本発明の第１の実施形態におけるキャリブレーション装置の構成を示すブロック図である。 本発明の第１の実施形態における半球中心座標取得手段の動作を説明するための図である。 本発明の第１の実施形態におけるキャリブレーション装置の動作を示すフローチャートである。 本発明の第２の実施形態の概要を説明するための説明図である。 本発明の第２の実施形態におけるキャリブレーションパターンの一例を示す図である。（ａ）は、正面図である。（ｂ）は、側面図である。 本発明の第２の実施形態における世界座標系の一例を示す図である。（ａ）は、正面図である。（ｂ）は、側面図である。 本発明の第２の実施形態におけるカラーカメラ座標系の一例を示す図である。 本発明の第２の実施形態におけるキャリブレーション装置の構成を示すブロック図である。 本発明の第２の実施形態におけるキャリブレーション装置の動作を示すフローチャートである。

以降、本発明を実施するための形態を、各図を参照して詳細に説明する。
（第１の実施形態）
［第１の実施形態の概要］
最初に、図１を参照して、本発明の第１の実施形態の概要について説明する。

キャリブレーションパターン１００は、平面板１０１に対して予め定められた４以上のキャリブレーションを行うための特徴点のそれぞれに対して、前記特徴点を中心点とする半径が予め定められた球形状半径Ｒである球面の全体またはその一部である球形状が配置されるものである。ここで、球形状半径Ｒは正であり、球形状の立体表面の面積は０ではない。なお、キャリブレーションパターン１００は、地面等に固定されて設置されている。また、後記するように、キャリブレーションを行うためには、４以上の特徴点が必要である。

距離カメラ２₁および距離カメラ２₂は、それぞれキャリブレーション装置１にケーブル等で接続されており、キャリブレーションパターン１００を撮像し、それぞれ計測点の距離マップを取得し、取得した距離マップをキャリブレーション装置１に出力するものである。以下、距離カメラ２₁および距離カメラ２₂を、特に区別しないときは、単に、距離カメラ２と記載する。ここで、距離マップは、距離カメラ２の不図示の撮像画面上に２次元に配列された画素ごとに、不図示の光学主点Ｏｃ（図６参照）から、当該光学主点Ｏｃと当該画素を結ぶ直線上の計測点へ伸びるベクトルと、撮像画面の単位法線ベクトルとの内積値を奥行き情報として示すものである。なお、距離カメラ２₁および距離カメラ２₂は、キャリブレーションパターン１００に対して固定され設置されている。また、３台以上の距離カメラ２が、キャリブレーションパターン１００を撮像し、キャリブレーション装置１に接続されていてもよい。

キャリブレーション装置１は、距離カメラ２₁および距離カメラ２₂が取得した計測点の距離マップに基づいて、距離カメラ２₁および距離カメラ２₂の距離カメラパラメータを推定し、推定した距離カメラパラメータを外部に出力するものである。ここで、距離カメラ２の距離カメラパラメータとは、後記するように、距離カメラ２のキャリブレーションパターン１００に対する設置姿勢および設置位置を示すパラメータである。

次に、図２を参照して、キャリブレーションパターン１００の一例について説明する。
キャリブレーションパターン１００は、２４個の特徴点が平面板１０１上に配置され、その特徴点のそれぞれに対して配置される球形状が、当該特徴点を中心点とする半径Ｒの半球であるように構成されている。なお、キャリブレーションパターン１００では、平面板１０１は、略正方形であるが、平面板１０１は底辺と高さが異なる長方形でもよいし、その他の形状でもよい。また、球形状が半球であるため、球形状を平面板１０１上に安定的に配置し、かつ、球形状の立体表面の面積を広くすることができる。これにより、距離カメラ２は、球形状の立体表面からより多くの計測点の距離マップを取得することができる。また、球形状が平面板１０１に固定されることで、キャリブレーションパターン１００は形がくずれず、長期間使用することができる。

ここで、平面板１０１上の予め定められた基準点から平面板１０１上の底辺方向に間隔Ｌのｍ倍であるｍ・Ｌだけ離れた点から、さらに、平面板１０１の高さ方向に間隔Ｌのｎ倍であるｎ・Ｌだけ離れた点を、格子点Ｐ（ｍ，ｎ）とする（ただし、ｍおよびｎは整数である）。ここで、平面板１０１は、ｍおよびｎが、−２以上で２以下である２５個の格子点Ｐ（ｍ，ｎ）を含む。なお、基準点は、格子点Ｐ（０，０）と一致する。
なお、キャリブレーションパターン１００では、平面板１０１上の底辺方向の間隔と、平面板１０１上の高さ方向の間隔は等しいが、異なっていてもよい。

また、ｍおよびｎは−２以上で２以下の整数であり、かつ、（ｍ，ｎ）＝（１，１）ではない２４個の格子点Ｐ（ｍ，ｎ）が、特徴点１０２₁〜１０２₂₄として予め定められる。ここで、格子点Ｐ（０，０）は特徴点１０２₁として定められる。なお、特徴点１０２₁は、２５個の格子点Ｐ（ｍ，ｎ）の全体の重心である格子点中心と一致する。
そして、格子点Ｐ（１，１）は、特徴点ではない格子点である除去点１０２₂₅として定められる。

なお、キャリブレーションパターン１００では、除去点は単数であるが、除去点は複数であってもよい。キャリブレーションパターン１００は、特徴点が、平面板１０１上の格子点であり、特徴点でない格子点である除去点全体の重心である除去点中心と格子点全体の重心である格子点中心とが一致しないように構成されていればよい。

以下、特徴点の総数をｓとする（キャリブレーションパターン１００では、ｓは、２４である）。また、特徴点１０２₁〜１０２₂₄を特に区別しないときは、単に特徴点１０２と記載する。そして、格子点１０２₁〜１０２₂₅を特に区別しないときは、単に格子点１０２と記載する。

ここで、特徴点１０２_iに対して、当該特徴点１０２_iを中心点とする半径Ｒの半球１０３_iが平面板１０１上に配置されている（ただし、ｉは、１以上２４以下の整数とする）。ここで、半径Ｒは、Ｌ／２より小さい。以下、半球１０３₁〜１０３₂₄を特に区別しないときは、単に半球１０３と記載する。

次に、図３を参照して、第１の実施形態における世界座標系の一例について説明する。
図３に示す世界座標系は、キャリブレーションパターン１００に対して、以下のように定められている。図３に示す世界座標系の原点は、平面板１０１の格子点中心であり、半球１０３₁の中心点１０２₁と一致する。Ｘ軸は、平面板１０１の底辺方向と平行であり、Ｙ軸は、平面板１０１の高さ方向と平行であり、Ｚ軸は、平面板１０１に垂直であり、半球１０３上の点のＺ座標は正である。以下、世界座標系における座標を世界座標と呼ぶ。

図３に示すように、平面板１０１上の点Ｐの世界座標は、（Ｘ，Ｙ，０）と表される。
また、格子点１０２_iの世界座標系における世界座標Ｐ_w（ｉ）は、次の式（１）で表される。ここで、格子点１０２_iは、格子点Ｐ（ｍ（ｉ），ｎ（ｉ））であるとして、ｍ（ｉ）およびｎ（ｉ）は、−２以上２以下の整数であるとする。ただし、ｉは、１以上２５以下の整数とする。

次に、図４を参照して、第１の実施形態におけるカメラ座標系の一例について説明する。
図４に示すように、距離カメラ２₁および距離カメラ２₂は、世界座標系において固定されており、それぞれの画角内にキャリブレーションパターン１００を撮像する。なお、距離カメラ２₁の不図示の光学主点Ｏｃ₁および距離カメラ２₂の不図示の光学主点Ｏｃ₂の世界座標系におけるＺ座標は、正である。光学主点Ｏｃ₁および光学主点Ｏｃ₂を特に区別しないときは、単に光学主点Ｏｃと記載する。

距離カメラ２のカメラ座標系の原点は、距離カメラ２の光学主点Ｏｃである。距離カメラ２₁のカメラ座標系のＺ_d1軸は、距離カメラ２₁の光軸であり、距離カメラ２₂のカメラ座標系のＺ_d2軸は、距離カメラ２₂の光軸である。ここで、Ｚ_d1軸およびＺ_d2軸を特に区別しないときは、Ｚ_d軸と記載する。距離カメラ２₁のＸ_d1軸は、距離カメラ２₁の不図示の撮像画面に２次元に画素が配列される水平方向に平行であり、距離カメラ２₂のＸ_d2軸は、距離カメラ２₂の不図示の撮像画面に２次元に画素が配列される水平方向に平行である。ここで、Ｘ_d1軸およびＸ_d2軸を特に区別しないときは、Ｘ_d軸と記載する。距離カメラ２₁のＹ_d1軸は、距離カメラ２₁の不図示の撮像画面に２次元に画素が配列される垂直方向に平行であり、距離カメラ２₂のＹ_d2軸は、距離カメラ２₂の不図示の撮像画面に２次元に画素が配列される垂直方向に平行である。ここで、Ｙ_d1軸およびＹ_d2軸を特に区別しないときは、Ｙ_d軸と記載する。以下、距離カメラ２のカメラ座標系における座標をカメラ座標と呼ぶ。

ここで、回転行列Ｒ_d1を距離カメラ２₁の姿勢を示し、回転行列Ｒ_d2を、距離カメラ２₂の姿勢を示すものとする。以下、回転行列Ｒ_d1および回転行列Ｒ_d2を、特に区別しないときは、単に回転行列Ｒ_dと記載する。具体的には、回転行列Ｒ_dは、世界座標系のＸ軸の周りのピッチ角φの回転を示す回転行列Ｒ_d（φ）、世界座標系のＹ軸の周りのヨー角θの回転を示す回転行列Ｒ_d（θ）、および、世界座標系のＺ軸の周りのチルト角ψの回転を示す回転行列Ｒ_d（ψ）を用いて、以下の式（２）により表される（非特許文献１参照）。

また、並進ベクトルＴ_d1は、距離カメラ２₁のカメラ座標系の原点である光学主点Ｏｃ₁を始点として、特徴点１０２₁と一致する世界座標系の原点を終点とするベクトルであり、距離カメラ２₁の位置を表すものとする。そして、並進ベクトルＴ_d2は、距離カメラ２₂のカメラ座標系の原点である光学主点Ｏｃ₂を始点として、特徴点１０２₁と一致する世界座標系の原点を終点とするベクトルであり、距離カメラ２₂の位置を表すものとする。以下、並進ベクトルＴ_d1および並進ベクトルＴ_d2を、特に区別しないときは、単に並進ベクトルＴ_dと記載する。
以下、距離カメラ２のカメラ座標における回転行列Ｒ_dおよび並進ベクトルＴ_dを距離カメラパラメータと呼ぶ。
このとき、世界座標がＰ_wである点Ｐの、距離カメラ２₁のカメラ座標系におけるカメラ座標がＰ_d1であり、距離カメラ２₂のカメラ座標系におけるカメラ座標がＰ_d2であるとき、次の式（３）が成り立つ。

ここで、４以上の個数の特徴点１０２のカメラ座標および世界座標が既知である場合、上記の式（３）により、回転行列Ｒ_dおよび並進ベクトルＴ_dを求めることができる。そして、任意の点Ｐのカメラ座標Ｐ_dから、その世界座標Ｐ_wを、下記の式（４）により求めることができる。これにより、距離カメラ２₁における世界座標Ｐ_d1を世界座標Ｐ_w1に変換し、距離カメラ２₂における世界座標Ｐ_d2を世界座標Ｐ_w2に変換して、共通の世界座標系において、世界座標Ｐ_w1および世界座標Ｐ_w2を扱うことができる。

［第１の実施形態のキャリブレーション装置の構成］
次に、図５を参照して、第１の実施形態におけるキャリブレーション装置１の構成について説明する。
キャリブレーション装置１は、半球中心座標取得手段（特徴点座標取得手段）１０と、距離カメラパラメータ推定手段２０とを備える。なお、キャリブレーション装置１は、２つの半球中心座標取得手段１０と、２つの距離カメラパラメータ推定手段２０とを備える。

半球中心座標取得手段（特徴点座標取得手段）１０は、それぞれの球形状の立体表面における計測点からの距離が球形状半径Ｒであるカメラ座標系の座標を、特徴点１０２のカメラ座標系における座標であるカメラ中心座標（カメラ特徴点座標）として取得するものである。
半球中心座標取得手段（特徴点座標取得手段）１０は、距離マップ取得手段１１と、投票手段１２と、上位抽出手段１３とを備える。なお、一方の半球中心座標取得手段１０は、距離カメラ２₁が取得した計測点の距離マップを取得し、他方の半球中心座標取得手段１０は、距離カメラ２₂が取得した計測点の距離マップを取得する。

距離マップ取得手段１１は、距離カメラ２が取得した計測点の距離マップを、ケーブル等を介して取得するものである。なお、計測点は、キャリブレーションパターン１００の立体表面上の計測点に限られない。そして、距離マップを用いて、各計測点のカメラ座標を算出することができるため、以下、計測点の距離マップを、計測点のカメラ座標として扱う。

投票手段１２は、距離マップ取得手段１１が取得した計測点の距離マップに基づいて、それぞれの計測点からの距離が半球の半径（球形状半径）Ｒであるカメラ座標点に投票するものである。具体的には、投票手段１２は、各計測点を中心点とする半径Ｒの球面と交わる各カメラ座標単位内の、その球面に最も近いカメラ座標点に投票する。ここで、カメラ座標点とは、カメラ座標系のＸ_d軸、Ｙ_d軸、およびＺ_d軸方向に、所定のピッチ間隔で３次元に配列されたカメラ座標であり、カメラ座標単位は、カメラ座標点を頂点とする最小単位の立方体である。

図６に示すように、半球１０３上の各計測点を中心とする半径Ｒの球は、半球１０３の中心点１０２を含む。よって、上記の半球１０３の中心点１０２と考えられるカメラ座標点には、投票が集中する。これにより、全てのカメラ座標点の中で投票数が、上位から特徴点１０２の総数個であるｓ個までのカメラ座標点は、半球１０３₁〜１０３₂₄の中心点１０２₁〜１０２₂₄のカメラ座標であると考えられる。

上位抽出手段１３は、カメラ座標点の投票数が上位から特徴点１０２の総数個であるｓ個までのカメラ座標点を抽出するものである。そして、上位抽出手段１３は、さらに、抽出したカメラ特徴点座標に基づいて、除去点のカメラ座標を算出し、算出した除去点のカメラ座標に基づいて、格子点中心および除去点中心のカメラ座標を算出し、算出した格子点中心および除去点中心のカメラ座標に基づいて、特徴点１０２全体の格子点中心に関する回転方向を検出することにより、抽出したカメラ特徴点座標と、特徴点１０２を対応付けるものである。

例えば、上位抽出手段１３は、抽出した特徴点１０２の総数個であるｓ個のカメラ中心座標に基づいて、除去点１０２₂₅のカメラ座標を算出する。ここで、除去点１０２₂₅は、いずれか２つの特徴点１０２の中点と一致する。これにより、上位抽出手段１３は、算出した特徴点１０２のカメラ座標のうち、当該２つの特徴点１０２のカメラ座標の平均を算出することにより、除去点１０２₂₅のカメラ座標を算出することができる。そして、上位抽出手段１３は、当該算出した除去点１０２₂₅のカメラ座標に基づいて、特徴点１０２全体の格子点中心１０２₁に関する回転方向を検出することにより、抽出したカメラ中心座標と、特徴点１０２₁〜１０２₂₄とを対応付ける。

なお、除去点が複数である場合にも、上位抽出手段１３は、抽出したカメラ特徴点座標に基づいて、除去点のカメラ座標を算出する。ここで、除去点は、いずれか２つの特徴点の中点などの内分点や、いずれか２つの特徴点の外分点と一致する。これにより、上位抽出手段１３は、算出した特徴点１０２のカメラ座標のうち、当該２つの特徴点１０２のカメラ座標から、除去点のカメラ座標を算出することができる。そして、上位抽出手段１３は、算出した除去点のカメラ座標に基づいて、格子点中心および除去点中心のカメラ座標を算出する。さらに、上位抽出手段１３は、当該算出した格子点中心および除去点中心のカメラ座標に基づいて、前記特徴点全体の前記格子点中心に関する回転方向を検出することにより、抽出したカメラ特徴点座標と、特徴点１０２とを対応付ける。これにより、上位抽出手段１３は、抽出したカメラ特徴点座標と、特徴点１０２とを、正確に対応付けることができる。

以上のように、上位抽出手段１３は、抽出した特徴点１０２の総数個であるｓ個のカメラ座標点を、半球１０３₁の中心点１０２_iと対応付けて、対応付けたカメラ座標点を、半球１０３₁の中心点１０２_iのカメラ座標であるカメラ中心座標Ｐ_d（ｉ）として取得する（ただし、ｉは１以上２４以下の整数である）。

このように、投票手段１２は、距離マップ取得手段１１が取得した計測点からの距離が球形状半径Ｒであるカメラ座標点に投票するので、特徴点１０２のカメラ座標の投票数は、他のカメラ座標よりも高くなる。これにより、上位抽出手段１３は、特徴点１０２のカメラ座標を、カメラ特徴点座標として、高精度に抽出することができる。
また、球形状が半球であるため、球形状の立体表面の面積を広くすることができる。よって、距離マップ取得手段１１は、球形状の立体表面からより多くの計測点の距離マップを取得することができる。これにより、特徴点１０２のカメラ座標の投票数は、他のカメラ座標よりもより高くなる。

また、距離マップ取得手段１１は、取得した計測点の距離マップに基づいて、平面板１０１の境界を検出することにより、キャリブレーションパターン１００の立体表面における計測点を選定してもよい。具体的には、距離マップ取得手段１１は、公知のハフ変換や一般化ハフ変換を用いて、平面板１０１の境界に含まれる直線等の形状を検出する。そして、距離マップ取得手段１１は、検出した境界に囲まれる立体表面上の計測点を選定する。
そして、投票手段１２は、距離マップ取得手段１１が選定した計測点からの距離が半径（球形状半径）Ｒであるカメラ座標点に投票する。

このように、距離マップ取得手段１１は、キャリブレーションパターン１００の立体表面のおける計測点を選定するので、投票手段１２が投票を行う計測点を限定することができる。これにより、投票手段１２が処理を行うために使用するメモリ等の容量を低減することができる。

以上により、半球中心座標取得手段１０は、距離カメラ２が出力した計測点の距離マップに基づいて、それぞれの半球１０３_iの立体表面における計測点からの距離が半球１０３の半径Ｒであるカメラ座標系の座標を、半球１０３_iの中心点である特徴点１０２_iのカメラ座標系における座標であるカメラ中心座標Ｐ_d（ｉ）として取得し、取得したカメラ中心座標Ｐ_d（ｉ）を出力する（ただし、ｉは１以上２４以下の整数である）。

距離カメラパラメータ推定手段２０は、半球中心座標取得手段（特徴点座標取得手段）１０が取得したカメラ中心座標（カメラ特徴点座標）に基づいて、距離カメラパラメータを推定するものである。なお、一方の距離カメラパラメータ推定手段２０は、一方の半球中心座標取得手段１０からカメラ中心座標を取得し、他方の距離カメラパラメータ推定手段２０は、他方の半球中心座標取得手段１０からカメラ中心座標を取得する。
距離カメラパラメータ推定手段２０は、世界中心座標記憶手段（世界特徴点座標記憶手段）２１と、距離カメラパラメータ最小二乗算出手段２２とを備える。

世界中心座標記憶手段（世界特徴点座標記憶手段）２１は、特徴点１０２の世界座標系における座標を、世界中心座標（世界特徴点座標）として記憶するものである。
具体的には、世界中心座標記憶手段２１は、半球１０３_iの中心点（特徴点）１０２_iの世界座標Ｐ_w（ｉ）（ただし、ｉは１以上２４以下の整数である）を、世界中心座標（世界特徴点座標）として記憶する。また、半球１０３の中心点１０２全体は、同一平面である平面板１０１上にあるため、世界特徴点座標Ｐ_w（ｉ）（ただし、ｉは１以上２４以下の整数である）は、Ｚ座標が０であるという拘束条件を有する。これにより、距離カメラパラメータ推定手段２０は、高精度に距離カメラパラメータを算出することができる。

距離カメラパラメータ最小二乗算出手段２２は、特徴点１０２のカメラ中心座標（カメラ特徴点座標）および世界中心座標（世界特徴点座標）により定まる距離カメラ評価関数を最小化する距離カメラパラメータを最小二乗法で算出するものである。
具体的には、下記の式（５）で、距離カメラ評価関数Ｉ（Ｒ_d，Ｔ_d）を定義する。ここで、｜｜は、ユークリッド距離を示す。なお、距離カメラ評価関数Ｉ（Ｒ_d，Ｔ_d）は、半球１０３_iの中心点１０２_iのカメラ中心座標Ｐ_d（ｉ）および世界座標Ｐ_w（ｉ）により定まる（ただし、ｉは１以上２４以下の整数である）。

ここで、式（２）により、距離カメラ評価関数Ｉ（Ｒ_d，Ｔ_d）を、（φ，θ，ψ，ｔ１，ｔ２，ｔ３）の関数Ｉ（φ，θ，ψ，ｔ１，ｔ２，ｔ３）として扱うことができる。ここで、ｔ１はＴ_dのＸ成分であり、ｔ２はＴ_dのＹ成分であり、ｔ３はＴ_dのＺ成分である。
距離カメラパラメータ最小二乗算出手段２２は、この関数Ｉ（φ，θ，ψ，ｔ１，ｔ２，ｔ３）を最小化する（φ，θ，ψ，ｔ１，ｔ２，ｔ３）を公知の非線形最小二乗法により算出する。そして、距離カメラ評価関数Ｉ（Ｒ_d，Ｔ_d）を最小化する距離カメラパラメータである回転行列Ｒ_dおよび並進ベクトルＴ_dを算出する。

このように、距離カメラパラメータ最小二乗算出手段２２は、特徴点１０２のカメラ特徴点座標から、高精度に距離カメラパラメータを算出することができる。

以上により、距離カメラパラメータ推定手段２０は、半球中心座標取得手段１０が取得した、半球１０３_iの中心点１０２_iのカメラ座標であるカメラ中心座標Ｐ_d（ｉ）に基づいて（ただし、ｉは１以上２４以下の整数である）、距離カメラパラメータを算出し、算出した距離カメラパラメータを出力する。

以上のように、キャリブレーション装置１は、半球中心座標取得手段１０によって、高精度にカメラ特徴点座標を取得することができる。これにより、共通の座標である世界座標において、距離カメラ２₁および距離カメラ２₂の距離カメラパラメータを高精度に取得することができる。そして、距離カメラ２₁および距離カメラ２₂で取得された距離マップを、世界座標系に変換して扱うことにより、それぞれの距離マップから得られた情報を統合することができる。さらに、例えば、それぞれの距離マップから得られた被写体の形状に関する情報を改善することができる。

［第１の実施形態のキャリブレーション装置の動作］
次に、図７を参照して、第１の実施形態におけるキャリブレーション装置１の動作を説明する。
距離カメラ２は、キャリブレーションパターン１００を撮像する。距離カメラ２は、計測点の距離マップを取得し、取得した計測点の距離マップをキャリブレーション装置１に出力する。キャリブレーション装置１は、計測点の距離マップが入力されると、以下の動作を開始する。

ステップＳ１１において、キャリブレーション装置１は、半球中心座標取得手段１０において、距離マップ取得手段１１によって、距離カメラ２が出力した計測点の距離マップを取得する。この距離マップ取得手段１１によって取得された計測点の距離マップは、不図示のメモリ等によって記憶される。

ステップＳ１２において、キャリブレーション装置１は、半球中心座標取得手段１０において、投票手段１２によって、不図示のメモリ等によって記憶された計測点の距離マップを参照して、当該計測点の距離マップに基づいて、それぞれの計測点から距離が半径Ｒであるカメラ座標点に投票する。この投票手段１２によって取得されたカメラ座標点の投票数は、不図示のメモリ等によって記憶される。具体的には、投票手段１２は、各計測点を中心点とする半径Ｒの球面と交わる各カメラ座標単位内の、その球面に最も近いカメラ座標点に投票する。

ステップＳ１３において、キャリブレーション装置１は、半球中心座標取得手段１０において、上位抽出手段１３によって、不図示のメモリ等によって記憶された計測点のカメラ座標点の投票数を参照して、カメラ座標点の投票数が上位から特徴点の総数個であるｓ個までのカメラ座標点を、カメラ特徴点座標として抽出する。

ステップＳ１４において、キャリブレーション装置１は、半球中心座標取得手段１０において、上位抽出手段１３によって抽出されたｓ個のカメラ座標点を、半球１０３_iの中心点である特徴点１０２_iと対応付け、カメラ中心座標Ｐ_d（ｉ）として取得する（ここで、ｉは１以上２４以下の整数である）。キャリブレーション装置１は、半球中心座標取得手段１０において、上位抽出手段１３によって取得したカメラ中心座標Ｐ_d（ｉ）（ここで、ｉは１以上２４以下の整数である）を不図示のメモリ等に記憶する。

ステップＳ１５において、キャリブレーション装置１は、距離カメラパラメータ推定手段２０において、世界中心座標記憶手段２１に予め記憶しておいたｓ個の世界中心座標Ｐ_w（ｉ）、および、不図示のメモリに記憶されたカメラ中心座標Ｐ_d（ｉ）（ただし、ｉは１以上２４以下の整数である）を参照する。キャリブレーション装置１は、距離カメラパラメータ推定手段２０において、式（５）で定義される距離カメラ評価関数Ｉ（Ｒ_d，Ｔ_d）を最小化する、距離カメラパラメータである回転行列Ｒ_dおよび並進ベクトルＴ_dを、公知の非線形最小二乗法により算出する。そして、キャリブレーション装置１は、距離カメラパラメータ推定手段２０において、距離カメラパラメータ最小二乗算出手段２２によって、算出した距離カメラパラメータを出力する。
ステップＳ１５の処理の終了後、キャリブレーション装置１は、動作を終了する。

（第２の実施形態）
［第２の実施形態の概要］
最初に、図８を参照して、本発明の第２の実施形態の概要について説明する。
キャリブレーションパターン１００Ａは、平面板１０１に対して予め定められた４以上のキャリブレーションを行うための特徴点のそれぞれに対して、この特徴点を中心点とする半径が予め定められた球形状半径Ｒである球面の全体またはその一部である球形状が配置され、さらに、平面板１０１に対して予め定められた７以上のキャリブレーションを行うため小形状体中心のそれぞれに対して、この小形状体中心を中心点とする半径が球形状半径Ｒよりも小さい半径ｒの球の全体またはその一部であり、所定色を有し、球形状とは異なる小形状体が配置されるものである。ここで、球形状半径Ｒは、小形状体の半径ｒより大であり、小形状体の半径ｒは正である。また、球形状の立体表面の面積は０ではなく、小形状体の立体表面の面積も０ではない。なお、キャリブレーションパターン１００Ａは、地面等に固定されて設置されている。また、後記するように、キャリブレーションを行うためには、４以上の特徴点および７以上の小形状体中心が必要である。

距離カメラ２は、それぞれキャリブレーション装置１Ａにケーブル等で接続されており、キャリブレーションパターン１００Ａを撮像し、計測点の距離マップを取得し、取得した距離マップをキャリブレーション装置１Ａに出力するものである。なお、距離カメラ２は、キャリブレーションパターン１００Ａに対して固定されている。また、２台以上の距離カメラ２が、キャリブレーションパターン１００を撮像し、キャリブレーション装置１に接続されていてもよい。

カラーカメラ３は、キャリブレーション装置１Ａにケーブル等で接続されており、キャリブレーションパターン１００Ａを撮像し、計測点のカラー画像を取得し、取得したカラー画像をキャリブレーション装置１Ａに出力するものである。ここで、カラーカメラ３は、例えばＣＣＤやＣＭＯＳ素子を用いたカメラである。また、カラー画像は、撮像画面の予め定められた画像座標系の２次元座標に対して、画素値を示すものである。なお、カラーカメラ３は、キャリブレーションパターン１００Ａに対して固定されている。また、２台以上の距離カメラ２が、キャリブレーションパターン１００を撮像し、キャリブレーション装置１に接続されていてもよい。

キャリブレーション装置１Ａは、距離カメラ２が取得した計測点の距離マップに基づいて、距離カメラパラメータを推定し、さらに、カラーカメラ３が取得したカラー画像に基づいて、カラーカメラパラメータを推定し、推定した距離カメラパラメータおよびカラーカメラパラメータを外部に出力するものである。なお、カラーカメラパラメータは、後記するように、カラーカメラ３の設置姿勢、設置位置や焦点距離等である。

次に、図９を参照して、第２の実施形態におけるキャリブレーションパターン１００Ａの一例を説明する。図２と同一の要素には同一の符号を付与し、説明を省略することもある。

キャリブレーションパターン１００Ａは、キャリブレーションパターン１００において、さらに、半径Ｒの半球１０３_iの頂点上に、小形状体として半径ｒの小球１０５_iが配置され、小形状体中心は、小球１０５_iの中心点１０４_iであるように構成されている（ただし、ｉは１以上２４以下の整数である）。ここで、ｒは、Ｒより小である。
カラー格子点１０４_iは、小球１０５の中心点（小形状体中心）１０４₁〜１０４₂₄全体と同一平面上にあり、かつ、平面板１０１に垂直な格子点１０２_iを通る直線上にある点である（ただし、ｉは１以上２５以下の整数である）。カラー除去点１０４₂₅は、小球１０５の中心点１０４ではないカラー格子点である。カラー格子点中心は、カラー格子点全体の重心であり、キャリブレーションパターン１００Ａでは、カラー格子点１０４₁と一致する。カラー除去点中心は、カラー除去点全体の重心であり、キャリブレーションパターン１００Ａでは、カラー除去点１０４₂₅である。

次に、図１０を参照して、第２の実施形態における世界座標系の一例を説明する。
図１０に示す世界座標系は、図２に示す世界座標系と同様に定められている。
図１０に示すように、小球１０５の中心点１０４₁〜１０４₂₄全体と同一平面上にある点Ｐの世界座標は、（Ｘ，Ｙ，Ｒ＋ｒ）と表される。
小球１０５_iの中心点の世界座標ｐ_w（ｉ）は、次の式（６）で表される（ただし、ｉは１以上２４以下の整数である）。

次に、図１１を参照して、第２の実施形態におけるカラーカメラ座標系の一例を説明する。
図１１に示すように、距離カメラ２およびカラーカメラ３は、世界座標系において固定されており、それぞれの画角内にキャリブレーションパターン１００Ａを撮像する。
距離カメラ２のカメラ座標系は、図３に示すカメラ座標と同様に定められている。
カラーカメラ３のカラーカメラ座標系は、以下のように定められる。カラーカメラ座標系の原点は、不図示のカラーカメラ３の光学主点であり、Ｚ_c軸は、カラーカメラ３の光軸であり、Ｘ_c軸は、カラーカメラ３の不図示の撮像画面に２次元に画素が配列される水平方向に平行とし、Ｙ_c軸は、カラーカメラ３の不図示の撮像画面に２次元に画素が配列される垂直方向に平行とする。任意のＰの上記のＸ_cＹ_cＺ_c座標系における座標をＰ_cとする。このとき、上記の点Ｐのカラーカメラ座標系における座標であるカラーカメラ座標ｐ_cは、次の式（７）で示される。

ここで、内部パラメータ行列Ｆ_cは、下記の式（８）で示される。ただし、ａはカラー画像の画素のアスペクト比であり、Ｆは、画素数で表された焦点距離であり、（Ｃ_u，Ｃ_v）は、カラー画像の画像座標系における光軸の画像座標である。

ここで、ｚを計測点Ｐの上記のＸ_cＹ_cＺ_c座標系におけるＺ_c成分として、カラー画像の撮像画面の画像座標系における２次元座標を（ｕ，ｖ）とする。このとき、計測点のカラーカメラ座標ｐ_cは、次の式（９）で表される。

ここで、回転行列Ｒ_cを、上記のＸ_cＹ_cＺ_c座標系におけるカラーカメラ３の姿勢を表すものとする。具体的には、回転行列Ｒ_cは、世界座標系のＸ軸の周りのピッチ角φの回転を示す回転行列Ｒ_c（φ）、世界座標系のＹ軸の周りのヨー角θの回転を示す回転行列Ｒ_c（θ）、および、世界座標系のＺ軸の周りのチルト角ψの回転を示す回転行列Ｒ_c（ψ）を用いて、以下の式（１０）により表される（非特許文献１参照）。

また、並進ベクトルＴ_cは、カラーカメラ３の不図示の光学主点を始点として、特徴点１０２₁と一致する世界座標系の原点を終点とするベクトルであり、カラーカメラ３の位置を表すものとする。このとき、次の式（１１）が成り立つ。以下、回転行列Ｒ_cおよび並進ベクトルＴ_cを外部パラメータとし、内部パラメータ行列Ｆ_cを内部パラメータとし、外部パラメータおよび内部パラメータをカラーカメラパラメータとする。

例えば、７個以上の特徴点１０２のカラーカメラ座標が既知であれば、上記の式（１１）により、外部パラメータおよび内部パラメータを求めることができる（非特許文献１参照）。そして、任意の点Ｐのカラーカメラ座標ｐ_cから、その点Ｐの世界座標ｐ_wを、上記の式（１１）により求めることができる。そして、任意の点Ｐのカラーカメラ座標ｐ_cを、世界座標ｐ_wに変換して扱うことができる。
以下、カラーカメラ座標系における座標をカラーカメラ座標と呼ぶ。

［第２の実施形態のキャリブレーション装置の構成］
次に、図１２を参照して、第２の実施形態におけるキャリブレーション装置１Ａの構成を説明する。図５と同一の要素には同一の符号を付与し、説明を省略することもある。
キャリブレーション装置１Ａは、半球中心座標取得手段（特徴点座標取得手段）１０と、距離カメラパラメータ推定手段２０と、小球中心座標取得手段（カラーカメラ座標取得手段）３０と、カラーカメラパラメータ推定手段４０と、を備える。

半球中心座標取得手段１０が、距離マップ取得手段１１によって、距離マップを取得する計測点は、小球１０５が半球１０３上に配置されているため、半球１０３の立体表面全体に亘らない。しかし、小球１０５の半径ｒは、半球１０３の半径Ｒよりも小さいため、距離マップ取得手段１１は、半球１０３の立体表面から十分な数の計測点の距離マップを取得することができる。また、小球１０５の半径ｒは半球１０３の半径Ｒより小さいため、小球１０５の立体表面の計測点から距離がＲである点に、投票手段１２による投票が集中する虞はない。これにより、半球中心座標取得手段１０は、投票手段１２および上位抽出手段１３により、カメラ中心座標を取得することができる。

小球中心座標取得手段（カラーカメラ座標取得手段）３０は、カラー画像に基づいて、小球（小形状体）の所定色を検出することにより、小球の中心点（小形状体中心）のカラーカメラ座標系における座標を、カラーカメラ小球中心座標（カメラ小形状体中心座標）として取得するものである。小球中心座標取得手段（カラーカメラ座標取得手段）３０は、さらに、取得したカラーカメラ小球中心座標（カメラ小形状体中心座標）に基づいて、カラー除去点のカラーカメラ座標を算出し、算出した前記カラー除去点のカラーカメラ座標に基づいて、カラー格子点中心およびカラー除去点中心のカラーカメラ座標を算出し、算出したカラー格子点中心およびカラー除去点中心のカラーカメラ座標に基づいて、小形状体中心全体のカラー格子点中心に関する回転方向を検出することにより、取得したカラーカメラ小球中心座標（カメラ小形状体中心座標）と、小形状体中心と、を対応付けるものである。

具体的には、小球中心座標取得手段３０は、カラーカメラ３が取得したカラー画像を、ケーブル等を介して取得し、取得したカラー画像に基づいて、小球１０５の所定色を検出することにより、小球１０５の中心点１０４のカラーカメラ座標系における座標を、カラーカメラ小球中心座標として取得する。例えば、小球中心座標取得手段３０は、取得したカラー画像の計測点の輝度情報に基づいて、小球１０５の立体表面上の計測点の所定色を検出する。さらに、小球中心座標取得手段３０は、所定色を検出した計測点の輝度情報に基づいて、小球１０５の立体表面上の計測点のカラーカメラ座標を取得する。そして、小球中心座標取得手段３０は、小球１０５の中心点１０４のカラーカメラ座標を、カラーカメラ小球中心座標として取得する。このように、小形状体は、小形状体中心を中心点１０４とする小球１０５であるため、小球中心座標取得手段３０は、高精度にカラーカメラ座標を取得することができる。なお、小球中心座標取得手段３０は、小球１０５の立体表面上の計測点の所定色を検出する際に、さらに、円形状を検出する処理、カラー画像における小球１０５の周辺の画素値を参照する処理や、カラー格子点１０５の配置に関する情報を用いる処理を行ってもよい。これにより、小球中心座標取得手段３０は、小球１０５の立体表面上の計測点の所定色を正確に検出することができ、より高精度にカラーカメラ座標を取得することができる。

そして、小球中心座標取得手段３０は、さらに、取得したカラーカメラ小球中心座標に基づいて、カラー除去点１０４₂₅のカラーカメラ座標を算出する。ここで、カラー除去点１０４₂₅は、いずれか２つの小球１０５の中心点１０４の中点と一致する。これにより、小球中心座標取得手段３０は、算出した小球１０５の中心点１０４のカラーカメラ座標のうち、当該２つの小球１０５の中心点１０４のカラーカメラ座標から、カラー除去点１０４₂₅のカラーカメラ座標を算出することができる。そして、小球中心座標取得手段３０は、算出したカラー除去点１０４₂₅のカラーカメラ座標に基づいて、小球１０５の中心点１０４全体のカラー格子点中心（小球１０５₁の中心点１０４₁に一致する）に関する回転方向を検出することにより、抽出したカラーカメラ小球中心座標と、小球１０５の中心点１０４₁〜１０４₂₄とを対応付ける。

なお、カラー除去点が複数である場合にも、小球中心座標取得手段３０は、取得したカラーカメラ小球中心座標に基づいて、カラー除去点のカラーカメラ座標を算出する。ここで、カラー除去点は、いずれか２つの小球１０５の中心点１０４の中点などの内分点や、いずれか２つの小球１０５の中心点１０４の外分点と一致する。これにより、小球中心座標取得手段３０は、算出した小球１０５の中心点１０４のカラーカメラ座標のうち、当該２つの小球１０５の中心点１０４のカラーカメラ座標から、カラー除去点のカラーカメラ座標を算出することができる。そして、小球中心座標取得手段３０は、算出したカラー除去点のカラーカメラ座標に基づいて、カラー格子点中心およびカラー除去点中心のカラーカメラ座標を算出する。さらに、小球中心座標取得手段３０は、算出したカラー格子点中心およびカラー除去点中心のカラーカメラ座標に基づいて、小球１０５の中心点１０４全体のカラー格子点中心に関する回転方向を検出することにより、取得したカラーカメラ小球中心座標と、小球１０５の中心点１０４とを対応付ける。これにより、小球中心座標取得手段３０は、取得したカラーカメラ小球中心座標と、小球１０５の中心点１０４とを、正確に対応付けることができる。

以上により、小球中心座標取得手段３０は、取得したカラーカメラ小球中心座標を、小球（小形状体）１０５_iの中心点（小形状体中心）１０４_iと対応付けて、カラーカメラ小球中心座標ｐ_c（ｉ）として取得する（ただし、ｉは１以上２４以下の整数である）。

カラーカメラパラメータ推定手段４０は、小球中心座標取得手段（カラーカメラ座標取得手段）３０が取得したカラーカメラ座標に基づいて、カラーカメラパラメータを推定するものである。カラーカメラパラメータ推定手段４０は、世界小球中心座標記憶手段（世界カラーカメラ座標記憶手段）４１と、カラーカメラパラメータ最小二乗算出手段４２と、を備える。

世界小球中心座標記憶手段（世界カラーカメラ座標記憶手段）４１は、小形状体中心の世界座標系における座標を、世界小形状体中心座標として記憶するものである。
具体的には、世界小球中心座標記憶手段４１は、キャリブレーションパターン１００Ａの小球１０５_iの中心点１０４_iの世界座標ｐ_w（ｉ）全体を、世界小球中心座標（世界カラーカメラ座標）ｐ_w（ｉ）として記憶する（ただし、ｉは１以上２４以下の整数である）。また、小球１０５の中心点１０４全体は、同一平面上にあるため、世界特徴点座標Ｐ_w（ｉ）（ただし、ｉは１以上２４以下の整数である）は、Ｚ座標がＲ＋ｒであるという拘束条件を有する。これにより、カラーカメラパラメータ推定手段４０は、高精度にカラーカメラパラメータを算出することができる。

カラーカメラパラメータ最小二乗算出手段４２は、カラーカメラ小球中心座標（カメラ小形状体中心座標）および世界小球中心座標（世界小形状体中心座標）により定まるカラーカメラ評価関数を最小化するカラーカメラパラメータを最小二乗法で算出するものである。
具体的には、下記の式（１２）で、カラーカメラ評価関数Ｊ（Ｒ_c，Ｔ_c，Ｆ_c）を定義する。ここで、｜｜は、ユークリッド距離を示す。なお、距離カメラ評価関数Ｉ（Ｒ_d，Ｔ_d）は、小球１０５_iの中心点１０４_iのカラーカメラ座標であるカラーカメラ小球中心座標ｐ_c（ｉ）および世界小球中心座標ｐ_w（ｉ）により定まる（ただし、ｉは１以上２４以下の整数である）。

ここで、式（８）および式（１０）により、カラーカメラ評価関数Ｊ（Ｒ_c，Ｔ_c，Ｆ_c）を、（φ，θ，ψ，ｔ１，ｔ２，ｔ３，ａ，Ｆ，Ｃ_u，Ｃ_v）の関数Ｊ（φ，θ，ψ，ｔ１，ｔ２，ｔ３，ａ，Ｆ，Ｃ_u，Ｃ_v）として扱うことができる。ここで、ｔ１はＴ_cのＸ成分であり、ｔ２はＴ_cのＹ成分であり、ｔ３はＴ_cのＺ成分である。
カラーカメラパラメータ最小二乗算出手段４２は、関数Ｊ（φ，θ，ψ，ｔ１，ｔ２，ｔ３，ａ，Ｆ，Ｃ_u，Ｃ_v）を最小化する（φ，θ，ψ，ｔ１，ｔ２，ｔ３，ａ，Ｆ，Ｃ_u，Ｃ_v）を公知の非線形最小二乗法により算出する。そして、カラーカメラ評価関数Ｊ（Ｒ_c，Ｔ_c，Ｆ_c）を最小化する、外部パラメータである回転行列Ｒ_cおよび並進ベクトルＴ_cおよび内部パラメータである内部パラメータ行列Ｆ_cを算出する。
このように、カラーカメラパラメータ最小二乗算出手段４２は、小球１０５の中心点１０４のカラーカメラ座標であるカラーカメラ小球中心座標から、高精度にカラーカメラパラメータを算出することができる。

以上のように、キャリブレーション装置１Ａは、さらに、小球中心座標取得手段３０によって、高精度にカラーカメラ座標を取得することができるため、カラーカメラ３のカラーカメラパラメータを高精度に取得することができる。これにより、共通の座標である世界座標において、距離カメラ２と、カラーカメラ３のカメラパラメータを高精度に取得することができる。そして、距離カメラ２で取得された距離マップおよびカラーカメラ３で取得されたカラー画像を、世界座標系に変換して扱うことにより、距離マップおよびカラー画像から得られた情報を統合することができる。さらに、例えば、距離マップから取得された情報に、カラー画像から取得されたテクスチャに関する情報を付与することができる。

［第２の実施形態のキャリブレーション装置の動作］
次に、図１３を参照して、第２の実施形態におけるキャリブレーション装置１Ａの動作を説明する。
距離カメラ２およびカラーカメラ３は、キャリブレーションパターン１００Ａを撮像する。距離カメラ２は、計測点の距離マップを取得し、取得した計測点の距離マップをキャリブレーション装置１Ａに出力する。キャリブレーション装置１Ａは、計測点の距離マップが入力されると、半球中心座標取得手段１０および距離カメラパラメータ推定手段２０によって、図７のフローチャートに示す動作と同様の動作を行い、距離カメラパラメータを出力する。カラーカメラ３は、計測点のカラー画像を取得し、取得した計測点のカラー画像をキャリブレーション装置１Ａに出力する。キャリブレーション装置１Ａは、計測点のカラー画像が入力されると、以下の動作を開始する。

ステップＳ２１において、キャリブレーション装置１Ａは、小球中心座標取得手段３０によって、計測点のカラー画像を取得する。小球中心座標取得手段３０により取得された計測点のカラー画像は、不図示のメモリ等によって記憶される。

ステップＳ２２において、キャリブレーション装置１Ａは、小球中心座標取得手段３０によって、不図示のメモリ等によって記憶されたカラー画像を参照して、当該カラー画像に基づいて、小球１０５の所定色を検出することにより、小球１０５の中心点１０４₁〜１０４₂₄のカラーカメラ座標を、カラーカメラ小球中心座標として取得する。小球中心座標取得手段３０により取得されたｓ個のカラーカメラ小球中心座標は、不図示のメモリ等によって記憶される。

ステップＳ２３において、キャリブレーション装置１Ａは、小球中心座標取得手段３０によって、不図示のメモリ等によって記憶されたｓ個のカラーカメラ小球中心座標を参照して、小球１０５の中心点１０４₁〜１０４₂₄と対応付ける。そして、キャリブレーション装置１Ａは、小球中心座標取得手段３０によって、小球１０５_iの中心点１０４_iのカラーカメラ座標を、カラーカメラ小球中心座標ｐ_c（ｉ）として取得する（ただし、ｉは１以上２４以下の整数である）。小球中心座標取得手段３０によりカラーカメラ小球中心座標ｐ_c（ｉ）（ただし、ｉは１以上２４以下の整数である）として取得されたカラーカメラ小球中心座標は、不図示のメモリ等によって記憶される。

ステップＳ２４において、キャリブレーション装置１Ａは、カラーカメラパラメータ推定手段４０において、世界小球中心座標記憶手段４１に予め記憶しておいたｓ個の世界小球中心座標ｐ_w（ｉ）、および、不図示のメモリ等によって記憶されたカラーカメラ小球中心座標ｐ_c（ｉ）（ただし、ｉは１以上２４以下の整数である）を参照する。キャリブレーション装置１Ａは、カラーカメラパラメータ推定手段４０において、式（１２）で定義されるカラーカメラ評価関数Ｊ（Ｒ_c，Ｔ_c，Ｆ_c）を最小化する回転行列Ｒ_c、並進ベクトルＴ_c、および、内部パラメータ行列Ｆ_cを、公知の非線形最小二乗法により算出する。そして、キャリブレーション装置１Ａは、カラーカメラパラメータ推定手段４０において、カラーカメラパラメータ最小二乗算出手段４２によって、算出した回転行列Ｒ_c、並進ベクトルＴ_c、および、内部パラメータ行列Ｆ_cを、カラーカメラパラメータとして出力する。
ステップＳ２４の処理の終了後、キャリブレーション装置１Ａは、動作を終了する。

上記の第１および第２本実施形態に係るキャリブレーション装置１，１Ａは、は、ＣＰＵ（Central Processing Unit）、記憶手段（例えば、メモリ、ハードディスク）等のハードウェア資源を備えるコンピュータを、前記した各手段として協調動作させるための、それぞれキャリブレーションプログラムによって実現することもできる。このプログラムは、通信回線を介して配布してもよく、光ディスクや磁気ディスク、フラッシュメモリ等の記録媒体に書き込んで配布してもよい。

（変形例）
本発明は、上記実施形態に限定されることなく、本発明の趣旨を逸脱しない範囲で、変更実施が可能であり、例えば、次の（ａ）〜（ｄ）のようなものがある。
（ａ）第２の実施形態において、カラーカメラパラメータ推定手段４０は、カラーカメラパラメータ最小二乗算出手段４２にて、公知の非線形最小二乗法により、カラーカメラパラメータを算出するかわりに、非特許文献１に記載の手法により、カラーカメラパラメータを算出することもできる。これにより、小球１０５の中心点１０４の総数が比較的少ない場合に、カラーカメラパラメータを高精度に取得することができる。
（ｂ）第１および第２の実施形態の投票手段１２における、所定のピッチ間隔は、任意に設定できるようにしてもよい。投票手段１２が使用するメモリの容量等により定まる許容範囲内で、この所定のピッチ間隔を最も小さく設定することにより、上位抽出手段１３が抽出するカメラ中心座標の精度を高めることができる。
（ｃ）第１および第２の実施形態の投票手段１２は、各計測点からの距離が、Ｒ−ＣからＲ＋Ｃであるカメラ座標点に投票してもよい。ただし、Ｃは所定の閾値であり、Ｃを任意に設定できるようにしてもよい。この所定の閾値Ｃを、所定のピッチ間隔と同程度に設定することにより、第１および第２の実施形態と同様に、上位抽出手段１３が抽出するカメラ中心座標の精度を高めることができる。
（ｄ）第１および第２実施形態に係るキャリブレーションパターン１００、１００Ａにおいて、球形状半径Ｒの球面が平面板１０１と交わり、球形状は平面板１０１に関していずれか一方の側にある当該球面の部分であってもよい。ここで、特徴点は、当該球面の中心点である。そして、球形状の頂点に、小形状体として、半径Ｒより小さい半径ｒの小球を配置してもよい。これにより、球形状を平面板１０１により安定的に配置する、または、より多くの球形状体の立体表面の計測点を確保するようにキャリブレーションパターン１００，１００Ａを構成することができる。なお、キャリブレーションパターン１００、１００Ａは、凸の球形状として説明したが、例えば、半球であれば凹の球形状であっても構わない。

１，１Ａ キャリブレーション装置
２ 距離カメラ
３ カラーカメラ
１０ 半球中心座標取得手段（特徴点座標取得手段）
１１ 距離マップ取得手段
１２ 投票手段
１３ 上位抽出手段
２０ 距離カメラパラメータ推定手段
２１ 世界中心座標記憶手段（世界特徴点座標記憶手段）
２２ 距離カメラパラメータ最小二乗算出手段
３０ 小球中心座標取得手段（カラーカメラ座標取得手段）
４０ カラーカメラパラメータ推定手段
４１ 世界小球中心座記憶手段（世界カラーカメラ座標記憶手段）
４２ カラーカメラパラメータ最小二乗算出手段
１００，１００Ａ キャリブレーションパターン
１０１ 平面板
１０２ 特徴点，格子点，中心点
１０３ 半球
１０４ 中心点，小形状体中心
１０５ 小球

Claims (11)

1. 平面板に対して予め定められた４以上のキャリブレーションを行うための特徴点のそれぞれに対して、前記特徴点を中心点とする半径が予め定められた球形状半径である球面の全体またはその一部である球形状が配置されるキャリブレーションパターンを撮像する距離カメラが取得した計測点の距離マップに基づいて、距離カメラパラメータを推定するキャリブレーション装置であって、
   前記球形状の立体表面におけるそれぞれの計測点からの距離が前記球形状半径であるカメラ座標系の座標を、前記特徴点のカメラ座標系における座標であるカメラ特徴点座標として取得する特徴点座標取得手段と、
   前記特徴点座標取得手段が取得したカメラ特徴点座標に基づいて、距離カメラパラメータを推定する距離カメラパラメータ推定手段と、
   を備えることを特徴とするキャリブレーション装置。
2. 平面板に対して予め定められた４以上のキャリブレーションを行うための特徴点のそれぞれに対して、前記特徴点を中心点とする半径が予め定められた球形状半径である球面の全体またはその一部である球形状が配置され、さらに、前記平面板に対して予め定められた７以上のキャリブレーションを行うため小形状体中心のそれぞれに対して、前記小形状体中心を中心点とする半径が前記球形状半径よりも小さい球の全体またはその一部であり、所定色を有し、前記球形状とは異なる小形状体が配置されるキャリブレーションパターンを撮像する距離カメラが取得した計測点の距離マップに基づいて、距離カメラパラメータを推定し、さらに、前記キャリブレーションパターンを撮像するカラーカメラが取得したカラー画像に基づいて、カラーカメラパラメータを推定するキャリブレーション装置であって、
   前記球形状の立体表面におけるそれぞれの計測点からの距離が前記球形状半径であるカメラ座標系の座標を、前記特徴点のカメラ座標系における座標であるカメラ特徴点座標として取得する特徴点座標取得手段と、
   前記特徴点座標取得手段が取得したカメラ特徴点座標に基づいて、距離カメラパラメータを推定する距離カメラパラメータ推定手段と、
   前記カラー画像に基づいて、前記小形状体の所定色を検出することにより、前記小形状体中心のカラーカメラ座標系における座標を、カメラ小形状体中心座標として取得するカラーカメラ座標取得手段と、
   前記カラーカメラ座標取得手段が取得したカメラ小形状体中心座標に基づいて、カラーカメラパラメータを推定するするカラーカメラパラメータ推定手段と、
   を備えることを特徴とするキャリブレーション装置。
3. 前記カラーカメラパラメータ推定手段は、
   前記小形状体中心の世界座標系における座標を、世界小形状体中心座標として記憶する世界カラーカメラ座標記憶手段と、
   前記カメラ小形状体中心座標および世界小形状体中心座標により定まるカラーカメラ評価関数を最小化するカラーカメラパラメータを最小二乗法で算出するカラーカメラパラメータ最小二乗算出手段と、
   を備えることを特徴とする請求項２に記載のキャリブレーション装置。
4. 前記距離カメラパラメータ推定手段は、前記特徴点の世界座標系における座標を、世界特徴点座標として記憶する世界特徴点座標記憶手段と、
   前記特徴点のカメラ特徴点座標および世界特徴点座標により定まる距離カメラ評価関数を最小化する距離カメラパラメータを最小二乗法で算出する距離カメラパラメータ最小二乗算出手段と、
   を備えることを特徴とする請求項１ないし請求項３のいずれか１項に記載のキャリブレーション装置。
5. 前記特徴点座標取得手段は、前記キャリブレーションパターンを撮像する距離カメラが取得した計測点の距離マップを取得する距離マップ取得手段と、
   前記距離マップ取得手段が取得した計測点の距離マップに基づいて、それぞれの計測点からの距離が前記球形状半径であるカメラ座標点に投票する投票手段と、
   前記カメラ座標点の投票数が上位から前記特徴点の総数個までのカメラ座標点を、カメラ特徴点座標として抽出する上位抽出手段と、
   を備えることを特徴とする請求項１ないし請求項４のいずれか１項に記載のキャリブレーション装置。
6. 前記特徴点座標取得手段は、前記キャリブレーションパターンを撮像する距離カメラが取得した計測点の距離マップを取得し、さらに、当該距離マップに基づいて前記平面板の境界を検出することにより、前記キャリブレーションパターンの立体表面における計測点を選定する距離マップ取得手段と、
   前記距離マップ取得手段が選定した計測点の距離マップに基づいて、前記選定した計測点からの距離が前記球形状半径であるカメラ座標点に投票する投票手段と、
   前記カメラ座標点の投票数が上位から前記特徴点の総数個までのカメラ座標点を、カメラ特徴点座標として抽出する上位抽出手段と、
   を備えることを特徴とする請求項１ないし請求項４のいずれか１項に記載のキャリブレーション装置。
7. 前記キャリブレーションパターンは、前記特徴点が、前記平面板上の格子点であり、前記特徴点でない格子点である除去点全体の重心である除去点中心と前記格子点全体の重心である格子点中心とが一致しないように構成され、
   前記上位抽出手段は、さらに、前記抽出したカメラ特徴点座標に基づいて、前記除去点のカメラ座標を算出し、当該算出した前記除去点のカメラ座標に基づいて、前記格子点中心および前記除去点中心のカメラ座標を算出し、当該算出した前記格子点中心および前記除去点中心のカメラ座標に基づいて、前記特徴点全体の前記格子点中心に関する回転方向を検出することにより、前記抽出したカメラ特徴点座標と、前記特徴点とを対応付けることを特徴とする請求項５または請求項６に記載のキャリブレーション装置。
8. 前記キャリブレーションパターンは、前記特徴点が前記平面板上に配置され、前記特徴点のそれぞれに対して配置される前記球形状が、前記特徴点を中心点とする半球であるように構成されていることを特徴とする請求項１ないし請求項７のいずれか１項に記載のキャリブレーション装置。
9. 前記キャリブレーションパターンは、前記特徴点が前記平面板上に配置され、前記特徴点のそれぞれに対して配置される前記球形状が、前記特徴点を中心点とする半球であり、さらに、前記小形状体が、前記半球の頂点上に配置された小球であり、前記小形状体中心は、当該小球の中心点であるように構成されていることを特徴とする請求項２または請求項３に記載のキャリブレーション装置。
10. 前記キャリブレーションパターンは、前記特徴点が、前記平面板上の格子点であり、前記特徴点でない格子点である除去点全体の重心である除去点中心と前記格子点全体の重心である格子点中心とが一致しないように構成され、
    カラー格子点は、前記小形状体中心全体と同一平面上にあり、かつ、前記平面板に垂直な前記格子点を通る直線上にある点であり、カラー除去点は、前記小形状体中心ではない前記カラー格子点であり、カラー格子点中心は、前記カラー格子点全体の重心であり、カラー除去点中心は、前記カラー除去点全体の重心であり、
    前記カラーカメラ座標取得手段は、さらに、前記取得したカメラ小形状体中心座標に基づいて、前記カラー除去点のカラーカメラ座標を算出し、当該算出した前記カラー除去点のカラーカメラ座標に基づいて、前記カラー格子点中心および前記カラー除去点中心のカラーカメラ座標を算出し、当該算出した前記カラー格子点中心および前記カラー除去点中心のカラーカメラ座標に基づいて、前記小形状体中心全体の前記カラー格子点中心に関する回転方向を検出することにより、前記取得したカメラ小形状体座標と、前記小形状体中心と、を対応付けることを特徴とする請求項９に記載のキャリブレーション装置。
11. コンピュータを、請求項１ないし請求項１０のいずれか１項に記載のキャリブレーション装置として機能させるためのキャリブレーションプログラム。

Images