JP6692321B2 - 幾何的合わせこみ装置、方法、及びプログラム - Google Patents

Description

本発明は、幾何的合わせこみ装置、方法、及びプログラムに係り、特に、入力画像を撮影した際のカメラの位置及び姿勢を推定するための幾何的合わせこみ装置、方法、及びプログラムに関する。

近年、ジャイロセンサやＧＰＳを搭載したスマートフォンや、レーザー計測機器やカメラ、ＧＰＳ、ＩＭＵといった複数のセンサを具備した計測車両の出現により、撮影した際のカメラの位置姿勢情報が付与された画像が容易に得られるようになった。しかし、これら位置姿勢情報を計測するセンサに含まれる誤差や計測時のセンサの振動のために、画像に付随する位置姿勢情報と実世界の三次元モデルを用いて、実際に撮影された画像と同様の画像を透視投影によって仮想的に復元しようとしても、位置姿勢情報に含まれる誤差のために三次元モデルが投影される画像平面上の位置がずれてしまい、まったく同じ画像が得られるということは無い。本発明はこのような位置姿勢情報に含まれるズレを解消し、三次元モデルを投影した結果が実際の画像に限りなく一致するように位置姿勢情報を修正するものである。

あらかじめ用意した三次元点群など、この点群の一部分に相当する実世界の物体を撮影した画像が入力として与えられたとき、入力画像に写っているように三次元物体を撮影した際のカメラの位置姿勢を推定する問題はコンピュータビジョン分野において古くから取り組まれている。

このような問題を解決する手法として、三次元物体と画像に写った対象物体のそれぞれからコーナーなどの手がかりとなる特徴点とその対応関係が与えられたときに、三次元物体に対するカメラの位置姿勢情報を計算するDirect Linear Transform(ＤＬＴ)や線形ＰｎＰアルゴリズムといった手法が存在する（非特許文献１参照）。しかし、三次元情報と画像情報の両方から自動的に同一の箇所に対応する可能性のある特徴点を取り出すことは極めて困難であることに加え、仮にそのようなことが出来たとしても抽出した特徴点を記述するような特徴量を計算し、その類似度等を用いて対応関係を自動的に推定するような方法はこれまで確立されていないため、実用的にはほぼ不可能であると考えられる。

形状特徴を必要とはするもののその正確な対応関係までは必要としない手法として、三次元物体と画像に写った対象物体のそれぞれからコーナーやエッジといった物体を表現する形状特徴を検出し、三次元物体側の特徴的な情報を画像平面上に投影した結果得られる画像上の位置と、画像から検出した形状情報の位置のズレが小さくなるように繰り返し位置姿勢情報を推定する手法がある（非特許文献２，３参照）。これはＣＡＤやメッシュのように物体の境界や輪郭、あるいはコーナーが取得しやすい形式によって表現された三次元物体を対象とするときには形状情報を正確に取得することが出来るため、このような手法は適していると考えられる。しかし、今回対象としている三次元点群からこれら形状特徴を正確に取り出すことにはいまだ大きな困難をともなうため、これらの特徴を用いてカメラの位置姿勢情報を推定することは難しいと考えられる。

コンピュータビジョン―アルゴリズムと応用―、Richard Szeliski 著・玉木 徹・福嶋 慶繁・飯山 将晃・鳥居 秋彦・栗田 多喜夫・波部 斉・林 昌希・野田 雅文訳、共立出版、2013. SoftPOSIT: Simultaneous Pose and Correspondence Determination, P. David et al., International Journal of Computer Vision, vol. 59, no. 3, pp. 259 - 284, 2004. バンドルアジャストメント、岡谷貴之、研究報告コンピュータビジョンとイメージメディア（CVIM） 2009-CVIM-167(37), pp. 1-16, 2009.

三次元点群と点群が表す物体が写った画像が入力されたとき、三次元点群と画像のそれぞれから特徴点やエッジを抽出し、三次元点群から抽出された特徴点やエッジを画像平面上に投影した位置が、画像から抽出された特徴点やエッジに一致するようにカメラの位置姿勢情報を求めるのが一般的であるが、実世界で計測された三次元点群から特徴点やエッジを正確に抽出することは困難であり、既存手法を用いて三次元点群に対する画像の正確な位置姿勢情報を求めることは現実的ではない。

本発明は、上記問題点を解決するために成されたものであり、精度よく、入力画像を撮影した際のカメラの位置及び姿勢を推定できる幾何的合わせこみ装置、方法、及びプログラムを提供することを目的とする。

上記目的を達成するために、第１の発明に係る幾何的合わせこみ装置は、実世界のあるシーンを表す三次元点群と、当該シーン内で撮影された入力画像と、計測機器によって得られた前記入力画像を撮影した際のカメラの位置及び姿勢に関する位置姿勢情報とを用いて、前記入力画像を撮影した際のカメラの位置及び姿勢を推定する幾何的合わせこみ装置であって、前記三次元点群に含まれる少なくとも１つの物体上の三次元点の三次元座標の集合を得る物体抽出部と、前記入力画像から前記入力画像に含まれるエッジの端点を表す座標の集合を得る画像エッジ抽出部と、前記三次元座標の集合と前記エッジの端点を表す座標の集合とを用いて、前記位置姿勢情報を修正することにより、前記入力画像を撮影した際のカメラの位置及び姿勢を推定するパラメータ最適化部と、を含んで構成されている。

また、第１の発明に係る幾何的合わせこみ装置において、前記パラメータ最適化部は、前記位置姿勢情報の修正量に関する探索範囲を入力として受け取り、修正後の前記位置姿勢情報と、前記三次元座標と、前記エッジの端点を表す座標とを用いてインライアと判定される前記三次元座標の数が最大となるように前記探索範囲内で前記位置姿勢情報の修正量を探索するようにしてもよい。

また、第１の発明に係る幾何的合わせこみ装置において、前記パラメータ最適化部は、前記位置姿勢情報の修正量の探索を、分枝限定法を用いて行うようにしてもよい。

また、第１の発明に係る幾何的合わせこみ装置において、前記パラメータ最適化部は、前記位置姿勢情報に含まれる回転行列の修正量と、前記位置姿勢情報に含まれる三次元ベクトルの修正量とのそれぞれの修正量について、分枝限定法を用いて別個に探索するようにしてもよい。

また、第１の発明に係る幾何的合わせこみ装置において、前記パラメータ最適化部は、前記位置姿勢情報の修正量を探索する際に、前記位置姿勢情報の修正量の探索範囲の大きさと、前記三次元座標の値と、前記位置姿勢情報と、前記エッジの端点を表す座標と、を用いて前記入力画像上に存在するエッジと前記三次元座標が表す点とが成す角度の最小値を推定し、推定された最小値が予め定められた閾値より小さい前記三次元座標をインライアと判定して探索を行うようにしてもよい。

