JP5538667B2 - 位置姿勢計測装置及びその制御方法 - Google Patents

Description

本発明は、計測対象物体のモデルを用いて、撮像装置が計測対象物体を撮像した時の撮像装置の位置姿勢を計測する位置姿勢計測技術に関する。

現実空間を撮像するカメラなどの撮像装置（以下適宜カメラと言い換える）の位置姿勢計測は、例えば現実空間と仮想空間とを融合表示する複合現実感システムにおいて必要となる。

従来から撮像装置の位置姿勢を計測する方法として、３次元位置が既知の指標を用いる方法が知られている。この方法では、撮像装置のおおよその位置姿勢を用いて各指標の３次元位置を撮像面へ投影した投影位置と、撮像画像から検出される指標の撮像画像上の位置との距離を誤差として、この誤差を小さくする目的関数を最適化するように位置姿勢を推定する。また、指標としては、予め検出が容易な幾何学的または色相が特異な特徴を有するものを利用することが多い。

さらに、計測対象物体の構造を利用して、計測対象物体を構成する平面の境界をエッジとして観察し、計測対象物体のモデルを用いて位置姿勢を計測する方法が開示されている。なお、ここでは撮像画像により観察される濃度の変化が大きい領域をエッジと呼称する。

非特許文献１には、計測対象物体の幾何特徴として線分（計測線分という）を用いて位置姿勢を推定する方法が記載されている。この方法では、計測線分の３次元位置をおおよその撮像装置の位置姿勢から観察される撮像面上に投影し、投影した計測線分と撮像画像から検出されたエッジとの距離を目的関数として位置姿勢を推定する。

この手法の概略は以下のとおりである。
・おおよその推定中の位置姿勢を用いて、計測対象物体の計測線分を撮像画像上に投影する。
・投影した計測線分周辺の撮像画像上の画素を探索して濃度が局所的に変化するエッジ領域の位置を求める。
・エッジ領域の位置と投影した計測線分との距離を小さくするように最適化する。
・推定中の位置姿勢の値を更新する。

このような、計測対象物体の計測線分を用いる位置姿勢推定は、対象の３次元モデルが既知であれば位置姿勢計測装置の計測対象とすることができるので利用範囲が広い。さらに、上記の位置姿勢推定では、撮像画像上のエッジを探索する範囲が、計測線分が投影された周辺の画像のみとなる。そのため、撮像画像全体から画像処理によりエッジを検出してからモデルとの距離を求める方法に比べて、処理時間を短くできる利点がある。そのため、複合現実感における頭部位置推定などの実時間処理が必要な撮像装置の位置合わせで利用されている。

非特許文献１では、計測対象物体として比較的簡素な形態を有する計測対象物体を対象としており、撮像装置と計測対象物体との距離もそれほど変化しない。そのため、計測対象物体のモデルが視野内で僅かに姿勢変化している間では、観察されるエッジの変化も小さく位置姿勢の推定が可能となっている。

実際の環境では、計測対象物体の影などがエッジとして観察されることが多いため、位置姿勢の推定が不安定になることが多い。非特許文献２では、複数の観察されたエッジから、対応する線分のエッジの一つを対応づけて、対応されたエッジと撮像面上へ投影された計測線分との距離を小さくするように最適化計算が行われる。非特許文献２によれば、上記の対応付けに関する想定を何度か繰り返して最も誤差が少なくなるように収束させることで、計測対象外のエッジが混在した環境でも頑健に位置姿勢の推定が行える。
Tom Drummond and Roberto Cipolla. "Real-time visual tracking of complexstructures." IEEE Transactionof Pattern Analysis and Machine Intelligence. Vol.24, No.7,pp.932-946, 2002 L. Vacchetti, V. Lepetit and P. Fua、"Combining edge and texture information forreal-time accurate 3D camera tracking." Proceedings of International Symposium on Mixed and AugmentedReality, pp.48-57, 2004

これまでに提案されている手法は、計測対象物体とそれを観察する撮像装置の相対位置関係がそれほど変化しない場合においてうまく動作する。しかし、撮像装置を人の動きに合わせる場合、例えば、ナビゲーションなどの目的で撮像装置自体を人が保持しながら移動する場合には、問題がある。それは、建物や屋外を歩き回る場合において、計測対象物体と撮像装置の相対位置が大きく変化することに起因する。

ここで例として、撮像装置を持った人が屋内の廊下を歩き回ったときの様子を図２に示す。図２の（ａ）から（ｄ）は、歩行者の視点位置から観察される建物構造物のエッジである。図２の（１）から（４）は、屋内環境を上から俯瞰した図で、黒丸が歩行者の位置、それに付する三角形が歩行者の視線方向を示している。図２の（ａ）〜（ｄ）のそれぞれの位置における俯瞰図は図２の（１）〜（４）に対応している。

図２の（ａ）における扉のエッジと、図２の（ｂ）における扉のエッジでは、同一の扉という観測対象を見ているにもかかわらず、視点と対象物との相対位置の違いにより、観察できる画像幾何特徴の構成が変化している。さらに、図２の（ｃ）、（ｄ）は、歩いて来た方向を急に振り返るような動作を示している。その際、歩行者が観察している対象は、近接している扉のエッジから遠方まで伸びる廊下のエッジに急に変化することがわかる。このように、屋内・屋外の構造物を計測対象物体とし、撮像装置を持ち運ぶような利用形態において、図２に示すような視点変化は頻繁に起きる。

観測対象と撮像装置との相対位置が変化すると、計測対象物体の幾何特徴と撮像画像上への投影において問題が生じる。即ち、撮影装置との相対位置が遠い詳細な幾何特徴を観察した場合は、互いに近接する投影幾何特徴は投影画像上の間隔は狭くなり、場合によっては１画素未満に複数の幾何特徴が投影されてしまう場合もある。このような事態に対処するために、幾何特徴と撮像装置との相対位置関係に応じて幾何特徴を切り換えることも考えられるが、事前に幾何特徴と撮像装置との相対位置との関係を網羅的に設定しておくことが必要となる。

しかしながら、廊下などの見回すことができる環境においては、複数モデルの切換処理の遅延が課題となる。また、基本的には１画素未満の線分を撮像画像から検出することは困難である。そのため、近接する幾何特徴が遠方に存在するときには、撮像画像におけるそれぞれの投影幾何特徴から、双方の画像幾何特徴を検出することができなくなり、位置姿勢の推定結果に影響を与える。同時に、近接する投影幾何特徴から求まる画像幾何特徴の探索領域が重複するため、効率的な処理の実現が困難になる。

