JP2013117451A - 位置姿勢推定マーク、およびこれを用いた位置姿勢推定装置 - Google Patents

Abstract

【課題】構成が複雑なものとなることを抑制するとともに、対象物の位置および姿勢を推定することができる位置姿勢推定マーク、およびこれを用いた位置姿勢推定装置を提供する。
【解決手段】位置姿勢推定装置１０は、マーカー３３を含む空間を撮像するカメラ１３と、カメラ１３を通じて撮像される撮影画像に基づいてマーカー３３のカメラ１３に対する相対的な位置および姿勢を推定するＰＣ１４とを備える。マーカー３３を構成する面のうち予め定められた面には、四箇所以上の頂点を備える多角形からなるマークが付されている。ＰＣ１４は、撮影画像からマークを抽出したか否かを判定するマーク抽出判定を行う。また、ＰＣ１４は、マークを抽出したと判定したときに、マークを構成する各頂点の位置座標に基づいてマーカー３３のカメラ１３に対する相対的な位置および姿勢を推定する位置座標推定処理。
【選択図】図１

Description

本発明は、位置姿勢推定マーク、およびこれを用いた位置姿勢推定装置に関するものである。

従来、カメラなどの撮像装置を用いて被写体を撮像し、この被写体の実空間における位置および姿勢を推定する装置が提供されている。
このような装置としては、例えば、特許文献１に記載の装置がある。この装置では、まず、マーカーを付した被写体を含む実空間を、予め定められた場所に配設した複数の撮像装置で撮像する。そして、各撮影画像におけるマーカーの位置座標の違いに基づいて、当該マーカーの位置および姿勢を推定するようにしている。このようにマーカーの位置および姿勢を推定することで、被写体の位置および姿勢を推定することができる。

特開２０００−２９８４２７号公報

ところで、撮像装置によって撮像される撮影画像には、多くの場合、被写体やマーカーとともに、これら被写体やマーカー以外のものも撮像される。例えば、特許文献１に記載の発明では、歯列模型が固定される人体頭部模型やテーブル等が、対象物とともに撮像される。このため、マーカーを適切に抽出することが困難であったり、マーカー以外のものを誤ってマーカーと認識してしまうおそれがある。

上述した特許文献１では、対象物に複数の球状のマーカーを所定の形状をなすように取り付けており、撮影画像においてこれらマーカーを適切に抽出するために、マーカーに固有の色を着色したり、固有の色を発色するランプ等を使用している。しかしながら、このような構成を採用すると、位置姿勢推定装置の構成が複雑なものとなってしまう。

これに対して、マーカーの構成を単純化すると、マーカーを撮影画像から適切に抽出することは、非常に困難なものとなってしまう。

本発明は、上記従来技術の問題を解決するためになされたものであって、構成が複雑なものとなることを抑制するとともに、対象物の位置および姿勢を推定することができる位置姿勢推定マーク、およびこれを用いた位置姿勢推定装置を提供することを課題とする。

本発明の第一態様は、上記課題を解決するためになされたものであり、対象物を含む空間を撮像する撮像手段と、前記撮像手段を通じて撮像される撮影画像に基づいて、前記対象物の前記撮像手段に対する相対的な位置および姿勢を推定する推定手段とを備える位置姿勢推定装置であって、前記対象物は、当該対象物を構成する面のうち予め定められた面に、四箇所以上の角部を備える図形からなるマークが設定されるものであり、前記推定手段は、前記撮影画像から前記図形からなるマークを抽出したか否かを判定するマーク抽出判定を行うべく構成されており、前記図形からなるマークを抽出したと判定したときに、前記マークを構成する各角部の位置座標に基づいて前記対象物の前記撮像手段に対する相対的な位置および姿勢を推定する推定処理を行うことを特徴としている。

同構成では、対象物の撮像手段に対する相対的な位置および姿勢を推定するためのマークを、四箇所以上の角部を備える図形とし、この図形を抽出したことをもって推定処理を行うようにしている。このようにマークの形状をマーク以外のものの形状と混同し難い四箇所以上の角部を備える図形としたため、マーク以外のものを誤ってマークを構成するものであると判定し、これに基づいて推定処理を行ってしまうことを抑制することができる。したがって、対象物の位置および姿勢を推定することができなくなったり、誤って推定してしまうことを抑制することができるようになる。

本発明の第二態様は、第一態様にかかる位置姿勢推定装置であって、前記推定手段は、前記撮影画像から前記マークを構成する複数の線分を抽出し、前記抽出した各線分を端部と中央部分とにそれぞれ区画して、前記中央部分の画像上の位置に基づいて前記各線分同士の交点を算出し、前記各線分同士の交点を前記角部の画像上の位置とすることを特徴としている。

マークを抽出する際、角部の画像上の位置は誤差が大きくなることが多い。この点、同構成によれば、各線分の中央部分に基づいて角部の位置座標を算出するようにしているため、角部の画像上の位置を抽出する際に誤差が発生することを抑制することができる。

本発明の第三態様は、第一態様または第二態様にかかる位置姿勢推定装置であって、前記対象物は多面体からなるものであり、前記マークは前記対象物の少なくとも二面に設定されていることを特徴としている。

対象物が撮像手段に対して傾斜している場合など、マークを付された面以外の面が撮像手段を通じて撮像されることがある。このような場合、推定手段はマークを適切に抽出することが困難となる。この点、同構成によれば、マークは対象物の少なくとも二面に設定されているため、マークが撮像手段に撮像され難くなったり、撮像されなくなることを抑制することができるようになる。