また、第１の発明に係る幾何的合わせこみ装置において、前記物体抽出部は、前記三次元点群から少なくとも１つのケーブル状又は円柱状の物体を抽出し、抽出した物体上の三次元点の三次元座標の集合を得るようにしてもよい。

また、第１の発明に係る幾何的合わせこみ装置において、前記パラメータ最適化部は、前記インライアと判定される前記三次元座標の数が最大となるように、前記探索範囲の部分空間を探索し、探索された前記部分空間内の修正量に基づいて、修正後の前記位置姿勢情報を用いて前記三次元座標の三次元点を画像平面上に透視投影することによって得られた点から最も近いエッジまでの距離が小さくなるように前記位置姿勢情報を修正する処理を行うようにしてもよい。

第２の発明に係る幾何的合わせこみ方法は、物体抽出部、画像エッジ抽出部、及びパラメータ最適化部を含み、実世界のあるシーンを表す三次元点群と、当該シーン内で撮影された入力画像と前記入力画像を撮影した際のカメラの位置及び姿勢に関する位置姿勢情報とを用いて、前記入力画像を撮影した際のカメラの位置及び姿勢を推定する幾何的合わせこみ方法であって、前記物体抽出部が、前記三次元点群に含まれる少なくとも１つの物体上の三次元点の三次元座標の集合を得るステップと、前記画像エッジ抽出部が、前記入力画像から前記入力画像に含まれるエッジの端点を表す座標の集合を得るステップと、前記パラメータ最適化部が、前記三次元座標の集合と前記エッジの端点を表す座標の集合とを用いて、前記位置姿勢情報を修正することにより、前記入力画像を撮影した際のカメラの位置及び姿勢を推定するステップと、を含んで実行することを特徴とする。

第３の発明に係るプログラムは、コンピュータを、第１の発明に係る幾何的合わせこみ装置の各部として機能させるためのプログラムである。

本発明の幾何的合わせこみ装置、方法、及びプログラムによれば、三次元点群に含まれる少なくとも１つの物体上の三次元点の三次元座標の集合を得て、入力画像から入力画像に含まれるエッジの端点を表す座標の集合を得て、三次元座標の集合とエッジの端点を表す座標の集合とを用いて、位置姿勢情報を修正することにより、入力画像を撮影した際のカメラの位置及び姿勢を推定することにより、精度よく、入力画像を撮影した際のカメラの位置及び姿勢を推定できる、という効果が得られる。

本発明の実施の形態に係る三次元点群に対する画像の幾何的合わせこみ装置の構成を示すブロック図である。 パラメータ探索部２７で用いられる、三次元点と画像上のエッジの成す角θ_ijおよびカメラ情報修正部２８で用いられるe_ijを説明するための図である。 パラメータ探索部２７およびカメラ情報修正部２８で用いられる投影点から最も近いあるエッジ上１点の定義を説明する図である。 パラメータ探索部２７で、三次元部分空間として表されたカメラの位置姿勢情報に関するパラメータの探索範囲を、各次元について２分割し、８つの部分空間を新たに生成する操作を説明する図である。 点群データ記憶部３０の一例を示す図である。 三次元物体記憶部３３の一例を示す図である。 画像エッジ記憶部３４の一例を示す図である。 本発明の実施の形態に係るモデル抽出ルーチンを示すフローチャートである。 本発明の実施の形態に係るエッジ抽出ルーチンを示すフローチャートである。 本発明の第１の実施の形態に係る合わせこみルーチンを示すフローチャートである。 本発明の実施の形態に係るカメラ情報修正ルーチンを示すフローチャートである。 本発明の第２の実施の形態に係る回転行列の修正量の探索ルーチンを示すフローチャートである。 本発明の第２の実施の形態に係る三次元ベクトルの修正量の探索ルーチンを示すフローチャートである。

以下、図面を参照して本発明の実施の形態を詳細に説明する。

＜本発明の実施の形態に係る概要＞

まず、本発明の実施の形態における概要を説明する。

本実施の形態に係る幾何的合わせこみ装置は、実世界のあるシーンに表す三次元点群と当該シーン内で内部パラメータが既知のカメラで撮影された入力画像、及び計測機器で得られた、入力画像が撮影されたときのカメラの位置姿勢に関する位置姿勢情報が入力として与えられたときに、入力画像が撮影されたときのカメラの位置姿勢を正しく推定するための幾何的合わせこみ装置である。

本実施の形態では、認識が比較的容易であり、かつ、細長い形状をした物体（道路脇を走るケーブルや電柱、白線、ガードレールなど）を点群から抽出し、入力画像との合わせこみに利用する。これにより、三次元点群から合わせこみに利用するための特徴点やエッジを正確に抽出できないという困難を回避しつつ、精度よく、三次元点群と画像を合わせこむことが出来る。

以下、図面を参照して本発明の実施の形態を詳細に説明する。

＜本発明の第１の実施の形態に係る幾何的合わせこみ装置の構成及び作用＞

まず、本発明の第１の実施の形態に係る三次元点群に対する画像の幾何的合わせこみ装置の構成について説明する。図１に示すように、本発明の第１の実施の形態に係る三次元点群に対する画像の幾何的合わせこみ装置１００は、ＣＰＵと、ＲＡＭと、後述する合わせこみルーチンを実行するためのプログラムや各種データを記憶したＲＯＭと、を含むコンピュータで構成することが出来る。この幾何的合わせこみ装置１００は、機能的には図１に示すように入力部１０と、演算部２０と、出力部４０とを備えている。

入力部１０は、実世界のあるシーンに相当する三次元点群を受け付け、点群データ記憶部３０へ格納する。三次元点群は、三次元点の三次元座標（Ｘ,Ｙ,Ｚ）のＮ_ａｌｌ個の集合として表現される（図５参照）。このような三次元点群としては、レーザーを用いた計測機器を具備した計測車両によって屋外を計測することによって得られた三次元点群や、屋外において撮影された大量の画像から三次元再構成技術などを用いて復元された三次元点群などがある。点群データ記憶部３０には、三次元点群の三次元点毎に、三次元点の識別番号（Ｎｏ．）、三次元点群の三次元座標系におけるＸ軸の座標（Ｘ）、Ｙ軸の座標（Ｙ）、Ｚ軸の座標（Ｚ）が格納される。