さらに、位置姿勢を求めるための最適化計算において非線形性を有する計算を繰り返し処理しなくてはならない。そのため、画像幾何特徴検出などの処理に費やされる時間が長くなると、十分な精度に至る前に繰り返しが中断されることになる。これらは、位置姿勢の精度とビデオフレームレートでのリアルタイム処理が必要な複合現実感技術においては好ましくない。

本発明は上記の課題に鑑みてなされたものであり、撮像装置と計測対象物体との相対位置に影響されることなく効率的に撮像装置の位置姿勢を求めることが可能な位置計測装置及び方法を提供することを目的とする。

上記の目的を達成するための本発明の一態様による位置姿勢計測装置は以下の構成を備える。すなわち、
一つ以上の計測対象物体を撮像する撮像装置と前記計測対象物体との間の相対的な位置姿勢を、前記計測対象物体の３次元モデルを利用して計測する位置姿勢計測装置であって、
前記撮像装置により撮像画像を取得する画像取得手段と、
前記撮像装置の位置姿勢に基づいて、前記３次元モデルにおける複数の幾何特徴の夫々を前記撮像画像上に投影して、前記撮像画像上に複数の投影幾何特徴を得る投影手段と、
前記撮像画像上の投影幾何特徴の相互の距離が所定の距離よりも大きい投影幾何特徴を、前記投影手段で得られた前記複数の投影幾何特徴から位置姿勢の算出に利用すべき投影幾何特徴として選択する選択手段と、
前記選択手段により選択された投影幾何特徴と、前記撮像画像上から検出される前記選択された投影幾何特徴に対応する画像幾何特徴とを用いて、前記撮像装置と該計測対象物体との間の相対的な位置姿勢を算出する算出手段とを備える。

また、上記の目的を達成するための本発明の他の態様による位置姿勢計測装置の制御方法は、
一つ以上の計測対象物体を撮像する撮像装置と前記計測対象物体との間の相対的な位置姿勢を、前記計測対象物体の３次元モデルを利用して計測する位置姿勢計測装置の制御方法であって、
前記位置姿勢計測装置の画像取得手段が、前記撮像装置から撮像画像を取得する画像取得工程と、
前記位置姿勢計測装置の投影手段が、前記撮像装置の位置姿勢に基づいて、前記３次元モデルにおける複数の幾何特徴の夫々を前記撮像画像上に投影して、前記撮像画像上に複数の投影幾何特徴を得る投影工程と、
前記位置姿勢計測装置の選択手段が、前記撮像画像上の投影幾何特徴の相互の距離が所定の距離よりも大きい投影幾何特徴を、前記投影工程で得られた前記複数の投影幾何特徴から位置姿勢の算出に利用すべき投影幾何特徴として選択する選択工程と、
前記位置姿勢計測装置の選択手段が、前記投影工程で得られた前記投影幾何特徴の夫々に関して、他の投影幾何特徴との間の前記撮像画像上の距離に基づいて投影幾何特徴を選択する選択工程と、
前記位置姿勢計測装置の算出手段が、前記選択工程で選択された投影幾何特徴と、前記撮像画像上から検出される前記選択された投影幾何特徴に対応する画像幾何特徴とを用いて、前記撮像装置と該計測対象物体との間の相対的な位置姿勢を算出する算出工程とを有する。

本発明によれば、撮像装置と計測対象物体との相対位置に影響されることなく効率的に撮像装置の位置姿勢を求めることができる。

以下添付図面を参照して、本発明の好適な実施形態について詳細に説明する。

［第１実施形態］
図１は、第１実施形態による位置姿勢計測装置１００を撮像装置５０に適用した場合の構成例を示すブロック図である。図１では、位置姿勢計測装置１００における処理の流れやデータの流れも示されている。なお、以下に示される位置姿勢計測装置１００の各構成の一部或いは全てが、不図示のＣＰＵが不図示のＲＯＭ或いはＲＡＭに格納された制御プログラムを実行することにより実現されてもよい。即ち、位置姿勢計測装置１００は一般的なコンピュータにおいて後述する処理を実行させる制御プログラムを実行させることにより実現されても良い。

本実施形態の位置姿勢計測装置１００には、情景を撮像する撮像装置５０と、撮像装置５０のおおよその位置姿勢を取得する概略位置姿勢計測装置７０とが、撮像装置入力部１１０及び概略位置姿勢入力部１３０を介して接続されている。また、計測対象物体の位置合わせに用いる幾何特徴を記述した、計測対象物体の３次元モデル６０が幾何特徴入力部１２０から取得される。幾何特徴入力部１２０は、装着された記憶媒体から３次元モデル６０を入力する（読み出す）構成でも良いし、ネットワーク等により外部から３次元モデル６０を入力する構成でも良い。位置姿勢計測装置１００の目的は、撮像装置５０が計測対象物体の３次元モデル６０に記述された計測対象物体を含む情景を撮像している時の、撮像装置５０の計測対象物体に対する相対的な位置姿勢を撮像画像の情報を用いて求めることである。

なお、本実施形態において、位置姿勢計測装置１００による位置姿勢推定処理を適用できる条件は、撮像装置５０が撮像した情景の撮像画像中に当該計測対象物体が映っていることである。なお、撮像画像に当該計測対象物体が映されていない場合は、概略位置姿勢計測装置７０の計測値をそのまま利用するようにしても構わない。

撮像装置５０はＣＣＤやＣＭＯＳで構成される光電子変換素子を用いた２次元の撮像装置である。また、撮像装置５０は、撮像装置入力部１１０を介して位置姿勢計測装置１００に映像信号を送る。撮像装置入力部１１０は、信号ケーブル（有線）或いは無線により映像信号を送信可能とする構成を有する。さらに、撮像装置５０が位置姿勢計測装置１００に内蔵される形態であってもよいし、撮像装置５０の処理部に位置姿勢計測装置１００の処理を実現する構成が内蔵さる形態であってもよい。