本発明の第四態様は、第一態様〜第三態様のいずれかにかかる位置姿勢推定装置であって、当該位置姿勢推定装置は、歯列模型を備える頭部模型と、歯科治療に用いる治療器具とを備える歯科実習装置に適用されるものであり、前記対象物は、前記治療器具に固定されていることを特徴としている。

歯科実習においては、指導者が口腔内を直接視認することは困難であるため、実習者が治療器具を適切な位置および姿勢で歯列模型に当接させているか否かを指導者がリアルタイムで評価することは困難である。この点、同構成によれば、撮影画像に基づいて治療器具の位置および姿勢を明らかにすることができるため、実習者が適切な位置および姿勢で治療器具を歯列模型に当接させているか否かを指導者がリアルタイムに評価することができるようになる。

本発明の第五態様は、第四態様にかかる位置姿勢推定装置であって、前記対象物は、前記歯列模型に固定されていることを特徴としている。

歯科実習においては、実習中に歯列模型の位置および姿勢を変化させる場合がある。しかしながら、歯列模型の位置および姿勢が変化すると、治療器具と歯列模型との位置関係が変化するため、指導者が歯列模型と治療位置との関係を外部から視認することは困難なものとなってしまう。この点、同構成によれば、歯列模型の位置および姿勢をリアルタイムに明らかにすることができるため、実習中に歯列模型の位置および姿勢が変化しても、指導者は、歯列模型と治療装置との位置関係を視認ことができるようになる。

本発明の第六態様は、第四態様または第五態様にかかる位置姿勢推定装置であって、前記治療器具と前記歯列模型とをＣＧ画像として表示することができることを特徴としている。

同構成によれば、指導者は歯列模型と治療器具との位置関係を容易に視認することができるようになる。

本発明の第七態様は、対象物を含む空間を撮像する撮像手段と、前記撮像手段を通じて撮像される撮影画像に基づいて、前記対象物の前記撮像手段に対する相対的な位置および姿勢を推定する推定手段とを備える位置姿勢推定装置に適用されるマークであって、前記推定手段は、前記撮影画像から当該マークを抽出したか否かを判定するマーク抽出判定を行うべく構成されており、当該マークを抽出したと判定したときに、前記マークを構成する各角部の位置座標に基づいて前記対象物の前記撮像手段に対する相対的な位置および姿勢を推定する推定処理を行うものであり、当該マークは、当該対象物を構成する面のうち予め定められた面に付され、四箇所以上の角部を備える図形からなることを特徴としている。

本発明によれば、構成が複雑なものとなることを抑制するとともに、対象物の位置および姿勢を推定することができる位置姿勢推定マーク、およびこれを用いた位置姿勢推定装置を提供することができる。

（ａ）は、本発明の第一実施形態にかかる位置姿勢推定装置を適用した歯科実習装置の概略構成を示す全体図、（ｂ）は、同実施形態にかかる歯列模型の概略構成を示す全体図。 （ａ）および（ｂ）は、同実施形態にかかるマーカーおよび治療器具の概略構成を示す斜視図。 同実施形態にかかる処理全体の流れを示すフローチャート。 同実施形態にかかる位置姿勢推定処理の処理手順を示すフローチャート。 （ａ）は、本発明の第二実施形態にかかる撮影画像に表示されたマークの全体図、（ｂ）〜（ｄ）は、マークの頂点を拡大して示す拡大図。 同実施形態にかかる処理全体の流れを示すフローチャート。 同実施形態にかかる頂点座標算出処理の処理手順を示すフローチャート。 本発明の第三実施形態にかかるマーカーおよび治療器具の概略構成を示す斜視図。 （ａ）は、同実施形態にかかるカメラ、マーカーおよび治療器具の概略構成図、（ｂ）は同実施形態にかかる撮影画像に表示されたマーカーの上面図。 （ａ）は、本発明の第四実施形態にかかるマーカーの全体構成を示す斜視図、（ｂ）〜（ｄ）は、同実施形態にかかるマークの全体図。 同実施形態にかかる処理全体の流れを示すフローチャート。 本発明の他の実施形態にかかるカメラ、歯列模型およびマーカーの概略構成を示す全体図。 本発明の他の実施形態にかかるマークの全体図。

（第一実施形態）
以下、本発明を具体化した第一実施形態を図１〜図４に基づいて説明する。なお、本実施形態では、位置姿勢推定装置１０を歯科実習装置５０に適用した例について説明する。
まず、図１を参照しながら、本実施形態にかかる歯科実習装置５０について説明する。

同図１に示されるように、本実施形態にかかる歯科実習装置５０は、実習者が着座する椅子５１と、歯科治療台５２と、位置姿勢推定装置１０とを含んで構成されている。なお、椅子５１と歯科治療台５２とは、実習者の身長等に応じて鉛直方向に移動させることができる。歯科治療台５２は、当該歯科治療台５２の端部の鉛直方向上方まで延出する支柱５３を含んで構成される。

次に位置姿勢推定装置１０について説明する。
同図１（ａ）に示されるように、本実施形態にかかる位置姿勢推定装置１０は、頭部模型１１と、頭部模型１１に脱着可能な歯列模型１２（図示せず）と、頭部模型１１を撮像することができるカメラ１３（本発明の「撮像手段」に相当）と、カメラ１３に接続されるパーソナルコンピュータ（以下「ＰＣ」、本発明の「推定手段」に相当）１４と、治療器具３１とを含んで構成される。