また、入力部１０は、上記の三次元点群が表す実世界のあるシーンにおいて撮影された入力画像、計測機器で得られた当該入力画像を撮影した際のカメラの位置及び姿勢に関する位置姿勢情報、並びにカメラの内部パラメータを入力として受け付ける。入力画像は画像データ記憶部３１へ、カメラの位置姿勢情報及び内部パラメータはカメラ情報記憶部３２へ格納する。このような入力画像と、入力画像を撮影した際のカメラの位置姿勢情報及び内部パラメータとのペアは、スマートフォンによる画像の撮影と撮影時のジャイロ情報及びＧＰＳの計測値から得られる。あるいは同様の情報を計測可能な計測機器を具備した計測車両からも得ることができる。また、カメラの位置姿勢情報は３×３のサイズの回転行列と３次元ベクトルで与えられるとする。ここで入力されたカメラの位置姿勢情報に含まれる回転行列をＲ_init、三次元ベクトルをｔ_initとする。カメラの内部パラメータについては焦点距離と画像中心は必須であるが、歪みに関するパラメータについてはあっても無くてもよい。

また、入力部１０は、修正量に関する探索範囲を受け付け、位置姿勢情報と共にカメラ情報記憶部３２に格納しておく。探索範囲は、６次元の有限部分空間Ｃとする。

演算部２０は物体抽出部２２と、画像エッジ抽出部２４と、パラメータ最適化部２６と、点群データ記憶部３０と、画像データ記憶部３１と、カメラ情報記憶部３２と、三次元物体記憶部３３と、画像エッジ記憶部３４とを含んで構成されている。

パラメータ最適化部２６は、パラメータ探索部２７と、カメラ情報修正部２８とを含んで構成されている。

以降、各部の動作をフローチャートも参照しながら説明する。図８は物体抽出部２２が行う動作のフローチャート、図９は画像エッジ抽出部２４が行う動作のフローチャート、図１０はパラメータ最適化部２６が行う動作のフローチャート、図１１は図１０のＳ３１６においてカメラ情報修正部２８が行う詳細動作のフローチャートである。各フローチャートは、物体抽出部２２、画像エッジ抽出部２４、及びパラメータ最適化部２６の順で処理すればよい。

物体抽出部２２は、入力部１０で受け付けた三次元点群から、合わせこみの際の手がかりとして利用する細長い形状の物体を少なくとも１つ抽出し、抽出した物体上の三次元点の三次元座標の集合を得る（図８：Ｓ１００）。このような物体としてはケーブル状又は円柱状のものがある。このような物体を三次元点群から抽出するための技術は盛んに研究されており、たとえば特許文献１や特許文献２に記載の技術などを利用することが考えられる。なお、細長い形状の物体は、複数抽出してもよく、抽出数が多いほど正確に位置姿勢情報を修正することができる。

［特許文献１］特開２０１４−１０６７２５号公報

［特許文献２］特開２０１５−１９０１号公報

ここで抽出された複数の物体のうちの少なくとも１つの物体上に存在する、三次元座標を持つＮ個の点をＸ_１,…,Ｘ_Ｎとする。物体抽出部２２は、抽出した物体を特定する情報として、三次元物体記憶部３３に当該物体上の三次元点の三次元座標（Ｘ,Ｙ,Ｚ）の集合（図６参照）を記憶する（図８：Ｓ１０２）。三次元物体記憶部３３には、物体上の三次元点毎に、三次元点の識別番号（Ｎｏ．）、三次元点群の三次元座標系におけるＸ軸の座標（Ｘ）、Ｙ軸の座標（Ｙ）、Ｚ軸の座標（Ｚ）が格納される。

画像エッジ抽出部２４は、入力部１０で受け付けた画像から入力画像に含まれるエッジの端点を表す座標の集合を検出する（図９：Ｓ２００）。画像からエッジを検出する手法としてはハフ変換や確率的ハフ変換、Line Segment Detector（ＬＳＤ）があり、これらの手法を状況に応じて利用して必要な量のエッジを検出する。画像エッジ抽出部２４は得られたＮ_Ｅ個のエッジを、エッジの端点を表す二つの点をカメラ座標系における三次元ベクトルを用いて（ｘ,ｙ,１）、（ｘ',ｙ',１）と表現し、エッジの端点の座標（ｘ、ｙ、ｘ'、ｙ'）の集合（図７参照）を画像エッジ記憶部３４に記憶する（図９：Ｓ２０２）。なお、すべてのエッジはｚ＝１である画像平面上に存在しているため、エッジの端点を表す三次元ベクトルのｚ座標も１となる。また、エッジの端点を表す三次元ベクトルのｚ座標が１であることは自明であるため、図７のように、画像エッジ記憶部３４に記憶する際にはｚ座標の値を省略している。画像エッジ記憶部３４には、エッジ毎に、エッジの識別番号（Ｎｏ．）、画像座標系におけるエッジの一方の端点のＸ軸の座標（Ｘ）、Ｙ軸の座標（Ｙ）、及びエッジの他方の端点のＸ軸の座標（Ｘ’）、Ｙ軸の座標（Ｙ’）が格納される。

パラメータ最適化部２６は、以下に説明する各部の処理によって、物体抽出部２２で抽出した物体上の三次元点の三次元座標の集合と、画像エッジ抽出部２４で入力画像から抽出したエッジの端点を表す座標の集合とを用いて、計測機器で得られた位置姿勢情報を修正することにより、入力画像を撮影した際のカメラの位置及び姿勢を推定する。

位置姿勢情報の修正を行う前に、三次元物体記憶部３３に記憶された三次元点群の座標系を、Ｒ_initとｔ_initを用いて、カメラを中心とするカメラ座標系に変更する。

パラメータ探索部２７は、カメラの位置姿勢情報を修正する修正量の探索範囲と、カメラ情報記憶部３２に記憶されているカメラの位置姿勢情報とを入力として受け取り、入力された探索範囲内で最もよいカメラの位置姿勢情報の修正量を、分枝限定法を用いて探索する。

以下、位置姿勢情報の修正量の探索範囲及び位置姿勢情報の良さの評価方法について説明する。

カメラの位置姿勢情報の修正量は、回転行列の修正量Ｒに関する３つのパラメータと、三次元ベクトルの修正量ｔに関する３つのパラメータとで表現される。ただし、ここでＲは３×３のサイズの回転行列、ｔは三次元ベクトルである。したがって、ここで入力されるカメラの位置姿勢情報の修正量に関する探索範囲は６次元の有限部分空間Ｃとなる。以下では、実数θ_ｍａｘとｄ_ｍａｘを用いて、回転行列の修正量Ｒの探索範囲Ｃ_ｒを[−θ_ｍａｘ,θ_ｍａｘ]^３、三次元ベクトルの修正量ｔの探索範囲Ｃ_ｔを[−ｄ_ｍａｘ,ｄ_ｍａｘ]^３とする。またこれらの直積を用いてＣはＣ＝Ｃ_ｒ×Ｃ_ｔと表される。

回転行列の修正量Ｒを３つのパラメータを用いて表現する方法は複数存在するが、以下では回転軸及び回転角表現を利用する（非特許文献１参照）。回転軸及び回転角表現を用いることにより、任意の回転行列を立方体[−π,π]^３に含まれる三次元ベクトルで表現することが出来る。三次元ベクトルと回転行列との間の変換方法については非特許文献１を参照されたい。