計測対象物体の３次元モデル６０は、計測対象物体の３次元の幾何的な情報を有するモデルであり、撮像装置５０により撮像された画像上でエッジとして検出される幾何特徴を含む。一般には、ＣＡＤ等の設計データを３次元モデル６０として流用することが可能である。また、設計データを用いずに、計測器で実測した値を３次元モデルとして入力可能に構成することもできる。なお、３次元モデルの編集には、市販の３次元計測アプリケーションを用いればよい。さらに、建築物中のドアや壁面のエッジは測量器具により計測することも可能である。本実施形態においては、位置姿勢計測装置１００が対象とする計測対象物体を変更できるようにするために、位置姿勢計測装置１００に着脱可能な記憶媒体により提供されるものとする。従って、位置姿勢計測装置１００の幾何特徴入力部１２０は着脱可能な記憶媒体のデータを読み出せるインターフェースを具備する。

一方、既存の固定した計測対象物体を扱う場合は、位置姿勢計測装置１００に３次元モデルの情報が不揮発メモリや記憶媒体により内蔵されていても構わない。つまり、位置姿勢計測装置１００においては、計測対象物体の３次元モデル６０が位置姿勢計測を実施する前に何らかの方法で提供されていればよい。そのため、外部記憶媒体により保存された複数の３次元モデルデータから指定されたものでも良いし、ネットワークによりリモートサーバから取得されたものであっても構わない。

さらに、位置姿勢計測装置１００が撮像画像により検出が可能な幾何特徴データを含んでいれば良いので、任意の３次元モデラーのデータフォーマットを変換する処理部を位置姿勢計測装置１００に含む構成としても良い。

概略位置姿勢計測装置７０は、撮像装置５０のおおよその位置姿勢を与えるための装置である。磁界或いは超音波を利用してセンサの空間中の位置・姿勢である６ＤＯＦを計測する装置を用いることができる。なお、既知の位置に設置した図形的な特徴を有する指標を利用して、撮像画像上からその指標を検出することによって得られる撮像装置５０の概略位置姿勢を利用しても構わない。

また、撮像装置５０と計測対象物体との間のおおよその相対位置姿勢がわかれば良いので、一度、位置姿勢計測装置１００により精度の高い位置姿勢を求めることができれば、以降は、その位置姿勢結果を利用することが可能となる。従って、実際には、予め既定した初期位置に撮像装置５０を置いてから位置姿勢計測を開始する場合には、概略位置姿勢計測装置７０を不要とすることができる。このように、概略位置姿勢を求める方法自体は、本発明の実施に制限を与えるものでは無く、どのような方法により取得しても良い。

次に、位置姿勢計測装置１００の構成について説明する。

位置姿勢計測装置１００は、他の装置からの情報を入力する入力部、入力部からの情報を元に撮像装置と計測対象物体との相対的な位置姿勢を推定する処理部、撮像装置の位置姿勢を他の装置で利用できるように出力する出力部とで構成されている。以下、入力部を構成する撮像装置入力部１１０、幾何特徴入力部１２０、そして概略位置姿勢入力部１３０について説明する。

まず、撮像装置５０の撮像画像を取得する構成について説明する。撮像装置５０は、撮像装置入力部１１０により位置姿勢計測装置１００と接続される。本実施形態では、撮像装置入力部１１０は映像信号を入力するためのコネクタを有し、映像信号の出力規格に準拠した撮像装置５０の映像出力端子を接続するものとする。

次に、計測対象物体の３次元モデル６０の読み込みについて説明する。３次元モデル６０の形式は、位置姿勢計測装置１００がサポートする形式であることを前提とする。なお、設計ＣＡＤデータを扱う場合は、幾何特徴入力部１２０において対応するＣＡＤフォーマットの解析機能を有することで、任意のモデルデータを取り扱うことも可能となる。各モデル形式から、本装置の位置姿勢推定に利用する計測対象物体が撮像画像上で検出される画像幾何特徴に対応したものを抽出し、保持するものとする。すなわち、幾何特徴入力部１２０は、概略位置姿勢計測装置７０からの撮像装置５０の位置姿勢に基づいて撮像画像上に存在する計測対象物体のモデルを選択し、保持する。

本実施形態においては、幾何特徴は１本以上の線分で構成され、各線分の３次元の始点および終点の２点を少なくとも含んでいるものとする。なお、後述するように、始点、終点のいずれかはクリッピングにより生成されたものでも良い。線分を構成する各点の座標は、各幾何特徴に設定されているモデル座標系での表現であれば、それを実際の空間の尺度に拡大するときの倍率およびモデル座標系の位置姿勢を参照することで実際の空間に対応させることができる。

次に、概略位置姿勢計測装置７０からの概略位置姿勢の取得について説明する。概略位置姿勢計測装置７０は、撮像装置５０と動きを供にするセンサの情報から、撮像装置のおおよその位置姿勢を計測するものとする。本実施形態では、概略位置姿勢計測装置７０の情報をシリアル通信に取得するものとする。概略位置姿勢入力部１３０は、センサが設置された撮像装置５０のおおよその位置姿勢を、概略位置姿勢計測装置７０からシリアル通信で取得する。さらに、概略位置姿勢入力部１３０或いは概略位置姿勢計測装置７０において、適当なセンサのオフセット調整を実施しても構わない。

次に、本実施形態である位置姿勢計測装置１００の位置姿勢推定の処理について説明する。

画像取得部２００は、撮像装置入力部１１０を経由して伝送される撮像装置５０の映像信号を水平同期信号、垂直同期信号を参照して、撮像画像としてサンプリングし、２次元の画像データとしての撮像画像を保持する。ここで、位置姿勢計測装置１００の各部の処理は、撮像装置５０の撮像タイミングと同期して処理が行われるものとする。撮像タイミングの確認には、撮像装置５０から出力される映像同期信号を用いれば良い。また、撮像装置５０で使用する光学素子は歪みが少ないものが望ましいが、歪みがある光学素子を利用する場合は、事前にレンズの歪みを校正しておき、それを利用して補正すれば良い。より具体的には画像取得部２００には、使用するレンズの歪みを数式モデルで近似したパラメータを保持しておき、別の処理部から撮像画像の画素濃度を参照するようにする。これにより、レンズ歪みの補正式により補正された画素位置での濃度を参照するようにしておけば、近似できる歪みの範囲においては撮像した画像中のエッジの直線性も維持することが可能となる。

次に、本実施形態の位置姿勢計測装置１００における幾何特徴投影部３００の処理について説明する。ここで幾何特徴は、始点と終点の２点で構成されているものとする。さらに、幾何特徴の座標系は、基準座標系での表現に変換されているものとする。

基準座標系の点の座標［ｘ_w ｙ_w ｚ_w］^Tとカメラ座標系における撮像画像上の座標［ｕ_xｕ_y］^Tとは、透視投影変換の行列Ｐにより以下のような関係がある。