同図１（ｂ）に、歯列模型１２の概略構成を示す。同図１（ｂ）に示されるように、歯列模型１２は、上顎部２１と、下顎部２２とを含んで構成される。本実施形態における歯列模型１２は、上顎部２１と下顎部２２の位置および姿勢を固定した状態で頭部模型１１に取り付けられる。すなわち、本実施形態において、歯列模型１２の位置および姿勢は予め所定の位置および姿勢に定められる。なお、歯列模型１２は、習得すべき治療技術等に応じて実習者が適宜選択し、頭部模型１１に対して脱着することができる。

カメラ１３は、歯科治療台５２の支柱５３に固定されている。このカメラ１３は、歯列模型１２を均等に撮像することができるように、カメラ１３の中心線が歯列模型１２の正中線上方に位置するように固定されることが好ましい。カメラ１３にて撮像された画像（以下、「撮影画像」）は、ＰＣ１４に出力される。
なお、カメラ１３の内部パラメータ（例えば、レンズ歪み、焦点距離およびカメラ中心位置）は、予めＰＣ１４に入力されている。

ＰＣ１４は、歯列模型１２をＣＧ画像として表示面に表示する。上述したように、本実施形態においては、歯列模型１２の位置および姿勢は予め定められている。このため、実習者がいずれの歯列模型１２を選択したかをＰＣ１４に入力すると、ＰＣ１４は、選択された歯列模型１２に応じて予め定められた歯列模型１２のＣＧ画像を表示面に表示する。

次に図２を参照しながらマーカー３３および治療器具３１について説明する。
同図２（ａ）および（ｂ）に示されるように、治療器具３１の基端部には、マーカー３３（本発明の「対象物」に相当）が固定されている。このように、マーカー３３が治療器具３１に対して固定されているため、治療器具３１の先端部の位置および姿勢は、マーカー３３の位置および姿勢に基づいて決定することができる。

マーカー３３を構成する面のうち、治療器具３１に当接する面の反対側の面には、星型の閉じた多角形（本発明の「図形」に相当）からなるマーク３２が付されている。なお、このマーク３２の形状は、ＰＣ１４の計算量を少なくするため、正多角形とすることが好ましい。

また、このマーク３２にはマーク３２を構成する頂点を識別するための基準線Ｓが付されている。この基準線Ｓは、マーク３２の中心点と、マーク３２を構成する頂点のうちのひとつの頂点（基準頂点）とを結ぶ線分となっている。なお、中心点とは、各マーク３２を構成する各頂点の座標を平均した座標とする。

上述したように、マーク３２は正多角形からなる。このため、基準線Ｓが付されていない場合には、マーク３２の中心点を中心としてマーカー３３を回転させた場合に、回転前と回転後とで撮影画像に表示されるマーク３２の形状は同一の形状であっても、治療器具３１の向きは異なるものとなる場合がある。すなわち、各頂点を識別することができないと、撮影画像に表示されるマーク３２の形状は同一であっても、治療器具３１の姿勢は異なる場合がある。
そこで、本実施形態では、基準線Ｓを付すことにより基準頂点を他の頂点と識別することができるようにしている。このように、基準頂点を他の頂点と識別することができると、マーク３２を構成する各頂点を識別することができるようになる。

ＰＣ１４は、撮影画像に表示されたマーク３２に基づいて治療器具３１の位置および姿勢を推定し、ＣＧ画像（以下、「ツールＣＧモデル」）として歯列模型１２とともに表示面に表示する。

ところで、従来技術において、マーカーをカメラにて撮像することで、当該マーカーの位置および姿勢を推定するときには、一般に、予め定められた位置に固定された複数のカメラにてマーカーを撮像する方法が用いられる。

この方法では、各カメラの三次元空間における位置関係（例えば、カメラ同士の距離）、および少なくとも一方のカメラの座標系（以下、「カメラ座標系」、カメラの中心から撮像したときの視点位置と視点の向きで決まる座標系）を予め求めておく。
ＰＣは、各カメラから出力された各撮影画像からマークを抽出し、各撮影画像におけるマークの位置座標をそれぞれ求める。その後、各撮影画像におけるマークの位置座標の違いと、予め求めておいた各カメラの三次元空間における位置関係とに基づいて、カメラ座標系におけるマークの位置および姿勢、ひいてはマーカーの位置および姿勢を求める。

これに対して、本発明では、単一のカメラ１３を用いてマーク３２を検出し、マーカー３３の座標系（マーカー座標系）を推定するようにしている。
すなわち、ＰＣ１４は、まず同図２（ａ）に示されるように、マーク３２近傍の点を原点とし、マーカー３３の一辺に平行であるｘ軸、当該ｘ軸に直交するｙ軸、当該ｘ軸およびｙ軸に直交するｚ軸からなる座標系（マーカー座標系）を設定する。そして、ＰＣ１４は、マーク３２を構成する各頂点のマーカー座標系における座標（Ｘｉ，Ｙｉ，Ｚｉ）を予め算出しておく。なお、マーク３２はマーカー３３に付されたものであるため、Ｚｉは「０」となる。

位置姿勢推定装置１０が起動され、カメラ１３にて撮像された撮影画像がＰＣ１４に入力されると、ＰＣ１４は、当該撮影画像からマーク３２を抽出する。そして、この抽出したマーク３２を構成する各頂点（本発明の「角部」に相当）の撮影画像上の座標（Ｕｉ，Ｖｉ）をそれぞれ算出する。