続いて、カメラの位置姿勢情報の修正量の良さを評価するために使用する目的関数について述べる。三次元物体記憶部３３に記憶された物体の三次元座標を表す点Ｘ_ｉ（ただし１≦ｉ≦Ｎで、カメラ座標系で表現されているものとする）を、カメラの位置姿勢情報の修正量Ｒおよびｔを用いて剛体変換したものをＸ'_ｉ＝Ｒ(Ｘ_ｉ−ｔ)（ただし１≦ｉ≦Ｎ）とする。

このＸ'_ｉを画像平面上へ透視投影して得られる点を表す三次元ベクトルをＰ（Ｘ'_ｉ）とし、Ｐ（Ｘ'_ｉ）と画像エッジ記憶部３４に保存されているエッジＥ_ｊ（ただし１≦ｊ≦Ｎ_Ｅ）とが成す角を計算する。Ｐ（Ｘ'_ｉ）から最も近いＥ_ｊ上の点を表す三次元ベクトルをＮ_ｉｊとして、Ｐ（Ｘ'_ｉ）とエッジＥ_ｊの成す角θ_ｉｊを次の（１）式のように定義する（図２参照）。

・・・（１）

ここで、||・||はユークリッドノルムを、・はベクトルの内積を表す。Ｘ'_ｉとすべての画像エッジＥ_ｊについてθ_ｉｊを計算し、θ_ｉｊ（１≦ｊ≦Ｎ_Ｅ）の中で最小のものをθ_ｉとすると、次の（２）式ようにθ_ｉを表すことができる。

・・・（２）

このθ_ｉ（１≦ｉ≦Ｎ）があらかじめ定められた閾値θ_ｄよりも小さな点Ｘ_ｉ（１≦ｉ≦Ｎ）をインライアとして定義し、このインライアの数を目的関数として使用する。インライアの数を表現する目的関数ｆは次の（３）式のように表すことができる。

・・・（３）

・・・（４）

以上の説明のように、パラメータ探索部２７は、位置姿勢情報の修正量に関する探索範囲として有限部分空間Ｃを受け取り、カメラ情報記憶部３２に記憶された位置姿勢情報と、三次元物体記憶部３３に記憶された三次元座標と、画像エッジ記憶部３４に記憶されたエッジの端点を表す座標とを用いてインライアと判定される三次元座標の数が最大となるように探索範囲内で位置姿勢情報の修正量を探索する。

続いて、探索範囲として入力された６次元の有限部分空間Ｃ内で、分枝限定法を用いて、ｆを最大化するようなカメラの位置姿勢情報の修正量を探索する方法について説明する。

分枝限定法を適用するためには、探索範囲として入力された６次元の有限部分空間Ｃに含まれる部分空間Ｃ'（すなわちＣ'⊆Ｃ）においてカメラの位置姿勢情報を修正したときの、ｆの最適値の上界と下界とが推定できなければならない。

まず、回転行列の修正量Ｒの探索範囲である三次元空間に部分空間として含まれる直方体Ｃ'_ｒ⊆Ｃ_ｒと、三次元ベクトル修正量ｔの探索範囲である三次元空間に部分空間として含まれる直方体Ｃ'_ｔ⊆Ｃ_ｔとが与えられる。カメラの位置姿勢情報は、この二つの部分空間の直積Ｃ'＝Ｃ'_ｒ×Ｃ'_ｔ上で修正される。このとき、ｆの最適値の上界と下界の推定方法は以下のようになる。

まず、ｆの下界

としては、Ｃ'_ｒの中心に存在する三次元ベクトルｒ_０が表す回転行列Ｒ_０と、Ｃ'_ｔの中心に存在する三次元ベクトルｔ_０を用いて計算したｆの値を用いる。すなわち、下界

は次の（５）式のように表される。

・・・（５）

次に、ｆの上界

の推定方法について説明する。非特許文献４記載のＴｈｅｏｒｅｍ１によれば、次の定理が成り立つ。

［非特許文献４］Globally Optimal 2D-3D Registration from Points or Lines Without Correspondences, M. Brown et al., In Proceedings of ICCV, 2015.

Ｒを回転行列とし、Ｒを表現する三次元ベクトルをｒとする。またｔを三次元ベクトル、Ｘ_ｉを三次元点を表すベクトルとする。ある定数δ_ｒとδ_ｔをもちいて||ｒ_０−ｒ||＜δ_ｒ及び||ｔ_０−ｔ||＜δ_ｔが成り立つとき、上で定義したＲ_０とｔ_０に対して以下（６）式の不等式が成り立つ。

・・・（６）

・・・（７）

・・・（８）

ここで、Ｃ'_ｒが１辺の長さがｄの立方体のときにはδ_ｒ＝√３ｄ/２とすればよい。また、Ｃ'_ｔが１辺の長さがｄの立方体のときにはδ_ｔ＝√３ｄ/２とすればよい。Ｃ'_ｒ、あるいはＣ'_ｔが立方体でないときには、それぞれを内包するような立方体を代わりに考え、その１辺の長さを用いてδ_ｒおよびδ_ｔを決定すればよい。

上記の定理より、カメラの位置姿勢情報が部分空間Ｃ'_ｒ×Ｃ'_ｔ上で修正されるとき、Ｒ_０（Ｘ_ｉ−ｔ_０）とＲ（Ｘ_ｉ−ｔ）の成す角度の最大値を計算することができる。すなわち、カメラの位置姿勢情報がＣ'ｒ×Ｃ'ｔ上で修正されるとき、ある点Ｘ_ｉを変換したＸ'_ｉが動く範囲は、Ｒ_０（Ｘ_ｉ−ｔ_０）と成す角度がε_ｒ＋ε_ｔよりも小さくなる範囲である。したがって、θ_ｉ（Ｒ_０，ｔ_０）がθ_ｄ＋ε_ｒ＋ε_ｔよりも小さければ、カメラの位置姿勢情報をＣ'_ｒ×Ｃ'_ｔ上で修正させることによって、θ_ｉ（Ｒ_０，ｔ_０）をθ_ｄよりも小さくし、点Ｘ_ｉをインライアとすることができる可能性がある。

この結果に基づき、カメラの位置姿勢情報がＣ'_ｒ×Ｃ'_ｔ上で修正されるときにインライアとすることができる可能性のある点を含めた目的関数ｆの値を、ｆの最適値の上界

とし、次の（９）式ように推定することができる。

・・・（９）

パラメータ探索部２７は、以上の目的関数及び探索範囲の定義から、（５）式及び（９）式に基づき、分枝限定法を用いて、ｆを最大化するようなカメラの位置姿勢情報の修正量の探索範囲の部分空間を探索し、探索された部分空間内の修正量に基づいて、位置姿勢情報を修正する。