ここで、ｈは同次座標変換の表現として、求めたｈを用いて［ｕ_x ｕ_y］^Tを計算する。また、Ｒ₁₁,Ｒ₁₂,Ｒ₁₃, Ｒ₂₁, Ｒ₂₂, Ｒ₂₃,Ｒ₃₁, Ｒ₃₂,Ｒ₃₃を要素とする行列Ｒ（３ｘ３行列）を基準座標系における姿勢の回転行列、ｔ＝［ｔ_xｔ_y ｔ_z］を平行移動ベクトルとすると上式のＰは以下のように表せる。

ここで、ｆ_x、ｆ_yは水平・垂直方向の焦点距離であり、ｐ_x、ｐ_yは撮像画像面における水平、垂直方向の主点座標とする。

撮像装置５０で使われている光学素子の歪みに関するパラメータが事前に校正済みであるとすると、上式のパラメータの内、ｆ_x、ｆ_yおよびｐ_x、ｐ_yは既知となる。さらに、カメラの姿勢成分であるＲは、９つの成分を持っているが、実際には３つの独立なベクトル周りの回転による表現を用いれば良い。そこで、回転軸回転角を３変数で表現する方法によって、回転変換を表現する。つまり、任意の回転軸のベクトルを、

とし、回転角をｒ_aとすると、回転行列Ｒの表現は以下のようになる。

なお、回転軸に関してはｒ_x ²＋ｒ_y ²＋ｒ_z ²＝１の条件で、姿勢をωとすると、

となる。

幾何特徴の始点・終点に対して、透視投影変換を行いカメラ座標系における撮像画像上の点に変換することができる。

実際には、撮像画像は光学素子を通してＣＣＤまたはＣＭＯＳなどの撮像素子に結像した光をサンプリングしたものである。そのため、撮像素子の範囲は既定されている。透視投影変換の計算の結果、撮像画像領域に始点もしくは終点が含まれる幾何特徴のみを幾何特徴投影部３００では保持する。幾何特徴の始点、終点のどちらか１つのみが撮像画像領域にある場合は、当該線分と撮像画像領域の境界との交点をあらたな始点もしくは終点とする。このような処理は、コンピュータグラフィックスにおいてはクリッピング処理と呼ばれ、一般的な処理である。

さらに、投影幾何特徴が実際の環境では別の物体に隠蔽されて観察できない位置に存在する場合がある。その場合は、非特許文献１に述べられているように、複数の幾何特徴で構成される３次元モデルの面の情報を用いて、投影幾何特徴が観察できるかどうかを判別することができる。

次に、投影幾何特徴選択部４００について説明する。投影幾何特徴選択部４００は、幾何特徴投影部３００により投影された夫々の幾何特徴について、幾何特徴間の画面上（撮像画像上）の距離に基づき、位置姿勢の算出に使用するか否かを決定する処理を行う。

まず、投影幾何特徴選択部４００は、投影幾何特徴の撮像画像上の距離を求める。ｉ番目の投影幾何特徴について説明する。ｉ番目の投影幾何特徴は、その始点をPs(i)=[xs_i ys_i]^Tとし、終点を Pe(i)=[xei yei]^Tとする。投影幾何特徴の撮像画像上の距離Ｄ_iは、以下により算出される。

この距離Ｄ_iが、既定のしきい値よりＤ’より短い場合には、当該投影幾何特徴は以降のエッジとの距離を求める処理には使わないと決定する。一方、Ｄ’以上の長さを有している場合は、当該投影幾何特徴は、引き続きエッジとの距離による位置姿勢推定の処理で利用されるものとする。

なお、位置姿勢が僅かに変化する場合においては、ほぼ同じ投影長さとなるので、位置姿勢推定の繰り返し処理時には距離Ｄ_iの値をキャッシュしておくことで効率的な処理が行える。

更に本実施形態においては、該投影幾何特徴と他の投影幾何特徴との相互の距離を利用して、該投影幾何特徴を使用するか否かを決定する。本実施形態では、投影変換済みの投影幾何特徴のｉ番目の投影幾何特徴の始点Ps(i)および終点Pe(i)に対して、ｉ−１番目までに処理された投影幾何特徴のうちで、使用することが決定している投影幾何特徴の始点および終点との距離を求める。なお、ｉ番目の投影幾何特徴の始点Ps(i)及び終点Pe(i)はそれぞれ、Ps(i)=[xs_i ys_i]^T、Pe(i)=[xe_i ye_i]^Tである。そして、最も短い距離と既定のしきい値との比較を行い、しきい値より小さいものは使わないように決定する。

以上の処理により投影画像上で平行に隣接する投影幾何特徴のうち、既に使用することが決定している線分と近接している、もしくはＤ’以下の長さを有していると判定された線分は使用されなくなる。これらの選択処理を行うことで、後述の画撮像画像からのエッジ探索で行われる画像領域への値参照回数が削減され、効率的な処理が行える。また、近接する投影幾何特徴のエッジ探索領域の重複を利用することで、撮像装置と計測対象物体との位置関係の変化による観察されるエッジの変化に対しても動的に処理することができる。また、Ｄ’以下の長さを有していると判定された線分を使用しないことにより、線分の方向を決定するのに十分な情報が得られないような線分を計算対象から除外できる。

次に、投影幾何特徴探索設定部５００は、幾何特徴投影部３００により透視投影変換された投影幾何特徴のうち、投影幾何特徴選択部４００により使用すると決定された投影幾何特徴に対して、撮像画像上のエッジ探索を行う点を計算する。以下、図３を参照して投影幾何特徴探索設定部５００による画像幾何特徴の探索開始点、探索対応点（探索終了点）の設定を説明する。

図３において、ｉ番目の投影幾何特徴３０１（本例では、投影計測線分）について説明する。ｉ番目の投影幾何特徴３０１は、その始点をPs(i)=[xs_i ys_i]^Tとし、終点をPe(i)=[xe_iye_i]^Tとする。Ps(i)とPe(i)を結ぶベクトルｖ_iは、

となり、その正規化ベクトルは、

となる。

次に、Ps(i)とPe(i)の線分上に等間隔な探索開始点ｃ_(i,m)を設定する。ここで、ｍの範囲は０からＭ_iとし、予め既定されている探索線分間隔Ｂを用いてＭ_iを次式のように求める。

上式において、[ ]（ガウス記号）は、ある値を越えない最大の整数値を表すものとする。得られたＭ_iを用いて、実際の探索間隔ｂ_iは、