次に、ＰＣ１４は、各頂点の座標（Ｕｉ，Ｖｉ）と、予め算出しておいたマーク３２の各頂点の座標（Ｘｉ，Ｙｉ，Ｚｉ）とをホモグラフィによって対応付ける。これにより、カメラ座標系におけるマーカー座標系の位置および姿勢を推定することができる。
例えば同図２（ｂ）に示されるようにマーカー３３がカメラ１３に対して傾けられると、同図２（ｂ）に示されるように、マーカー座標系もマーカー３３の位置および姿勢に応じた位置および姿勢となる。

ＰＣ１４は、このようにカメラ座標系におけるマーカー座標系を算出することで、カメラ座標系におけるマーカー３３の位置および姿勢、ひいてはカメラ座標系における治療器具３１の位置および姿勢を算出することができる。

次に、本実施形態にかかる処理全体の流れを図３を参照しながら説明する。
同図３に示されるように、ＰＣ１４は、まずカメラ１３から撮影画像を取得する（ステップＳ１０１）。
そして、治療器具３１の位置および姿勢を推定する位置姿勢推定処理（本発明の「推定処理」に相当）を行い（ステップＳ１０２）、上述した治療器具３１のＣＧ画像であるツールＣＧモデルをＰＣ１４の表示面に表示する（ステップＳ１０３）。

次に、位置姿勢推定装置について、図４を参照しながら詳細に説明する。
同図４に示されるように、ＰＣ１４は、まず撮影画像に対して二値化処理を施す（ステップＳ２０１）。

次に、この二値化処理を施した撮影画像から輪郭点列を抽出し（ステップＳ２０２）、この抽出した輪郭点列からマーク３２を抽出する（ステップＳ２０３）。
上述したように、マーク３２は星型の閉じた正多角形からなる。本実施形態では、輪郭点列を抽出した後、近似曲線の簡略化手法であるダグラス・ポイカーによる頂点抽出アルゴリズムを用いて、マーク３２を構成する頂点の点列を抽出する。

ＰＣ１４は、抽出されたマーク３２の輪郭に基づいて、抽出された多角形の頂点の数を算出し、この数がマーク３２の頂点数と同一であるか否かを判定する（本発明の「マーク抽出判定」に相当）。そして、抽出した多角形の頂点の数が、予め記憶しているマーク３２の頂点の数と同一であることをもって、輪郭点列からマーク３２を抽出したと判定する。

ＰＣ１４は、次に、基準線Ｓに基づいてマーク３２を構成する各頂点を識別する。そして、マーク３２を構成する各頂点の撮影画像上の座標に基づいて、カメラ座標系におけるマーカー座標系を算出する（ステップＳ２０４）。マーカー座標系の算出方法については、上述したため、ここでの詳しい説明は割愛する。

そして、ＰＣ１４は、算出したマーカー座標系に基づいて、治療器具３１の位置および姿勢を推定する（ステップＳ２０５）。

本実施形態にかかるツールＣＧモデルによれば、治療器具３１の位置および姿勢をＣＧ化することができるため、直接視認することは困難である口腔内における歯列模型１２と治療器具３１との位置関係を明らかにすることができる。

上記本実施形態によれば、以下に示す作用効果を奏することができるようになる。
（１）従来の歯科実習においては、指導者が口腔内を直接視認することは困難であるため、実習者が治療器具３１を適切な位置および姿勢で歯列模型１２に当接させているか否かを指導者がリアルタイムに評価することは困難であった。この点、本実施形態によれば、撮影画像に基づいて治療器具３１の位置および姿勢を明らかにすることができるため、実習者が適切な位置および姿勢で治療器具３１を歯列模型１２に当接させているか否かを指導者がリアルタイムに評価することができるようになる。

（２）また、歯列模型１２および治療器具３１をＣＧ画像にて表示するため、実習者が治療器具３１を適切な位置および姿勢で歯列模型１２に当接させているかを指導者が視認しやすくなる。

（３）また、マーク３２の形状をマーク３２以外のものの形状と混同し難い星型の閉じた正多角形としたため、マーク３２以外のものを誤ってマーク３２を構成するものであると判定し、これに基づいて位置姿勢推定処理を行ってしまうことを抑制することができる。したがって、マーカー３３の位置および姿勢を誤って推定することを抑制することができるようになる。

（５）また、ＰＣ１４は、撮影画像から抽出した各頂点の座標とマーク３２の各頂点の空間座標とをホモグラフィによって対応付けることで、カメラ座標系におけるマーカー座標系の位置および姿勢を推定するようにしている。このため、カメラ１３を複数用いることなく、単一のカメラ１３を用いることで、マーカー３３の位置および姿勢を推定することができるようになる。このため、位置姿勢推定装置１０の構成をより簡易なものとすることができる。

（６）また、基準線Ｓによってマーク３２を構成する各頂点を識別することができるため、治療器具３１の姿勢を誤って推定することを抑制することができるようになる。

（第二実施形態）
以下、本発明を具体化した第二実施形態を上記第一実施形態と異なる点を中心に図５〜図７に基づいて説明する。なお、これら図５〜図７において、第一実施形態にて説明した各要素と同一の要素についてはそれぞれ同一の符号を示しており、それら要素についての重複する説明は割愛する。

上記第一実施形態で説明したように、ＰＣ１４は、二値化処理を施した撮影画像に基づいて、マーク３２を構成する各頂点の撮影画像上の座標（Ｕｉ，Ｖｉ）を抽出するようにしている。
ところで、ＰＣ１４は画素を単位として撮影画像を表示する。このため、ＰＣ１４によって抽出されたマーク３２の各頂点は、僅かにずれた座標に表示されてしまうおそれがある。また、上述したように撮影画像に二値化処理を施すと、このようなずれは大きくなってしまうおそれがある。