以下に、パラメータ探索部２７における位置姿勢情報の修正量の探索及び修正の処理について、図１０のフローチャートを参照しつつ説明する。

最初に、探索対象となる部分空間Ｃ'と、当該部分空間を探索対象として計算された目的関数ｆの最適値の上界

と、当該部分空間を探索対象として計算された目的関数ｆの最適値の下界

と、からなる３つ組

を元として持つ集合Ｔを空集合として初期化する（図１０：Ｓ３００）。

次に、Ｒを単位行列とし、ｔをゼロベクトルとした上で、上記（３）式を計算することによって得られた目的関数ｆの値を、ｆの最適値の初期値とする（図１０：Ｓ３０２）。

次に、修正量に関する探索範囲として入力された６次元の有限部分空間Ｃ内の修正量でカメラの位置姿勢情報を修正するときの、目的関数ｆの最適値の上界と下界を上記（５）式及び（９）式を用いて計算し、有限部分空間Ｃと、ここで計算された、上界と、下界とからなる３つ組をＴに追加する（図１０：Ｓ３０４）。

続いて、以下Ｓ３０６〜Ｓ３１８の処理を、Ｔが空になるか、あらかじめ定められた停止条件を満たすまで繰り返し行う。

Ｔが空であればカメラの位置姿勢情報の修正量の探索を終了し（図１０：Ｓ３０６）、出力部４０へカメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量を用いてＲ_initとｔ_initに修正を加えたカメラの位置姿勢情報を出力し、処理を終了する（図１０：Ｓ３２０）。

Ｔが空でなければ、Ｔに含まれる元の中で、最も大きな上界を持つ３つ組τを取り出す（図１０：Ｓ３０８）。ここで探索の停止条件を満たしているか否かを判定する（図１０：Ｓ３１０）。考えられる停止条件としては、Ｔに含まれる元を取り出す回数があらかじめ定められた回数に到達する、あるいは、τに含まれる上界の値と、その時点で得られているｆの最適値の差があらかじめ定められた値よりも小さくなる、といったものが考えられる。

次に、τに含まれる探索対象となる部分空間Ｃ_τを、各次元について２分割して２^６＝６４個の部分空間Ｃ_τｉ（ｉは１≦ｉ≦６４である自然数）を生成する（図１０：Ｓ３１２）。ここで生成された部分空間Ｃ_τｉの各々について、以下の処理を行う。

まず、上記（５）式及び（９）式を用いて目的関数ｆの最適値の上界と下界を計算する（図１０：Ｓ３１４）。

計算された下界がｆの最適値よりも大きなときは、ｆの最適値を当該下界の値で更新し、カメラ情報記憶部３２に、当該下界の計算に用いた回転行列と三次元ベクトルをカメラの位置姿勢情報の修正量として記憶し、カメラ情報修正部２８を呼び出し、カメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量に修正を加える（図１０：Ｓ３１６）。カメラ情報修正部２８の処理の詳細については後述する。

続いて、計算された上界がｆの最適値よりも大きなときは、部分空間Ｃ_τｉと、ここで計算された上界と、下界とからなる３つ組をＴに加える（図１０：Ｓ３１８）。

以上の処理をすべてのＣ_τｉに行った後、Ｔが空でなければ再度３つ組を取り出し、同様の処理を行う（図１０：Ｓ３０６〜Ｓ３１８）。Ｔが空であれば探索を終了し、出力部４０へカメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量を用いてＲ_initとｔ_initに修正を加えたカメラの位置姿勢情報を出力し、処理を終了する（図１０：Ｓ３２０）。

次に図１０のＳ３１６のカメラ情報修正部２８の処理の詳細について、図１１のフローチャートを参照しつつ説明する。

カメラ情報修正部２８は、カメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量を入力として受け取り、画像エッジ記憶部３４に記憶されたエッジ情報と、三次元物体記憶部３３に記憶された、物体上の三次元点を画像平面上に透視投影した位置とのズレを定義し、このズレが小さくなるように、物体上の三次元点の三次元座標の集合を利用してカメラの位置姿勢情報の修正量を繰り返し更新する。

以下に具体的な計算法について説明する。まず、カメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量を用いて、三次元物体記憶部３３に保存された物体上の三次元点Ｘ_ｉ（ただし１≦ｉ≦Ｎ）を画像平面上に透視投影し、投影された点Ｘ_ｉから画像エッジ記憶部３４に記憶された各エッジに対するズレを計算し、ズレが最も小さなエッジと当該エッジ上の点で投影された点に最も近い点（図３参照）を求め、以下に説明する誤差Ｄを計算する（図１１：Ｓ４００）。

画像エッジ記憶部３４に記憶されたＮ_Ｅ個のエッジＥ_ｊ（ただし１≦ｊ≦Ｎ_Ｅ）の端点をＰ_ｊ１、Ｐ_ｊ２とし、Ｘ’_ｉを、点Ｘ_ｉ（ただし１≦ｉ≦Ｎ）をカメラの位置姿勢情報の修正量を用いて剛体変換したものとし（Ｘ'_ｉ＝Ｒ(Ｘ_ｉ−ｔ)）、このＸ’_ｉを画像平面上へ透視投影して得られる点をＰ（Ｘ'_ｉ）、Ｐ（Ｘ'_ｉ）から最も近いＥ_ｊ上の点をＮ_ｉｊとしたときに、ｅ_ｉｊを次のように定義する。

・・・（１０）

ここで、||・||はユークリッドノルムを表す。各画像エッジＥ_ｉについて、画像エッジ記憶部に記憶されたすべての画像エッジに対してこのズレｅ_ｉｊを計算し、ズレの最小値をｅ_ｉとすると、次のようにｅ_ｉを表すことができる。

・・・（１１）

・・・（１２）

以上のように、三次元点を画像平面上に透視投影することによって得られた点から最も近いエッジまでの距離が小さくなるように位置姿勢情報の修正量を更新する。

上記（１０）式で定義した量を用いて、ズレの二乗和として誤差Ｄを次のように定義する。

・・・（１３）

ここで、各ｅ_ｉに対して適切な重みｗ_ｉを、物体のカテゴリやパラメータの修正回数に応じて適切に設定してかけ合わせるようにしてもよい。また、その場合、（１３）式は、ｅ_ｉとなっているところをｗ_ｉ，ｅ_ｉと修正すればよい。

誤差Ｄの計算は、停止条件を満たすまで繰り返す（図１１：Ｓ４０２）。停止条件は、誤差が所定の値以下になるか、あるいは所定の回数だけ繰り返し位置姿勢情報の修正量の更新を行ったこととし、停止条件を満たした場合には、位置姿勢情報の修正量の更新を打ち切る。

