JP2008309631A - 情報処理方法及び情報処理装置 - Google Patents

Abstract

【課題】 撮像装置により撮影された画像情報に基づいて行う撮像装置又は物体の位置及び姿勢の計測を安定化させること。
【解決手段】 撮像装置１００で撮影した画像を画像取得部１１０で取得し、そのときの姿勢をセンサ計測値取得部１２０で取得し、直線検出１３０が撮影画像から直線を検出する。センサ計測値、検出した直線の３次元空間における方程式に基づき、位置姿勢計測部１５０が、撮像装置１００の位置姿勢、又は、観察対象物体１０の位置姿勢を算出する。
【選択図】 図１

Description

本発明は、情報処理方法及び情報処理装置に関し、特に、画像を用いた撮像装置または物体の位置及び姿勢を計測する情報処理方法及び情報処理装置に関する。

近年、現実空間に仮想空間の情報を重ね合わせて表示する拡張現実感（Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ、以下ＡＲ）技術の研究が盛んである。ＡＲ技術の情報提示装置としては、ビデオシースルー型のヘッドマウントディスプレイ（Ｈｅａｄ Ｍｏｕｎｔｅｄ Ｄｉｓｐｌａｙ、以下ＨＭＤ）が代表的である。ビデオシースルー型ＨＭＤには現実空間を撮影するカメラが内蔵されており、このカメラの現実空間における位置及び姿勢に応じて、仮想物体がコンピュータグラフィクスにより描画される。さらに描画された仮想物体を現実空間の画像上に重畳した合成画像が、液晶パネルなどのＨＭＤの表示デバイス上に表示される。この情報提示装置によって、利用者は、あたかも仮想物体が現実空間中に実在するかのように感じることができる。

ＡＲ技術を実現する上で解決しなければならない大きな問題の一つとして「位置合わせ」の問題がある。ＡＲにおける位置合わせとは、仮想物体と現実空間との間の幾何学的な整合性を取ることである。仮想物体が現実空間に実在するように利用者が感じるためには、位置合わせが正しく行われ、仮想物体が常に現実空間中の存在すべき位置に存在しているように利用者に提示されなければならない。ビデオシースルー型ＨＭＤを利用する場合には、位置合わせの問題は、ＨＭＤに内蔵したカメラの現実空間に設定された座標系における位置及び姿勢を計測する問題となる。カメラの位置及び姿勢の計測は、例えば磁気センサや超音波センサなど６自由度のカメラの位置及び姿勢を計測する物理センサによって行うことが可能である。

ところで、ビデオシースルー型ＨＭＤを利用する場合には、ＨＭＤに内蔵されているカメラからの画像情報を、カメラの位置及び姿勢を計測するために利用することが可能である。このような画像情報を利用する計測方法は、物理センサを利用する方法に比べて手軽でかつ低コストであるため広く利用されている。

非特許文献１では、現実空間や現実物体を線分によって記述した３次元幾何モデルと、カメラが撮影する画像上のエッジ情報を利用してカメラの位置及び姿勢を計測する方法が開示されている。ここでエッジとは、画像上において濃度が急激に変化する点のことを表す。この方法では、画像上で検出される多数のエッジが、カメラの位置及び姿勢に基づいて画像上に投影された３次元幾何モデルの線分に当てはまるようにカメラの位置及び姿勢を算出する。より具体的には、予め入力されるカメラの位置及び姿勢の予測値をもとに３次元幾何モデルの線分を画像に仮想的に投影し、投影された線分の近傍においてエッジ検出を行う。そして、検出されたエッジが投影された線分上に存在するように、カメラの位置及び姿勢の予測値を補正することを繰り返すことにより、カメラの位置及び姿勢を算出する。

前述のエッジを用いたカメラの位置及び姿勢の計測方法では、カメラの位置及び姿勢の予測値として、主に前フレームにおけるカメラの位置及び姿勢が利用される。そのため、あるフレームにおいて位置及び姿勢の計測に失敗した場合には、その後のフレームにおける位置及び姿勢の計測を行うことができない。計測の失敗は、例えばカメラを高速に移動させた場合に起こる。そのような場合には、画像にモーションブラーが発生してエッジが検出されなかったり、カメラの位置及び姿勢の予測値に基づいて投影される線分の近傍にエッジが存在しなかったりするため、位置及び姿勢の計測に失敗しやすい。また、最初のフレームにおいては、前フレームにおけるカメラの位置及び姿勢を得ることができない。

そのため、エッジを用いたカメラの位置及び姿勢を実際に利用するためには、最初のフレームや計測に失敗した直後に、カメラの位置及び姿勢の予測値を利用しないでカメラの位置及び姿勢の計測を行う「初期化処理」が必要となる。

その方策の一つとして、予め決められた初期位置及び初期姿勢を設定しておき、カメラを初期位置及び初期姿勢になるように移動した後に、該初期位置及び初期姿勢を予測値としてエッジを用いたカメラの位置及び姿勢を初期化することが行われている。

別の方策として、画像上で検出される特徴から、カメラの位置及び姿勢の予測値を利用しないでカメラの位置及び姿勢の計測を行う方法がある。本発明は、カメラの位置及び姿勢の予測値を利用しないでカメラの位置及び姿勢の計測を行う方法に関するものである。例えば非特許文献２では、画像上で検出される直線と、それに対応する３次元空間における直線の対応から、カメラの位置及び姿勢の計測を行う方法が開示されている。この方法では、少なくとも８本以上の直線の対応に基づいて立式される線形方程式を解くことにより、カメラの位置及び姿勢を算出する。非特許文献２の方法では、画像上で検出される直線とそれに対応する３次元空間における直線を対応付ける方法は示されていない。そのため、画像上の直線と３次元空間中の直線の対応が未知の状態から、非特許文献２で開示される方法を用いてカメラの位置及び姿勢を計測するためには、何らかの方策により画像上で検出される直線と３次元空間中での直線との対応を求める必要がある。一般的には、画像上での直線と３次元空間での直線とをランダムに対応付けてカメラの位置及び姿勢を算出し、最も整合性の高い対応を用いたときの算出結果を最終的なカメラの位置及び姿勢として出力する。

また、特許文献１では、カメラに傾斜角センサを装着し、傾斜角センサの計測値と画像情報（点の情報）を併用してカメラの位置及び姿勢を計測することで、位置及び姿勢の計測を安定化している。また、特許文献２では、カメラに傾斜角センサを装着し、傾斜角センサの計測値と画像上における直線の情報を利用して、カメラの方位角を算出している。
特開２００６−２９２４１７号公報 特開２００４−１０８８３６号公報 Ｔ．Ｄｒｕｍｍｏｎｄ ａｎｄ Ｒ．Ｃｉｐｏｌｌａ，"Ｒｅａｌ−ｔｉｍｅ ｖｉｓｕａｌ ｔｒａｃｋｉｎｇ ｏｆ ｃｏｍｐｌｅｘ ｓｔｒｕｃｔｕｒｅｓ"，ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．２４，ｎｏ．７，ｐｐ．９３２−９４６，２００２． Ｙ．Ｌｉｕ，Ｔ．Ｓ．Ｈｕａｎｇ，ａｎｄＯ．Ｄ．Ｆａｕｇｅｒａｓ，"Ｄｅｔｅｒｍｉｎａｔｉｏｎ ｏｆ ｃａｍｅｒａ ｌｏｃａｔｉｏｎ ｆｒｏｍ ２−Ｄ ｔｏ ３−Ｄ ｌｉｎｅ ａｎｄ ｐｏｉｎｔ ｃｏｒｒｅｓｐｏｎｄｅｎｃｅｓ"，ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．１２，ｎｏ．１，ｐｐ．２８−３７，１９９０．

非特許文献２に開示された方法によりカメラの位置及び姿勢を計測する方法では、直線の検出誤差や画像解像度が有限であることに起因して、安定に位置及び姿勢を算出できない場合がある。特許文献１で開示される方法は、位置及び姿勢の計測を安定化させる方法であるが、画像上の点と３次元空間中の点の対応を用いる方法であるため、直線の対応の場合には用いることができない。両端の３次元座標が既知である線分を画像から検出することが可能であれば、線分を２つの点とみなすことで特許文献１の方法を適用することができる。しかしながら、一般的に線分の端点を精度よく検出することは困難であるため、特許文献１の方法を適用することはできない。一方、特許文献２で開示される方法では、１枚の画像からはカメラの方位角しか算出することができず、カメラの位置を算出することができない。

また、画像上の直線と３次元空間中の直線との対応が未知である場合に、それらをランダムに対応付けて位置及び姿勢を複数算出しその中から最も整合性の高いものを選択する方法では、正解の位置及び姿勢を含むように多数の位置及び姿勢を算出する必要がある。さらに各算出された位置及び姿勢について、整合性を評価する必要がある。そのため、最終的な解を得るまでに多大な時間を必要とする。そのため、この方法をエッジを用いたカメラの位置及び姿勢の初期化に用いると、初期化に多大な時間がかかり、利用者は位置姿勢計測が失敗するたびに長時間待つ必要が生じるため、利用者の利便性を損なってしまうという解決すべき課題がある。

本発明は以上の問題に鑑みてなされたものであり、カメラの位置及び姿勢の予測値を利用せずに画像を用いて行うカメラの位置及び姿勢の計測を安定化することを目的とする。また、本発明の別の目的は、カメラの位置及び姿勢の予測値を利用せずに画像を用いて行うカメラの位置及び姿勢の計測の処理時間を短くすることを目的とする。