となる。すると、探索開始点ｃ_(i,m)は、

となる。

各探索開始点ｃ_(i,m)から投影計測線分に垂直な方向へ撮像画像上のエッジを探索する。その探索範囲は、各探索開始点ｃ_(i,m)から探索対応点ｓ⁺ _(i,m)およびｓ^- _(i,m)までの線分領域であり、この線分領域に沿って画像幾何特徴であるエッジが探索されるものとする。ここで、投影計測線分の正規化ベクトル、

の垂直ベクトルを、

とすると、

という関係がある。以下、上記の垂直ベクトルを「正規化垂直ベクトルｖ_⊥i」とも記載する。

探索対応点と探索開始点までの距離をｄとすると、探索対応点ｓ⁺ _(i,m)の位置は、

となり、反対側の探索対応点ｓ^- _(i,m)は、

となる。

なお、ここでは、正規化垂直ベクトルｖ_⊥iの方向を投影幾何特徴に対して垂直に設定したが、その探索方向については投影幾何特徴の近傍の画像幾何特徴が探索できる方向を設定できさえすれば良い。

具体的には、正規化垂直ベクトルｖ_⊥iを上、下、左、右の４方向もしくは、それに右上、右下、左上、左下を加えた８方向に量子化したベクトルを使っても構わない。正規化垂直ベクトルｖ_⊥iとの差が小さい量子化ベクトル方向を選択することにより、画像幾何特徴の探索を効率的に処理できる。

さらに、幾何特徴の探索方向を事前に３次元モデルに記述しておき、探索方向を透視投影変換したベクトルを用いても構わない。

画像幾何特徴探索部６００は、画像取得部２００から得られる撮像画像上の、投影幾何特徴探索設定部５００により設定された探索開始点から探索対応点までの画素の濃度に基づいて画像幾何特徴を検出する。例えば、読み取った隣接する画素の濃度の濃度勾配が大きい点（所定の閾値以上の点）を検出し、そのときの注目画素の位置を画像幾何特徴検出位置として保持する。

さらに、画像取得部２００により画像が取得された時間からの経過時間を参照し、その経過時間が既定された値を越えたかどうかを判別し、越えた場合は検出処理を中断するようにすることも全体の処理を効率化する上で有効である。

位置姿勢算出部７００は、画像幾何特徴探索部６００により検出された撮像画像上の画像幾何特徴の位置と、それに対応する計測対象物体の投影計測線分の位置との距離に基づいて、計測対象物体と撮像装置との相対的な位置姿勢を算出し更新する。ここで、位置姿勢算出部７００による撮像装置の位置姿勢の算出、更新処理について説明する。

推定している位置姿勢から観測されるであろう計測対象物体の幾何特徴の投影位置と、実際の撮像画像面上の画像幾何特徴の観測位置との差が小さくなるように最適化計算を行うことで求めることができる。本実施形態では、上述したように、幾何特徴として計測線分が用いられる。

ここでは、画像幾何特徴の観測位置と計測対象物体の幾何特徴の投影位置との距離を誤差として、これを誤差関数Ｅとする。誤差関数Ｅは、カメラの位置ｔ＝［ｔ_x ｔ_y ｔ_z］^T、回転軸回転角による姿勢ω＝［ω_x ω_y ω_z］^T、観測対象点の基準座標系での位置ｘ_w＝［ｘ_w ｙ_w ｚ_w］^Tを入力とする関数とする。

投影位置をｕ’＝［ｕ’_x ｕ’_y］^T、観測位置をｕ＝［ｕ_x ｕ_y］^Tとすると、撮像画像上での誤差関数Ｅは、それらの差として表現され得る。

一般に、位置姿勢および観測対象点の位置の関係は、非線形なので上式の誤差関数Ｅも入力に対して非線形関数となる。本実施形態では、投影幾何特徴として線分を用いているので、エッジ探索により検出された観測点ｕ＝［ｕ_x ｕ_y］^Tと、その点から投影線分へ垂線を下ろした交点を対応する投影点をｕ”＝［ｕ”_x ｕ”_y］^Tとする。そして、

を誤差関数として用いる。

撮像装置の位置姿勢が僅かに変化する範囲において、上式を１次のテイラー展開を用いて線形近似すると以下のように表せる。

Ｅ’₀は、カメラの位置ｔ⁰＝［ｔ_x ⁰ ｔ_y ⁰ ｔ_z ⁰］^T、姿勢ω⁰＝［ω_x ⁰ ω_y ⁰ ω_z ⁰］^T、対象点座標ｘ_w ⁰＝［ｘ_w ⁰ ｙ_w ⁰ｚ_w ⁰］^T、観測位置ｕ⁰＝［ｕ_x ⁰ｕ_y ⁰］^Tを代入したときの投影位置と観測位置の差、つまり誤差を示す。これにより、ｔ⁰,ω⁰,ｘ⁰の近傍において、線形化された式を用いてΔｔ_x,Δｔ_y,Δｔ_z,Δω_x,Δω_y,Δω_z,Δｘ_w,Δｙ_w,Δｚ_wを求め、ｔ⁰,ω⁰,ｘ⁰に補正値として加えることによって、誤差を少なくすることができる。

通常、方程式の数と変数の数は一致しないため、多数の参照点の情報を用いて最小二乗法によって補正値Δを求める。そこで、補正式Δについてまとめる。まず、各変数の偏微分をまとめた行列をＪとし、補正ベクトルΔと誤差ベクトルＥ’は上式より、

と書くことができる。Ｊの転置行列Ｊ^Tを左右に掛け合わせて、補正ベクトルΔの式としてまとめる以下のように表せる。

補正ベクトルΔの値が小さくなるまで更新することで、精度の良い位置姿勢を求めることが可能となる。上述した方法は、Ｎｅｗｔｏｎ法による求解法を示しているが他の方法により最適化計算を行っても構わないことは明らかである。

尚、本実施形態においては、画像幾何特徴の位置と計測対象物体の幾何特徴を撮像画像上に投影した投影幾何特徴との観測画像上での距離を小さくすることができれば良いので、公知の非線形最適化手法を適用することができる。例えば、おおよその推定中の位置姿勢に対して、その各変数値の周辺に僅かな差を有するランダムな組み合わせを多数生成して、それぞれの組み合わせに対して誤差を求めて、誤差の値が小さい組み合わせを利用する方法でも同様な効果は得られる。