ここで、カメラの位置姿勢情報の修正量の更新方法について説明する。

カメラの位置姿勢情報の修正量を修正するにあたって、カメラの位置姿勢情報の修正量に対する変化量をパラメータとして導入し、（１３）式で定義されるズレが小さくなるように変化量を非線形最適化手法によって求める。本実施の形態では非線形最適化手法としてガウス＝ニュートン法を用いた。こうして求めた微小変化を位置姿勢情報の修正量に適用することによって、位置姿勢情報の修正量の更新を行う。

カメラの位置姿勢情報の修正量に対する変化量を表すパラメータｐ_１，ｐ_２，ｐ_３，ｐ_４，ｐ_５，ｐ_６を導入し、これらのパラメータを用いてカメラの位置姿勢情報の修正量を次のように更新することを考える。

・・・（１４）

・・・（１５）

・・・（１６）

・・・（１７）

誤差Ｄを減少させるようなパラメータｐ_１〜ｐ_６をガウス＝ニュートン法によって求める（上記非特許文献２参照）。ここではガウス＝ニュートン法に基づく方法を説明するが、無論レベンバーグ＝マーカート法に代表されるニュートン法の派生アルゴリズムなど、利用するアルゴリズムは他のものでもよい。ガウス＝ニュートン法によって変化量を求めるには下記の２つの量が必要である。

・・・（１７）

・・・（１８）

・・・（１９）

以上の（１９）式及びｅ_ｉの定義式の（１１）式を用いてｅとＪを求めたら、次の（２０）式の方程式をパラメータｐ_１，ｐ_２，ｐ_３，ｐ_４，ｐ_５，ｐ_６について解き、パラメータの値を求める（図１１：Ｓ４０４）。

・・・（２０）

こうして求めたパラメータｐ_１〜ｐ_６の値を用いて、（１６）式及び（１７）式により、カメラ情報記憶部３２に記憶された位置姿勢情報の修正量を更新する（図１１：Ｓ４０６）。

更新したカメラの位置姿勢情報の修正量を用いて上記（１３）式により誤差Ｄを再計算し（図１１：Ｓ４０８）、誤差が所定の値以下になるか、あるいは所定の回数だけ繰り返し位置姿勢情報の修正量の更新を行ったところで位置姿勢情報の修正量の更新を打ち切る（図１１：Ｓ４０２）。更新により最終的に得られたカメラの位置姿勢情報の修正量をカメラ情報記憶部３２に記憶し処理を終了する（図１１：Ｓ４１０）。このとき、更新されたカメラの位置姿勢情報の修正量によって、（３）式のインライアと判定される三次元座標の数を計算したときに、インライアの数が更新前の値と比較して減少した場合に、カメラ情報記憶部３２に記憶しないようにするなどしてもよい。

以上説明したように、本発明の第１の実施の形態に係る幾何的合わせこみ装置によれば、三次元点群に含まれる少なくとも１つの物体上の三次元点の三次元座標の集合を得て、入力画像から入力画像に含まれるエッジの端点を表す座標の集合を得て、三次元座標の集合とエッジの端点を表す座標の集合とを用いて、位置姿勢情報を修正することにより、入力画像を撮影した際のカメラの位置及び姿勢を推定することにより、精度よく、入力画像を撮影した際のカメラの位置及び姿勢を推定できる。

＜本発明の第２の実施の形態に係る幾何的合わせこみ装置の構成及び作用＞

まず、本発明の第２の実施の形態に係る三次元点群に対する画像の幾何的合わせこみ装置の構成について説明する。第２の実施の形態では、６次元の有限部分空間Ｃを、回転行列の修正量Ｒの探索範囲Ｃ_ｒ＝［−θ_ｍａｘ，θ_ｍａｘ］^３と、三次元ベクトルの修正量ｔの探索範囲Ｃ_ｔ＝［−ｄ_ｍａｘ，ｄ_ｍａｘ］^３に分割した上で、修正量の探索を行う点が、第１の実施の形態と異なっている。これは、通常は第１の実施の形態のように、６次元の有限部分空間において分枝限定法を利用し、ｆを最大化するカメラの位置姿勢情報を探索するが、時間計算量および空間計算量に関する困難を伴うため、探索範囲を分割することによりこれを軽減する。

第２の実施の形態の構成は上記図１に示す第１の実施の形態と同様であるため説明を省略する。

以下、第２の実施の形態のパラメータ最適化部２６の処理について図１２及び図１３のフローチャートを参照しつつ説明する。

最初に、探索対象となる部分空間Ｓ_ｒと、当該部分空間を探索対象として計算された目的関数ｆの最適値の上界

と、当該部分空間を探索対象として計算された目的関数ｆの最適値の下界

と、からなる３つ組

を元として持つ集合Ｔ_ｒを空集合として初期化する（図１２：Ｓ５００）。

次に、Ｒを単位行列とし、ｔをゼロベクトルとした上で上記（３）式を計算することによって得られた目的関数ｆの値を、ｆの最適値の初期値とする（図１２：Ｓ５０２）。

次に、カメラの位置姿勢情報に含まれる回転行列Ｒ_initが、回転行列の修正量の探索範囲である三次元部分空間Ｃ_ｒ内で修正されるときの、目的関数ｆの最適値の上界と下界を以下に述べる方法で計算し、有限部分空間Ｃ_ｒと、ここで計算された上界と、下界と、からなる３つ組をＴ_ｒに追加する（図１２：Ｓ５０４）。上界及び下界の一部の計算方法については、図１３のフローチャートの説明において後述する。

ここで、カメラの位置姿勢情報に含まれる回転行列Ｒ_initが、回転行列の修正量Ｒの探索範囲である三次元部分空間Ｃ''_ｒ内で修正されるときのｆの上界と下界の定義、及び推定方法について説明する。

回転行列の修正量Ｒの探索範囲に含まれる三次元部分空間をＣ''_ｒとするとき、Ｃ''_ｒの中心に位置するパラメータをｒ''_０、ｒ''_０によって表現される回転行列の修正量をＲ''_０とする。Ｃ''_ｒを探索対象としたときの目的関数ｆの最適値の上界

と、Ｃ''_ｒを探索対象としたときの目的関数ｆの最適値の下界

を次の（２１）式及び（２２）式のように定義する。

・・・（２１）

・・・（２２）

（２１）式の右辺に含まれるｆ（Ｒ''_０，ｔ，θ_ｄ）を最大化するｔと、そのときの右辺の値、及び、（２２）式の右辺に含まれるｆ（Ｒ''_０，ｔ，θ_ｄ＋ε_ｒ）を最大化するｔと、そのときの右辺の値を計算する。計算方法については図１３のフローチャートの説明において後述する。

続いて、以下Ｓ５０６〜Ｓ５１８の処理を、Ｔ_rが空になるか、あらかじめ定められた停止条件を満たすまで繰り返し行う。