上記目的を達成するために、本発明の情報処理方法は、物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値の入力工程と、前記撮像装置から撮影画像を入力する画像の入力工程と、前記撮影画像から直線を検出する直線検出工程と、前記傾斜角計測値、前記検出された直線の撮影画像での方程式および該直線に対応する３次元空間中における直線の３次元空間中での方程式に基づき、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を算出する位置姿勢算出工程とを備える。

また、本発明の情報処理方法は、物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値の入力工程と、前記撮像装置から撮影画像を入力する画像の入力工程と、前記撮影画像から直線を検出する直線検出工程と、前記検出された直線の夫々と、前記物体を表す線分モデルを構成する線分の夫々の方向ベクトルとが仮に対応するとみなして、夫々の組合せについて方位角を求めて投票することにより方位角候補値を算出する方位角候補値算出工程とを備える。

また、本発明の情報処理方法は、物体または撮像装置の位置及び姿勢を計測する情報処理方法であって、物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値の入力工程と、前記撮像装置から撮影画像を入力する画像の入力工程と、前記撮影画像から検出される直線と前記傾斜角センサの計測値に基づいて物体または撮像装置の位置と姿勢を算出する第一の位置姿勢算出工程と、前記撮影画像から検出される特徴に基づいて物体または位置及び姿勢の予測値に対する補正値を算出することにより物体または撮像装置の位置と姿勢を算出する第二の位置姿勢算出工程とを備え、前記位置及び姿勢の予測値が得られない場合には第一の位置姿勢算出工程を、前記位置及び姿勢の予測値が得られる場合には第二の位置姿勢算出工程を選択する選択工程とを備える。

更に、本発明の情報処理装置は、物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値の入力手段と、前記撮像装置から撮影画像を入力する画像の入力手段と、前記撮影画像から直線を検出する直線検出手段と、前記傾斜角計測値、前記検出された直線の撮影画像での方程式および該直線に対応する３次元空間中における直線の３次元空間中での方程式に基づき、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を算出する位置姿勢算出手段を備える。

また、本発明の情報処理装置は、物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値の入力手段と、前記撮像装置から撮影画像を入力する画像の入力手段と、前記撮影画像から直線を検出する直線検出手段と、前記検出された直線の夫々と、前記物体を表す線分モデルを構成する線分の夫々の方向ベクトルとが仮に対応するとみなして、夫々の組合せについて方位角を求めて投票することにより方位角候補値を算出する方位角候補値算出手段とを備える。

また、本発明の情報処理装置は、物体または撮像装置の位置及び姿勢を計測する情報処理装置であって、物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値の入力手段と、前記撮像装置から撮影画像を入力する画像の入力手段と、前記撮影画像から検出される直線と前記傾斜角センサの計測値に基づいて物体または撮像装置の位置と姿勢を算出する第一の位置姿勢算出手段と、前記撮影画像から検出される特徴に基づいて物体または位置及び姿勢の予測値に対する補正値を算出することにより物体または撮像装置の位置と姿勢を算出する第二の位置姿勢算出手段とを備え、前記位置及び姿勢の予測値が得られない場合には第一の位置姿勢算出手段を、前記位置及び姿勢の予測値が得られる場合には第二の位置姿勢算出手段を選択する選択工程とを備える。

本発明により、位置及び姿勢の予測値を利用せずに画像を用いて行う撮像装置または物体の位置及び姿勢の計測を安定化させ、処理時間を短くすることが可能になる。

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

［第一の実施形態］
第一の実施形態では、傾斜角センサを撮像装置に装着し、本発明を撮像装置の位置及び姿勢の計測に適用した場合の実施形態について説明する。

図１は、第一の実施形態における位置姿勢計測装置１の構成を示している。同図に示すように、位置姿勢計測装置１は、画像取得部１１０、センサ計測値取得部１２０、直線検出部１３０、モデルデータ記憶部１４０、位置姿勢算出部１５０によって構成されている。

画像取得部１１０は撮像装置１００に接続されており、センサ計測値取得部１２０は傾斜角センサ１０５に接続されている。撮像装置１００は観察対象物体１０を撮影し、位置姿勢計測装置１は、観察対象物体１０を含む３次元空間中に規定される座標系（以下、基準座標系）における撮像装置１００の位置及び姿勢を計測する。また、傾斜角センサ１０５は撮像装置１００に固定されており、基準座標系に対する撮像装置の傾斜角を計測する。なお、観察対象物体は現実空間中に固定されているものとする。以下、位置姿勢計測装置１を構成する各部について説明する。

画像取得部１１０は、撮像装置１００によって撮像された撮影画像を位置姿勢計測装置１に入力する。画像取得部１１０は、撮像装置の出力がＮＴＳＣ信号などのアナログ出力であればアナログビデオキャプチャボードによって実現される。また撮像装置の出力がＩＥＥＥ１３９４などのデジタル出力であれば、例えばＩＥＥＥ１３９４インタフェースボードによって実現される。また、予め記憶装置に記憶してある静止画像や動画像のデジタルデータを読み出してもよい。

センサ計測値取得部１２０は、傾斜角センサ１０５が計測した傾斜角計測値を位置姿勢計測装置１に入力する。センサ計測値取得部１２０は、画像取得部１１０が取得する画像の撮影時刻とほぼ同一時刻の傾斜角計測値を取得できるものとする。また、画像取得部１１０が予め記憶装置に記憶してある静止画像や動画像のデジタルデータを読み出す場合には、傾斜角センサ１０５が出力する傾斜角計測値ではなく、予め記憶装置に記憶してある傾斜角計測値を読み出してもよい。撮像装置１００に対する傾斜角センサ１０５の姿勢は予め校正済みであるとし、傾斜角センサ１０５から得られる傾斜角計測値により、撮像装置１００の傾斜角が計測できるものとする。

直線検出部１３０は、画像取得部１１０によって取得された画像上において直線を検出する。撮像装置１００の焦点距離や歪みパラメータなどの固有パラメータは予め校正されているものとし、直線の検出結果は焦点距離が１となるように変換されているものとする。

モデルデータ記憶部１４０は、観察対象物体１０の３次元幾何モデルデータを記憶する。図２は本実施形態における３次元幾何モデルの定義方法を説明する図である。３次元モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。図２（ａ）に示すように、観察対象物体１０の３次元幾何モデルは点Ｐ１〜点Ｐ１４の１４点から構成される。３次元空間中に規定される基準座標系の原点をＰ１２に、ｘ軸を点Ｐ１２から点Ｐ１３に向かう方向に、ｙ軸を点Ｐ１２から点Ｐ８に向かう方向に、ｚ軸を点Ｐ１２から点Ｐ１１に向かう方向に取る。ｙ軸は鉛直上方向（重力軸の反対方向）であるとする。

また図２（ｂ）に示すように、観察対象物体１０の３次元幾何モデルは線分Ｌ１〜Ｌ１６により構成されている。図２（ｃ）に示すように、点Ｐ１〜点Ｐ１４は３次元座標値によって表される。また図２（ｄ）に示すように、線分Ｌ１〜Ｌ１６は、線分を構成する点のＩＤによって表される。さらに３次元幾何モデルは、陰線消去の問題を取り扱うため、面の情報も保持する。各面は、各面を構成する点のＩＤによって表される。図２に示す３次元幾何モデルでは、直方体の６つの各面の情報が記憶されている。位置姿勢算出部１５０は、センサ計測値取得部１２０によって取得された傾斜角計測値、直線検出部１３０によって検出された直線の方程式及び該直線に対応する基準座標系における直線の方程式に基づいて、撮像装置１００の位置及び姿勢を算出する。

次に、第一の実施形態における位置姿勢計測方法の処理手順について説明する。図３は、第一の実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。

まずステップＳ１０１０において、位置姿勢計測装置１は、画像取得部１１０及びセンサ計測値取得部１２０を介して、撮像装置１００が撮影する観察対象物体１０の画像及び画像が撮影された時刻とほぼ同時刻の撮像装置１００の傾斜角計測値を取り込む。

次にステップＳ１０２０において、直線検出部１３０は、ステップＳ１０１０で取得された画像上で直線検出を行う。

図４は、直線検出部１３０で実行される画像上の直線検出の処理手順を示すフローチャートである。

まずステップＳ１１１０において、画像からエッジ検出を行う。前述したように、エッジとは、画像上で濃度が急激に変化する点である。エッジ検出は、例えばＳｏｂｅｌオペレータ，Ｐｒｅｗｉｔｔオペレータなどの公知のエッジ検出オペレータにより行う。各画素について、エッジ検出オペレータを画像の水平方向、垂直方向について適用する。そして、その出力値をもとに、エッジ強度及びエッジ方向を算出する。エッジ検出オペレータの水平方向の出力値がｆ_ｘ、垂直方向の出力値がｆ_ｙである場合、エッジ強度Ｉは

、エッジ方向Φはφ＝ａｒｃｔａｎ（ｆ_ｙ／ｆ_ｘ）として算出する。

次にステップＳ１１２０において細線化を行う。ステップＳ１１１０では、エッジのぼけやエッジ検出オペレータのウィンドウサイズの影響により、エッジはその垂直方向にある幅を持って検出される。そこで、各画素について、エッジ方向に対して垂直な方向の隣接画素のエッジ強度を比較し、隣接画素よりもエッジ強度が大きい画素のみをエッジとして残すことでエッジ幅を１画素にする細線化を行う。このステップＳ１１２０の処理においてエッジとして残った画素には１を、それ以外の画素には０を割り当てることにより２値化画像を生成する。なお、細線化されたエッジ検出結果を出力する、Ｃａｎｎｙのエッジ検出器を用いてエッジ検出と細線化を行ってもよい。