また、位置姿勢算出部７００により処理された誤差が既定の値を下回ると、該当する撮像画像からの位置姿勢推定が終了したものとして、その結果を位置姿勢出力部１４０より出力する。誤差が既定の値より大きい場合は、更新された位置姿勢を幾何特徴投影部３００に渡し、更新された位置姿勢を用いてエッジの探索結果による位置姿勢推定を行う。位置姿勢算出部７００での更新回数が既定した値を越える場合は、位置姿勢の推定がうまくいかなかったことを意味する。その場合、本実施形態では、概略位置姿勢入力部１３０の値を位置姿勢出力部１４０に出力する。このような処理により、位置姿勢計測装置１００は、精度の悪い概略位置姿勢であるが、継続して位置姿勢の算出結果を出力することができる。その際、位置姿勢がうまくいったかどうかを示す値も一緒に位置姿勢出力部１４０に出力するようにしてもよい。そのようにすれば、位置姿勢計測装置１００の利用者が、出力された位置姿勢の計測値が高精度に補正されたものであるか否かを判断できるように構成することができる。例えば、複合現実感技術を利用するアプリケーションでは、位置姿勢の計測が継続して行われていることが前提となっているが、位置姿勢計測の処理がうまくいかなかったという情報をもとに、アプリケーションの処理を変更することが可能となる。

なお、位置姿勢の全６パラメータを求める場合について述べたが、他の手段で一部のパラメータを求め残りのパラメータを求める場合も同様に取り扱える。例えば、撮像装置が三脚に固定されている環境において姿勢を求める場合や、重力軸に対する傾斜を計測するセンサによる傾斜角度の計測値を利用して位置および方位の４パラメータを最適化する場合も同様に扱うことができる。

なお、上記の第１実施形態においては、幾何特徴として計測対象物体を構成する３次元モデルの線分を利用した例を示したが、実際には線分でなくても構わない。例えば、点や円や楕円などの幾何特徴であっても本発明は適用できる。

例えば、幾何特徴が点の場合においても、投影画像上における分布は撮像装置と計測対象物体の相対位置に応じて変化する。投影画像上に既に投影された点と、該幾何特徴の投影点の撮像画像上の距離が１画素未満である場合は、夫々の画像幾何特徴を分離して検出することが困難である。そのため、本実施形態における投影幾何特徴選択部４００において、上述の実施形態と同様な処理を行うことで、課題を解決することができる。

以上のように、第１実施形態によれば、投影された幾何特徴の位置関係に基づいて、位置姿勢計算に利用すべき幾何特徴が適切に選択される。そのため、撮像装置と計測対象物体との相対位置に影響されることなく効率的に撮像装置の位置姿勢を求めることが可能となる。

［第２実施形態］
前述の第１実施形態においては、投影幾何特徴の位置に対応する撮像画像上での画像幾何特徴を探索し、それらの距離が小さくするように位置姿勢を推定する方法について述べた。撮像画像上の画像幾何特徴検出における線分検出では、画像処理にて一般的なSobelオペレータやCannyオペレータを用いることが可能である。ここでは、撮像画像から画像処理により検出された線分（適宜エッジと呼称する）をエッジ素と呼ぶ。

投影幾何特徴とエッジ素との対応は、概略位置姿勢が得られているとすると、投影幾何特徴の近傍のエッジ素のうち、長さ、方向などが類似するものを対応付けし、そのエッジ素との距離が小さくなるように位置姿勢を推定することができる。この場合も、幾何特徴について、撮像画像上での距離に応じて使用するか否かを決定することで、冗長な対応付け処理を省くことができる。

［第３実施形態］
計測対象物体が建物の構造全てを記述しているような膨大な線分を有する３次元モデルである場合、撮像装置５０と計測対象物体との距離に応じて位置姿勢推定に必要な幾何特徴が大幅に変化する。そのため、効率的に処理を行わないとカメラの撮像間隔内に位置姿勢推定を実施することが難しくなる。実際には、位置姿勢の推定に必要な幾何特徴は、撮像画像上においてある程度の長さがあり、方向も様々なものがあったほうが良い。

そこで、位置姿勢推定に必要な幾何特徴の処理順序を考慮して処理することで、画像幾何特徴探索処理が中断したとしても位置姿勢推定に必要な幾何特徴の処理が済んでいるので位置姿勢精度を維持しながら、膨大な幾何特徴の処理を実施することが可能となる。

図４はモデルの処理順序を決める幾何特徴ソート手段を実現する幾何特徴ソート部を有する、第３実施形態の位置姿勢計測装置１００の構成を示すブロック図である。なお、幾何特徴ソート部８００以外の構成は、第１実施形態において説明しているので、ここでは、幾何特徴ソート部８００の処理について述べる。

第３実施形態では、幾何特徴ソート部８００は、幾何特徴が線分の場合には、撮像画像上における投影幾何特徴の長さを算出し、長い順にソート処理を行うものとする。また、幾何特徴として点とその周辺の画素分布を利用する場合は、画素分布を既定する分布値を用いてソート処理することができる。なお、ここでは、投影幾何特徴の長さについて述べるが、実際には位置姿勢推定の際に有効な画像幾何特徴検出が行える線分を優先的に処理できるようにソートしさえすれば、第３実施形態の効果を達成することができる。

例えば、撮像画像上での画像幾何特徴検出が容易な幾何特徴の優先度を高くしてソートする方法がある。初期化時に優先度を全て未検出の値に設定し、一度全ての投影幾何特徴の検出を行う。次に、撮像画像上の画像幾何検出を行った際の隣接間画素の濃度差を保持しておき、その濃度差をソートのキーとすることができる。また、視線方向が変わり観察される幾何特徴が異なれば、優先度が未検出に設定されている幾何特徴はエッジ探索処理まで行うようにすることで動的に変化する場合でも対応できる。また、幾何特徴投影部３００におけるカメラ座標系への変換の際には、幾何特徴のカメラ視点位置からの奥行き方向も算出される。そこで、幾何特徴ソート部８００が奥行き方向が視点に近いものからソートすることで、位置姿勢精度に影響の大きいエッジを先の処理することが可能となる。

また、幾何特徴ソート部８００において既定する数の幾何特徴のソート結果を保持するテーブルを用意し、既定数を越えるソート結果は保持しないという方法を採用することも可能であり、これは、本発明の処理の効率化という点で有効である。

［第４実施形態］
次に、本実施形態である位置姿勢計測装置１００を複合現実感に適用した例を示す。