またさらに、二値化処理を施すと、ノイズが画像に残ってしまうことがあるため、二値化処理を施すときには、事前に平滑化処理等のノイズ除去処理を施すことがある。このような平滑化処理を施すことでも、同様に、マーク３２の頂点の座標は変化してしまうおそれがある。
そこで、本実施形態では、ＰＣ１４は、頂点座標算出処理を行うようにしている。

以下、図６を参照しながら、本実施形態にかかる処理全体の流れについて説明する。
同図６に示されるように、この処理が開始されると、ＰＣ１４は、まず、撮影画像に対して二値化処理を施す（ステップＳ３０１）。

例えば、図５（ｂ）および図５（ｃ）に示されるように、撮像画像を構成する画素のうち、所定の閾値以上の輝度を有する画素を黒画素に、所定の閾値未満の輝度を有する画素を白画素に変換する。
なお、同図５（ｂ）および（ｃ）に示されるように、頂点近傍の画素は、小さい値となることが多いため、このような二値化処理によって、白画素に変換されることが多い。

次に、この二値化処理を施した撮影画像から輪郭点列を抽出し（ステップＳ３０２、図５（ｄ）参照）、この輪郭点列からマーク３２を抽出する（ステップＳ３０３）。なお、マーク３２を抽出する方法は上記第一実施形態にて説明したため、詳しい説明は割愛する。

その後、ＰＣ１４は、頂点座標算出処理を行う（ステップＳ３０４）。
ＰＣ１４は、頂点座標算出処理（ステップＳ３０４）を行った後、カメラ座標系におけるマーカー座標系を算出し（ステップＳ３０５）、このマーカー座標系に基づいて治療器具３１の位置および姿勢を推定して（ステップＳ３０６）、この処理を一旦終了する。

次に、図７を参照しながら頂点座標算出処理について詳細に説明する。
同図７に示されるように、頂点座標算出処理が開始されると、ＰＣ１４は、まず抽出した辺毎の点列（本発明の「各線分」に相当、図５（ｄ）にて示す点列）を取得する（ステップＳ４０１）。

次に、各辺を構成する点列から辺の点列数を算出する。その後、各点列のうち、端点からの点列数が辺の点列数の５〜４５％（好ましくは、１０％）にある点列（本発明の「端部」に相当）を削除する（ステップＳ４０２）。なお、端点とは、マーク３２を構成する各点列のうち、頂点を構成する点を示す。

次に、ＰＣ１４は、端点から５〜４５％（好ましくは、１０％）にある点列を削除した点列（本発明の「中央部分」に相当）に基づいて、多角形を構成する各辺の方程式を算出し（ステップＳ４０３）、次に隣り合う辺の交点（図５（ｄ）に示す点Ａ）を算出して、この交点の座標を頂点の座標として（ステップＳ４０４）、この処理を一旦終了する。

図５（ｂ）に示されるように、ＰＣ１４は画素を単位として撮影画像を表示する。このため、ＰＣ１４によって抽出されたマーク３２の各頂点は、僅かにずれた座標（例えば、同図５（ｂ）に示す座標（Ｕｉ，Ｖｉ））に表示されてしまうおそれがある。
特に、カメラ１３やＰＣ１４の分解能が低い場合は、このようなずれは大きくなってしまうおそれがある。
また、二値化処理を施すことにより、このようなずれはさらに大きくなってしまうおそれがある。

しかしながら、本実施形態によれば、同図５（ｄ）に示されるように、端点から５〜４５％（好ましくは、１０％）にある点列を削除した点列に基づいて頂点（同図５（ｄ）に示される点Ａ）の座標を算出しているため、上述したように二値化処理等に起因して頂点の座標がずれてしまうことを抑制することができる。

上記説明した第二実施形態によれば、上述した作用効果（１）〜（５）に加えて、以下の作用効果を奏することができるようになる。
（６）本実施形態によれば、各辺を構成する点列のうち、端点からの点列数が辺の点列数の５〜４５％（好ましくは、１０％）以内にある点列を削除した点列に基づいて頂点の座標を算出しているため、抽出した頂点の座標が実際に撮像した座標と異なるものとなることを抑制することができるようになる。

（第三実施形態）
以下、本発明を具体化した第三実施形態を上記第一実施形態と異なる点を中心に図８および図９に基づいて説明する。なお、これら図８および図９において、第一実施形態にて説明した各要素と同一の要素についてはそれぞれ同一の符号を示しており、それら要素についての重複する説明は割愛する。

上述した第一実施形態では、マーク３２をマーカー３３の一面のみに付すようにした。これに対して、本実施形態では、図８に示されるように、マーカー３３を立方体とするとともに、マーカー３３を構成する全ての面にマーク３２を付すようにしている。
同図８に示されるように、これらマーク３２は、それぞれ異なる形状からなる。また、いずれも四点以上の頂点を備える閉じた多角形である。

本実施形態においては、ＰＣ１４は、まず同図８に示されるように、面Ｆ１に付されたマーク３２近傍の点を原点とし、面Ｆ１の一辺に平行なｘ軸、当該ｘ軸に直交するｙ軸、当該ｘ軸およびｙ軸に直交するｚ軸からなるマーカー座標系を設定する。そして、ＰＣ１４は、マーカー座標系におけるマーク３２を構成する各頂点の座標（Ｘｉ，Ｙｉ，Ｚｉ）予め算出しておく。なお、面Ｆ１に付されたマーク３２のＺｉは「０」となる。