次にステップＳ１１３０において、ステップＳ１１２０で生成された２値化画像について、隣接エッジのラベリングを行う。ラベリングは、例えばある画素の周囲８画素の中にエッジが存在すれば、同一ラベルを割り当てることにより行う。

次にステップＳ１１４０において、ステップＳ１１３０において同一ラベルに分類された隣接エッジの中から複数の枝が分岐する点を探索し、該分岐点において枝を切断し、各枝に異なるラベルを割り当てる。分岐点がないものについては、分岐点の処理を行わない。

さらにステップＳ１１５０では、ステップＳ１１４０においてラベルを割り当てられた各枝について、折れ線近似を行う。折れ線近似は例えば以下の方法により行う。まず、枝の両端を線分で結び、枝上の点のうち該線分との距離が最大かつ閾値以上の点に新たに分割点を設ける。次に、この分割点と枝の両端を線分で結び、該線分との距離が最大の点に分割点を設ける。この処理を、枝が折れ線によって十分近似されるまで再帰的に繰り返す。その後、折れ線を構成する各線分について、両端の座標を画像上の直線の通過点として出力する。

なお、ここではラベリング処理と折れ線近似により直線検出を行ったが、直線検出方法はこれに限るものではなく、画像上から直線を検出する方法であればいかなる方法であってもよい。例えば、ハフ変換によって直線を検出してもよい。なお、本ステップでは、観察対象物体１０上の直線だけでなく、背景に存在する直線も検出される。

図３に戻って、ステップＳ１０３０では、ステップＳ１０２０における直線の検出結果を利用して、観察対象物体１０に対する撮像装置１００の位置及び姿勢を算出する。姿勢のうち、傾斜角成分は傾斜角センサの計測値として得られているので、ここでは姿勢の方位角成分と位置を未知変数として算出する。なお、第１の実施形態においては、ステップＳ１０２０で検出された直線と、モデルデータ記憶部１４０に記憶されている３次元幾何モデルの線分との対応は、手動で対応付けるなどしてわかっているものと仮定する。また、手動で対応付けるのではなく、予め現実空間における線分の周辺の画像情報を記録しておき、該記録された周辺の画像情報と、画像上で検出された線分の周辺の画像情報とパターンマッチングなどによって比較し、対応付けを行ってもよい。

ここで、撮像装置１００の座標系（以下、カメラ座標系）を次のように定義する。すなわち、視点Ｃをカメラ座標系の原点とし、視軸ベクトルの反対方向をｚ軸、画像平面の水平方向をｘ軸、垂直方向をｙ軸とする。また、撮像装置１００の焦点距離を１として、画像平面をカメラ座標系のｚ＝−１にとり、画像座標系の原点を画像平面とカメラ座標系のｚ軸（すなわち視軸ベクトル）との交点とする。ステップＳ１０２０で検出された直線の情報は、以上のように定義された画像座標系において表す。

基準座標系における任意の３次元ベクトルｘ_ｗは、式１によりカメラ座標系における３次元ベクトルｘ_ｃに変換される。
ｘ_ｃ＝Ｒ_ｃｗｘ_ｗ＋ｔ_ｃｗ （式１）
Ｒ_ｃｗはカメラ座標系に対する基準座標系の姿勢を表す３×３回転行列である。基準座標系におけるカメラ座標系（すなわち撮像装置１００）の姿勢を表す３×３回転行列をＲ_ｗｃとすると、

の関係がある。ｔ_ｃｗはカメラ座標系に対する基準座標系の原点の位置を表す３次元ベクトルである。基準座標系におけるカメラ座標系（すなわち撮像装置１００）の位置を表す３次元ベクトルをｔ_ｗｃとすると、ｔ_ｗｃ＝−Ｒ_ｗｃｔ_ｃｗの関係がある。

図５は、画像上の直線と３次元空間中の直線との関係を説明する図である。一般に、３次元空間中の直線を撮像装置により撮像すると、その画像平面上における投影像は直線となる。図５に示すように、３次元空間中の点Ｐ及びＱを通過する直線Ｌの画像平面上における投影像である直線ｌは、直線Ｌ及び視点Ｃを含む平面πと画像平面との交線である。また平面πの法線ベクトルｎは、ベクトルＣＰ，ＣＱ，ＰＱと直交する。点Ｐ、点Ｑの基準座標系における位置を表す３次元ベクトルをそれぞれｐ、ｑ、直線Ｌの基準座標系における方向ベクトルをｄ（＝ｑ−ｐ）とすると、この３つの直交条件は式２〜式４のように表される。
ｎ・（Ｒ_ｃｗｐ＋ｔ_ｃｗ）＝０ （式２）
ｎ・（Ｒ_ｃｗｑ＋ｔ_ｃｗ）＝０ （式３）
ｎ・Ｒ_ｃｗｄ＝０ （式４）
ただし「・」はベクトルの内積である。

撮像装置１００の姿勢の傾斜角成分を表す３×３回転行列を

，方位角成分を表す３×３回転行列を

とすると，Ｒ_ｗｃは

のように分解できるため、

となる。ここで傾斜角とは、計測対象の座標系（ここではカメラ座標系）に対する重力軸の傾きを表し、２自由度で表現される。また方位角とは、計測対象の重力軸周りの回転を表し、１自由度で表現される。

を式５のように表す。

ｒ_１、ｒ_２、ｒ_３はそれぞれ行列

の各列ベクトルである。

の各要素の値は、傾斜角センサ１０５の計測値によって決定される。センサ計測値取得部１２０の説明で述べたように、撮像装置１００に対する傾斜角センサ１０５の姿勢は予め校正済みであると仮定しているため、傾斜角センサ１０５の計測値から撮像装置１００の傾斜角が得られる。一方、モデルデータ記憶部１４０の説明で述べたように、基準座標系のｙ軸は鉛直上ベクトルとしているので、方位角を表す回転行列

は、方位角をθ、α＝ｃｏｓθ、β＝ｓｉｎθとすると式６のように表される。

ｎ＝［ｎ_ｘ ｎ_ｙ ｎ_ｚ］^ｔ、ｐ＝［ｐ_ｘ ｐ_ｙ ｐ_ｚ］^ｔ、ｑ＝［ｑ_ｘ ｑ_ｙ ｑ_ｚ］^ｔ、ｔ_ｃｗ＝［ｔ_ｘ ｔ_ｙ ｔ_ｚ］^ｔとして、式５、式６を式２、式３に代入すると、式７、式８が得られる。

式７、式８は、未知変数α、β、ｔ_ｘ、ｔ_ｙ、ｔ_ｚについての線形方程式になっている。また、画像上で検出された直線の２つの通過点の、前述の焦点距離を１とした画像平面の座標系における座標を（ｘ_１，ｙ_１）、（ｘ_２，ｙ_２）とすると、そのカメラ座標はｘ_ｃ１＝［ｘ_１ ｙ_１ −１］^ｔ、ｘ_ｃ２＝［ｘ_２ ｙ_２ −１］^ｔ
となる。法線ベクトルｎは、ｘ_ｃ１、ｘ_ｃ２の双方に直交するベクトルであるので、ｎ＝ｘ_ｃ１×ｘ_ｃ２により求められる（×は外積を表す）。これによって、画像上で検出される直線と、３次元空間中の直線とが法線ベクトルｎを介して方程式として対応付けられる。

複数の画像上の直線と３次元空間中の直線との対応について成り立つ式７、式８をα、β、ｔ_ｘ、ｔ_ｙ、ｔ_ｚについての連立方程式として解くことにより、α、β、ｔ_ｘ、ｔ_ｙ、ｔ_ｚを算出することができる。以下、未知パラメータα、β、ｔ_ｘ、ｔ_ｙ、ｔ_ｚを表すベクトルをｓ＝［α β ｔ_ｘ ｔ_ｙ ｔ_ｚ］^ｔと表す。ｐ、ｑとしては、３次元幾何モデルにおける各線分の両端の座標を用いる。

（ｉ） 少なくとも一つの直線が同一水平面上に存在しない場合
位置及び姿勢の算出に用いる直線のうち、少なくとも一つの直線が３次元空間において同一水平面上に存在しない場合には，Ａｓ＝ｂ（Ａは式７、式８の左辺の係数の値からなる行列、ｂは式７、式８の右辺の値からなるベクトル）の形の連立方程式を解くことによりｓを算出する。通常Ａの行数は未知変数の数（すなわちｓの要素数）より多いため、式９に示すように最小二乗法によりｓを算出する。
ｓ＝（Ａ^ｔＡ）^−１Ａ^ｔｂ （式９）
また、ｓ’＝［ｓ^ｔ １］^ｔ、Ａ’＝［Ａ −ｂ］として、行列（Ａ’）^ｔＡ’の最小固有値に対応する固有ベクトルを求め、最後の要素が１になるようにｓ’の各要素を割ることで、ｓを算出してもよい。

得られたｓから方位角θ及び撮像装置１００の位置ｔ_ｗｃを算出する。方位角θはα、βの符号とｔａｎ^−１（β／α）から算出する。算出した方位角と傾斜角計測値から

により撮像装置１００の姿勢を表す回転行列Ｒ_ｗｃを計算する。撮像装置１００の位置ｔ_ｗｃは式１０により算出する。
ｔ_ｗｃ＝−Ｒ_ｗｃｔ_ｃｗ （式１０）
（Ｉ）の場合には、未知変数ｓの自由度は５である。１本の直線の対応からは式７、式８の２つの方程式が立式できるので、少なくとも３本の直線の対応が必要となる。但し、すべての直線が３次元空間において一点で交わらず、かつすべての直線が平行ではないことが一意な解を得る必要条件となる。