図５は、第４実施形態による位置姿勢計測装置１００を複合現実感技術による計測対象物体への仮想物体ＣＧの重合へ適用したときの様子を示す模式図である。図５において、計測対象物体１０は、既にその形状が既知の３次元の物体であるとする。曲面を有する構成面は、複数の面で構成されているものとする。また、計測対象物体の観察角度は特に既定していない。

撮像装置５０で撮像した画像は、撮像装置入力部１１０より位置姿勢計測装置１００へ入力される。また、画像を合成するためにクロマキー合成装置９２０の画像入力部９２１にも接続されている。位置姿勢計測装置１００では、計測対象物体１０の３次元モデル６０が既に登録されているものとする。

撮像画像中に計測対象物体１０が存在するときに、位置姿勢計測装置１００は撮像装置５０の位置姿勢測定結果を位置姿勢出力部１４０より出力する。ＣＧ描画装置９１０は、撮像装置５０の位置姿勢をＣＧの視点位置とするように構成された位置姿勢入力部９１１を有し、仮想物体をＣＧ描画した画像をＣＧ画像出力部９１２より出力する。ここでは、ＣＧ描画装置９１０は計測対象物体１０の上に仮想物体である２つの円柱９４１を描画し、仮想物体ＣＧ画像９４０のような画像をＣＧ画像出力部９１２より出力する。なお、仮想物体ＣＧ画像９４０の背景色はクロマキー合成装置９２０で設定したクロマキー対応色とする。

クロマキー合成装置９２０は、画像入力部９２１から撮像装置５０の撮像画像を入力し、クロマキー対象画像入力部９２２から仮想物体ＣＧ画像９４０を取り込む。そして、クロマキー合成装置９２０は、クロマキー対応色とした領域を透明にした仮想物体ＣＧ画像９４０を撮像画像上に合成して、得られた合成画像９３０をクロマキー合成画像出力部９２３より出力する。以上の処理の結果、出力された合成画像９３０は、撮像した計測対象物体の上に仮想物体ＣＧである円柱９４が２つ描画されたものとなる。

例えば、第４実施形態において、実際の部品とＣＡＤで設計中の仮想部品を画面で観察し、部品と部品の干渉などを確認することが可能となる。また、合成画像９３０を頭部装着型ディスプレイ（ＨＭＤ：Head Mount Display）を用いて観測することで、作業者による上記確認も容易になるので、好ましい。

その際、撮像装置と視線方向を同じ向きに設定していれば、頭部の位置姿勢と観察している現実空間を一致することができるため複合現実感による作業性の向上が期待できる。さらに、位置姿勢計測装置１００とＣＧ描画装置９１０とクロマキー合成装置９２０を同一の計算機上のプログラムで実現することも可能である。

［他の実施形態］
本発明の実施形態の一例として上述の構成例を説明したが、一般的にはカメラと本発明の処理手段を実施する装置があれば良い。例えば、本発明の処理構成を携帯電話や携帯型のコンピュータで実施するプログラムという形で処理を実行させても同じ効果が得られる。さらに、位置姿勢の計測結果を利用して、近隣の施設や場所に関する情報を呈示するような２次的なサービスを実施することも可能である。

以上、実施形態を詳述したが、本発明は、例えば、システム、装置、方法、プログラムもしくは記憶媒体等としての実施態様をとることが可能である。具体的には、複数の機器から構成されるシステムに適用しても良いし、また、一つの機器からなる装置に適用しても良い。

尚、本発明は、ソフトウェアのプログラムをシステム或いは装置に直接或いは遠隔から供給し、そのシステム或いは装置のコンピュータが該供給されたプログラムコードを読み出して実行することによって前述した実施形態の機能が達成される場合を含む。この場合、供給されるプログラムは実施形態で図に示したフローチャートに対応したプログラムである。

従って、本発明の機能処理をコンピュータで実現するために、該コンピュータにインストールされるプログラムコード自体も本発明を実現するものである。つまり、本発明は、本発明の機能処理を実現するためのコンピュータプログラム自体も含まれる。

その場合、プログラムの機能を有していれば、オブジェクトコード、インタプリタにより実行されるプログラム、ＯＳに供給するスクリプトデータ等の形態であっても良い。

プログラムを供給するための、コンピュータ読み取り可能な記憶媒体としては以下が挙げられる。例えば、フロッピー（登録商標）ディスク、ハードディスク、光ディスク、光磁気ディスク、ＭＯ、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、ＣＤ−ＲＷ、磁気テープ、不揮発性のメモリカード、ＲＯＭ、ＤＶＤ（ＤＶＤ−ＲＯＭ，ＤＶＤ−Ｒ）などである。

その他、プログラムの供給方法としては、クライアントコンピュータのブラウザを用いてインターネットのホームページに接続し、該ホームページから本発明のコンピュータプログラムをハードディスク等の記録媒体にダウンロードすることが挙げられる。この場合、ダウンロードされるプログラムは、圧縮され自動インストール機能を含むファイルであってもよい。また、本発明のプログラムを構成するプログラムコードを複数のファイルに分割し、それぞれのファイルを異なるホームページからダウンロードすることによっても実現可能である。つまり、本発明の機能処理をコンピュータで実現するためのプログラムファイルを複数のユーザに対してダウンロードさせるＷＷＷサーバも、本発明に含まれるものである。

また、本発明のプログラムを暗号化してＣＤ−ＲＯＭ等の記憶媒体に格納してユーザに配布するという形態をとることもできる。この場合、所定の条件をクリアしたユーザに、インターネットを介してホームページから暗号を解く鍵情報をダウンロードさせ、その鍵情報を使用して暗号化されたプログラムを実行し、プログラムをコンピュータにインストールさせるようにもできる。

また、コンピュータが、読み出したプログラムを実行することによって、前述した実施形態の機能が実現される他、そのプログラムの指示に基づき、コンピュータ上で稼動しているＯＳなどとの協働で実施形態の機能が実現されてもよい。この場合、ＯＳなどが、実際の処理の一部または全部を行ない、その処理によって前述した実施形態の機能が実現される。

さらに、記録媒体から読み出されたプログラムが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれて前述の実施形態の機能の一部或いは全てが実現されてもよい。この場合、機能拡張ボードや機能拡張ユニットにプログラムが書き込まれた後、そのプログラムの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行なう。