Ｔ_ｒが空であればカメラの位置姿勢情報の修正量の探索を終了し（図１２：Ｓ５０６）、出力部４０へカメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量を用いてＲ_initとｔ_initに修正を加えたカメラの位置姿勢情報を出力し、処理を終了する。

Ｔ_ｒが空でなければ、Ｔ_ｒに含まれる元の中で、最も大きな上界を持つ３つ組τ_ｒを取り出す（図１２：Ｓ５０８）。ここで探索の停止条件を満たしているか否かを判定する（図１２：Ｓ５１０）。考えられる停止条件としては、Ｔ_ｒに含まれる元を取り出す回数があらかじめ定められた回数に到達する、あるいは、τ_ｒに含まれる上界の値と、その時点で得られているｆの最適値の差があらかじめ定められた値よりも小さくなる、といったものが考えられる。

τ_ｒに含まれる探索対象となる部分空間

を、各次元について２分割して８個の部分空間

（ｉは１≦ｉ≦８である自然数）を生成する（図１２：Ｓ５１２）。ここで生成された各部分空間について、以下の処理を行う。

まず、（２１）式及び（２２）式を用いて目的関数ｆの最適値の上界と下界、及びそれぞれの値を達成する三次元ベクトルの修正量を計算する（図１２：Ｓ５１４）。

計算された下界がｆの最適値よりも大きなときは、ｆの最適値を当該下界の値で更新し、カメラ情報記憶部３２に当該下界の計算に用いた回転行列と三次元ベクトルを位置姿勢情報の修正量として記憶し、カメラ情報修正部２８を呼び出し、カメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量に修正を加える（図１２：Ｓ５１６）。修正方法は第１の実施の形態の図１１のフローチャートと同様である。

続いて、計算された上界がｆの最適値よりも大きなときは、部分空間Ｃ_ｒτｉと、ここで計算された上界と、下界とからなる３つ組をＴ_ｒに加える（図１２：Ｓ５１８）。

以上の処理をすべての

に行った後、Ｔ_ｒが空でなければ再度３つ組を取り出し、同様の処理を行う。Ｔ_ｒが空であれば探索を終了し、出力部４０へカメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量を用いてＲ_initとｔ_initに修正を加えたカメラの位置姿勢情報を出力し、処理を終了する（図１２：Ｓ５２０）。

続いて、上記（２１）式の右辺のｆ（Ｒ''_０，ｔ，θ_ｄ）を最大化するｔと、そのときのｆ（Ｒ''_０，ｔ，θ_ｄ）の値を計算する方法について、図１３のフローチャートを参照しつつ説明する。ｆ（Ｒ''_０，ｔ，θ_ｄ）の最大化は、Ｒ’’_０が与えられたときに、ｆ（Ｒ''_０，ｔ，θ_ｄ）を最大化する三次元ベクトルの修正量ｔを、三次元ベクトルの修正量ｔの探索範囲上で分枝限定法を用いて探索することによって実行する。

まず、探索対象となる部分空間Ｓ_ｔと、当該部分空間を探索対象として計算されたｆ（Ｒ''_０，ｔ，θ_ｄ）の最適値の上界

と、当該部分空間を探索対象として計算されたｆ（Ｒ''_０，ｔ，θ_ｄ）の最適値の下界

と、からなる３つ組

を元として持つ集合Ｔ_ｔを空集合として初期化する（図１３：Ｓ６００）。

また、ｆ（Ｒ''_０，ｔ，θ_ｄ）の最大値ｆ_ｍａｘ ^ｔを、その時点で得られている目的関数ｆの最適値で初期化する（図１３：Ｓ６０２）。

次に、カメラの位置姿勢情報に含まれる三次元ベクトルが、三次元ベクトルの修正量ｔの探索範囲である三次元部分空間Ｃ_ｔ内で修正されるときの、ｆ（Ｒ''_０，ｔ，θ_ｄ）の最適値の上界と下界を以下に述べる方法で計算し、有限部分空間Ｃ_ｔと、ここで計算された上界と下界と、からなる３つ組をＴ_ｔに追加する（図１３：Ｓ６０４）。

カメラの位置姿勢情報に含まれる三次元ベクトルが、三次元ベクトルの修正量ｔの探索範囲である三次元部分空間Ｃ''_ｔ内で修正されるときのｆ（Ｒ''_０，ｔ，θ_ｄ）の上界と下界の定義と推定方法について説明する。

三次元ベクトルの修正量ｔの探索範囲に含まれる三次元部分空間をＣ''_ｔとするとき、Ｃ’’_ｔの中心に位置する三次元ベクトルの修正量をｔ''_０とする。Ｃ''_ｔを探索対象としたときのｆ（Ｒ''_０，ｔ，θ_ｄ）の最適値の上界

と、Ｃ''_ｔを探索対象としたときのｆ（Ｒ''_０，ｔ，θ_ｄ）の最適値の下界

を次のように定義する。

・・・（２３）

・・・（２４）

続いて、以下Ｓ６０６〜６１８の処理を、Ｔ_ｔが空になるか、あらかじめ定められた停止条件を満たすまで繰り返し行う。

Ｔ_ｔが空であれば三次元ベクトルの修正量ｔの探索を終了し（図１３：Ｓ６０６）、その時点で得られているｆ_ｍａｘ ^ｔを（２１）式の値として、ｆ_ｍａｘ ^ｔを計算したときの三次元ベクトルの修正量ｔ''_０を、最大値を達成した三次元ベクトルの修正量として出力し、図１３の処理ルーチンを終了する（図１３：Ｓ６２０）。

Ｔ_ｔが空でなければ、Ｔ_ｔに含まれる元の中で、最も大きな上界を持つ３つ組τ_ｔを取り出す（図１３：Ｓ６０８）。ここで探索の停止条件を満たしているか否かを判定する（図１３：Ｓ６１０）。考えられる停止条件としては、Ｔ_ｔに含まれる元を取り出す回数があらかじめ定められた回数に到達する、あるいは、τ_ｔに含まれる上界の値と、ｆ_ｍａｘ ^ｔの差があらかじめ定められた値よりも小さくなる、といったものが考えられる。

探索の停止条件を満たしていないと判定された場合、τ_ｔに含まれる探索対象となる部分空間

を、各次元について２分割して８個の部分空間

（ｉは１≦ｉ≦８である自然数）を生成する（図１３：Ｓ６１２）。ここで生成された部分空間の各々について、以下の処理を行う。

まず、上記（２３）式及び（２４）式を用いてｆ（Ｒ''_０，ｔ，θ_ｄ）の最大値の上界と下界を計算する（図１３：Ｓ６１４）。