（ｉｉ） すべての直線が同一水平面上にある場合
位置及び姿勢の算出に用いる直線が３次元空間においてすべて同一水平面上（ｙ＝ｙ_１）にある場合には、ｐ_ｙ＝ｙ_１、ｑ_ｙ＝ｙ_１となる。そのため、Ｐ_ｙ＝０、ｑ_ｙ＝０とおいて式７、式８の右辺をすべて０とすることにより、式１１、式１２が得られる。

これにより、連立方程式をＡｓ＝０の形にすることができるため、ｓは行列Ａ^ｔＡの最小の固有値に対応する固有ベクトルとして算出できる。このときｓの符号を含むスケールファクターは不定である。そこで、α^２＋β^２＝１（すなわちｃｏｓ^２θ＋ｓｉｎ^２θ＝１）となるようにスケールを決定する。ｓの符号については、符号の異なる２つのｓ（ｓ_１＝［α_０ β_０ ｔ_ｘ０ ｔ_ｙ０ ｔ_ｚ０］^ｔ、ｓ_２＝［−α_０ −β_０ −ｔ_ｘ０ −ｔ_ｙ０ −ｔ_ｚ０］^ｔ）から以下の方法により選択する。まず、ｓ_１、ｓ_２それぞれについて、撮像装置１００の位置及び姿勢を算出する。方位角θはα、βの符号とｔａｎ^−１（β／α）から算出する。算出した方位角と傾斜角計測値から

により撮像装置１００の姿勢を表す回転行列Ｒ_ｗｃを計算する。位置ｔ_ｗｃは式１３により算出する。

次に、それぞれの位置及び姿勢について、画像平面上における直線の通過点（ｘ_１，ｙ_１）、（ｘ_２，ｙ_２）のカメラ座標における位置を求め、そのｚの値が負であるようなｓを解として選択する。

（ｉｉ）の場合には、未知変数ｓの自由度は４である。１本の直線の対応から式１１、式１２の２つの方程式が立式されるため、４自由度の未知変数を求めるためには２本の直線の対応があればよい。しかしながら、同一水平面上にある２本の直線は、平行であるか交わるかのどちらかであり、このような場合には未知変数を一意に決定することができない。そのため、すべての直線が同一水平面上にある場合には、少なくとも１本の直線が他の直線と平行ではなく、かつ一点で交わらないことが一意な解を得る必要条件であり、少なくとも３本の直線の対応を必要とする。

以上説明したように、第一の実施形態では、撮像装置に装着した傾斜角センサが計測する傾斜角計測値と、画像上で検出される直線とそれに対応する３次元幾何モデルの線分とから、直接的に撮像装置の位置及び姿勢を算出する。傾斜角センサを利用することで未知変数の自由度を６から４に減らすことができるため、より少ない直線の対応から一意な位置及び姿勢を算出することができる。また、多数の対応がある場合でも、画像のみを用いる場合に比べてより安定的に解を求めることができる。

［変形例１−１］
第一の実施形態において、すべての直線が同一水平面上に存在しない場合（（ｉｉ）の場合）には、本来独立ではないαとβを独立に求めていたため、αとβは必ずしもα^２＋β^２＝１（すなわちｃｏｓ^２θ＋ｓｉｎ^２θ＝１）という拘束条件を満たしていなかった。

そこで、第一の実施形態で説明した方法を用いて未知変数θ、ｔ_ｘ、ｔ_ｙ、ｔ_ｚを算出した後、非線形最適化を行うことによってθ、ｔ_ｘ、ｔ_ｙ、ｔ_ｚを補正してもよい。

第一の実施形態で説明した方法を用いて算出された未知変数をθ_０、ｔ_ｘ０、ｔ_ｙ０、ｔ_ｚ０とすると、その近傍において式７、式８を一次近似すると式１４、式１５が得られる。

ただし

であり、△θ、△_ｘ、△ｔ_ｙ、△ｔ_ｚはそれぞれθ、ｔ_ｘ、ｔ_ｙ、ｔ_ｚの微小変化量である。

式１４、式１５を用いて△θ、△ｔ_ｘ、△ｔ_ｙ、△ｔ_ｚについての線形連立方程式を立てて解くことによって得られる補正値△θ、△ｔ_ｘ、△ｔ_ｙ、△ｔ_ｚにより、θ_０，ｔ_ｘ０，ｔ_ｙ０，ｔ_ｚ０の補正を行う。なお、この処理は１回だけ行ってもよいし、補正した結果をもとに再度式１４、式１５をもとに連立方程式を立てて補正値を求め補正することを繰り返してもよい。

［変形例１−２］
第一の実施形態では、式２、式３の拘束条件を利用し、撮像装置の位置と方位角を同時に算出していた。しかしながら、位置と方位角を同時に算出する必要はなく、両者を別々に算出してもよい。すなわち、式４の拘束条件を用いて方位角を算出した後、式２または式３の拘束条件を用いて位置を算出してもよい。

ｄ＝［ｄ_ｘ ｄ_ｙ ｄ_ｚ］^ｔとして、式５、式６を式２、式３に代入すると、式１６が得られる。

複数の画像上の直線と３次元空間中の直線との対応について成り立つ式１６をα、βについての連立方程式として解くことにより、α、βを算出することができる。なお、直線の方向が鉛直ベクトルと平行である場合にはｄ_ｘ＝ｄ_ｚ＝０であるため、左辺の係数がすべて０になってしまうのでそのような直線は用いることができない。α、βから方位角θを算出する方法は第一の実施形態で説明した方法と同じ方法を用いる。直線の方向がすべて水平方向である場合にはｄ_ｙ＝０であり、式１６の右辺がすべて０になる。この場合、未知変数の自由度は１となるので、少なくとも１本の直線によりα、βを算出することが出来る。また、少なくとも１本の直線の方向が水平方向でない場合には、自由度は２であるので、少なくとも２本の直線によりα、βを算出することが出来る。式１６の右辺がすべて０である場合には、α、βの符号を決定することができない。そのような場合には、符号の異なる２種類のα、βから２個の方位角を求め、以降で説明する位置の算出をそれぞれの方位角について行い、第一の実施形態で説明した方法と同じ方法によりどちらか一方を選択する。

算出した方位角と傾斜角計測値から

により撮像装置１００の姿勢を表す回転行列Ｒ_ｗｃを計算する。ここでｐ’＝Ｒ_ＣＷｐ、ｑ’＝Ｒ_ＣＷｑとすると、式２、式３より式１７、式１８が得られる。式４より、式１７、式１８は同じ方程式を表す。

複数の画像上の直線と３次元空間中の直線との対応について成り立つ式１７または式１８を連立方程式として解くことにより、ｔ_ｘ、ｔ_ｙ、ｔ_ｚを算出することができる。一意な解を得るためには、一点で交わらず、かつ平行でない直線が少なくとも３本必要である。

なお、本変形例の方法により撮像装置の位置と方位角を算出した後、変形例１−１の方法によって補正を行ってもよい。

［第二の実施形態］
第一の実施形態は、画像上で検出された直線と３次元幾何モデルの線分との対応が既知である場合の位置及び姿勢の算出方法を取り扱うものである。それに対して、第二の実施形態では、画像上で検出された直線と３次元幾何モデルの線分との対応が未知の場合に、傾斜角センサの計測値を利用して位置及び姿勢の算出を行う方法について述べる。

図６は、第二の実施形態における位置姿勢計測装置２の構成を示している。同図に示すように、位置姿勢計測装置２は、画像取得部２１０、センサ計測値取得部２２０、直線検出部２３０、モデルデータ記憶部２４０、方位角候補算出部２５０、位置姿勢算出部２６０によって構成されている。画像取得部２１０は、撮像装置２００に接続されている。また、センサ計測値取得部２２０は、傾斜角センサ２０５に接続されている。第一の実施形態と同様に、撮像装置２００は観察対象物体２０を撮影し、位置姿勢計測装置２は、観察対象物体２０を含む３次元空間中に規定される基準座標系における撮像装置２００の位置及び姿勢を計測する。また、傾斜角センサ２０５は、撮像装置２００に固定されている。以下、位置姿勢計測装置２を構成する各部について説明する。

画像取得部２１０、センサ計測値取得部２２０、直線検出部２３０、モデルデータ記憶部２４０は、夫々第一の実施形態における画像取得部１１０、センサ計測値取得部１２０、直線検出部１３０、モデルデータ記憶物１４０と同一の処理を行うため説明を省略する。

方位角候補算出部２５０は、センサ計測値取得部２２０によって取得された傾斜角計測値と直線検出部２３０によって検出された直線の方程式に基づいて、撮像装置１００の方位角候補値を算出する。

位置姿勢算出部２６０は、センサ計測値取得部２２０によって取得された傾斜角計測値と直線検出部２３０によって検出された直線の方程式、及び方位角候補算出部２５０によって算出された方位角候補値に基づいて、撮像装置２００の位置及び姿勢を算出する。

第二の実施形態におけるモデルデータ記憶部２４０は、自身が記憶している線分モデルデータをその方向ベクトルによって予め分類し、得られた分類をモデルデータの一部として保持している。図９は、線分モデルデータの方向ベクトルによる分類を説明する図である。方向ベクトルＤ１、Ｄ２、Ｄ３の方向をそれぞれ（１，０，０）、（０，１，０）、（０，０，１）として、図２に示される物体を構成する線分Ｌ１〜Ｌ１６を方向ベクトルにより分類する。方向ベクトルの数は、例えば互いのなす角度が一定角度以上であれば異なるベクトルとして計数することにより決定する。線分Ｌ１〜Ｌ１６は、図９に示すように、方向ベクトルＤ１、Ｄ２、Ｄ３に分類される。