第１実施形態に係る位置姿勢計測装置の機能構成を示す図である。 視点の変化に伴い観察される画像幾何特徴の分布の違いを示した図である。 第１実施形態に係る位置姿勢計測装置における、投影幾何特徴としての線分における画像幾何探索領域の探索開始点および探索対応点を説明した図である。 本発明の一実施形態に係る位置姿勢計測装置の機能構成を示す図である。 本発明の一実施形態に係る位置姿勢計測装置を複合現実感利用に適用したときの構成を示す図である。

Claims (7)

1. 一つ以上の計測対象物体を撮像する撮像装置と前記計測対象物体との間の相対的な位置姿勢を、前記計測対象物体の３次元モデルを利用して計測する位置姿勢計測装置であって、
   前記撮像装置により撮像画像を取得する画像取得手段と、
   前記撮像装置の位置姿勢に基づいて、前記３次元モデルにおける複数の幾何特徴の夫々を前記撮像画像上に投影して、前記撮像画像上に複数の投影幾何特徴を得る投影手段と、
   前記撮像画像上の投影幾何特徴の相互の距離が所定の距離よりも大きい投影幾何特徴を、前記投影手段で得られた前記複数の投影幾何特徴から位置姿勢の算出に利用すべき投影幾何特徴として選択する選択手段と、
   前記選択手段により選択された投影幾何特徴と、前記撮像画像上から検出される前記選択された投影幾何特徴に対応する画像幾何特徴とを用いて、前記撮像装置と該計測対象物体との間の相対的な位置姿勢を算出する算出手段とを備えることを特徴とする位置姿勢計測装置。
2. 一つ以上の計測対象物体を撮像する撮像装置と前記計測対象物体との間の相対的な位置姿勢を、前記計測対象物体の３次元モデルを利用して計測する位置姿勢計測装置であって、
   前記撮像装置により撮像画像を取得する画像取得手段と、
   前記撮像装置の位置姿勢に基づいて、前記３次元モデルにおける複数の幾何特徴の夫々を前記撮像画像上に投影して、前記撮像画像上に複数の投影幾何特徴を得る投影手段と、
   前記撮像画像上の複数の投影幾何特徴の位置と状態に基づいて当該複数の投影幾何特徴のそれぞれの位置姿勢の算出への利用の適否を判断することにより、前記投影手段で得られた前記複数の投影幾何特徴より位置姿勢の算出に利用すべき投影幾何特徴を選択する選択手段と、
   前記選択手段により選択された投影幾何特徴と、前記撮像画像上から検出される前記選択された投影幾何特徴に対応する画像幾何特徴とを用いて、前記撮像装置と該計測対象物体との間の相対的な位置姿勢を算出する算出手段と、
   前記投影手段によって得られた前記計測対象物体の複数の投影幾何特徴について、前記選択手段によって選択されるか否かが決定される順序を設定する幾何特徴ソート手段とを備えることを特徴とする位置姿勢計測装置。
3. 一つ以上の計測対象物体を撮像する撮像装置と前記計測対象物体との間の相対的な位置姿勢を、前記計測対象物体の３次元モデルを利用して計測する位置姿勢計測装置であって、
   前記撮像装置により撮像画像を取得する画像取得手段と、
   前記撮像装置の位置姿勢に基づいて、前記３次元モデルにおける複数の幾何特徴の夫々を前記撮像画像上に投影して、前記撮像画像上に複数の投影幾何特徴を得る投影手段と、
   前記撮像画像上の複数の投影幾何特徴の位置と状態に基づいて当該複数の投影幾何特徴のそれぞれの位置姿勢の算出への利用の適否を判断することにより、前記投影手段で得られた前記複数の投影幾何特徴より位置姿勢の算出に利用すべき投影幾何特徴を選択する選択手段と、
   前記選択手段により選択された投影幾何特徴と、前記撮像画像上から検出される前記選択された投影幾何特徴に対応する画像幾何特徴とを用いて、前記撮像装置と該計測対象物体との間の相対的な位置姿勢を算出する算出手段とを備え、
   前記投影幾何特徴は線分であり、
   前記選択手段は、前記撮像画像上における線分の長さが所定の長さよりも長い投影幾何特徴を位置姿勢の算出に利用すべき投影幾何特徴として選択することを特徴とする位置姿勢計測装置。
4. 前記投影手段は、前記撮像画像上から検出することができる画像幾何特徴に対応する計測対象物体の３次元モデルを構成する線分を前記撮像画像に投影して投影幾何特徴を得ることを特徴とする請求項１乃至３のいずれか１項に記載の位置姿勢計測装置。
5. 前記算出手段は、
   前記投影手段により得られた投影幾何特徴に対して、前記撮像画像上における１つ以上の点を設定し、各点における探索方向を求める設定手段と、
   前記設定手段により設定された点と探索方向に基づき、前記撮像画像上の画像幾何特徴を探索する探索手段とを有することを特徴とする請求項１乃至４のいずれか１項に記載の位置姿勢計測装置。
6. 一つ以上の計測対象物体を撮像する撮像装置と前記計測対象物体との間の相対的な位置姿勢を、前記計測対象物体の３次元モデルを利用して計測する位置姿勢計測装置の制御方法であって、
   前記位置姿勢計測装置の画像取得手段が、前記撮像装置から撮像画像を取得する画像取得工程と、
   前記位置姿勢計測装置の投影手段が、前記撮像装置の位置姿勢に基づいて、前記３次元モデルにおける複数の幾何特徴の夫々を前記撮像画像上に投影して、前記撮像画像上に複数の投影幾何特徴を得る投影工程と、
   前記位置姿勢計測装置の選択手段が、前記撮像画像上の投影幾何特徴の相互の距離が所定の距離よりも大きい投影幾何特徴を、前記投影工程で得られた前記複数の投影幾何特徴から位置姿勢の算出に利用すべき投影幾何特徴として選択する選択工程と、
   前記位置姿勢計測装置の選択手段が、前記投影工程で得られた前記投影幾何特徴の夫々に関して、他の投影幾何特徴との間の前記撮像画像上の距離に基づいて投影幾何特徴を選択する選択工程と、
   前記位置姿勢計測装置の算出手段が、前記選択工程で選択された投影幾何特徴と、前記撮像画像上から検出される前記選択された投影幾何特徴に対応する画像幾何特徴とを用いて、前記撮像装置と該計測対象物体との間の相対的な位置姿勢を算出する算出工程とを有することを特徴とする位置姿勢計測装置の制御方法。
7. コンピュータを、請求項１乃至請求項５のいずれか１項に記載の位置姿勢計測装置の各手段として機能させるためのプログラム。

Images