上述したように、位置姿勢推定装置１０が起動され、カメラ１３にて撮像された撮影画像がＰＣ１４に入力されると、ＰＣ１４は、当該撮影画像から各マーク３２を抽出する。この際、ＰＣ１４は、基準線Ｓに基づいて、各マーク３２の頂点を識別する。そして、この抽出した各マーク３２を構成する各頂点の撮影画像上の座標（Ｕｉ，Ｖｉ）をそれぞれ算出する。

次に、ＰＣ１４は、各頂点の座標（Ｕｉ，Ｖｉ）と、予め算出しておいた各マーク３２の各頂点の座標（Ｘｉ，Ｙｉ，Ｚｉ）とを対応付ける。
上述した第一実施形態では、マーク３２を一面にのみに付したため、ホモグラフィを用いて座標（Ｕｉ，Ｖｉ）と座標（Ｘｉ，Ｙｉ，Ｚｉ）とを対応付けた。これに対して、本実施形態では、ホモグラフィを用いることなく、座標（Ｕｉ，Ｖｉ）と座標（Ｘｉ，Ｙｉ，Ｚｉ）とを対応付けて、カメラ座標系におけるマーカー座標系の位置および姿勢を求める。

また、本実施形態においては、ＰＣ１４は、抽出した多角形の頂点の数に基づいて、いずれの面を検出しているかを認識したうえで、当該検出した面のマーク３２の頂点の座標（Ｕｉ，Ｖｉ）と、マーク３２の各頂点の座標（Ｘｉ，Ｙｉ，Ｚｉ）とを対応付ける。

例えば、図９（ａ）に示されるように治療器具３１がカメラ１３に対して傾けられたときには、同図９（ｂ）に示されるようにマーカー３３を構成する面のうち面Ｆ１と面Ｆ２とが撮影画像に表示される。

このとき、ＰＣ１４は、正六角形からなるマーク３２と正七角形からなるマーク３２とを撮影画像から抽出する。ＰＣ１４は、正六角形の付された面は面Ｆ１であり、正七角形の付された面は面Ｆ２であると認識する。
そして、各多角形を構成する各頂点のマーカー座標系における座標（Ｘｉ，Ｙｉ，Ｚｉ）と、当該頂点の撮影画像における座標（Ｕｉ，Ｖｉ）とを対応付けることで、カメラ座標系におけるマーカー座標系の位置および姿勢を算出する。

上記本実施形態によれば、上述した作用効果（１）〜（５）に加えて、以下の作用効果を奏することができるようになる。
（７）本実施形態によれば、マーク３２がマーカー３３を構成する全ての面に付されているため、マーカー３３がカメラ１３に対して大きく傾けられたときであっても、いずれかのマーク３２が撮像されることとなる。このため、マーク３２が撮影画像に表示されなくなることによって、マーカー３３の位置および姿勢を推定することができなくなったり、推定し難くなることを抑制することができるようになる。

（８）また、複数の面に付されたマーク３２に基づいてマーカー３３の位置および姿勢を推定することができるため、マーカー３３の位置および姿勢をより正確に推定することができるようになる。

（第四実施形態）
以下、本発明を具体化した第四実施形態を上記第三実施形態と異なる点を中心に図１０および図１１に基づいて説明する。なお、これら図１０および図１１において、第三実施形態にて説明した各要素と同一の要素についてはそれぞれ同一の符号を示しており、それら要素についての重複する説明は割愛する。

上述した第三実施形態では、マーカー３３の各面に、それぞれ異なる形状からなる多角形を付すようにした。これに対して、本実施形態では、図１０（ａ）に示されるように、マーカー３３の各面に正六角形からなるマーク３２を付している。

また、同図１０（ａ）および（ｂ）に示されるように、面Ｆ１に付したマーク３２には、中心点Ｃ１と頂点Ｖ１とを結ぶ基準線Ｌ１を付している。また、同図１０（ａ）および（ｃ）に示されるように、面Ｆ２に付したマーク３２には、中心点Ｃ２と頂点Ｖ７とを結ぶ基準線Ｌ２、および中心点Ｃ２と頂点Ｖ８とを結ぶ基準線Ｌ３を付している。またさらに、同図１０（ａ）および（ｄ）に示されるように、面Ｆ３に付したマーク３２には、中心点Ｃ３と頂点Ｖ１３とを結ぶ基準線Ｌ４、中心点Ｃ３と頂点Ｖ１４とを結ぶ基準線Ｌ５、および中心点Ｃ３と頂点Ｖ１５とを結ぶ基準線Ｌ６を付している。

本実施形態では、ＰＣ１４は、中心点と頂点との間に付された線分に基づいて、各マーク３２の基準頂点を識別するとともに、抽出したマーク３２がいずれの面に付されたマーク３２であるかを認識するようにしている。
以下、図１１を参照しながら、同実施形態にかかる処理全体の流れを説明する。

同図１１に示されるように、本処理が開始されると、ＰＣ１４は、まず撮影画像に対して二値化処理を施し（ステップＳ５０１）、この二値化処理を施した撮影画像から輪郭点列を抽出する（ステップＳ５０２）。次に、ＰＣ１４は、この輪郭点列からマーク３２を抽出し（ステップＳ５０３）、抽出した各マーク３２について、中心点をそれぞれ算出する（ステップＳ５０４）。