次に、第二の実施形態における位置姿勢計測方法の処理手順について説明する。図７は、第二の実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。

まずステップＳ２０１０において、位置姿勢計測装置２は、画像取得部２１０及びセンサ計測値取得部２２０を介して、撮像装置２００が撮影する観察対象物体２０の画像及び画像が撮影された時刻とほぼ同時刻の撮像装置２００の傾斜角計測値を取り込む。

次にステップＳ２０２０において、直線検出部２３０は、ステップＳ２０１０で取得された画像上で直線検出を行う。直線検出の方法は第一の実施形態で説明した方法と同様であるので説明を省略する。ただし、本実施形態においては、検出された直線の線分としての特徴（端点の情報）を、後に述べるステップＳ２２４５等の処理において利用する。そのため、ハフ変換のように直線を直接検出する方法を用いる場合には、検出された直線から連続領域を線分として抽出し、その端点を直線の通過点として登録しておく。

ステップＳ２０３０では、方位角候補算出部２５０は、傾斜角計測値及びステップＳ２０２０における直線の検出結果を利用して、基準座標系における撮像装置２００の方位角候補値を算出する。第二の実施形態では、画像上で検出される直線と３次元幾何モデルにおける線分の対応は未知であるため、第一の実施形態の方法を用いて直接位置及び姿勢を算出することはできない。そこで、ある検出直線が３次元幾何モデルにおけるある線分と対応すると仮定して（仮の対応）、その場合の方位角を算出してみる。これをすべての３次元幾何モデルにおける線分に対して行い、算出される方位角を投票する。さらに、この処理をすべての検出直線について行い、投票結果から最も尤もらしい方位角を求める。

なお、ある検出直線を、同一の方向ベクトルを持つ線分と対応付けた場合には同じ方位角が得られる。そこで、各検出された直線について、３次元幾何モデルにおいて分類されている方向ベクトルをそれぞれ対応付けて方位角を算出して方位角を投票し、投票数の多い方位角を方位角候補値として出力する。

図８は、方位角候補値算出の処理方法を説明するフローチャートである。ここでは、ｉを現在処理している直線検出結果、ｊを現在処理している方向ベクトルとする。ここで、ステップＳ２０２０において検出された直線の数をＮ、３次元幾何モデルの線分の方向ベクトル数をＭとする。すなわち、１≦ｉ≦Ｎ、１≦ｊ≦Ｍである。

まずステップＳ２１０５において、方位角のヒストグラムを初期化する。方位角の範囲をあらかじめ決められたステップで量子化し、ヒストグラムの各階級の度数を０にセットする。量子化ステップは例えば１度である。しかしながら、量子化ステップの大きさはこれに限るものではない。

次に、ステップＳ２１１０においてｉを１にセットし、ステップＳ２１２０において、ｊを１にセットする。

次にステップＳ２１３０において方位角の算出と投票を行う。方位角の算出は、画像上での直線ｉと方向ベクトルｊの対応から式１６に基づいて行う。簡単のため、式１６を式１９のように表す。
ａα＋ｂβ＋ｃ＝０ （式１９）
式１９は、α−β平面における直線の方程式とみなすことができる。また、α^２＋β^２＝１（すなわちｃｏｓ^２θ＋ｓｉｎ^２θ＝１）であることから、図１０に示すように、α、βは円α^２＋β^２＝１と直線ａα＋ｂβ＋ｃ＝０の交点から求めることができ、方位角θ_１、θ_２が得られる。円α^２＋β^２＝１と直線ａα＋ｂβ＋ｃ＝０が接する場合には、一つの方位角しか得られない。また、円α^２＋β^２＝１と直線ａα＋ｂβ＋ｃ＝０が交わらない場合は、方位角は得られない。さらには、方向ベクトルが鉛直方向のベクトルである場合には、ａ＝ｂ＝０になるので方位角が得られない。

このようにして得られた方位角θ_１、θ_２のそれぞれに最も近い代表値を持つ方位角ヒストグラムの階級の度数に１を加える。このとき、投票した階級について、方位角の計算に用いた画像上での直線と方向ベクトルの組合せも合わせて記憶しておく。

次に、ステップＳ２１４０において、ｊに１を加算する。ステップＳ２１５０において、すべての方向ベクトルについて処理が終わったと判断された場合には、ステップＳ２１６０に進み、そうでない場合にはステップＳ２１３０に戻って新たな組合せで方位角の算出及び投票を行う。

ステップＳ２１６０では、ｉに１を加算する。ステップＳ２１７０において、すべての画像上での直線について処理が終わったと判断された場合には、ステップＳ２１８０に進み、そうでない場合にはステップＳ２１２０に戻って次の直線について処理を行う。

すべての画像上での直線と方向ベクトルの組合せにより方位角を算出しヒストグラムへの投票が終了すると、ステップＳ２１８０において方位角候補値の選択を行う。方位角候補値として、方位角ヒストグラムにおける各階級の度数の最も大きい階級の代表値を採用する。

図７に戻って、ステップＳ２０４０において、ステップＳ２０２０における直線の検出結果とステップＳ２０３０における方位角候補値に基づいて、基準座標系に対する撮像装置２００の位置及び姿勢を算出する。

図１１は、ステップＳ２０４０における位置及び姿勢算出の処理の手順を示すフローチャートである。ステップＳ２０４０では、検証する仮説の数ｎ_ｍａｘを予め設定し、ｎ_ｍａｘ通りの３個の直線の組合せからそれぞれ撮像装置の位置を算出し、最も妥当な位置を選択することにより位置を算出する。

まずステップＳ２２０５において、対応データリストの作成を行う。対応データは、画像上での直線ｌと３次元幾何モデルにおける線分Ｌの一対一の組合せであり、対応データリストは複数の対応データを保持する。対応データリストは、画像上での直線と３次元幾何モデルの線分との対応のうち、対応する可能性のあるものの組合せをすべて対応データとして持つ。この組合せは、ステップＳ２０３０において方位角候補を算出したときに、該方位角候補に投票した画像上での直線と方向ベクトルとの組合せから生成する。

図１２は、対応データリストの一例を示す図である。図９で示すように、一つの方向ベクトルには複数の線分が分類されているため、図１２では、一本の画像上での直線に対して複数の線分が対応付けられて対応データリストに記憶されている。

次にステップＳ２２１０において、検証済み仮説数ｎを０にセットする。

ステップＳ２２２０では、ステップＳ２２０５で生成した対応データリストから、無作為に３個の対応データを抽出する。なお、データ抽出を無作為に行わないで、ｎ_ｍａｘをすべての組合せ数とし、すべての組合せについて位置を求めてもよい。

次にステップＳ２２３０では、ステップＳ２２２０において抽出された３個の対応データから位置を計算可能かどうかの判定を行う。具体的には、
・同じ画像上の直線が含まれている場合
・同じ線分が含まれている場合
・３個の線分がすべて平行である場合
・３個の線分が一点で交わる場合
・過去に用いた組合せである場合
のいずれかに該当する場合には計算不可能と判定する。また、３個の線分がすべて平行である場合、３個の線分が一点で交わる場合の判定は、式１７、式１８の連立方程式を立式し、その係数行列のランクが３未満であるかどうかにより判定してもよい。ステップＳ２２３０において計算可能と判定された場合にはステップＳ２２４０に進み、そうでない場合にはステップＳ２２２０において新たに３個の対応データを抽出し直す。

次にステップＳ２２４０において、撮像装置２００の位置を算出する。位置ｔ_ｃｗ＝［ｔ_ｘ ｔ_ｙ ｔ_ｚ］^ｔは、変形例１−２と同様にして、傾斜角センサから得られる傾斜角と方位角候補値をもとに撮像装置１００の姿勢を表す回転行列Ｒ_ｗｃを計算した後、複数の式１７または式１８から構成される連立方程式を解くことにより算出する。

次にステップＳ２２４５において、算出された位置及び姿勢の妥当性の評価を行う。まず、算出された位置及び姿勢を用いて、位置算出に用いた３つの線分を仮想的に画像上に投影する。さらに、陰線消去を取り扱うために、３次元幾何モデルを構成する面をデプスマップ上に描画する。そして、次の３つの条件のうち１つでも満たす場合には、算出された位置及び姿勢は妥当性がないと判定し、ステップＳ２２６０に進む。そうでない場合には、ステップＳ２２５０に進む。条件は次の通りである。
条件１：投影された線分と、それに対応する画像上の直線のどちらかの通過点（線分の端点）との距離が閾値を越える。
条件２：投影された線分と、それに対応する画像上の線分との間にオーバラップがない。
条件３：画像上の直線の通過点に相当する３次元幾何モデルの線分上の点の奥行きが、デプスマップにおける奥行きより大きい。

ステップＳ２２５０では、ステップＳ２２４０で算出した位置の評価値の算出を行う。ここでは、算出された撮像装置の位置及び姿勢に基づいて３次元幾何モデルのすべての線分を仮想的に画像上に投影し、その近傍に画像上で検出された直線が存在するかどうかを判定する。そして、近傍に画像上での直線が存在する３次元幾何モデルの数を該位置の評価値とする。ここでは、ステップＳ２２４５で描画したデプスマップを利用し、面に隠される線分は描画しない。画像上での直線の近傍に３次元幾何モデルの投影線分が存在するかどうかは、直線の両端と投影線分との距離が一定値以内かどうかにより判定する。

ステップＳ２２６０において検証済み仮説数ｎに１を加えた後、ステップＳ２２７０においてｎがｎ_ｍａｘに等しいかどうか判定する。ｎがｎ_ｍａｘに等しいと判定された場合にはステップＳ２２８０に進み、そうでない場合にはステップＳ２２２０に戻って新たに３個の対応データを選択し直す。

