(57)【要約】【課題】精度よく、入力画像を撮影した際のカメラの位置及び姿勢を推定できる。【解決手段】三次元点群に含まれる少なくとも１つの物体上の三次元点の三次元座標の集合を得て、入力画像から入力画像に含まれるエッジの端点を表す座標の集合を得て、三次元座標の集合とエッジの端点を表す座標の集合とを用いて、位置姿勢情報を修正することにより、入力画像を撮影した際のカメラの位置及び姿勢を推定する。【選択図】図１
2018200504.tif 000002
【特許請求の範囲】
【請求項１】
実世界のあるシーンを表す三次元点群と、当該シーン内で撮影された入力画像と、計測機器によって得られた前記入力画像を撮影した際のカメラの位置及び姿勢に関する位置姿勢情報とを用いて、前記入力画像を撮影した際のカメラの位置及び姿勢を推定する幾何的合わせこみ装置であって、
前記三次元点群に含まれる少なくとも１つの物体上の三次元点の三次元座標の集合を得る物体抽出部と、
前記入力画像から前記入力画像に含まれるエッジの端点を表す座標の集合を得る画像エッジ抽出部と、
前記三次元座標の集合と前記エッジの端点を表す座標の集合とを用いて、前記位置姿勢情報を修正することにより、前記入力画像を撮影した際のカメラの位置及び姿勢を推定するパラメータ最適化部と、
を含む幾何的合わせこみ装置。
【請求項２】
前記パラメータ最適化部は、前記位置姿勢情報の修正量に関する探索範囲を入力として受け取り、修正後の前記位置姿勢情報と、前記三次元座標と、前記エッジの端点を表す座標とを用いてインライアと判定される前記三次元座標の数が最大となるように前記探索範囲内で前記位置姿勢情報の修正量を探索する請求項１に記載の幾何的合わせこみ装置。
【請求項３】
前記パラメータ最適化部は、前記位置姿勢情報の修正量の探索を、分枝限定法を用いて行う請求項２に記載の幾何的合わせこみ装置。
【請求項４】
前記パラメータ最適化部は、前記位置姿勢情報に含まれる回転行列の修正量と、前記位置姿勢情報に含まれる三次元ベクトルの修正量とのそれぞれの修正量について、分枝限定法を用いて別個に探索する請求項３に記載の幾何的合わせこみ装置。
【請求項５】
前記パラメータ最適化部は、前記位置姿勢情報の修正量を探索する際に、前記位置姿勢情報の修正量の探索範囲の大きさと、前記三次元座標の値と、前記位置姿勢情報と、前記エッジの端点を表す座標と、を用いて前記入力画像上に存在するエッジと前記三次元座標が表す点とが成す角度の最小値を推定し、推定された最小値が予め定められた閾値より小さい前記三次元座標をインライアと判定して探索を行う、請求項２〜４の何れか１項に記載の幾何的合わせこみ装置。
【請求項６】
前記物体抽出部は、前記三次元点群から少なくとも１つのケーブル状又は円柱状の物体を抽出し、抽出した物体上の三次元点の三次元座標の集合を得る、請求項１〜５の何れか１項に記載の幾何的合わせこみ装置。
【請求項７】
前記パラメータ最適化部は、前記インライアと判定される前記三次元座標の数が最大となるように、前記探索範囲の部分空間を探索し、探索された前記部分空間内の修正量に基づいて、修正後の前記位置姿勢情報を用いて前記三次元座標の三次元点を画像平面上に透視投影することによって得られた点から最も近いエッジまでの距離が小さくなるように前記位置姿勢情報を修正する処理を行う、請求項２〜５のいずれか１項に記載の幾何的合わせこみ装置。
【請求項８】
物体抽出部、画像エッジ抽出部、及びパラメータ最適化部を含み、実世界のあるシーンを表す三次元点群と、当該シーン内で撮影された入力画像と前記入力画像を撮影した際のカメラの位置及び姿勢に関する位置姿勢情報とを用いて、前記入力画像を撮影した際のカメラの位置及び姿勢を推定する幾何的合わせこみ方法であって、
前記物体抽出部が、前記三次元点群に含まれる少なくとも１つの物体上の三次元点の三次元座標の集合を得るステップと、
前記画像エッジ抽出部が、前記入力画像から前記入力画像に含まれるエッジの端点を表す座標の集合を得るステップと、
前記パラメータ最適化部が、前記三次元座標の集合と前記エッジの端点を表す座標の集合とを用いて、前記位置姿勢情報を修正することにより、前記入力画像を撮影した際のカメラの位置及び姿勢を推定するステップと、
を含む幾何的合わせこみ方法。
【請求項９】
コンピュータを、請求項１〜請求項７の何れか１項に記載の幾何的合わせこみ装置の各部として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、幾何的合わせこみ装置、方法、及びプログラムに係り、特に、入力画像を撮影した際のカメラの位置及び姿勢を推定するための幾何的合わせこみ装置、方法、及びプログラムに関する。
【背景技術】
【０００２】
近年、ジャイロセンサやＧＰＳを搭載したスマートフォンや、レーザー計測機器やカメラ、ＧＰＳ、ＩＭＵといった複数のセンサを具備した計測車両の出現により、撮影した際のカメラの位置姿勢情報が付与された画像が容易に得られるようになった。しかし、これら位置姿勢情報を計測するセンサに含まれる誤差や計測時のセンサの振動のために、画像に付随する位置姿勢情報と実世界の三次元モデルを用いて、実際に撮影された画像と同様の画像を透視投影によって仮想的に復元しようとしても、位置姿勢情報に含まれる誤差のために三次元モデルが投影される画像平面上の位置がずれてしまい、まったく同じ画像が得られるということは無い。本発明はこのような位置姿勢情報に含まれるズレを解消し、三次元モデルを投影した結果が実際の画像に限りなく一致するように位置姿勢情報を修正するものである。
【０００３】
あらかじめ用意した三次元点群など、この点群の一部分に相当する実世界の物体を撮影した画像が入力として与えられたとき、入力画像に写っているように三次元物体を撮影した際のカメラの位置姿勢を推定する問題はコンピュータビジョン分野において古くから取り組まれている。
【０００４】
このような問題を解決する手法として、三次元物体と画像に写った対象物体のそれぞれからコーナーなどの手がかりとなる特徴点とその対応関係が与えられたときに、三次元物体に対するカメラの位置姿勢情報を計算する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およびカメラ情報修正部２８で用いられるeijを説明するための図である。
【図３】パラメータ探索部２７およびカメラ情報修正部２８で用いられる投影点から最も近いあるエッジ上１点の定義を説明する図である。
【図４】パラメータ探索部２７で、三次元部分空間として表されたカメラの位置姿勢情報に関するパラメータの探索範囲を、各次元について２分割し、８つの部分空間を新たに生成する操作を説明する図である。
【図５】点群データ記憶部３０の一例を示す図である。
【図６】三次元物体記憶部３３の一例を示す図である。
【図７】画像エッジ記憶部３４の一例を示す図である。
【図８】本発明の実施の形態に係るモデル抽出ルーチンを示すフローチャートである。
【図９】本発明の実施の形態に係るエッジ抽出ルーチンを示すフローチャートである。
【図１０】本発明の第１の実施の形態に係る合わせこみルーチンを示すフローチャートである。
【図１１】本発明の実施の形態に係るカメラ情報修正ルーチンを示すフローチャートである。
【図１２】本発明の第２の実施の形態に係る回転行列の修正量の探索ルーチンを示すフローチャートである。
【図１３】本発明の第２の実施の形態に係る三次元ベクトルの修正量の探索ルーチンを示すフローチャートである。
【発明を実施するための形態】
【００２０】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００２１】
＜本発明の実施の形態に係る概要＞
【００２２】
まず、本発明の実施の形態における概要を説明する。
【００２３】
本実施の形態に係る幾何的合わせこみ装置は、実世界のあるシーンに表す三次元点群と当該シーン内で内部パラメータが既知のカメラで撮影された入力画像、及び計測機器で得られた、入力画像が撮影されたときのカメラの位置姿勢に関する位置姿勢情報が入力として与えられたときに、入力画像が撮影されたときのカメラの位置姿勢を正しく推定するための幾何的合わせこみ装置である。
【００２４】
本実施の形態では、認識が比較的容易であり、かつ、細長い形状をした物体（道路脇を走るケーブルや電柱、白線、ガードレールなど）を点群から抽出し、入力画像との合わせこみに利用する。これにより、三次元点群から合わせこみに利用するための特徴点やエッジを正確に抽出できないという困難を回避しつつ、精度よく、三次元点群と画像を合わせこむことが出来る。
【００２５】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００２６】
＜本発明の第１の実施の形態に係る幾何的合わせこみ装置の構成及び作用＞
【００２７】
まず、本発明の第１の実施の形態に係る三次元点群に対する画像の幾何的合わせこみ装置の構成について説明する。図１に示すように、本発明の第１の実施の形態に係る三次元点群に対する画像の幾何的合わせこみ装置１００は、ＣＰＵと、ＲＡＭと、後述する合わせこみルーチンを実行するためのプログラムや各種データを記憶したＲＯＭと、を含むコンピュータで構成することが出来る。この幾何的合わせこみ装置１００は、機能的には図１に示すように入力部１０と、演算部２０と、出力部４０とを備えている。
【００２８】
入力部１０は、実世界のあるシーンに相当する三次元点群を受け付け、点群データ記憶部３０へ格納する。三次元点群は、三次元点の三次元座標（Ｘ,Ｙ,Ｚ）のＮａｌｌ個の集合として表現される（図５参照）。このような三次元点群としては、レーザーを用いた計測機器を具備した計測車両によって屋外を計測することによって得られた三次元点群や、屋外において撮影された大量の画像から三次元再構成技術などを用いて復元された三次元点群などがある。点群データ記憶部３０には、三次元点群の三次元点毎に、三次元点の識別番号（Ｎｏ．）、三次元点群の三次元座標系におけるＸ軸の座標（Ｘ）、Ｙ軸の座標（Ｙ）、Ｚ軸の座標（Ｚ）が格納される。
【００２９】
また、入力部１０は、上記の三次元点群が表す実世界のあるシーンにおいて撮影された入力画像、計測機器で得られた当該入力画像を撮影した際のカメラの位置及び姿勢に関する位置姿勢情報、並びにカメラの内部パラメータを入力として受け付ける。入力画像は画像データ記憶部３１へ、カメラの位置姿勢情報及び内部パラメータはカメラ情報記憶部３２へ格納する。このような入力画像と、入力画像を撮影した際のカメラの位置姿勢情報及び内部パラメータとのペアは、スマートフォンによる画像の撮影と撮影時のジャイロ情報及びＧＰＳの計測値から得られる。あるいは同様の情報を計測可能な計測機器を具備した計測車両からも得ることができる。また、カメラの位置姿勢情報は３×３のサイズの回転行列と３次元ベクトルで与えられるとする。ここで入力されたカメラの位置姿勢情報に含まれる回転行列をＲinit、三次元ベクトルをｔinitとする。カメラの内部パラメータについては焦点距離と画像中心は必須であるが、歪みに関するパラメータについてはあっても無くてもよい。
【００３０】
また、入力部１０は、修正量に関する探索範囲を受け付け、位置姿勢情報と共にカメラ情報記憶部３２に格納しておく。探索範囲は、６次元の有限部分空間Ｃとする。
【００３１】
演算部２０は物体抽出部２２と、画像エッジ抽出部２４と、パラメータ最適化部２６と、点群データ記憶部３０と、画像データ記憶部３１と、カメラ情報記憶部３２と、三次元物体記憶部３３と、画像エッジ記憶部３４とを含んで構成されている。
【００３２】
パラメータ最適化部２６は、パラメータ探索部２７と、カメラ情報修正部２８とを含んで構成されている。
【００３３】
以降、各部の動作をフローチャートも参照しながら説明する。図８は物体抽出部２２が行う動作のフローチャート、図９は画像エッジ抽出部２４が行う動作のフローチャート、図１０はパラメータ最適化部２６が行う動作のフローチャート、図１１は図１０のＳ３１６においてカメラ情報修正部２８が行う詳細動作のフローチャートである。各フローチャートは、物体抽出部２２、画像エッジ抽出部２４、及びパラメータ最適化部２６の順で処理すればよい。
【００３４】
物体抽出部２２は、入力部１０で受け付けた三次元点群から、合わせこみの際の手がかりとして利用する細長い形状の物体を少なくとも１つ抽出し、抽出した物体上の三次元点の三次元座標の集合を得る（図８：Ｓ１００）。このような物体としてはケーブル状又は円柱状のものがある。このような物体を三次元点群から抽出するための技術は盛んに研究されており、たとえば特許文献１や特許文献２に記載の技術などを利用することが考えられる。なお、細長い形状の物体は、複数抽出してもよく、抽出数が多いほど正確に位置姿勢情報を修正することができる。
【００３５】
［特許文献１］特開２０１４−１０６７２５号公報
【００３６】
［特許文献２］特開２０１５−１９０１号公報
【００３７】
ここで抽出された複数の物体のうちの少なくとも１つの物体上に存在する、三次元座標を持つＮ個の点をＸ１,…,ＸＮとする。物体抽出部２２は、抽出した物体を特定する情報として、三次元物体記憶部３３に当該物体上の三次元点の三次元座標（Ｘ,Ｙ,Ｚ）の集合（図６参照）を記憶する（図８：Ｓ１０２）。三次元物体記憶部３３には、物体上の三次元点毎に、三次元点の識別番号（Ｎｏ．）、三次元点群の三次元座標系におけるＸ軸の座標（Ｘ）、Ｙ軸の座標（Ｙ）、Ｚ軸の座標（Ｚ）が格納される。
【００３８】
画像エッジ抽出部２４は、入力部１０で受け付けた画像から入力画像に含まれるエッジの端点を表す座標の集合を検出する（図９：Ｓ２００）。画像からエッジを検出する手法としてはハフ変換や確率的ハフ変換、Line Segment Detector（ＬＳＤ）があり、これらの手法を状況に応じて利用して必要な量のエッジを検出する。画像エッジ抽出部２４は得られたＮＥ個のエッジを、エッジの端点を表す二つの点をカメラ座標系における三次元ベクトルを用いて（ｘ,ｙ,１）、（ｘ',ｙ',１）と表現し、エッジの端点の座標（ｘ、ｙ、ｘ'、ｙ'）の集合（図７参照）を画像エッジ記憶部３４に記憶する（図９：Ｓ２０２）。なお、すべてのエッジはｚ＝１である画像平面上に存在しているため、エッジの端点を表す三次元ベクトルのｚ座標も１となる。また、エッジの端点を表す三次元ベクトルのｚ座標が１であることは自明であるため、図７のように、画像エッジ記憶部３４に記憶する際にはｚ座標の値を省略している。画像エッジ記憶部３４には、エッジ毎に、エッジの識別番号（Ｎｏ．）、画像座標系におけるエッジの一方の端点のＸ軸の座標（Ｘ）、Ｙ軸の座標（Ｙ）、及びエッジの他方の端点のＸ軸の座標（Ｘ’）、Ｙ軸の座標（Ｙ’）が格納される。
【００３９】
パラメータ最適化部２６は、以下に説明する各部の処理によって、物体抽出部２２で抽出した物体上の三次元点の三次元座標の集合と、画像エッジ抽出部２４で入力画像から抽出したエッジの端点を表す座標の集合とを用いて、計測機器で得られた位置姿勢情報を修正することにより、入力画像を撮影した際のカメラの位置及び姿勢を推定する。
【００４０】
位置姿勢情報の修正を行う前に、三次元物体記憶部３３に記憶された三次元点群の座標系を、Ｒinitとｔinitを用いて、カメラを中心とするカメラ座標系に変更する。
【００４１】
パラメータ探索部２７は、カメラの位置姿勢情報を修正する修正量の探索範囲と、カメラ情報記憶部３２に記憶されているカメラの位置姿勢情報とを入力として受け取り、入力された探索範囲内で最もよいカメラの位置姿勢情報の修正量を、分枝限定法を用いて探索する。
【００４２】
以下、位置姿勢情報の修正量の探索範囲及び位置姿勢情報の良さの評価方法について説明する。
【００４３】
カメラの位置姿勢情報の修正量は、回転行列の修正量Ｒに関する３つのパラメータと、三次元ベクトルの修正量ｔに関する３つのパラメータとで表現される。ただし、ここでＲは３×３のサイズの回転行列、ｔは三次元ベクトルである。したがって、ここで入力されるカメラの位置姿勢情報の修正量に関する探索範囲は６次元の有限部分空間Ｃとなる。以下では、実数θｍａｘとｄｍａｘを用いて、回転行列の修正量Ｒの探索範囲Ｃｒを[−θｍａｘ,θｍａｘ]３、三次元ベクトルの修正量ｔの探索範囲Ｃｔを[−ｄｍａｘ,ｄｍａｘ]３とする。またこれらの直積を用いてＣはＣ＝Ｃｒ×Ｃｔと表される。
【００４４】
回転行列の修正量Ｒを３つのパラメータを用いて表現する方法は複数存在するが、以下では回転軸及び回転角表現を利用する（非特許文献１参照）。回転軸及び回転角表現を用いることにより、任意の回転行列を立方体[−π,π]３に含まれる三次元ベクトルで表現することが出来る。三次元ベクトルと回転行列との間の変換方法については非特許文献１を参照されたい。
【００４５】
続いて、カメラの位置姿勢情報の修正量の良さを評価するために使用する目的関数について述べる。三次元物体記憶部３３に記憶された物体の三次元座標を表す点Ｘｉ（ただし１≦ｉ≦Ｎで、カメラ座標系で表現されているものとする）を、カメラの位置姿勢情報の修正量Ｒおよびｔを用いて剛体変換したものをＸ'ｉ＝Ｒ(Ｘｉ−ｔ)（ただし１≦ｉ≦Ｎ）とする。
【００４６】
このＸ'ｉを画像平面上へ透視投影して得られる点を表す三次元ベクトルをＰ（Ｘ'ｉ）とし、Ｐ（Ｘ'ｉ）と画像エッジ記憶部３４に保存されているエッジＥｊ（ただし１≦ｊ≦ＮＥ）とが成す角を計算する。Ｐ（Ｘ'ｉ）から最も近いＥｊ上の点を表す三次元ベクトルをＮｉｊとして、Ｐ（Ｘ'ｉ）とエッジＥｊの成す角θｉｊを次の（１）式のように定義する（図２参照）。
【００４７】
【数１】
2018200504.tif 000003
・・・（１）
【００４８】
ここで、||・||はユークリッドノルムを、・はベクトルの内積を表す。Ｘ'ｉとすべての画像エッジＥｊについてθｉｊを計算し、θｉｊ（１≦ｊ≦ＮＥ）の中で最小のものをθｉとすると、次の（２）式ようにθｉを表すことができる。
【００４９】
【数２】
2018200504.tif 000004
・・・（２）
【００５０】
このθｉ（１≦ｉ≦Ｎ）があらかじめ定められた閾値θｄよりも小さな点Ｘｉ（１≦ｉ≦Ｎ）をインライアとして定義し、このインライアの数を目的関数として使用する。インライアの数を表現する目的関数ｆは次の（３）式のように表すことができる。
【００５１】
【数３】
2018200504.tif 000005
・・・（３）
2018200504.tif 000006
・・・（４）
【００５２】
以上の説明のように、パラメータ探索部２７は、位置姿勢情報の修正量に関する探索範囲として有限部分空間Ｃを受け取り、カメラ情報記憶部３２に記憶された位置姿勢情報と、三次元物体記憶部３３に記憶された三次元座標と、画像エッジ記憶部３４に記憶されたエッジの端点を表す座標とを用いてインライアと判定される三次元座標の数が最大となるように探索範囲内で位置姿勢情報の修正量を探索する。
【００５３】
続いて、探索範囲として入力された６次元の有限部分空間Ｃ内で、分枝限定法を用いて、ｆを最大化するようなカメラの位置姿勢情報の修正量を探索する方法について説明する。
【００５４】
分枝限定法を適用するためには、探索範囲として入力された６次元の有限部分空間Ｃに含まれる部分空間Ｃ'（すなわちＣ'⊆Ｃ）においてカメラの位置姿勢情報を修正したときの、ｆの最適値の上界と下界とが推定できなければならない。
【００５５】
まず、回転行列の修正量Ｒの探索範囲である三次元空間に部分空間として含まれる直方体Ｃ'ｒ⊆Ｃｒと、三次元ベクトル修正量ｔの探索範囲である三次元空間に部分空間として含まれる直方体Ｃ'ｔ⊆Ｃｔとが与えられる。カメラの位置姿勢情報は、この二つの部分空間の直積Ｃ'＝Ｃ'ｒ×Ｃ'ｔ上で修正される。このとき、ｆの最適値の上界と下界の推定方法は以下のようになる。
【００５６】
まず、ｆの下界
【００５７】
【数４】
2018200504.tif 000007
【００５８】
としては、Ｃ'ｒの中心に存在する三次元ベクトルｒ０が表す回転行列Ｒ０と、Ｃ'ｔの中心に存在する三次元ベクトルｔ０を用いて計算したｆの値を用いる。すなわち、下界
【００５９】
【数５】
2018200504.tif 000008
【００６０】
は次の（５）式のように表される。
【００６１】
【数６】
2018200504.tif 000009
・・・（５）
【００６２】
次に、ｆの上界
【００６３】
【数７】
2018200504.tif 000010
【００６４】
の推定方法について説明する。非特許文献４記載のＴｈｅｏｒｅｍ１によれば、次の定理が成り立つ。
【００６５】
［非特許文献４］Globally Optimal 2D-3D Registration from Points or Lines Without Correspondences, M. Brown et al., In Proceedings of ICCV, 2015.
【００６６】
Ｒを回転行列とし、Ｒを表現する三次元ベクトルをｒとする。またｔを三次元ベクトル、Ｘｉを三次元点を表すベクトルとする。ある定数δｒとδｔをもちいて||ｒ０−ｒ||＜δｒ及び||ｔ０−ｔ||＜δｔが成り立つとき、上で定義したＲ０とｔ０に対して以下（６）式の不等式が成り立つ。
【００６７】
【数８】
2018200504.tif 000011
・・・（６）
2018200504.tif 000012
・・・（７）
2018200504.tif 000013
・・・（８）
【００６８】
ここで、Ｃ'ｒが１辺の長さがｄの立方体のときにはδｒ＝√３ｄ/２とすればよい。また、Ｃ'ｔが１辺の長さがｄの立方体のときにはδｔ＝√３ｄ/２とすればよい。Ｃ'ｒ、あるいはＣ'ｔが立方体でないときには、それぞれを内包するような立方体を代わりに考え、その１辺の長さを用いてδｒおよびδｔを決定すればよい。
【００６９】
上記の定理より、カメラの位置姿勢情報が部分空間Ｃ'ｒ×Ｃ'ｔ上で修正されるとき、Ｒ０（Ｘｉ−ｔ０）とＲ（Ｘｉ−ｔ）の成す角度の最大値を計算することができる。すなわち、カメラの位置姿勢情報がＣ'ｒ×Ｃ'ｔ上で修正されるとき、ある点Ｘｉを変換したＸ'ｉが動く範囲は、Ｒ０（Ｘｉ−ｔ０）と成す角度がεｒ＋εｔよりも小さくなる範囲である。したがって、θｉ（Ｒ０，ｔ０）がθｄ＋εｒ＋εｔよりも小さければ、カメラの位置姿勢情報をＣ'ｒ×Ｃ'ｔ上で修正させることによって、θｉ（Ｒ０，ｔ０）をθｄよりも小さくし、点Ｘｉをインライアとすることができる可能性がある。
【００７０】
この結果に基づき、カメラの位置姿勢情報がＣ'ｒ×Ｃ'ｔ上で修正されるときにインライアとすることができる可能性のある点を含めた目的関数ｆの値を、ｆの最適値の上界
【００７１】
【数９】
2018200504.tif 000014
【００７２】
とし、次の（９）式ように推定することができる。
【００７３】
【数１０】
2018200504.tif 000015
・・・（９）
【００７４】
パラメータ探索部２７は、以上の目的関数及び探索範囲の定義から、（５）式及び（９）式に基づき、分枝限定法を用いて、ｆを最大化するようなカメラの位置姿勢情報の修正量の探索範囲の部分空間を探索し、探索された部分空間内の修正量に基づいて、位置姿勢情報を修正する。
【００７５】
以下に、パラメータ探索部２７における位置姿勢情報の修正量の探索及び修正の処理について、図１０のフローチャートを参照しつつ説明する。
【００７６】
最初に、探索対象となる部分空間Ｃ'と、当該部分空間を探索対象として計算された目的関数ｆの最適値の上界
【００７７】
【数１１】
2018200504.tif 000016
【００７８】
と、当該部分空間を探索対象として計算された目的関数ｆの最適値の下界
【００７９】
【数１２】
2018200504.tif 000017
【００８０】
と、からなる３つ組
【００８１】
【数１３】
2018200504.tif 000018
【００８２】
を元として持つ集合Ｔを空集合として初期化する（図１０：Ｓ３００）。
【００８３】
次に、Ｒを単位行列とし、ｔをゼロベクトルとした上で、上記（３）式を計算することによって得られた目的関数ｆの値を、ｆの最適値の初期値とする（図１０：Ｓ３０２）。
【００８４】
次に、修正量に関する探索範囲として入力された６次元の有限部分空間Ｃ内の修正量でカメラの位置姿勢情報を修正するときの、目的関数ｆの最適値の上界と下界を上記（５）式及び（９）式を用いて計算し、有限部分空間Ｃと、ここで計算された、上界と、下界とからなる３つ組をＴに追加する（図１０：Ｓ３０４）。
【００８５】
続いて、以下Ｓ３０６〜Ｓ３１８の処理を、Ｔが空になるか、あらかじめ定められた停止条件を満たすまで繰り返し行う。
【００８６】
Ｔが空であればカメラの位置姿勢情報の修正量の探索を終了し（図１０：Ｓ３０６）、出力部４０へカメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量を用いてＲinitとｔinitに修正を加えたカメラの位置姿勢情報を出力し、処理を終了する（図１０：Ｓ３２０）。
【００８７】
Ｔが空でなければ、Ｔに含まれる元の中で、最も大きな上界を持つ３つ組τを取り出す（図１０：Ｓ３０８）。ここで探索の停止条件を満たしているか否かを判定する（図１０：Ｓ３１０）。考えられる停止条件としては、Ｔに含まれる元を取り出す回数があらかじめ定められた回数に到達する、あるいは、τに含まれる上界の値と、その時点で得られているｆの最適値の差があらかじめ定められた値よりも小さくなる、といったものが考えられる。
【００８８】
次に、τに含まれる探索対象となる部分空間Ｃτを、各次元について２分割して２６＝６４個の部分空間Ｃτｉ（ｉは１≦ｉ≦６４である自然数）を生成する（図１０：Ｓ３１２）。ここで生成された部分空間Ｃτｉの各々について、以下の処理を行う。
【００８９】
まず、上記（５）式及び（９）式を用いて目的関数ｆの最適値の上界と下界を計算する（図１０：Ｓ３１４）。
【００９０】
計算された下界がｆの最適値よりも大きなときは、ｆの最適値を当該下界の値で更新し、カメラ情報記憶部３２に、当該下界の計算に用いた回転行列と三次元ベクトルをカメラの位置姿勢情報の修正量として記憶し、カメラ情報修正部２８を呼び出し、カメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量に修正を加える（図１０：Ｓ３１６）。カメラ情報修正部２８の処理の詳細については後述する。
【００９１】
続いて、計算された上界がｆの最適値よりも大きなときは、部分空間Ｃτｉと、ここで計算された上界と、下界とからなる３つ組をＴに加える（図１０：Ｓ３１８）。
【００９２】
以上の処理をすべてのＣτｉに行った後、Ｔが空でなければ再度３つ組を取り出し、同様の処理を行う（図１０：Ｓ３０６〜Ｓ３１８）。Ｔが空であれば探索を終了し、出力部４０へカメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量を用いてＲinitとｔinitに修正を加えたカメラの位置姿勢情報を出力し、処理を終了する（図１０：Ｓ３２０）。
【００９３】
次に図１０のＳ３１６のカメラ情報修正部２８の処理の詳細について、図１１のフローチャートを参照しつつ説明する。
【００９４】
カメラ情報修正部２８は、カメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量を入力として受け取り、画像エッジ記憶部３４に記憶されたエッジ情報と、三次元物体記憶部３３に記憶された、物体上の三次元点を画像平面上に透視投影した位置とのズレを定義し、このズレが小さくなるように、物体上の三次元点の三次元座標の集合を利用してカメラの位置姿勢情報の修正量を繰り返し更新する。
【００９５】
以下に具体的な計算法について説明する。まず、カメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量を用いて、三次元物体記憶部３３に保存された物体上の三次元点Ｘｉ（ただし１≦ｉ≦Ｎ）を画像平面上に透視投影し、投影された点Ｘｉから画像エッジ記憶部３４に記憶された各エッジに対するズレを計算し、ズレが最も小さなエッジと当該エッジ上の点で投影された点に最も近い点（図３参照）を求め、以下に説明する誤差Ｄを計算する（図１１：Ｓ４００）。
【００９６】
画像エッジ記憶部３４に記憶されたＮＥ個のエッジＥｊ（ただし１≦ｊ≦ＮＥ）の端点をＰｊ１、Ｐｊ２とし、Ｘ’ｉを、点Ｘｉ（ただし１≦ｉ≦Ｎ）をカメラの位置姿勢情報の修正量を用いて剛体変換したものとし（Ｘ'ｉ＝Ｒ(Ｘｉ−ｔ)）、このＸ’ｉを画像平面上へ透視投影して得られる点をＰ（Ｘ'ｉ）、Ｐ（Ｘ'ｉ）から最も近いＥｊ上の点をＮｉｊとしたときに、ｅｉｊを次のように定義する。
【００９７】
【数１４】
2018200504.tif 000019
・・・（１０）
【００９８】
ここで、||・||はユークリッドノルムを表す。各画像エッジＥｉについて、画像エッジ記憶部に記憶されたすべての画像エッジに対してこのズレｅｉｊを計算し、ズレの最小値をｅｉとすると、次のようにｅｉを表すことができる。
【００９９】
【数１５】
2018200504.tif 000020
・・・（１１）
2018200504.tif 000021
・・・（１２）
【０１００】
以上のように、三次元点を画像平面上に透視投影することによって得られた点から最も近いエッジまでの距離が小さくなるように位置姿勢情報の修正量を更新する。
【０１０１】
上記（１０）式で定義した量を用いて、ズレの二乗和として誤差Ｄを次のように定義する。
【０１０２】
【数１６】
2018200504.tif 000022
・・・（１３）
【０１０３】
ここで、各ｅｉに対して適切な重みｗｉを、物体のカテゴリやパラメータの修正回数に応じて適切に設定してかけ合わせるようにしてもよい。また、その場合、（１３）式は、ｅｉとなっているところをｗｉ，ｅｉと修正すればよい。
【０１０４】
誤差Ｄの計算は、停止条件を満たすまで繰り返す（図１１：Ｓ４０２）。停止条件は、誤差が所定の値以下になるか、あるいは所定の回数だけ繰り返し位置姿勢情報の修正量の更新を行ったこととし、停止条件を満たした場合には、位置姿勢情報の修正量の更新を打ち切る。
【０１０５】
ここで、カメラの位置姿勢情報の修正量の更新方法について説明する。
【０１０６】
カメラの位置姿勢情報の修正量を修正するにあたって、カメラの位置姿勢情報の修正量に対する変化量をパラメータとして導入し、（１３）式で定義されるズレが小さくなるように変化量を非線形最適化手法によって求める。本実施の形態では非線形最適化手法としてガウス＝ニュートン法を用いた。こうして求めた微小変化を位置姿勢情報の修正量に適用することによって、位置姿勢情報の修正量の更新を行う。
【０１０７】
カメラの位置姿勢情報の修正量に対する変化量を表すパラメータｐ１，ｐ２，ｐ３，ｐ４，ｐ５，ｐ６を導入し、これらのパラメータを用いてカメラの位置姿勢情報の修正量を次のように更新することを考える。
【０１０８】
【数１７】
2018200504.tif 000023
・・・（１４）
2018200504.tif 000024
・・・（１５）
2018200504.tif 000025
・・・（１６）
2018200504.tif 000026
・・・（１７）
【０１０９】
誤差Ｄを減少させるようなパラメータｐ１〜ｐ６をガウス＝ニュートン法によって求める（上記非特許文献２参照）。ここではガウス＝ニュートン法に基づく方法を説明するが、無論レベンバーグ＝マーカート法に代表されるニュートン法の派生アルゴリズムなど、利用するアルゴリズムは他のものでもよい。ガウス＝ニュートン法によって変化量を求めるには下記の２つの量が必要である。
【０１１０】
【数１８】
2018200504.tif 000027
・・・（１７）
2018200504.tif 000028
・・・（１８）
2018200504.tif 000029
・・・（１９）
【０１１１】
以上の（１９）式及びｅｉの定義式の（１１）式を用いてｅとＪを求めたら、次の（２０）式の方程式をパラメータｐ１，ｐ２，ｐ３，ｐ４，ｐ５，ｐ６について解き、パラメータの値を求める（図１１：Ｓ４０４）。
【０１１２】
【数１９】
2018200504.tif 000030
・・・（２０）
【０１１３】
こうして求めたパラメータｐ１〜ｐ６の値を用いて、（１６）式及び（１７）式により、カメラ情報記憶部３２に記憶された位置姿勢情報の修正量を更新する（図１１：Ｓ４０６）。
【０１１４】
更新したカメラの位置姿勢情報の修正量を用いて上記（１３）式により誤差Ｄを再計算し（図１１：Ｓ４０８）、誤差が所定の値以下になるか、あるいは所定の回数だけ繰り返し位置姿勢情報の修正量の更新を行ったところで位置姿勢情報の修正量の更新を打ち切る（図１１：Ｓ４０２）。更新により最終的に得られたカメラの位置姿勢情報の修正量をカメラ情報記憶部３２に記憶し処理を終了する（図１１：Ｓ４１０）。このとき、更新されたカメラの位置姿勢情報の修正量によって、（３）式のインライアと判定される三次元座標の数を計算したときに、インライアの数が更新前の値と比較して減少した場合に、カメラ情報記憶部３２に記憶しないようにするなどしてもよい。
【０１１５】
以上説明したように、本発明の第１の実施の形態に係る幾何的合わせこみ装置によれば、三次元点群に含まれる少なくとも１つの物体上の三次元点の三次元座標の集合を得て、入力画像から入力画像に含まれるエッジの端点を表す座標の集合を得て、三次元座標の集合とエッジの端点を表す座標の集合とを用いて、位置姿勢情報を修正することにより、入力画像を撮影した際のカメラの位置及び姿勢を推定することにより、精度よく、入力画像を撮影した際のカメラの位置及び姿勢を推定できる。
【０１１６】
＜本発明の第２の実施の形態に係る幾何的合わせこみ装置の構成及び作用＞
【０１１７】
まず、本発明の第２の実施の形態に係る三次元点群に対する画像の幾何的合わせこみ装置の構成について説明する。第２の実施の形態では、６次元の有限部分空間Ｃを、回転行列の修正量Ｒの探索範囲Ｃｒ＝［−θｍａｘ，θｍａｘ］３と、三次元ベクトルの修正量ｔの探索範囲Ｃｔ＝［−ｄｍａｘ，ｄｍａｘ］３に分割した上で、修正量の探索を行う点が、第１の実施の形態と異なっている。これは、通常は第１の実施の形態のように、６次元の有限部分空間において分枝限定法を利用し、ｆを最大化するカメラの位置姿勢情報を探索するが、時間計算量および空間計算量に関する困難を伴うため、探索範囲を分割することによりこれを軽減する。
【０１１８】
第２の実施の形態の構成は上記図１に示す第１の実施の形態と同様であるため説明を省略する。
【０１１９】
以下、第２の実施の形態のパラメータ最適化部２６の処理について図１２及び図１３のフローチャートを参照しつつ説明する。
【０１２０】
最初に、探索対象となる部分空間Ｓｒと、当該部分空間を探索対象として計算された目的関数ｆの最適値の上界
【０１２１】
【数２０】
2018200504.tif 000031
【０１２２】
と、当該部分空間を探索対象として計算された目的関数ｆの最適値の下界
【０１２３】
【数２１】
2018200504.tif 000032
【０１２４】
と、からなる３つ組
【０１２５】
【数２２】
2018200504.tif 000033
【０１２６】
を元として持つ集合Ｔｒを空集合として初期化する（図１２：Ｓ５００）。
【０１２７】
次に、Ｒを単位行列とし、ｔをゼロベクトルとした上で上記（３）式を計算することによって得られた目的関数ｆの値を、ｆの最適値の初期値とする（図１２：Ｓ５０２）。
【０１２８】
次に、カメラの位置姿勢情報に含まれる回転行列Ｒinitが、回転行列の修正量の探索範囲である三次元部分空間Ｃｒ内で修正されるときの、目的関数ｆの最適値の上界と下界を以下に述べる方法で計算し、有限部分空間Ｃｒと、ここで計算された上界と、下界と、からなる３つ組をＴｒに追加する（図１２：Ｓ５０４）。上界及び下界の一部の計算方法については、図１３のフローチャートの説明において後述する。
【０１２９】
ここで、カメラの位置姿勢情報に含まれる回転行列Ｒinitが、回転行列の修正量Ｒの探索範囲である三次元部分空間Ｃ''ｒ内で修正されるときのｆの上界と下界の定義、及び推定方法について説明する。
【０１３０】
回転行列の修正量Ｒの探索範囲に含まれる三次元部分空間をＣ''ｒとするとき、Ｃ''ｒの中心に位置するパラメータをｒ''０、ｒ''０によって表現される回転行列の修正量をＲ''０とする。Ｃ''ｒを探索対象としたときの目的関数ｆの最適値の上界
【０１３１】
【数２３】
2018200504.tif 000034
【０１３２】
と、Ｃ''ｒを探索対象としたときの目的関数ｆの最適値の下界
【０１３３】
【数２４】
2018200504.tif 000035
【０１３４】
を次の（２１）式及び（２２）式のように定義する。
【０１３５】
【数２５】
2018200504.tif 000036
・・・（２１）
【０１３６】
2018200504.tif 000037
・・・（２２）
【０１３７】
（２１）式の右辺に含まれるｆ（Ｒ''０，ｔ，θｄ）を最大化するｔと、そのときの右辺の値、及び、（２２）式の右辺に含まれるｆ（Ｒ''０，ｔ，θｄ＋εｒ）を最大化するｔと、そのときの右辺の値を計算する。計算方法については図１３のフローチャートの説明において後述する。
【０１３８】
続いて、以下Ｓ５０６〜Ｓ５１８の処理を、Ｔrが空になるか、あらかじめ定められた停止条件を満たすまで繰り返し行う。
【０１３９】
Ｔｒが空であればカメラの位置姿勢情報の修正量の探索を終了し（図１２：Ｓ５０６）、出力部４０へカメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量を用いてＲinitとｔinitに修正を加えたカメラの位置姿勢情報を出力し、処理を終了する。
【０１４０】
Ｔｒが空でなければ、Ｔｒに含まれる元の中で、最も大きな上界を持つ３つ組τｒを取り出す（図１２：Ｓ５０８）。ここで探索の停止条件を満たしているか否かを判定する（図１２：Ｓ５１０）。考えられる停止条件としては、Ｔｒに含まれる元を取り出す回数があらかじめ定められた回数に到達する、あるいは、τｒに含まれる上界の値と、その時点で得られているｆの最適値の差があらかじめ定められた値よりも小さくなる、といったものが考えられる。
【０１４１】
τｒに含まれる探索対象となる部分空間
【０１４２】
【数２６】
2018200504.tif 000038
【０１４３】
を、各次元について２分割して８個の部分空間
【０１４４】
【数２７】
2018200504.tif 000039
【０１４５】
（ｉは１≦ｉ≦８である自然数）を生成する（図１２：Ｓ５１２）。ここで生成された各部分空間について、以下の処理を行う。
【０１４６】
まず、（２１）式及び（２２）式を用いて目的関数ｆの最適値の上界と下界、及びそれぞれの値を達成する三次元ベクトルの修正量を計算する（図１２：Ｓ５１４）。
【０１４７】
計算された下界がｆの最適値よりも大きなときは、ｆの最適値を当該下界の値で更新し、カメラ情報記憶部３２に当該下界の計算に用いた回転行列と三次元ベクトルを位置姿勢情報の修正量として記憶し、カメラ情報修正部２８を呼び出し、カメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量に修正を加える（図１２：Ｓ５１６）。修正方法は第１の実施の形態の図１１のフローチャートと同様である。
【０１４８】
続いて、計算された上界がｆの最適値よりも大きなときは、部分空間Ｃｒτｉと、ここで計算された上界と、下界とからなる３つ組をＴｒに加える（図１２：Ｓ５１８）。
【０１４９】
以上の処理をすべての
【０１５０】
【数２８】
2018200504.tif 000040
【０１５１】
に行った後、Ｔｒが空でなければ再度３つ組を取り出し、同様の処理を行う。Ｔｒが空であれば探索を終了し、出力部４０へカメラ情報記憶部３２に記憶されたカメラの位置姿勢情報の修正量を用いてＲinitとｔinitに修正を加えたカメラの位置姿勢情報を出力し、処理を終了する（図１２：Ｓ５２０）。
【０１５２】
続いて、上記（２１）式の右辺のｆ（Ｒ''０，ｔ，θｄ）を最大化するｔと、そのときのｆ（Ｒ''０，ｔ，θｄ）の値を計算する方法について、図１３のフローチャートを参照しつつ説明する。ｆ（Ｒ''０，ｔ，θｄ）の最大化は、Ｒ’’０が与えられたときに、ｆ（Ｒ''０，ｔ，θｄ）を最大化する三次元ベクトルの修正量ｔを、三次元ベクトルの修正量ｔの探索範囲上で分枝限定法を用いて探索することによって実行する。
【０１５３】
まず、探索対象となる部分空間Ｓｔと、当該部分空間を探索対象として計算されたｆ（Ｒ''０，ｔ，θｄ）の最適値の上界
【０１５４】
【数２９】
2018200504.tif 000041
【０１５５】
と、当該部分空間を探索対象として計算されたｆ（Ｒ''０，ｔ，θｄ）の最適値の下界
【０１５６】
【数３０】
2018200504.tif 000042
【０１５７】
と、からなる３つ組
【０１５８】
【数３１】
2018200504.tif 000043
【０１５９】
を元として持つ集合Ｔｔを空集合として初期化する（図１３：Ｓ６００）。
【０１６０】
また、ｆ（Ｒ''０，ｔ，θｄ）の最大値ｆｍａｘｔを、その時点で得られている目的関数ｆの最適値で初期化する（図１３：Ｓ６０２）。
【０１６１】
次に、カメラの位置姿勢情報に含まれる三次元ベクトルが、三次元ベクトルの修正量ｔの探索範囲である三次元部分空間Ｃｔ内で修正されるときの、ｆ（Ｒ''０，ｔ，θｄ）の最適値の上界と下界を以下に述べる方法で計算し、有限部分空間Ｃｔと、ここで計算された上界と下界と、からなる３つ組をＴｔに追加する（図１３：Ｓ６０４）。
【０１６２】
カメラの位置姿勢情報に含まれる三次元ベクトルが、三次元ベクトルの修正量ｔの探索範囲である三次元部分空間Ｃ''ｔ内で修正されるときのｆ（Ｒ''０，ｔ，θｄ）の上界と下界の定義と推定方法について説明する。
【０１６３】
三次元ベクトルの修正量ｔの探索範囲に含まれる三次元部分空間をＣ''ｔとするとき、Ｃ’’ｔの中心に位置する三次元ベクトルの修正量をｔ''０とする。Ｃ''ｔを探索対象としたときのｆ（Ｒ''０，ｔ，θｄ）の最適値の上界
【０１６４】
【数３２】
2018200504.tif 000044
【０１６５】
と、Ｃ''ｔを探索対象としたときのｆ（Ｒ''０，ｔ，θｄ）の最適値の下界
【０１６６】
【数３３】
2018200504.tif 000045
【０１６７】
を次のように定義する。
【０１６８】
【数３４】
2018200504.tif 000046
・・・（２３）
2018200504.tif 000047
・・・（２４）
【０１６９】
続いて、以下Ｓ６０６〜６１８の処理を、Ｔｔが空になるか、あらかじめ定められた停止条件を満たすまで繰り返し行う。
【０１７０】
Ｔｔが空であれば三次元ベクトルの修正量ｔの探索を終了し（図１３：Ｓ６０６）、その時点で得られているｆｍａｘｔを（２１）式の値として、ｆｍａｘｔを計算したときの三次元ベクトルの修正量ｔ''０を、最大値を達成した三次元ベクトルの修正量として出力し、図１３の処理ルーチンを終了する（図１３：Ｓ６２０）。
【０１７１】
Ｔｔが空でなければ、Ｔｔに含まれる元の中で、最も大きな上界を持つ３つ組τｔを取り出す（図１３：Ｓ６０８）。ここで探索の停止条件を満たしているか否かを判定する（図１３：Ｓ６１０）。考えられる停止条件としては、Ｔｔに含まれる元を取り出す回数があらかじめ定められた回数に到達する、あるいは、τｔに含まれる上界の値と、ｆｍａｘｔの差があらかじめ定められた値よりも小さくなる、といったものが考えられる。
【０１７２】
探索の停止条件を満たしていないと判定された場合、τｔに含まれる探索対象となる部分空間
【０１７３】
【数３５】
2018200504.tif 000048
【０１７４】
を、各次元について２分割して８個の部分空間
【０１７５】
【数３６】
2018200504.tif 000049
【０１７６】
（ｉは１≦ｉ≦８である自然数）を生成する（図１３：Ｓ６１２）。ここで生成された部分空間の各々について、以下の処理を行う。
【０１７７】
まず、上記（２３）式及び（２４）式を用いてｆ（Ｒ''０，ｔ，θｄ）の最大値の上界と下界を計算する（図１３：Ｓ６１４）。
【０１７８】
ここで計算された下界が、ｆｍａｘｔよりも大きなときは、ｆｍａｘｔを下界の値で更新し、下界の計算に用いた三次元ベクトルの修正量ｔ''０を記憶しておく（図１３：Ｓ６１６）。
【０１７９】
続いて、計算された上界がｆｍａｘｔよりも大きなときは、部分空間
【０１８０】
【数３７】
2018200504.tif 000050
【０１８１】
と、ここで計算された上界と、下界とからなる３つ組をＴｔに加える（図１３：Ｓ６１８）。
【０１８２】
以上の処理をすべての
【０１８３】
【数３８】
2018200504.tif 000051
【０１８４】
に行った後、Ｔｔが空でなければ再度３つ組を取り出し、同様の処理を行う。Ｔｔが空であれば探索を終了し、その時点で得られているｆｍａｘｔを（２１）式の値として、ｆｍａｘｔを計算したときの三次元ベクトルの修正量ｔ''０を、最大値を達成した三次元ベクトルの修正量として出力し、処理を終了する（図１３：Ｓ６２０）。
【０１８５】
また、（２２）式の右辺のｆ（Ｒ''０，ｔ，θｄ＋εｒ）の最大値を計算するときには、以下の（２５）式及び（２６）式のように、上記（２３）式及び（２４）式を修正した上で、（２１）式の右辺の最大化を計算するときと同様の操作を行えばよい。
【０１８６】
【数３９】
2018200504.tif 000052
・・・（２５）
2018200504.tif 000053
・・・（２６）
【０１８７】
以上説明したように、本発明の第２の実施の形態に係る幾何的合わせこみ装置によれば、三次元点群に含まれる少なくとも１つの物体上の三次元点の三次元座標の集合を得て、入力画像から入力画像に含まれるエッジの端点を表す座標の集合を得て、三次元座標の集合とエッジの端点を表す座標の集合とを用いて、探索範囲の部分空間を、回転行列の修正量Ｒの探索範囲と、三次元ベクトルの修正量ｔの探索範囲とに分割してそれぞれについて探索し、位置姿勢情報を修正することにより、入力画像を撮影した際のカメラの位置及び姿勢を推定することにより、精度よく、入力画像を撮影した際のカメラの位置及び姿勢を推定できる。
【０１８８】
なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。
【符号の説明】
【０１８９】
１０   入力部
２０   演算部
２２   物体抽出部
２４   画像エッジ抽出部
２６   パラメータ最適化部
２７   パラメータ探索部
２８   カメラ情報修正部
３０   点群データ記憶部
３１   画像データ記憶部
３２   カメラ情報記憶部
３３   三次元物体記憶部
３４   画像エッジ記憶部
４０   出力部
１００ 幾何的合わせこみ装置
【図１】
2018200504.tif 000054
【図２】
2018200504.tif 000055
【図３】
2018200504.tif 000056
【図４】
2018200504.tif 000057
【図５】
2018200504.tif 000058
【図６】
2018200504.tif 000059
【図７】
2018200504.tif 000060
【図８】
2018200504.tif 000061
【図９】
2018200504.tif 000062
【図１０】
2018200504.tif 000063
【図１１】
2018200504.tif 000064
【図１２】
2018200504.tif 000065
【図１３】
2018200504.tif 000066