例えば、Ｆ１に付したマーク３２（図１０（ｂ）参照）を抽出し、頂点Ｖ１〜Ｖ６に基づいて中心点Ｃ１の座標を算出した場合（ステップＳ５０４）、マーク３２を構成する各頂点Ｖ１〜Ｖ６と、中心点Ｃ１とを結ぶ各線分のうち、基準線Ｌ１のみが黒色の線分として表示されている。
また、例えば、Ｆ２に付したマーク３２（図１０（ｃ）参照）を抽出し、頂点Ｖ７〜Ｖ１２に基づいて中心点Ｃ２の座標を算出した場合（ステップＳ５０４）、マーク３２を構成する各頂点Ｖ７〜Ｖ１２と、中心点Ｃ２とを結ぶ各線分のうち、基準線Ｌ２および基準線Ｌ３のみが黒色の線分として表示されている。

そこで、ＰＣ１４は、マーク３２を構成する各頂点と中心点とを結ぶ線分の点列を算出し（ステップＳ５０５）、各頂点と中心点とを結ぶ点列のうち、黒色の画素からなる点を所定割合以上含む点列を基準線とする（ステップＳ５０６）。

そして、この基準線に基づいて、基準頂点およびマーク３２同士を識別する（ステップＳ５０７）

その後、ＰＣ１４は、カメラ座標系におけるマーカー座標系の位置および姿勢を算出し（ステップＳ５０８）、治療器具３１の位置および姿勢を推定する（ステップＳ５０９）。

上記実施形態によれば、上述した作用効果（１）〜（５）、（７）および（８）に加えて、以下の作用効果を奏することができるようになる。
（９）本実施形態によれば、同じ形状からなるマーク３２同士を区別することができる。このため、マーカー３３の位置および姿勢を推定するにあたって、マーク３２同士を混同してしまうことによりマーカー３３の位置および姿勢を誤って推定してしまうことを抑制することができるようになる。

（その他の実施形態）
なお、この発明にかかる位置姿勢推定装置１０は、上記実施形態にて例示した構成に限定されるものではなく、同実施の形態を適宜変更した例えば次のような形態として実施することもできる。また以下の各変形例は、上記実施形態についてのみ適用されるものではなく、異なる変形例同士を互いに組み合わせて実施することもできる。

・上記各実施形態では、歯列模型１２の位置および姿勢を固定するようにしたが、本発明はこれに限られるものではなく、歯列模型１２を構成する上顎部２１および下顎部２２の位置および姿勢を変更することができるようにしてもよい。この場合は、図１２に示されるように、上顎部２１および下顎部２２にマーカー３３を固定して、カメラ１３にてこれらマーカー３３を撮像し、このマーカー３３の位置および姿勢に基づいて上顎部２１および下顎部２２の位置および姿勢を推定することが好ましい。

歯科実習においては、実習中に歯列模型１２の位置および姿勢を変化させる場合がある。本変形例によれば、歯列模型１２の位置および姿勢をリアルタイムで明らかにすることができるため、実習中に歯列模型１２の位置および姿勢が変化しても、指導者は、歯列模型１２と治療器具３１との位置関係を視認することができることができるようになる。

・上記変形例では、上顎部２１および下顎部２２の位置および姿勢を変更することができるようにしたが、本発明はこれに限られるものではなく、下顎部２２の位置および姿勢は固定し、上顎部２１の位置および姿勢のみを変更することができるようにしてもよい。この場合は、マーカー３３を上顎部２１のみに固定して、下顎部２２にはマーカー３３を取り付けないようにしてもよい。

・または、上顎部２１の位置および姿勢は固定し、下顎部２２の位置および姿勢のみを変更することができるようにしてもよい。この場合は、マーカー３３を下顎部２２のみに固定して、上顎部２１にはマーカー３３を取り付けないようにしてもよい。

・上述した第三実施形態および第四実施形態において、上述した第二実施形態にて説明したとおり、マーク３２の各辺を構成する点列のうち、端点からの点列数が辺の点列数の４〜４５％（好ましくは、１０％）にある点列を削除した点列に基づいて各辺の方程式を算出し、各辺の交点の座標を頂点の座標としてもよい。

・上述した第三実施形態では、マーカー３３を構成する全ての面にマーク３２を付すようにしたが、本発明はこれに限られるものではなく、マーカー３３の二面のみにマーク３２を付すようにしてもよい。また、マーカー３３の三面のみにマーク３２を付すようにしてもよい。さらに、マーカー３３の四面のみにマーク３２を付すようにしてもよい。

・上記第二実施形態では、端点からの点列数が辺の点列数の４〜４５％（好ましくは、１０％）にある点列を削除するようにしたが、本発明はこれに限られるものではなく、抽出した点列を全て用いて、頂点の座標を再度算出するようにしてもよい。

・上記第三実施形態では、マーカー３３の形状と立方体としたが、本発明におけるマーカー３３の形状は立方体に限られるものではなく、他の多面体であってもよい。

・上記各実施形態および各変形例では、ワールド座標系（カメラ１３の位置および姿勢を表わす座標系）をカメラ座標系に一致させたが、本発明におけるワールド座標系はカメラ座標系に限られるものではなく、任意に決定することができる。

・上記各実施形態および各変形例では、マーカー座標系の原点を平面Ｆ１においたが、本発明におけるマーカー座標系は、これに限られるものではなく、マーカー３３やマーカー３３と一体となっている治療器具３１内で任意に決定することができる。