ステップＳ２２８０では、算出されたｎ_ｍａｘ個の撮像装置の位置の中で、最も妥当なものを選択する。最も妥当かどうかの判定は、ステップＳ２２５０において計算された評価値に基づいて行う。

以上説明したように、第二の実施形態では、画像上で検出される直線と３次元幾何モデルの線分の対応が未知な場合に、傾斜角計測値を利用して方位角候補値と対応候補を求め、撮像装置の位置及び姿勢を算出する。計算負荷の小さい方法によって得られる方位角を投票することで方位角候補値を算出し、その結果得られる画像上の直線と３次元幾何モデルの方向ベクトルとの対応によって可能な対応数を限定することにより、位置及び姿勢の算出を効率化する。

［変形例２−１］
以上で説明した第二の実施形態では、算出された撮像装置の位置及び姿勢に基づいて３次元幾何モデルのすべての線分を仮想的に画像上に投影し、その近傍に画像上で検出された直線が存在する３次元幾何モデルの数を該位置の評価値としていた。ただし、この評価値は直線の数という離散的な値であるため、同一の数である場合には比較を行うことができない。そのため、直線の数とともに、図１３（ａ）に示す直線の両端から投影線分までの距離ｄ_１、ｄ_２の和（ｄ_１＋ｄ_２）を評価値として保持する。図１３に示すように、画像上の直線ｌの両端に対応する投影線分Ｌ’の位置が線分外である場合には、投影線分Ｌ’の端点から直線ｌへ下ろした垂線の長さの和を距離として保持する。また、距離ではなく、図１３に斜線で示される直線と線分が囲む部分の面積を評価値として用いてもよい。

そして、最も妥当な位置を選択する際には、まず、近傍に３次元幾何モデルの投影線分が存在する画像上での直線の数により候補の位置を選択し、その中で直線の両端から投影線分までの距離の和が最も小さい位置を選択して出力する。また、直線の両端から投影線分までの距離の和だけに基づいて位置を選択してもよい。

また、前述の評価値の計算は計算負荷が比較的小さいため、多数の計算に適している。しかしながら、計算負荷と評価の精度はトレードオフの関係にある。そこで、前述の評価値に基づいて限定された位置及び姿勢を選択した後に、さらにより評価の精度が高い評価値に基づいて最終的な解となる位置及び姿勢を選択してもよい。これによって、計算負荷をそれほど高めることなく、正しい位置及び姿勢が選択される可能性を増すことができる。

別の評価値決定方法として、算出された撮像装置の位置及び姿勢に基づいて画像上に投影される線分上の各点の近傍において、実際にエッジが存在するかどうかによって評価値を決定する。すなわち、図１６に示すように、画像上に投影される線分を画像上で等間隔になるように分割点を設定する。そして、各分割点において周辺のエッジを探索し、エッジが近傍にある分割点の数を計数する。そして、総分割点数に対するエッジが近傍にある分割点数の割合を該位置及び姿勢の評価値とする。このとき、エッジ探索には、ステップＳ２０２０で検出されたエッジの情報を用いてもよいし、撮影画像において分割点の近傍で再度エッジ検出を行ってもよい。

［変形例２−２］
以上で説明した第二の実施形態では、最も近い代表値を持つ方位角ヒストグラムの階級への投票を行っていた。しかしながら、画像上での直線の検出誤差や、センサの計測誤差などの影響から、算出される方位角の精度が低い場合がある。そのため、最も近い代表値を持つ階級への投票を行うのではなく、その周囲の階級への投票も同時に行ってもよい。また、周囲の階級の度数を均等に１ずつ増やすのではなく、算出された方位角からの差に応じて重み付けして投票してもよい。さらには、ある方向ベクトルに分類されている線分の実際の方向ベクトルに基づいてそれぞれ方位角を算出し、投票の範囲を決定してもよい。

［変形例２−３］
以上で説明した第二の実施形態では、鉛直方向に平行な方向ベクトルは式１６の左辺が０になって方位角を算出できないために、該方向ベクトルを持つ線分の情報を後段の位置算出において利用していなかった。しかしながら、式１６の左辺だけではなく式１６の右辺も０になる場合には、該画像上の直線が鉛直方向に平行な方向ベクトルと対応する可能性がある。そこで、そのような対応を対応データリストに追加し、位置の算出において鉛直方向に平行な方向ベクトルを持つ線分の情報も利用するようにしてもよい。

［変形例２−４］
以上で説明した第二の実施形態では、画像上で検出されたすべての線分について方向ベクトルと組み合わせて方位角の算出を行っていた。しかしながら、画像上で多数の線分が検出されるような場合には計算負荷が大きくなる。そのため、すべての線分を用いるのではなく、一部の線分を無作為に抽出し、方向ベクトルと対応付けて方位角を算出して投票を行うことにより計算負荷の増大を抑制してもよい。

［変形例２−５］
以上で説明した第二の実施形態では、１つの対応をもとに方位角を算出し、投票を行っていた。しかしながら、１つの対応からではなく、複数の対応をもとに方位角を算出し、投票を行ってもよい。例えば、画像上で検出された線分と方向ベクトルの全ての組合せの集合を生成し、該集合から任意のｋ個（例えば２個）を抽出し、ｋ個の式１６からなる連立方程式を解くことにより方位角を算出し投票を行う。

［変形例２−６］
以上で説明した第二の実施形態では、方位角候補値を算出した後、方位角は既知として、位置のみを算出していた。しかしながら、方位角候補値は方位角ヒストグラムの各階級の代表値を用いているため、離散的な値であり実際の方位角とは量子化ステップ分だけ誤っている可能性がある。そのため、抽出された３つの対応から位置のみを算出するのではなく、位置及び方位角の補正値を算出してもよい。この際には、式７、式８を方位角候補値θ_０の近傍で線形化した式２０、式２１を用いる。

ただし、

である。３つの対応からこの式２０、２１の方程式を連立方程式として立式し、△θ、ｔ_ｘ、ｔ_ｙ、ｔ_ｚを未知変数として求めてもよい。このとき方位角はθ＝θ＋△θとなる。

［変形例２−７］
以上で説明した第二の実施形態では、方位角ヒストグラムから最も度数の大きい階級の代表値を方位角候補値として選択していた。しかしながら、方位角候補値は１つである必要はなく、投票数上位の複数の方位角を方位角候補値として選択してもよい。例えば、各方位角候補値についてステップＳ２０４０の処理を行って位置を算出した後、最も評価値の高い位置とそれを算出したときの方位角を最終的な算出結果として出力する。また、変形例２−１で述べたように、各方位角候補値について複数の位置を算出した後、計算負荷が軽い処理によって算出された評価値に基づいて位置を限定した後、異なる評価値に基づいて最終的な位置及び姿勢を決定してもよい。

［変形例２−８］
以上で説明した第二の実施形態では、ｎ_ｍａｘ回組合せを変えて撮像装置の位置を算出していた。しかしながら、ｎ_ｍａｘ回の試行の途中で、十分よい位置が算出された場合には、その時点で処理を打ち切って、その位置を最終的な出力としてもよい。十分よい位置かどうかの判定は、例えば画像中で検出された線分の５０％以上が撮像装置の位置及び姿勢に基づいて仮想的に画像上に投影された線分の近傍にあるかどうかを判定することにより行う。

［変形例２−９］
以上で説明した第二の実施形態では、方位角候補値を算出した後、画像上の線分と３次元幾何モデルとの線分の対応をもとに位置を算出していた。しかしながら、位置の算出方法はこれに限るものではない。

例えば、画像上でＨａｒｒｉｓオペレータなどによって検出されるコーナー点と、３次元幾何モデルの頂点との対応をもとに位置を算出してもよい。すなわち、前述の方法により姿勢を決定した後、画像上で検出されたコーナー点と３次元幾何モデルの頂点をランダムに対応付けて複数の位置を算出し、最も整合性の高い位置を選択してもよい。

また、画像全体のアピアランスを利用する固有空間法などにより位置を算出してもよい。すなわち、３次元幾何モデルで表される現実物体の画像を、あらかじめ撮像装置の位置及び姿勢を変えて多数撮影しておき、画像とともに位置及び姿勢を記録しておく。さらに、画像の各画素の値をベクトルとした特徴ベクトルとして、多数の画像から特徴ベクトルの固有空間を求めておく。そして、位置を算出する際には、撮像された画像を固有空間内の一点に射影し、射影された点の近傍で、最も姿勢が近い予め撮影された画像の位置を求める位置として出力する。

［変形例２−１０］
以上で説明した第二の実施形態では、撮像装置の傾斜角をセンサから得て、位置及び方位角を未知変数として算出した。しかしながら、例えば光学式の位置センサなどによって撮像装置の位置がわかっている場合には、方位角のみが未知変数となる。このような場合には、ステップＳ２０４０における位置算出の処理を省略してもよい。

［変形例２−１１］
以上で説明した第二の実施形態では、撮像装置の方位角及び位置を全く未知なものとして算出していた。しかしながら、方位角や位置の範囲が事前にわかっている場合には、その範囲外の方位角や位置が算出された場合には、その算出結果を無視してもよい。

［第三の実施形態］
第三の実施形態では、本発明による位置及び姿勢計測方法を、非特許文献１に開示されているエッジを用いた位置及び姿勢計測方法の初期化方法として用いる。ここでは、初期化とは、第１フレーム及び計測破綻時に、エッジを用いた位置及び姿勢計測の位置及び姿勢の予測値を現フレームの情報のみから算出することと定義する。位置姿勢計測装置３は、破綻または初期状態であるか否かを示すフラグＦを保持する。初期状態ではＦ＝ＴＲＵＥである。