ここで計算された下界が、ｆ_ｍａｘ ^ｔよりも大きなときは、ｆ_ｍａｘ ^ｔを下界の値で更新し、下界の計算に用いた三次元ベクトルの修正量ｔ''_０を記憶しておく（図１３：Ｓ６１６）。

続いて、計算された上界がｆ_ｍａｘ ^ｔよりも大きなときは、部分空間

と、ここで計算された上界と、下界とからなる３つ組をＴ_ｔに加える（図１３：Ｓ６１８）。

以上の処理をすべての

に行った後、Ｔ_ｔが空でなければ再度３つ組を取り出し、同様の処理を行う。Ｔ_ｔが空であれば探索を終了し、その時点で得られているｆ_ｍａｘ ^ｔを（２１）式の値として、ｆ_ｍａｘ ^ｔを計算したときの三次元ベクトルの修正量ｔ''_０を、最大値を達成した三次元ベクトルの修正量として出力し、処理を終了する（図１３：Ｓ６２０）。

また、（２２）式の右辺のｆ（Ｒ''_０，ｔ，θ_ｄ＋ε_ｒ）の最大値を計算するときには、以下の（２５）式及び（２６）式のように、上記（２３）式及び（２４）式を修正した上で、（２１）式の右辺の最大化を計算するときと同様の操作を行えばよい。

・・・（２５）

・・・（２６）

以上説明したように、本発明の第２の実施の形態に係る幾何的合わせこみ装置によれば、三次元点群に含まれる少なくとも１つの物体上の三次元点の三次元座標の集合を得て、入力画像から入力画像に含まれるエッジの端点を表す座標の集合を得て、三次元座標の集合とエッジの端点を表す座標の集合とを用いて、探索範囲の部分空間を、回転行列の修正量Ｒの探索範囲と、三次元ベクトルの修正量ｔの探索範囲とに分割してそれぞれについて探索し、位置姿勢情報を修正することにより、入力画像を撮影した際のカメラの位置及び姿勢を推定することにより、精度よく、入力画像を撮影した際のカメラの位置及び姿勢を推定できる。

なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。

１０ 入力部
２０ 演算部
２２ 物体抽出部
２４ 画像エッジ抽出部
２６ パラメータ最適化部
２７ パラメータ探索部
２８ カメラ情報修正部
３０ 点群データ記憶部
３１ 画像データ記憶部
３２ カメラ情報記憶部
３３ 三次元物体記憶部
３４ 画像エッジ記憶部
４０ 出力部
１００ 幾何的合わせこみ装置

Claims (8)

1. 実世界のあるシーンを表す三次元点群と、当該シーン内で撮影された入力画像と、計測機器によって得られた前記入力画像を撮影した際のカメラの位置及び姿勢に関する位置姿勢情報とを用いて、前記入力画像を撮影した際のカメラの位置及び姿勢を推定する幾何的合わせこみ装置であって、
   前記三次元点群に含まれる少なくとも１つの物体上の三次元点の三次元座標の集合を得る物体抽出部と、
   前記入力画像から前記入力画像に含まれるエッジの端点を表す座標の集合を得る画像エッジ抽出部と、
   前記三次元座標の集合と前記エッジの端点を表す座標の集合と、前記位置姿勢情報の修正量に関する探索範囲と、を入力として受け取り、修正後の前記位置姿勢情報と、前記三次元座標と、前記エッジの端点を表す座標とを用いてインライアと判定される前記三次元座標の数が最大となるように前記探索範囲内で前記位置姿勢情報の修正量を探索し、前記位置姿勢情報を修正することにより、前記入力画像を撮影した際のカメラの位置及び姿勢を推定するパラメータ最適化部と、
   を含む幾何的合わせこみ装置。
2. 前記パラメータ最適化部は、前記位置姿勢情報の修正量の探索を、分枝限定法を用いて行う請求項１に記載の幾何的合わせこみ装置。
3. 前記パラメータ最適化部は、前記位置姿勢情報に含まれる回転行列の修正量と、前記位置姿勢情報に含まれる三次元ベクトルの修正量とのそれぞれの修正量について、分枝限定法を用いて別個に探索する請求項２に記載の幾何的合わせこみ装置。
4. 前記パラメータ最適化部は、前記位置姿勢情報の修正量を探索する際に、前記位置姿勢情報の修正量の探索範囲の大きさと、前記三次元座標の値と、前記位置姿勢情報と、前記エッジの端点を表す座標と、を用いて前記入力画像上に存在するエッジと前記三次元座標が表す点とが成す角度の最小値を推定し、推定された最小値が予め定められた閾値より小さい前記三次元座標をインライアと判定して探索を行う、請求項１〜３の何れか１項に記載の幾何的合わせこみ装置。
5. 前記物体抽出部は、前記三次元点群から少なくとも１つのケーブル状又は円柱状の物体を抽出し、抽出した物体上の三次元点の三次元座標の集合を得る、請求項１〜４の何れか１項に記載の幾何的合わせこみ装置。
6. 前記パラメータ最適化部は、前記インライアと判定される前記三次元座標の数が最大となるように、前記探索範囲の部分空間を探索し、探索された前記部分空間内の修正量に基づいて、修正後の前記位置姿勢情報を用いて前記三次元座標の三次元点を画像平面上に透視投影することによって得られた点から最も近いエッジまでの距離が小さくなるように前記位置姿勢情報を修正する処理を行う、請求項１〜５の何れか１項に記載の幾何的合わせこみ装置。
7. 物体抽出部、画像エッジ抽出部、及びパラメータ最適化部を含み、実世界のあるシーンを表す三次元点群と、当該シーン内で撮影された入力画像と前記入力画像を撮影した際のカメラの位置及び姿勢に関する位置姿勢情報とを用いて、前記入力画像を撮影した際のカメラの位置及び姿勢を推定する幾何的合わせこみ方法であって、
   前記物体抽出部が、前記三次元点群に含まれる少なくとも１つの物体上の三次元点の三次元座標の集合を得るステップと、
   前記画像エッジ抽出部が、前記入力画像から前記入力画像に含まれるエッジの端点を表す座標の集合を得るステップと、
   前記パラメータ最適化部が、前記三次元座標の集合と前記エッジの端点を表す座標の集合と、前記位置姿勢情報の修正量に関する探索範囲と、を入力として受け取り、修正後の前記位置姿勢情報と、前記三次元座標と、前記エッジの端点を表す座標とを用いてインライアと判定される前記三次元座標の数が最大となるように前記探索範囲内で前記位置姿勢情報の修正量を探索し、前記位置姿勢情報を修正することにより、前記入力画像を撮影した際のカメラの位置及び姿勢を推定するステップと、
   を含む幾何的合わせこみ方法。
8. コンピュータを、請求項１〜請求項６の何れか１項に記載の幾何的合わせこみ装置の各部として機能させるためのプログラム。