・上記各実施形態および各変形例では、マーク３２の形状を星型の正多角形または五角形、または六角形または七角形としたが、本発明におけるマーク３２の形状はこれらに限られるものではなく、四点以上の頂点を備える多角形であればよい。

・上記各実施形態および各変形例では、マーク３２を所定の太さを備える直線からなるものとしたが、本発明のマーク３２はこれに限られるものではなく、ＰＣ１４が最外周の点列を抽出することができるものであればよい。例えば、マーク３２の内部をマーク３２の外周の輪郭と同じ色で塗りつぶしたものであってもよい。

・上記各実施形態および各変形例では、基準線Ｓ、基準線Ｌ１、基準線Ｌ２もしくは基準線Ｌ３を用いて基準頂点を識別することで、マーク３２の向きを識別するようにしたが、本発明はこれに限られるものではない。例えば、図１３に記載されるように、マーカー３３を回転させた場合であっても、回転後と回転前とで同じ形状となるおそれがない形状のマーク３２を用いるようにしてもよい。本変形例によれば、基準線を用いることなく、マーク３２の向きを識別することができるようになる。

・上記各実施形態および各変形例では、基準頂点を識別するために、基準線Ｓを用いるようにしたが、本発明における基準頂点の識別方法はこれに限られるものではない。例えば、マーク３２の一部を塗りつぶすようにしてもよい。

・上記各実施形態および各変形例では、マーク３２をマーカー３３に直接付すようにしたが、本発明はこれに限られるものではなく、マーク３２は、マーカー３３と離れた位置に設定されていてもよい。マーカー３３の位置および座標をマーク３２の位置および座標に基づいて決定することができればよい。

・上記各実施形態および各変形例では、治療器具３１の位置および姿勢をツールＣＧモデルにてＰＣ１４の表示面に表示するようにしたが、本発明における治療器具３１の位置および姿勢の表示方法はＣＧモデルに限られるものではなく、他の方法を用いるようにしてもよい。例えば、アニメーションや簡易モデルにて表示するようにしてもよい。

・上記各実施形態および各変形例では、位置姿勢推定装置１０を歯科実習装置５０に適用した例について説明したが、本発明における位置姿勢推定装置１０はこれに限られるものではなく、ＡＶ機器やゲーム装置等、他の装置に適用することはできる。

１０…位置姿勢推定装置
１１…頭部模型
１２…歯列模型
１３…カメラ
１４…パーソナルコンピュータ（ＰＣ）
２１…上顎部
２２…下顎部
３１…治療器具
３２…マーク
３３…マーカー
５０…歯科実習装置
５１…椅子
５２…歯科治療台
５３…支柱

Claims (7)

1. 対象物を含む空間を撮像する撮像手段と、前記撮像手段を通じて撮像される撮影画像に基づいて、前記対象物の前記撮像手段に対する相対的な位置および姿勢を推定する推定手段とを備える位置姿勢推定装置であって、
   前記対象物は、当該対象物を構成する面のうち予め定められた面に、四箇所以上の角部を備える図形からなるマークが設定されるものであり、
   前記推定手段は、前記撮影画像から前記図形からなるマークを抽出したか否かを判定するマーク抽出判定を行うべく構成されており、
   前記図形からなるマークを抽出したと判定したときに、前記マークを構成する各角部の位置座標に基づいて前記対象物の前記撮像手段に対する相対的な位置および姿勢を推定する推定処理を行う
   ことを特徴とする位置姿勢推定装置。
2. 請求項１に記載の位置姿勢推定装置であって、
   前記推定手段は、前記撮影画像から前記マークを構成する複数の線分を抽出し、前記抽出した各線分を端部と中央部分とにそれぞれ区画して、前記中央部分の位置座標に基づいて前記各線分同士の交点を算出し、前記各線分同士の交点を前記角部の位置座標とする
   ことを特徴とする位置姿勢推定装置。
3. 請求項１または２に記載の位置姿勢推定装置であって、
   前記対象物は多面体からなるものであり、
   前記マークは前記対象物の少なくとも二面に設定されている
   ことを特徴とする位置姿勢推定装置。
4. 請求項１〜３のいずれか一項に記載の位置姿勢推定装置であって、
   当該位置姿勢推定装置は、歯列模型を備える頭部模型と、歯科治療に用いる治療器具とを備える歯科実習装置に適用されるものであり、
   前記対象物は、前記治療器具に固定されている
   ことを特徴とする位置姿勢推定装置。
5. 請求項４に記載の位置姿勢推定装置であって、
   前記対象物は、前記歯列模型に固定されている
   ことを特徴とする位置姿勢推定装置。
6. 請求項４および５に記載の位置姿勢推定装置であって、
   前記治療器具と前記歯列模型とをＣＧ画像として表示することができる
   ことを特徴とする位置姿勢推定装置。
7. 対象物を含む空間を撮像する撮像手段と、前記撮像手段を通じて撮像される撮影画像に基づいて、前記対象物の前記撮像手段に対する相対的な位置および姿勢を推定する推定手段とを備える位置姿勢推定装置に適用されるマークであって、
   前記推定手段は、前記撮影画像から当該マークを抽出したか否かを判定するマーク抽出判定を行うべく構成されており、
   当該マークを抽出したと判定したときに、前記マークを構成する各角部の位置座標に基づいて前記対象物の前記撮像手段に対する相対的な位置および姿勢を推定する推定処理を行うものであり、
   当該マークは、当該対象物を構成する面のうち予め定められた面に付され、四箇所以上の角部を備える図形からなる
   ことを特徴とするマーク。

Images