図１４は、第三の実施形態における処理の手順を説明するフローチャートである。まずステップＳ４０１０において、フラグＦの値を見ることにより、現在の位置姿勢計測の状態が初期状態または破綻直後の状態であるかどうか判定を行う。初期状態または破綻直後の状態である場合には、ステップＳ４０２０に進む。また、初期状態でも破綻直後でもない場合には、ステップＳ４０３０に進む。

ステップＳ４０２０では、第二の実施形態で説明した方法により撮像装置の位置及び姿勢の計測を行い、フラグＦをＦＡＬＳＥにセットして、ステップＳ４０５０に進む。

ステップＳ４０３０では、非特許文献１で開示されるエッジを用いた撮像装置の位置及び姿勢計測を行う。ステップＳ４０３０では、次の処理を行う。

１．図１５に示すように、撮像装置の位置及び姿勢の予測値及び予め校正済みのカメラの固有パラメータに基づいて、３次元幾何モデルの線分を画像上に投影する。図１５において、（ａ）は、撮像装置により撮像された画像を示し、（ｂ）は、撮像装置により撮像された画像に３次元幾何モデルの線分（太い実線と点線：点線は見えない部分を示している）を投影した画像を示している。
２．投影された各線分を、画像上で一定間隔となるように分割し、分割点を設定する（図１６）。そして各分割点について、該分割点を通過し向きが投影された線分の法線方向である線分（探索ライン）上でエッジ探索を行い、探索ライン上における輝度値の勾配が極大でありかつ分割点に最も近い点を対応エッジとして検出する（図１７）
３．分割点毎に検出された対応エッジと、投影される線分との間の画像上での距離の総和が最小となるような撮像装置の位置及び姿勢の補正値を算出し、撮像装置の位置及び姿勢を補正する。
４． ３．の処理を収束するまで繰り返す最適化計算を行う。

ステップＳ４０３０の処理が終了したらステップＳ４０４０に進み、破綻検出を行う。ここでは、位置及び姿勢の計測に失敗しているかどうかを検出する。破綻検出は、例えば算出された位置及び姿勢に基づき画像上に投影した線分と、上記のステップＳ４０３０で検出されたエッジとの距離に基づいて行う。具体的には、該線分とそれに対応する検出エッジとの距離が予め設定された閾値以上であるエッジの割合が一定値以上であれば破綻していると判定する。ステップＳ４０４０において破綻していると検出された場合には、フラグＦをＴＲＵＥにセットする。ステップＳ４０４０の処理が終わったら、ステップＳ４０５０に進む。

ステップＳ４０５０では、位置姿勢計測を終了するか否かの判定を行う。終了しない場合にはステップＳ４０１０に戻る。

以上説明したように、第三の実施形態では、本発明による位置及び姿勢計測方法を、エッジを用いた位置及び姿勢計測方法の初期化方法として用いることで、エッジを用いた位置及び姿勢計測方法の初期化を自動化することができる。

［変形例１］
以上で説明した実施形態では、傾斜角センサを撮像装置に装着することにより、撮像装置の傾斜角を計測していた。しかしながら、傾斜角を計測するセンサは傾斜角センサに限るものではなく、傾斜角を計測可能なセンサであればいかなるセンサであってもよい。例えば、３自由度の姿勢センサを撮像装置に装着して姿勢を計測し、該姿勢から傾斜角成分を抽出して傾斜角計測値として利用してもよい。

また、第一の実施形態では、撮像装置の位置と方位角を未知変数として算出していた。しかしながら、上記の３自由度の姿勢センサを用いる場合など方位角がまったく未知でない場合には、撮像装置の位置と方位角計測値の補正値を求めてもよい。位置と方位角の補正値の算出は、変形例２−６で説明した方法により行うことができる。さらに、上記の３自由度の姿勢センサによって計測される方位角の精度が高い場合には、撮像装置の位置だけを算出してもよい。位置の算出は、変形例１−２で説明した方法により行うことができる。

［変形例２］
以上で説明した実施形態では、１台の撮像装置を利用していた。しかしながら、撮像装置は１台に限るものではなく、撮像装置間の相対的な位置及び姿勢が固定されており既知であれば、撮像装置が何台であっても本発明の本質を損なうことはない。ここでは、２台の撮像装置１００及び１０１を用いて位置姿勢計測を行う場合を考える。傾斜角センサ１０５は撮像装置１００の傾斜角を計測するものとする。撮像装置１００に対する基準座標系の原点の位置を表す３次元ベクトルをｔ_Ｃ１Ｗ、基準座標系の姿勢を表す３×３回転行列をＲ_Ｃ１Ｗとすると、基準座標系ｘ_ｗから撮像装置１００の座標系ｘ_Ｃ１への変換は次のように表される。
ｘ_Ｃ１＝Ｒ_Ｃ１Ｗｘ_ｗ＋ｔ_Ｃ１Ｗ

また、撮像装置１０１に対する撮像装置１００の位置を表す３次元ベクトルをｔ_Ｃ２Ｃ１、姿勢を表す３×３回転行列をＲ_Ｃ２Ｃ１とすると、基準座標系ｘ_ｗから撮像装置１０１の座標系ｘ_Ｃ２への変換は式２２のように表される。

第一の実施形態で式２〜式４を導いたのと同様に、基準座標系における直線Ｌ上の点ｐ、ｑ、直線Ｌの基準座標系における方向ベクトルｄ（＝ｑ−ｐ）は、
ｎ・（Ｒ_Ｃ２Ｃ１Ｒ_Ｃ１Ｗｐ＋Ｒ_Ｃ２Ｃ１ｔ_Ｃ１Ｗ＋ｔ_Ｃ２Ｃ１）＝０ （式２３）
ｎ・（Ｒ_Ｃ２Ｃ１Ｒ_Ｃ１Ｗｑ＋Ｒ_Ｃ２Ｃ１ｔ_Ｃ１Ｗ＋ｔ_Ｃ２Ｃ１）＝０ （式２４）
ｎ・Ｒ_Ｃ２Ｃ１Ｒ_Ｃ１Ｗｄ＝０ （式２５）
を満たす。ここでｎは、直線Ｌ及び撮像装置１０１の視点位置を通る平面の法線ベクトルである。ｎ＝［ｎ_ｘ ｎ_ｙ ｎ_ｚ］^ｔ、ｐ＝［ｐ_ｘ ｐ_ｙ ｐ_ｚ］^ｔ、ｑ＝［ｑ_ｘ ｑ_ｙ ｑ_ｚ］^ｔ、ｄ＝［ｄ_ｘ ｄ_ｙ ｄ_ｚ］^ｔ、ｔ_ｃｗ＝［ｔ_ｘ ｔ_ｙ ｔ_ｚ］^ｔとすると、式２６、式２７が得られる。

ただし

である。Ｒ_１、Ｒ_２、Ｒ_３は、Ｒ_Ｃ２Ｃ１の各列ベクトルである。式２６、式２７、式７、式８によって立式されるα、β、ｔ_ｘ、ｔ_ｙ、ｔ_ｚについての連立方程式を解くことにより、撮像装置が複数の場合でも、方位角及び位置の算出が可能となる。

［変形例３］
以上で説明した実施形態では、撮像装置に傾斜角センサを装着し、観察対象物体に対する撮像装置の位置及び姿勢を計測していた。しかしながら、本発明はこれに限るものではない。例えば、移動する観察対象物体に傾斜角センサを装着し、基準座標系に固定された撮像装置によって撮影される画像に基づいて、観察対象物体の位置及び姿勢を計測してもよい。

撮像装置の座標系における観察対象物体の位置を表す３次元ベクトルをｔ_ｃｏ、基準座標系における観察対象物体の姿勢を表す３×３回転行列をＲ_ｗｏとすると、観察対象物体の座標系ｘ_ｏから撮像装置の座標系ｘ_ｃへの変換は次のように表される。
ｘ_ｏ＝Ｒ_ＣＷＲ_ＷＯｘ_Ｏ＋ｔ_ＣＯ
ここでＲ_ｃｗは、撮像装置の座標系に対する基準座標系の姿勢を表す３×３回転行列である。観察対象物体の姿勢を表す３×３回転行列Ｒ_ｗｏは、傾斜角成分を表す３×３回転行列を

，方位角成分を表す３×３回転行列を

とすると，

のように分解できる。

は観察対象物体に装着した傾斜角センサの計測値から得ることができる。第一の実施形態で式２〜式４を導いたのと同様に、物体座標系における直線Ｌ上の点ｐ、ｑ、直線Ｌの物体座標系における方向ベクトルｄ（＝ｑ−ｐ）は、
ｎ・（Ｒ_ＣＷＲ_ＷＯｐ＋ｔ_ＣＯ）＝０ （式２８）
ｎ・（Ｒ_ＣＷＲ_ＷＯｑ＋ｔ_ＣＯ）＝０ （式２９）
ｎ・Ｒ_ＣＷＲ_ＷＯｄ＝０ （式３０）
を満たす。ここでｎは、直線Ｌ及び撮像装置の視点位置を通る平面の法線ベクトルである。

とすると、式３１、式３２、式３３が得られる。

ただし、Ｒ_ｃｗ＝［Ｒ_１ Ｒ_２ Ｒ_３］、方位角をθ、α＝ｃｏｓθ、β＝ｓｉｎθとする。式３１、式３２、式３３を用いることにより、以上で説明した実施形態を移動物体の位置及び姿勢の計測に適用することができる。また、基準座標系に固定した撮像装置が複数ある場合であっても、変形例２で説明したのと同様に、本発明を適用することができる。

［変形例４］
以上で説明した実施形態では、撮像装置に傾斜角センサを装着し、観察対象物体に対する撮像装置の位置及び姿勢を計測していた。しかしながら、本発明はこれに限るものではなく、移動する観察対象物体と撮像装置の双方に傾斜角センサを装着し、観察対象物体と撮像装置との間の相対的なの位置及び姿勢を計測してもよい。

撮像装置の座標系における基準座標系の姿勢を表す３×３回転行列をＲ_ｃｗ、基準座標系における観察対象物体の姿勢を表す３×３回転行列をＲ_ｗｏ、撮像装置の座標系における観察対象物体の位置を表す３次元ベクトルをｔ_ｃｏとすると、観察対象物体の座標系ｘ_ｏから撮像装置の座標系ｘ_ｃへの変換は次のように表される。
ｘ_Ｏ＝Ｒ_ＣＷＲ_ＷＯｘ_Ｏ＋ｔ_ＣＯ
Ｒ_ｗｏは、傾斜角成分を表す３×３回転行列を

，方位角成分を表す３×３回転行列を

とすると，

のように分解できる。また、Ｒ_ｃｗは、傾斜角成分を表す３×３回転行列を

，方位角成分を表す３×３回転行列を

とすると，

のように分解できる。これにより、

と表せる。

は観察対象物体と撮像装置の方位角の差であり、一つの行列

として表すことができる。

は観察対象物体及び撮像装置に装着した傾斜角センサの計測値から得ることができる。第一の実施形態で式２〜式４を導いたのと同様に、物体座標系における直線Ｌ上の点ｐ、ｑ、直線Ｌの物体座標系における方向ベクトルｄ（＝ｑ−ｐ）は、

を満たす。ここでｎは、直線Ｌ及び撮像装置の視点位置を通る平面の法線ベクトルである。

とすると、式３７、式３８、式３９が得られる。

ただし、Ｒ^αのｙ軸周りの回転角をθ、α＝ｃｏｓθ、β＝ｓｉｎθとする。式３７、式３８、式３９を用いることにより、以上で説明した実施形態を移動物体の位置及び姿勢の計測に適用することができる。また、また、撮像装置に対して相対的な位置及び姿勢が固定されていて既知な他の撮像装置が複数ある場合であっても、変形例２で説明したのと同様に、本発明を適用することができる。

［変形例５］
以上で説明した第一の実施形態及び第二の実施形態では、基準座標系のｙ軸を鉛直上方向（重力軸の逆方向）としていた。しかしながら、基準座標系のとり方はこれに限るものではなく、いかなるとり方であってもよい。基準座標系のｙ軸が鉛直上方向でない場合には、その座標系とｙ軸が鉛直上方向の座標系との相対姿勢を予め算出しておき、基準座標系をｙ軸が鉛直上方向の座標系に変換することにより本発明を適用可能である。

［その他の実施の形態］
なお、本発明の目的は、前述した実施例の機能を実現するソフトウェアのプログラムコードを記録したコンピュータ可読記憶媒体を、システムあるいは装置に供給し、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）がコンピュータ可読記憶媒体に記憶されたプログラムコードを読出し実行することによっても、達成されることは言うまでもない。

この場合、コンピュータ可読記憶媒体から読出されたプログラムコード自体が前述した実施例の機能を実現することになり、そのプログラムコードを記憶したコンピュータ可読記憶媒体は本発明を構成することになる。

プログラムコードを供給するためのコンピュータ可読記憶媒体としては、例えば、フレキシブルディスク，ハードディスク，光ディスク，光磁気ディスク，ＣＤ−ＲＯＭ，ＣＤ−Ｒ，ＤＶＤ−ＲＯＭ，ＤＶＤ−Ｒ，磁気テープ，不揮発性のメモリカード，ＲＯＭなどを用いることができる。

また、コンピュータが読出したプログラムコードを実行することにより、前述した実施例の機能が実現されるだけでなく、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているＯＳ（オペレーティングシステム）などが実際の処理の一部または全部を行い、その処理によって前述した実施例の機能が実現される場合も含まれることは言うまでもない。

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

第一の実施形態における位置姿勢計測装置１の構成を示す図である。 ３次元幾何モデルの定義方法を説明する図である。 第一の実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。 画像上の直線検出の処理手順を示すフローチャートである。 画像上の直線と３次元空間中の直線との関係を説明する図である。 第二の実施形態における位置姿勢計測装置１の構成を示す図である。 第二の実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。 方位角候補値算出の処理方法を説明するフローチャートである。 線分データの方向ベクトルによる分類を説明する図である。 方位角の算出方法を説明する図である。 ステップＳ２０４０における位置及び姿勢算出の処理の手順を示すフローチャートである。 対応データリストの一例を示す図である。 評価値の算出方法を示す図である。 第三の実施形態における処理の手順を説明するフローチャートである。 画像上への３次元モデルの投影を説明する図である。 ３次元モデルの画像上での分割点を説明する図である。 エッジ検出方法を説明する図である。

Claims (13)

1. 物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値の入力工程と、
   前記撮像装置から撮影画像を入力する画像の入力工程と、
   前記撮影画像から直線を検出する直線検出工程と、
   前記傾斜角計測値、前記検出された直線の撮影画像での方程式および該直線に対応する３次元空間中における直線の３次元空間中での方程式に基づき、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を算出する位置姿勢算出工程とを備えることを特徴とする情報処理方法。
2. 前記位置姿勢算出工程は、前記傾斜角センサが装着された物体または撮像装置の位置と方位角を算出することを特徴とする請求項１に記載の情報処理方法。
3. 前記位置姿勢算出工程は、前記位置と前記方位角を同時に算出することを特徴とする請求項２に記載の情報処理方法。
4. 前記位置姿勢算出工程は、前記方位角を算出した後、前記位置を算出することを特徴とする請求項２に記載の情報処理方法。
5. 前記位置姿勢算出工程は、少なくとも３つの直線の対応をもとに前記位置と前記方位角を算出することを特徴とする請求項２乃至４に記載の情報処理方法。
6. 物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値の入力工程と、
   前記撮像装置から撮影画像を入力する画像の入力工程と、
   前記撮影画像から直線を検出する直線検出工程と、
   前記検出された直線の夫々と、前記物体を表す線分モデルを構成する線分の夫々の方向ベクトルとが仮に対応するとみなして、夫々の組合せについて方位角を求めて投票することにより方位角候補値を算出する方位角候補値算出工程とを備えることを特徴とする情報処理方法。
7. 前記傾斜角計測値、前記方位角候補値及び前記検出された直線と方向ベクトルとの対応に基づき、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を算出する位置姿勢算出工程を備えることを特徴とする請求項６に記載の情報処理方法。
8. 前記方位角候補値算出工程は、投票数が最大の方位角を方位角候補値として出力することを特徴とする請求項６乃至７に記載の情報処理方法。
9. 前記方位角候補値算出工程は、投票数が上位の複数の方位角を方位角候補値として出力することを特徴とする請求項６乃至７に記載の情報処理方法。
10. 物体または撮像装置の位置及び姿勢を計測する情報処理方法であって、
    物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値の入力工程と、
    前記撮像装置から撮影画像を入力する画像の入力工程と、
    前記撮影画像から検出される直線と前記傾斜角センサの計測値に基づいて物体または撮像装置の位置と姿勢を算出する第一の位置姿勢算出工程と、
    前記撮影画像から検出される特徴に基づいて物体または位置及び姿勢の予測値に対する補正値を算出することにより物体または撮像装置の位置と姿勢を算出する第二の位置姿勢算出工程とを備え、
    前記位置及び姿勢の予測値が得られない場合には第一の位置姿勢算出工程を、前記位置及び姿勢の予測値が得られる場合には第二の位置姿勢算出工程を選択する選択工程とを備えることを特徴とする情報処理方法。
11. 物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値の入力手段と、
    前記撮像装置から撮影画像を入力する画像の入力手段と、
    前記撮影画像から直線を検出する直線検出手段と、
    前記傾斜角計測値、前記検出された直線の撮影画像での方程式および該直線に対応する３次元空間中における直線の３次元空間中での方程式に基づき、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を算出する位置姿勢算出手段とを備えることを特徴とする情報処理装置。
12. 物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値の入力手段と、
    前記撮像装置から撮影画像を入力する画像の入力手段と、
    前記撮影画像から前記物体の直線を検出する直線検出手段と、
    前記検出された直線の夫々と、前記物体を表す線分モデルを構成する線分の夫々の方向ベクトルとが仮に対応するとみなして、夫々の組合せについて方位角を求めて投票することにより方位角候補値を算出する方位角候補値算出手段とを備えることを特徴とする情報処理装置。
13. 物体または撮像装置の位置及び姿勢を計測する情報処理装置であって、
    物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値の入力手段と、
    前記撮像装置から撮影画像を入力する画像の入力手段と、
    前記撮影画像から検出される直線と前記傾斜角センサの計測値に基づいて物体または撮像装置の位置と姿勢を算出する第一の位置姿勢算出手段と、
    前記撮影画像から検出される特徴に基づいて物体または位置及び姿勢の予測値に対する補正値を算出することにより物体または撮像装置の位置と姿勢を算出する第二の位置姿勢算出手段とを備え、
    前記位置及び姿勢の予測値が得られない場合には第一の位置姿勢算出手段を、前記位置及び姿勢の予測値が得られる場合には第二の位置姿勢算出手段を選択する選択工程とを備えることを特徴とする情報処理装置。

Images