(57)【要約】【課題】所定の対象の３次元座標を容易且つ適切に推定できるようにする。【解決手段】複数の画像と、当該画像の撮影時のカメラ３の位置情報及び撮影方向情報とを記憶するとともに、対象の１以上の対象点についての３次元座標を記憶する記憶部１５と、記憶部１５に３次元座標が記憶されている対象点に対応する特徴点の或る画像中の実位置と、対象点の３次元座標と、或る画像の撮影時のカメラの位置情報及び撮影方向情報とに基づいて、或る画像の撮影時のカメラの焦点距離を推定し、推定された焦点距離に基づいて、或る画像と他の画像とに含まれている、３次元座標が未知である対象点についての３次元座標を推定する画像処理部１３とを有するように構成する。【選択図】図１
2011085971.tif 000002
【特許請求の範囲】
【請求項１】
所定の対象に存在する複数の対象点についての３次元座標を、前記対象の少なくとも一部が含まれる複数の画像に基づいて、推定する画像処理装置であって、
前記複数の画像と、当該画像の撮影時の前記カメラの位置情報及び撮影方向情報とを記憶する画像記憶手段と、
前記対象の１以上の対象点についての３次元座標を記憶する座標記憶手段と、
前記座標記憶手段に前記３次元座標が記憶されている前記対象点に対応する特徴点の或る画像中の実位置と、前記対象点の前記３次元座標と、前記或る画像の撮影時の前記カメラの位置情報及び撮影方向情報とに基づいて、前記或る画像の撮影時の前記カメラの焦点距離を推定する焦点距離推定手段と、
前記推定された前記焦点距離に基づいて、前記或る画像と他の画像とに含まれている、３次元座標が未知である対象点についての３次元座標を推定する座標推定手段と
を有する画像処理装置。
【請求項２】
前記焦点距離推定手段は、前記位置情報、前記撮影方向情報及び、前記３次元座標に基づいて推定される前記対象点に対応する特徴点の前記或る画像上の推定位置と、前記特徴点の前記或る画像上の前記実位置とに基づいて、前記焦点距離を推定する
請求項１に記載の画像処理装置。
【請求項３】
前記焦点距離推定手段は、複数の対象点に対応するそれぞれの特徴点の前記推定位置と、前記複数の対象点に対応するそれぞれの特徴点の前記実位置とのずれに関する関数の和がもっとも小さくなるように、前記焦点距離を推定する
請求項２に記載の画像処理装置。
【請求項４】
前記焦点距離推定手段は、共役勾配法を用いて、前記ずれに関する関数の和がもっとも小さくなるような焦点距離を推定する
請求項３に記載の画像処理装置。
【請求項５】
前記画像を撮影する前記カメラは、前記カメラの位置及び撮影方向を変更可能なデバイスに装着されており、
前記デバイスの状態を特定することのできる状態特定可能データに基づいて、前記カメラの前記位置情報及び前記撮影方向情報を特定するカメラ情報特定手段と、
前記カメラにより撮影された前記画像と、前記撮影時における前記カメラの前記位置情報及び前記撮影方向情報とを前記画像記憶手段に格納する格納制御手段と
を更に有する請求項１乃至請求項４のいずれか一項に記載の画像処理装置。
【請求項６】
処理対象の複数の画像を用いて複数の対象点の３次元座標を推定した後に、各画像に含まれる複数の対象点の前記３次元座標を用いて、各画像の撮影時の前記カメラの位置情報及び撮影方向情報を最適化するカメラ情報最適化手段を
更に有する請求項１乃至請求項５のいずれか一項に記載の画像処理装置。
【請求項７】
処理対象の複数の画像を用いて対象点の３次元座標を推定した後に、同一の対象点が含まれている複数の画像における前記カメラの位置情報及び撮影方向情報に基づいて、前記対象点の３次元座標を最適化する３次元座標最適化手段を
更に有する請求項１乃至請求項６のいずれか一項に記載の画像処理装置。
【請求項８】
複数の前記対象点の前記３次元座標に基づいて、前記対象についての３次元モデルを形成するモデル形成手段を更に有する
請求項１乃至請求項７のいずれか一項に記載の画像処理装置。
【請求項９】
所定の対象に存在する複数の対象点についての３次元座標を、前記対象の少なくとも一部が含まれる複数の画像に基づいて、推定する画像処理装置による画像処理方法であって、
前記画像処理装置の画像記憶手段が、前記複数の画像と、当該画像の撮影時の前記カメラの位置情報及び撮影方向情報とを記憶する画像記憶ステップと、
前記画像処理装置の座標記憶手段が、前記対象の１以上の対象点についての３次元座標を記憶する座標記憶ステップと、
前記画像処理装置の焦点距離推定手段が、前記座標記憶手段に前記３次元座標が記憶されている前記対象点に対応する特徴点の或る画像中の実位置と、前記対象点の前記３次元座標と、前記或る画像の撮影時の前記カメラの位置情報及び撮影方向情報とに基づいて、前記或る画像の撮影時の前記カメラの焦点距離を推定する焦点距離推定ステップと、
前記画像処理装置の座標推定手段が、前記推定された前記焦点距離に基づいて、前記或る画像と他の画像とに含まれている、３次元座標が未知である対象点についての３次元座標を推定する座標推定ステップと
を有する画像処理方法。
【請求項１０】
コンピューターに、所定の対象に存在する複数の対象点についての３次元座標を、前記対象の少なくとも一部が含まれる複数の画像に基づいて、推定させるための画像処理プログラムであって、
前記コンピューターを
前記複数の画像と、当該画像の撮影時の前記カメラの位置情報及び撮影方向情報とを記憶する画像記憶手段と、
前記対象の１以上の対象点についての３次元座標を記憶する座標記憶手段と、
前記座標記憶手段に前記３次元座標が記憶されている前記対象点に対応する特徴点の或る画像中の実位置と、前記対象点の前記３次元座標と、前記或る画像の撮影時の前記カメラの位置情報及び撮影方向情報とに基づいて、前記或る画像の撮影時の前記カメラの焦点距離を推定する焦点距離推定手段と、
前記推定された前記焦点距離に基づいて、前記或る画像と他の画像とに含まれている、３次元座標が未知である対象点についての３次元座標を推定する座標推定手段と
して機能させる画像処理プログラム。
【請求項１１】
コンピューターに、所定の対象に存在する複数の対象点についての３次元座標を、前記対象の少なくとも一部が含まれる複数の画像に基づいて、推定させるための画像処理プログラムを格納する記録媒体であって、
前記コンピューターを
前記複数の画像と、当該画像の撮影時の前記カメラの位置情報及び撮影方向情報とを記憶する画像記憶手段と、
前記対象の１以上の対象点についての３次元座標を記憶する座標記憶手段と、
前記座標記憶手段に前記３次元座標が記憶されている前記対象点に対応する特徴点の或る画像中の実位置と、前記対象点の前記３次元座標と、前記或る画像の撮影時の前記カメラの位置情報及び撮影方向情報とに基づいて、前記或る画像の撮影時の前記カメラの焦点距離を推定する焦点距離推定手段と、
前記推定された前記焦点距離に基づいて、前記或る画像と他の画像とに含まれている、３次元座標が未知である対象点についての３次元座標を推定する座標推定手段と
して機能させる画像処理プログラムを格納する記録媒体。
【請求項１２】
所定の対象に存在する複数の対象点についての３次元座標を、前記対象の少なくとも一部が含まれる複数の画像に基づいて、推定する画像処理システムであって、
前記画像を撮影する前記カメラと、
前記カメラを装着し、前記カメラの位置及び撮影方向を変更可能なデバイスと、
前記デバイスの状態を特定することのできる状態特定可能データに基づいて、前記カメラの前記位置情報及び前記撮影方向情報を特定するカメラ情報特定手段と、
前記カメラにより撮影された前記画像と、前記撮影時における前記カメラの前記位置情報及び前記撮影方向情報とを画像記憶手段に格納する格納制御手段と、
前記対象の１以上の対象点についての３次元座標を記憶する座標記憶手段と、
前記座標記憶手段に前記３次元座標が記憶されている前記対象点に対応する特徴点の或る画像中の実位置と、前記対象点の前記３次元座標と、前記或る画像の撮影時の前記カメラの位置情報及び撮影方向情報とに基づいて、前記或る画像の撮影時の前記カメラの焦点距離を推定する焦点距離推定手段と、
前記推定された前記焦点距離に基づいて、前記或る画像と他の画像とに含まれている、３次元座標が未知である対象点についての３次元座標を推定する座標推定手段と
を有する画像処理システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、周囲の環境等の対象に存在する複数の対象点についての３次元座標を推定する画像処理装置等に関する。
【背景技術】
【０００２】
従来、複数の画像に基づいて、画像中に含まれる対象についての３次元座標を求め、対象についての３次元モデルを形成する技術が知られている。
【０００３】
例えば、特許文献１には、カメラのオリエンテーションを固定させて、カメラを直線のパス上を移動させ、規則的な間隔をおいて画像を逐次撮影することにより、対象についての一連のイメージ（イメージシーケンス）を取得するようにして、対象についての３次元モデルの作成処理を単純化する技術が開示されている。
【０００４】
また、特許文献２には、場面の３Ｄ（３次元）モデルを、未知の外部値を備えた１セットのイメージから復元する技術が開示されている。
【０００５】
また、非特許文献１には、ハンドヘルドカメラを使用して、オブジェクトの３Ｄモデルを復元する方法が開示されている。非特許文献１に示されたアルゴリズムでは、各フレームのカメラの位置及びオリエンテーションは把握されていないために、エピポーラー拘束を使用して、２つの視界の関係を解決して、３Ｄモデルの復元を行っている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】米国特許出願公開第２００７／０２４２８７２号明細書
【特許文献２】米国特許出願公開第２００７／０２５３６１８号明細書
【非特許文献】
【０００７】
【非特許文献１】マーク  ポレフェイズ(Marc Pollefeys)著、「ハンドヘルドカメラによる視覚的モデリング（Visual modeling with a hand-held camera）」、コンピュータービジョン国際ジャーナル（InternationalJournal of Computer Vision）、２００４、５９（３）、Ｐ２０７−２３２
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかしながら、特許文献１の技術によると、カメラのオリエンテーションが固定され、直線上の一定間隔の位置でしか対象の画像を撮影しないので、対象の複雑な形状を適切に復元することができないという問題がある。また、特許文献２においては、スケルトン化等の複雑な処理を行っており、容易に３Ｄモデルを復元することができないという問題がある。また、非特許文献１においては、エピポーラー拘束を使用して、２つの画像の視界の関係を解決する必要があるので、処理が複雑になるという問題がある。
【０００９】
本発明は、上記課題に鑑みなされたものであり、その目的は、所定の対象の３次元座標を容易且つ適切に推定することのできる技術を提供することにある。
【課題を解決するための手段】
【００１０】
上記目的達成のため、本発明の第１の観点に係る画像処理装置は、所定の対象に存在する複数の対象点についての３次元座標を、対象の少なくとも一部が含まれる複数の画像に基づいて、推定する画像処理装置であって、複数の画像と、当該画像の撮影時のカメラの位置情報及び撮影方向情報とを記憶する画像記憶手段と、対象の１以上の対象点についての３次元座標を記憶する座標記憶手段と、座標記憶手段に３次元座標が記憶されている対象点に対応する特徴点の或る画像中の実位置と、対象点の３次元座標と、或る画像の撮影時の前記カメラの位置情報及び撮影方向情報とに基づいて、或る画像の撮影時のカメラの焦点距離を推定する焦点距離推定手段と、推定された焦点距離に基づいて、或る画像と他の画像とに含まれている、３次元座標が未知である対象点についての３次元座標を推定する座標推定手段とを有する。係る画像処理装置によると、或る画像における焦点距離を容易且つ適切に推定することができ、当該或る画像と、他の画像とに基づいて、これら画像に含まれる対象点の３次元座標を推定することができる。
【００１１】
上記画像処理装置において、焦点距離推定手段は、位置情報、撮影方向情報及び、３次元座標に基づいて推定される対象点に対応する特徴点の或る画像上の推定位置と、特徴点の或る画像上の実位置とに基づいて、焦点距離を推定するようにしてもよい。係る画像形成装置によると、推定位置と、或る画像上の実位置とに基づいて、適切な焦点距離を推定することができる。
【００１２】
また、上記画像処理装置において、焦点距離推定手段は、複数の対象点に対応するそれぞれの特徴点の推定位置と、複数の対象点に対応するそれぞれの特徴点の実位置とのずれに関する関数の和がもっとも小さくなるように、焦点距離を推定するようにしてもよい。係る画像処理装置によると、複数の対象点に対応する特徴点の推定位置と、対象点の実位置とのずれに関する関数の和が小さくなるように、焦点距離を推定するので、より適切な焦点距離を推定することができる。
【００１３】
また、上記画像処理装置において、焦点距離推定手段は、共役勾配法を用いて、ずれに関する関数の和がもっとも小さくなるような焦点距離を推定するようにしてもよい。係る画像処理装置によると、容易且つ適切に焦点距離を推定することができる。
【００１４】
また、上記画像処理装置において、画像を撮影するカメラは、カメラの位置及び撮影方向を変更可能なデバイスに装着されており、デバイスの状態を特定することのできる状態特定可能データに基づいて、カメラの位置情報及び撮影方向情報を特定するカメラ情報特定手段と、カメラにより撮影された画像と、撮影時におけるカメラの位置情報及び撮影方向情報とを画像記憶手段に格納する格納制御手段とを更に有するようにしてもよい。係る画像処理装置によると、カメラの位置情報及び撮影方向情報を容易且つ適切に取得することができる。このため、推定する対象点の３次元座標の精度を向上することができる。
【００１５】
また、上記画像処理装置において、処理対象の複数の画像を用いて複数の対象点の３次元座標を推定した後に、各画像に含まれる複数の対象点の３次元座標を用いて、各画像の撮影時のカメラの位置情報及び撮影方向情報を最適化するカメラ情報最適化手段を更に有するようにしてもよい。係る画像処理装置によると、各画像の撮影時のカメラの位置情報及び撮影方向情報を最適化することができ、最適化された始点位置座標及び撮影方向情報に基づいて、複数の対象点の３次元座標をより適切に推定することができる。
【００１６】
また、上記画像処理装置において、処理対象の複数の画像を用いて対象点の３次元座標を推定した後に、同一の対象点が含まれている複数の画像におけるカメラの位置情報及び撮影方向情報に基づいて、対象点の３次元座標を最適化する３次元座標最適化手段を更に有するようにしてもよい。係る画像処理装置によると、対象点の３次元座標を適切に最適化することができる。
【００１７】
また、上記画像処理装置において、複数の対象点の３次元座標に基づいて、対象についての３次元モデルを形成するモデル形成手段を更に有するようにしてもよい。係る画像処理装置によると、複数の対象点を含む対象の３次元モデルを適切に形成することができる。
【００１８】
上記目的達成のため、本発明の第２の観点に係る画像処理方法は、所定の対象に存在する複数の対象点についての３次元座標を、対象の少なくとも一部が含まれる複数の画像に基づいて、推定する画像処理装置による画像処理方法であって、画像処理装置の画像記憶手段が、複数の画像と、当該画像の撮影時のカメラの位置情報及び撮影方向情報とを記憶する画像記憶ステップと、画像処理装置の座標記憶手段が、対象の１以上の対象点についての３次元座標を記憶する座標記憶ステップと、画像処理装置の焦点距離推定手段が、座標記憶手段に３次元座標が記憶されている対象点に対応する特徴点の或る画像中の実位置と、対象点の３次元座標と、或る画像の撮影時のカメラの位置情報及び撮影方向情報とに基づいて、或る画像の撮影時のカメラの焦点距離を推定する焦点距離推定ステップと、画像処理装置の座標推定手段が、推定された焦点距離に基づいて、或る画像と他の画像とに含まれている、３次元座標が未知である対象点についての３次元座標を推定する座標推定ステップとを有する。係る画像処理方法によると、或る画像における焦点距離を容易且つ適切に推定することができ、当該或る画像と、他の画像とに基づいて、これら画像に含まれる対象点の３次元座標を推定することができる。
【００１９】
上記目的達成のため、本発明の第３の観点に係る画像処理プログラムは、コンピューターに、所定の対象に存在する複数の対象点についての３次元座標を、前記対象の少なくとも一部が含まれる複数の画像に基づいて、推定させるための画像処理プログラムであって、コンピューターを複数の画像と、当該画像の撮影時のカメラの位置情報及び撮影方向情報とを記憶する画像記憶手段と、対象の１以上の対象点についての３次元座標を記憶する座標記憶手段と、座標記憶手段に３次元座標が記憶されている対象点に対応する特徴点の或る画像中の実位置と、対象点の３次元座標と、或る画像の撮影時のカメラの位置情報及び撮影方向情報とに基づいて、或る画像の撮影時のカメラの焦点距離を推定する焦点距離推定手段と、推定された焦点距離に基づいて、或る画像と他の画像とに含まれている、３次元座標が未知である対象点についての３次元座標を推定する座標推定手段として機能させる。係る画像処理プログラムを実行するコンピューターによると、或る画像における焦点距離を容易且つ適切に推定することができ、当該或る画像と、他の画像とに基づいて、これら画像に含まれる対象点の３次元座標を推定することができる。
【００２０】
上記目的達成のため、本発明の第４の観点に係る記録媒体は、コンピューターに、所定の対象に存在する複数の対象点についての３次元座標を、対象の少なくとも一部が含まれる複数の画像に基づいて、推定させるための画像処理プログラムを格納する記録媒体であって、コンピューターを複数の画像と、当該画像の撮影時のカメラの位置情報及び撮影方向情報とを記憶する画像記憶手段と、対象の１以上の対象点についての３次元座標を記憶する座標記憶手段と、座標記憶手段に３次元座標が記憶されている対象点に対応する特徴点の或る画像中の実位置と、対象点の３次元座標と、或る画像の撮影時のカメラの位置情報及び撮影方向情報とに基づいて、或る画像の撮影時のカメラの焦点距離を推定する焦点距離推定手段と、推定された焦点距離に基づいて、或る画像と他の画像とに含まれている、３次元座標が未知である対象点についての３次元座標を推定する座標推定手段として機能させる画像処理プログラムを格納する。係る画像処理プログラムを実行するコンピューターによると、或る画像における焦点距離を容易且つ適切に推定することができ、当該或る画像と、他の画像とに基づいて、これら画像に含まれる対象点の３次元座標を推定することができる。
【００２１】
上記目的達成のため、本発明の第５の観点に係る画像処理システムは、所定の対象に存在する複数の対象点についての３次元座標を、対象の少なくとも一部が含まれる複数の画像に基づいて、推定する画像処理システムであって、画像を撮影するカメラと、カメラを装着し、カメラの位置及び撮影方向を変更可能なデバイスと、デバイスの状態を特定することのできる状態特定可能データに基づいて、カメラの位置情報及び前記撮影方向情報を特定するカメラ情報特定手段と、カメラにより撮影された画像と、撮影時におけるカメラの位置情報及び撮影方向情報とを画像記憶手段に格納する格納制御手段と、対象の１以上の対象点についての３次元座標を記憶する座標記憶手段と、座標記憶手段に３次元座標が記憶されている対象点に対応する特徴点の或る画像中の実位置と、対象点の３次元座標と、或る画像の撮影時の前記カメラの位置情報及び撮影方向情報とに基づいて、或る画像の撮影時のカメラの焦点距離を推定する焦点距離推定手段と、推定された焦点距離に基づいて、或る画像と他の画像とに含まれている、３次元座標が未知である対象点についての３次元座標を推定する座標推定手段とを有する。係る画像処理システムによると、或る画像における焦点距離を容易且つ適切に推定することができ、当該或る画像と、他の画像とに基づいて、これら画像に含まれる対象点の３次元座標を推定することができる。
【図面の簡単な説明】
【００２２】
【図１】本発明の一実施形態に係る画像処理システムの機能構成図である。
【図２】本発明の一実施形態に係る画像処理システムにおけるデバイス、カメラの一例を示す図である。
【図３】本発明の一実施形態に係る画像処理システムの一連の処理を示すフローチャートである。
【図４】本発明の一実施形態に係るフレームシーケンスの一例を示す図である。
【図５】本発明の一実施形態に係る校正マークの一例を示す図である。
【図６】本発明の一実施形態に係るイメージシーケンスの最初の２つのフレームの一例を説明する図である。
【図７】本発明の一実施形態に係る特徴点マッチングを説明する図である。
【図８】本発明の一実施形態に係る異なる画像のマッチする特徴点の組から３次元座標を算出する方法を説明する図である。
【図９】本発明の一実施形態に係る一対の画像から得られた対象点の３次元座標の一例を示す図である。
【図１０】本発明の一実施形態に係る焦点距離を算出する方法を説明する図である。
【図１１】本発明の一実施形態に係る複数の画像から得られた対象点の３次元座標を示す図である。
【図１２】本発明の一実施形態に係る対象点に対応する法線を示す図である。
【図１３】本発明の一実施形態に係るマーチングキューブ法を説明する図である。
【図１４】本発明の一実施形態に係る周囲環境における複数のキューブの一例を示す図である。
【図１５】本発明の一実施形態に係る周囲環境におけるメッシュの一例を示す図である。
【図１６】本発明の一実施形態に係るテクスチャーが貼られた３次元モデルの一例を示す図である。
【発明を実施するための形態】
【００２３】
本発明の実施形態について、図面を参照して説明する。なお、以下に説明する実施形態は特許請求の範囲に係る発明を限定するものではなく、また実施形態の中で説明されている諸要素及びその組み合わせの全てが発明の解決手段に必須であるとは限らない。
【００２４】
まず、本発明の一実施形態に係る画像処理システムについて説明する。
【００２５】
図１は、本発明の一実施形態に係る画像処理システムの機能構成図である。
【００２６】
画像処理システム１は、デバイス２と、周囲の画像を取り込むことのできるカメラ３と、デバイス２の動作を制御するためのデバイスコントローラー４と、画像処理装置５とを有する。
【００２７】
デバイス２は、所定の環境に配置され、姿勢を変更させる動作ができるようになっている。本実施形態では、デバイス２には、カメラ３が装着され、デバイス２を動作させることにより、カメラ３の位置（視点位置）及びカメラ３のオリエンテーション(撮影方向：ヨー、ピッチ、ロール）を変更することができる。デバイス２の装着の部位に対するカメラ３の位置は、決まっているので、デバイス２の姿勢からカメラ３の位置及びオリエンテーションを把握することができる。
【００２８】
デバイス２の一例としては、作業スペースの内部に置かれたロボットがある。例えば、ロボットの場合には、カメラ３は、ロボットのアームに対して配置される。ロボットのアームにおけるカメラ３の位置及び向きは、ロボットへの固定時に決まることとなるので、ロボットのアームの状態からカメラ３の位置及びオリエンテーションを把握することができる。例えば、ロボットのアームが、複数の関節を介して接続された複数のメンバーにより構成されていれば、アームの各関節の角度からカメラ３の位置及びオリエンテーションを把握することができる。デバイス２は、デバイスコントローラー４によって、その姿勢を変更することができる。
【００２９】
カメラ３は、デバイス２を介して、画像の取り込み指示を受け、周囲環境(対象)の画像（画像データ）を取り込んで画像処理装置５に渡す。カメラ３は、焦点距離を変えて画像を取り込むことができる。例えば、カメラ３は、撮影範囲の或る物体に対して焦点が合うように、焦点距離を変更させる。
【００３０】
デバイスコントローラー４は、画像処理装置５のメイン制御部１１からコマンドを受け取り、それに従って、デバイス２の姿勢を制御する。
【００３１】
画像処理装置５は、例えば、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）等を含むコンピューターにより構成される。画像処理装置５は、カメラ情報特定手段の一例としてのメイン制御部１１と、格納制御手段の一例としての画像取得部（フレームグラバー）１２と、焦点距離推定手段、座標推定手段、カメラ情報最適化手段、３次元座標最適化手段及びモデル形成手段の一例としての画像処理部１３と、シミュレーション部１４と、画像記憶手段及び座標記憶手段の一例としての記憶部１５とを有する。なお、本実施形態では、メイン制御部１１と、画像処理部１３と、シミュレーション部１４とは、同一の画像処理装置５内に備えるようにしているが、例えば、少なくともいずれか１つが別の装置内に備えられてもよく、各部がそれぞれ異なる装置内に備えられてもよい。
【００３２】
記憶部１５は、種々のデータを記憶する。本実施形態では、記憶部１５は、カメラ情報１６と、イメージシーケンス１７と、環境３次元（３Ｄ）モデル１８と、デバイス３次元（３Ｄ）モデル１９と、シミュレーション結果２０とを記憶する。また、記憶部１５は、メイン制御部１１、画像取得部１２、画像処理部１３、又はシミュレーション部１４による処理に利用するデータについても記憶する。
【００３３】
メイン制御部１１は、主に、デバイスコントロールソフトウェアをＣＰＵが実行することにより構成され、図示しない入力手段によるユーザーの指示又は設定内容に従って、デバイス２の姿勢を制御するためのコマンド（例えば、デバイス２の各関節の角度を含むコマンド）をデバイスコントローラー４に出力する。また、メイン制御部１１は、図示しない入力手段によるユーザーの指示又は設定内容に従って、画像取得部１２に画像を取り込ませるコマンドを逐次出力する。ここで、後続の処理の迅速化を考慮すると、取り込まれる一連の画像（イメージ、フレーム）の内の連続する画像間に十分なオーバーラップがあり、各画像が異なるカメラ位置（同じカメラ位置でオリエンテーションのみを変更したものではない）から得られたものとなるようにコマンドを出力することが好ましい。例えば、ユーザーがデバイス２を移動させて撮影させる指示を逐次行う場合には、上記したような画像が得られるように、メイン制御部１１に対して指示することが必要である。
【００３４】
また、メイン制御部１１は、画像を取り込ませるコマンドを出力する度に、デバイス２の姿勢を制御したコマンド（状態特定可能データ）に基づいて、デバイス２に装着されたカメラ３のその時点の位置（視点位置）及びオリエンテーションを算出し、当該位置を示す位置情報及びオリエンテーションを示すオリエンテーション情報を含むカメラ情報を画像取得部１２に渡す。
【００３５】
画像取得部１２は、主に、フレームグラバーアプリケーションをＣＰＵが実行することにより構成され、メイン制御部１１から画像を取り込ませるコマンドを受け付ける度に、カメラ３からその時点の画像を取り込み、当該画像と、メイン制御部１１から渡されたカメラ情報（すなわち、当該画像を撮影した際のカメラの位置情報及びオリエンテーション情報）とを対応付けて記憶部１５に格納する。このような処理により、画像取得部１２は、デバイス２の周囲環境における種々の視点からの一連の画像（イメージシーケンス１７）を記憶部１５に格納することとなる。
【００３６】
画像処理部１３は、主に、処理プログラムをＣＰＵが実行することにより構成され、カメラ情報１６と、イメージシーケンス１７を用いて、周囲環境の３次元モデル（環境３次元モデル）を作成し、作成した環境３次元モデル１８を記憶部１５に記憶する。３次元モデルを作成する処理の詳細は、後述する。
【００３７】
シミュレーション部１４は、主に、シミュレーションプログラムをＣＰＵが実行することにより構成され、環境３次元モデル１８と、デバイスについての３次元モデル(デバイス３次元モデル)１９とに基づいて、各種シミュレーション処理を実行し、シミュレーション結果２０を記憶部１５に記憶する。ここで、デバイス３次元モデル１９の対象としては、環境３次元モデル１８を作成するためにカメラ３を装着したデバイス２に限られず、任意のデバイスであってよく、例えば、環境において使用することを検討したいデバイスであってもよい。また、シミュレーション処理としては、例えば、ポイントティーチング処理、経路探索処理、デバイスの環境に対する衝突検出処理等であってもよい。
【００３８】
図２は、本発明の一実施形態に係る画像処理システムにおけるデバイス、カメラの一例を示す図である。
【００３９】
デバイス２は、例えば、図２に示すような種々の物体が存在する周囲環境ＳＥ下で使用される。この周囲環境ＳＥにおいては、デバイス２が、周囲環境ＳＥに存在する物体と衝突しないようにすることが要求される。そこで、周囲環境ＳＥの３次元モデルを作成し、衝突処理等を行うことが有益である。
【００４０】
本実施形態では、周囲環境ＳＥの画像を得るために、デバイス２のアーム部に対してカメラ３が装着される。カメラ３は、図２に示すように、デバイス２のアーム部に装着されるので、アーム部との相対位置を予め測定等して把握しておくことにより、デバイス２のアーム部の状態（アーム部における各関節の角度等）に基づいて、カメラ３の位置及びオリエンテーションを比較的正確に把握することができることとなる。そこで、本実施形態では、メイン制御部１１が、デバイス２の状態に基づいて、カメラ３の位置及びオリエンテーションを算出するようにしている。
【００４１】
まず、画像処理システムの一連の処理についての概略を説明する。
【００４２】
図３は、本発明の一実施形態に係る画像処理システムの一連の処理を示すフローチャートである。
【００４３】
画像処理システム１は、図４に示すような周囲の環境の一連の画像（イメージシーケンス）を取得して記憶する処理を実行する(ステップＳ１)。ここで、イメージシーケンスとしては、連続するイメージ間に十分なオーバーラップがあり、且つ後続の処理で必要となる深さ情報を確実に生成できるように各イメージを取得した際のカメラ視点が実質的に離れていることが好ましい。なお、ユーザーの指示によって一連の画像を取得していく場合には、この点をユーザーが注意する必要がある。なお、このようにすることは、ユーザーにとっては、実行して経験を積むことで、難しいタスクではない。
【００４４】
また、このステップにおいては、画像処理システム１は、各画像の撮影時のカメラ３の位置及びオリエンテーションを、例えば、デバイス２のアームの各関節角に基づいて計算して、画像とともに記憶する。
【００４５】
次いで、フレームシーケンスの最初の２つのフレームを用いて、各種初期設定用の値を算出する初期処理を実行する(ステップＳ２)。ここで、最初の２つのフレームは、所定の校正用パターン（図５参照）が含まれたフレームとなっており、校正用パターンを用いて、各フレームの撮影時のカメラ３の焦点距離や、これら２つの画像に含まれている一致（マッチ）する特徴点の組に対応する対象点の３次元座標を算出する。
【００４６】
次いで、以降の各フレームから複数の特徴点を検出し(ステップＳ３)、各フレームの特徴点と、前の順番のフレームの特徴点とにマッチする特徴点があるか否かを検出し(ステップＳ４)、これらマッチする特徴点の組に対応する対象点の３次元座標を推定し、最適化し、周囲環境の複数の対象点についての３次元座標の集合を復元する（ステップＳ５）。
【００４７】
次いで、周囲環境の各点の３次元座標に基づいて、周囲環境（対象）を示すメッシュ構造を調整する処理を実行し（ステップＳ６）、当該対象の表面を示すパッチに対して外観を示すテクスチャーを貼り付ける処理(ステップＳ７)を実行する。これにより、周囲環境の３次元モデルが復元される。
【００４８】
次いで、画像処理装置５は、ユーザーから３次元モデルの復元が成功したか否か(３次元モデルが満足するものであったか否か)を受け付け（ステップＳ８）、３次元モデルが成功でない場合には、３次元モデルの再構成に利用するための追加のイメージを取得する処理を実行し(ステップＳ９)、ステップＳ３からの処理を再び実行する。
【００４９】
一方、ユーザーから３次元モデルの復元が成功した旨を受け付けた場合（ステップＳ８：ＹＥＳ）には、シミュレーション部１４は、３次元モデルのデータをロードし(ステップＳ１０)、当該３次元モデルを用いて所定のシミュレーション処理を実行する(ステップＳ１１)。
【００５０】
次に、画像処理システムの一連の処理について詳細に説明する。なお、図３を適宜参照して説明する。
【００５１】
まず、イメージシーケンス取得ステップ（ステップＳ１）においては、以下のような処理が行われる。
【００５２】
すなわち、メイン制御部１１がデバイスコントローラー４にコマンドを発行し、デバイスコントローラーがデバイス２の姿勢を変える。そして、メイン制御部１１が画像取得部１２にカメラ３からの画像を取得させ、画像取得部１２が取得したイメージを記憶部１５に格納させる。この際、メイン制御部１１は、デバイスコントローラー４に発行したコマンド中のデバイス２の関節角度に基づいて、デバイス２に装着されているカメラ３の位置及びオリエンテーションを算出して画像取得部１２に通知しておき、画像取得部１２に、当該カメラ位置及びオリエンテーションを画像と対応付けて記憶部１５に記憶させる。なお、画像取得部１２においては、カメラ３から得られた画像から放射状のディストーションを除去する処理を実行し、実行後の画像を記憶部１５に記憶させるようにしている。なお、画像から放射状のディストーションを除去するために使用するパラメーターは、実際のカメラに適合すると仮定できるピンホールカメラモデルに基づいて、予め推定しておくことができる。
【００５３】
このような処理を繰り返し行うことにより、周囲環境についての複数の視点からの一連の画像群（イメージシーケンス）を記憶部１５に記憶させる。ここで、イメージシーケンスとしては、連続するイメージ（フレーム）間に十分なオーバーラップがあり、且つ後続の処理で必要となる深さ情報を確実に生成できるように各イメージを取得した際のカメラ視点が実質的に離れていることが好ましい。なお、ユーザーの指示によって一連の画像を取得していく場合には、この点をユーザーが注意する必要がある。なお、このようにすることは、ユーザーにとっては、実行して経験を積むことで、難しいタスクではない。
【００５４】
ここで、フレームｊ（ｊは任意の整数）に対応付けられて記憶されたカメラ３の位置ｔｊ＝（ｘｊ，ｙｊ，ｚｊ）及びオリエンテーションＲｊ（３×３の回転行列：ワールド座標系からカメラ座標系に変換する行列）は、フレームｊに対する初期値(初期の推測値)となる。フレームｊの取得時のカメラ３の位置およびオリエンテーションは、後述する処理で洗練されることとなる。
【００５５】
次に、処理に必要な他のカメラに関するパラメーターである各フレームｊの焦点距離ｆｊを推定する。ここで、本実施形態では、座標系については、次のような取り決めに従って記述することとする。イメージ空間（フレームにおける座標系）においては、各画素の座標は、フレームの左上の角を原点とした座標（ｘ，ｙ）により示される。このｘ値は、フレーム上の左から右に行くにしたがって増加し、ｙ値は、上から下に行くにしたがって増加するようになっている。周囲環境の基準となる３次元座標系であるワールド座標系は、ｘ軸の正方向が右を示し、ｙ軸の正方向が上を示し、ｚ軸の正方向が外側を示している右手系に従っている。このため、カメラ３の向きは、そのローカル座標系（カメラ座標系）のｚ軸の負の方向を常に向いている。
【００５６】
図４は、本発明の一実施形態に係るフレームシーケンスの一例を示す図である。
【００５７】
ステップＳ１において、図４に示すフレームシーケンスが得られた場合を例に後続するステップの説明をする。
【００５８】
まず、シーケンスの最初の２つのフレームを用いて、初期化処理(ステップＳ２)が実行される。本実施形態では、最初の２つのフレームは、図５に示すような２つのクロスＣＭ１、ＣＭ２（校正用パターン）がカメラ３の正面を向き、且つ既知の距離Ｚ（図６参照）だけ離れている点をカメラ３の視点として撮影されたフレームとなっている。２つのクロスＣＭ１及びＣＭ２の間の距離は、図５に示すように既知の距離Ｄとなっている。
【００５９】
図６は、本発明の一実施形態に係るイメージシーケンスの最初の２つのフレームの一例を説明する図である。
【００６０】
図６に示すように、最初のフレーム(フレーム１)は、クロスＣＭ１及びＣＭ２に対して正面を向き、且つこれらクロスから距離Ｚだけ離れた視点Ｐ１からカメラ３により、撮影したフレームであり、２番目のフレーム(フレーム２)は、クロスＣＭ１及びＣＭ２に対して正面を向き、且つこれらクロスから距離Ｚだけ視点Ｐ２からカメラ３により、撮影したフレームである。
【００６１】
ここで、クロスＣＭ１及びＣＭ２のフレーム上の位置をｘｉｊ（ｉは、クロスの別（ＣＭ１か、ＣＭ２であるか）を示し、ｊは、フレームの番号を示す）とすると、ｘ１１、ｘ２１、ｘ１２、ｘ２２の具体的な座標は、各フレーム１、フレーム２から把握することができる。フレーム１及びフレーム２のそれぞれの撮影時におけるカメラ３の焦点距離ｆ１、ｆ２は、以下の数式（１）、（２）のように表すことができる。画像処理部１３は、この数式（１）、（２）を用いてフレーム１及びフレーム２におけるカメラ３の焦点距離を算出する。
【００６２】
【数１】
2011085971.tif 000003
次いで、画像処理部１３は、各フレームについて、複数の特徴点を検出し（ステップＳ３）、２つのフレーム間での一致する特徴点の組を検出する処理（ステップＳ４）を行う。特徴点の組の検出方法としては、ＳＳＤ（sum of squared difference）やＺＭＣＣ（zero-mean cross-correlation）を利用したハリス・コーナーマッチングや、ＳＩＦＴ（scale invariant feature transform）マッチングなどが知られている。この方法により２つのフレームにおける複数の対応(マッチ)する特徴点の組が検出される。
【００６３】
図７は、本発明の一実施形態に係る特徴点マッチングを説明する図である。
【００６４】
上記した処理により、図７に示すように、マッチする特徴点の組が検出される。例えば、フレーム１の特徴点ＦＰＡ、ＦＰＢ、ＦＰＣは、それぞれフレーム２の特徴点ＦＰａ、ＦＰｂ、ＦＰｃとマッチするとして検出される。従って、特徴点ＦＰＡ及びＦＰａ、特徴点ＦＰＢ及びＦＰｂ、特徴点ＦＰＣ及びＦＰｃ、がそれぞれマッチする特徴点の組として検出される。
【００６５】
このようにして検出されたマッチする特徴点の組のそれぞれに対応する周囲環境の点（対象点）の３次元座標は、各フレームの撮影時のカメラ３の位置及びオリエンテーションと、既に算出されている焦点距離とにより、三角測量によって算出することができる。
【００６６】
ここで、或るマッチする特徴点の組についての３次元座標を算出する方法を説明する。
【００６７】
図８は、本発明の一実施形態に係る異なる画像のマッチする特徴点の組から３次元座標を算出する方法を説明する図である。
【００６８】
マッチする特徴点の組である第１のフレーム中の特徴点と第２フレーム中の特徴点とに対応する対象点は、以下に示す数式（３）及び数式（４）に示される線上に存在する。ここで、ｌＡ、ｌＢは、直線の方程式を示し、ｓ及びｔは、直線のパラメーターを示し、ＰＡ及びＰＢは、各フレームにおけるカメラ３の視点の座標を示し、ｖＡ及びｖＢは、ワールド座標系のＺ軸のマイナス方向への視点から各フレームにおける各特徴点へ向うベクトルを示している。
【００６９】
【数２】
2011085971.tif 000004
【００７０】
ここで、特徴点に対応する対象点は、１点であるので、理想的には、ｌＡ、ｌＢは、１点で交差することとなるが、実際には、種々の誤差要因により、直線ｌＡ、ｌＢは、交差しない場合がある。そこで、本実施形態では、図８に示すような、直線ｌＡ、ｌＢ同士が最も近接する点ＤＡ、ＤＢを求め、その中間点を、特徴点の組に対応する対象点とする。ここで、以下に示すように、数式（５）から特定の値であるｓ’と、ｔ’を求めることにより、数式（６）、数式（７）から点ＤＡ、ＤＢの３次元座標が求まり、その中間点である対象点の３次元座標を求めることができる。
【００７１】
【数３】
2011085971.tif 000005
上記した特徴点の組から対象点の３次元座標を算出する処理を、２つのフレームから得られた複数の特徴点の組に対して実行することにより、複数の対象点の３次元座標を算出することができる。
【００７２】
図９は、本発明の一実施形態に係る一対の画像から得られた対象点の３次元座標の一例を示す図である。
【００７３】
一対の画像（フレーム１、フレーム２）から、図９に示すように、複数の対象点ＯＰの３次元座標を算出することができる。
【００７４】
次に、上記処理に用いた１つのフレーム（例えば、第２フレーム）と、フレームシーケンスにおける次のフレーム（例えば、第３フレーム）とを用いてステップＳ３〜Ｓ５の処理を実行する。ここで、第３フレームからの特徴点の検出処理、及び第２フレームと第３フレームとの間でのマッチする特徴点の組の検出処理は、上記同様の処理により実行される。
【００７５】
次に、第３のフレームについての焦点距離を算出する処理を実行する。
【００７６】
図１０は、本発明の一実施形態に係る焦点距離を算出する方法を説明する図である。
【００７７】
本実施形態では、マッチする特徴点の組の検出処理(ステップＳ４)において、第２フレームと、第３フレームとの間で検出されたマッチする特徴点の組の中には、既に３次元座標が算出されている対象点に対応する特徴点が存在する。ここで、このような対象点をＸｉとする。なお、既に３次元座標が算出された対象点に対応する特徴点の組が存在するためには、既に特徴点の組を検出したフレーム(例えば、第１フレーム)と、第３フレームとでその対象点を含む一部の周囲環境がオーバーラップしている必要がある。なお、これについては、フレームシーケンスの取得する際に考慮しておくことで実現できる。
【００７８】
ここで、対象点Ｘｉの３次元座標（ワールド座標系）については、前の複数フレーム（現在のフレームをｊとすると、例えば、フレームｊ−１と、フレームｊ−２）によって、Ｘｉ（斜字体）＝（Ｘｉ，Ｙｉ，Ｚｉ）Ｔと算出されているものとして、以下の説明を行う。ここで、Ｔは、転置を示している。
【００７９】
フレームｊ（例えば、第３フレーム）におけるカメラ位置ｔｊとオリエンテーションＲｊとは、予め画像データと対応付けられて記憶部１５に記憶されているので把握することができる。また、フレームｊにおける対象点Ｘｉに対応する特徴点の位置（実位置）の座標ｘｉ，ｊ（斜字体）＝（ｘｉ，ｊ，ｙｉ，ｊ）は、フレームｊから把握することができる。
【００８０】
一方、対応する対象点の３次元座標Ｘｉ（斜字体）を用いると、この対象点についてのフレームｊの撮影時のカメラ座標系における３次元座標Ｙｉｊ（斜字体）は、以下の数式（８）で表される。ここで、Ｒｊは、オリエンテーションであり、本実施形態では、３×３回転行列として表現されている。そして、当該対象点に対応する特徴点がフレームｊにおいて位置すると推定される位置（推定位置）の座標Ｐｊ（Ｘｉ）は、数式（９）で示される。ここで、Ｐｊは、ワールド座標系からフレームｊのイメージ座標系への写像を示し、ｆｊは、フレームｊにおける焦点距離を示し、Ｎは、数式（１０）に示すカメラ座標系からフレームｊのフレーム座標系に平行な２次元平面への変換を表している。
【００８１】
【数４】
2011085971.tif 000006
ここで、焦点距離を推定するために、数式（１１）に示すような、既に算出された対象点の３次元座標から推定されたフレームｊのフレーム座標系への写像位置（推定位置）Ｐｊ（Ｘｉ）と、フレームｊにおいて実際に存在する対応する特徴点の位置（実位置）の座標ｘｉ，ｊ（斜字体）とのずれに関するエラー関数ｅｉｊを導入し、更に、エラー関数ｅｉｊを３次元座標が把握されている複数の対象点を対象にしたものの合計を表す数式（１２）に示す合計エラー値ｅｊを導入する。なお、エラー関数については、これに限られず、推定位置と実位置とのずれに関する関数であればよい。
【００８２】
【数５】
2011085971.tif 000007
ここで、フレームｊの焦点距離ｆｊを変化させると、数式（９）により算出される対象点の推定される写像の位置が変わり、結果として、数式（１１）のエラー関数ｅｉｊ及び数式（１２）の合計エラー値ｅｊに影響を及ぼすこととなる。このような合計エラー値ｅｊが最小となる場合における焦点距離を、フレームｊにおける焦点距離と推定するようにしてもよい。なお、合計エラー値ｅｊの最小化問題は、閉形（クローズドフォーム）において解決することができる。
【００８３】
本実施形態においては、カメラ位置及びオリエンテーションについてデバイス２から取得するようにしているが、カメラ位置及びオリエンテーションについても精度よくすることが好ましい。そこで、フレームｊにおける既に３次元座標を検出（推定）している全ての対象点を対象にして、カメラの位置、オリエンテーション、及び焦点距離を変化させて、数式（１２）の合計エラー値ｅｊを最小化させるために、共役勾配法（conjugate gradient method）を用いることとする。
【００８４】
数式（８）に示す座標Ｙｉｊは、回転と変換との変数がそれぞれ独立するようにするために、数式（１３）に示すように、グラディエントの表現に書き換えることができる。ここで、回転マトリクスＴｊは、数式（１４）に示すようになっており、ｓｊは、数式（１５）に示すようになっている。
【００８５】
【数６】
2011085971.tif 000008
また、回転マトリクスＴｊは、最適化処理において追加となる制限が必要とならないように、数式（１６）、（１７）、（１８）に示すような、ロドリゲス・フォームによる３次元ベクトルとして表現する。
【００８６】
【数７】
2011085971.tif 000009
この表現においては、回転は、軸ａ＝（ａｘ，ａｙ，ａｚ）に関する角θの回転によって定義される。ここで、ａｘ２＋ａｙ２＋ａｚ２＝１である。
【００８７】
回転マトリクスは、ロドリゲス・フォームの表現によると、数式（１９）〜（２５）に示すようになる。
【００８８】
【数８】
2011085971.tif 000010
この表現において、以下のグラディエント、δＰｊ（Ｘｉ）／δｖｊ、δＰｊ（Ｘｉ）／δｓｊ、δＰｊ（Ｘｉ）／δｆｊは、数式（２６）〜（３０）に示すようになる。ここで、数式（２６）における「：」は、列行列を示す。
【００８９】
【数９】
2011085971.tif 000011
このようにして、共役勾配法により求めた、数式（１２）の合計エラー値ｅｊを最小化させることのできる焦点距離、カメラ位置、及びオリエンテーションを用いて、上記した同様な処理（三角測量等）により、第３フレームと、第２フレームとの間でマッチした特徴点の複数の組のそれぞれに対応する対象点の３次元座標を算出（推定）する。これにより、第２フレームと、第３フレームとの間でマッチした特徴点の組のすべてに対応する対象点（３次元座標が未知である対象点を含む）の３次元座標が推定される。なお、各対象点についての３次元座標のデータについては、記憶部１５に記憶される。
【００９０】
また、第３フレーム以降の新しい処理対象のフレーム（新対象フレーム）についても、上記した第３フレームと同様な処理、すなわち、新対象フレームと、１つ前のフレームとの間での、マッチする特徴点の組の検出処理と、新対象フレームにおける焦点距離の推定処理と、推定した焦点距離を用いて、新対象フレームと１つ前のフレームとの間のマッチする特徴点の組に対応する対象点の３次元座標の検出処理を実行する。
【００９１】
このようにして、フレームシーケンスの処理対象の全てのフレームを用いて、複数の対象点についての３次元座標を推定することができる。
【００９２】
なお、上記実施形態においては、連続するフレーム間で特徴点のマッチングを行って、対象点の３次元座標を推定するようにしていたが、これに限られず、カメラ視点が類似する、すなわち、周囲環境の一部がオーバーラップする任意の２つのフレーム間で特徴点のマッチングを行なって、得られたマッチする特徴点の組に対応する対象点の３次元座標を推定するようにしてもよい。
【００９３】
本実施形態においては、対象点の３次元座標をより高精度に推定するために、画像処理部１３は、更に以下に示す大域的最適化処理を行う。
【００９４】
ここで、最適化するパラメーターは、各フレームｊのカメラ位置ｔｊ、オリエンテーションＲｊ、焦点距離ｆｊ、及び対象点の３次元座標Ｘｉ（斜字体）である。
【００９５】
ここで、これらを最適化するための目的関数Ｅは、数式（３１）に示すようになる。
【００９６】
【数１０】
2011085971.tif 000012
この目的関数を最適化するために、カメラパラメーター（カメラ位置、オリエンテーション、焦点距離）の最適化と、３次元座標の最適化との２段階のアプローチが実行される。このアプローチにより、３次元座標に含まれるエラーを縮小することができる。
【００９７】
このアプローチにおいては、カメラパラメーターを最適化する際には、対象点の３次元座標を固定値とし、また、対象点の３次元座標を最適化する際には、カメラパラメーターを固定値として取扱う。
【００９８】
このアプローチにおいては、カメラパラメーターを最適化する際には、各フレームにおけるカメラパラメーターは、他のフレームに対して無関係のもの（独立したもの）となり、３次元座標を最適化する場合には、各対象点は、他の対象点に対して無関係のもの(独立したもの)となる。
【００９９】
カメラパラメーターの最適化においては、対象とする１つのフレーム中で検出された複数の対象点の全てを使用して、上記したような共役勾配法を用いて最適化する。これにより、カメラパラメーターが最適化される。
【０１００】
一方、３次元座標の最適化ステップにおいては、以下のような処理を実行する。
【０１０１】
或る対象点ｉについて、最小化するエラー関数ｅｉは、数式（３２）に示すように、当該対象点が存在する全てのフレームｊを対象にした値の合計となる。ここで、Ｒｔｐは、行列Ｒの転置行列のｐ列目を示す。数式（３２）は、或る対象点ｉに対応する特徴点のフレーム上の位置（実位置）と、当該対象点ｉの３次元座標から推定される写像位置との距離（ずれ）に関する関数（本実施形態では、距離の２乗の関数）を、対象点ｉに対応する特徴点が存在する全てのフレームについて合計したものを意味している。
【０１０２】
【数１１】
2011085971.tif 000013
数式（３２）は、数式（３３−１）、（３３−２）に示すように、書き直すことができる。数式（３２）及び（３３−１）は、それらが１つの対象点を扱う（全ての対象点の最適化を同時に行うこととは正反対である）ので、等価である。この結果、それぞれの誤差項を変更する必要はない。数式（３３−１）は、閉形において最小化することができる。
【０１０３】
【数１２】
2011085971.tif 000014
ここで、マッチすると誤って検出された特徴点の組に対応すると考えられる点については、或るフレームにおける値が極端に大きくなる。そこで、このように極端に大きくなった場合には、このような対象点を周囲環境の対象点から除外する。このため、間違った判断がされた対象点の３次元座標を適切に取り除くことができ、これによる３次元モデル形成への悪影響を適切に防止することができる。このようにして、図１１に示すように、周囲環境における複数の対象点の３次元座標を適切に推定することができる。このように推定された各対象点の３次元座標のデータの集合は、記憶部１５に記憶される。これにより、ステップＳ５までの処理が終了する。
【０１０４】
次に、画像処理部１３により、周囲環境における対象を示すメッシュ構造をあてはめる処理（ステップＳ６）が実行される。
【０１０５】
まず、３次元座標が推定されている周囲環境の各対象点に対する接平面が推定される。これらの接平面は、周囲環境におけるオブジェクトの表面へのローカル線形近似として取扱われる。
【０１０６】
各対象点ｉについて、当該対象点の３次元座標Ｘｉから所定の距離ρ以内にある近傍の対象点が選択され、これら対象点に対してＮｉＸ＋ｃｉ＝０の式で表される平面があてはめられる。ここで、Ｎｉは、垂平面を示し、ｃｉは、対応する一定平面である。
【０１０７】
もし、近傍の対象点がほとんどない場合には、当該対象点に対しては、平面があてはめられない。
【０１０８】
平面をあてはめる方法としては、２つの方法がある。まず、第１の方法としては、数式（３４）、（３５）、（３６）により表される目的関数を最小化する方法がある。
【０１０９】
【数１３】
2011085971.tif 000015
ここで、最小二乗法により与えられた初期推定値による、Ｇｅｍａｎ−Ｍｃ−Ｃｌｕｒｅ推定量を用いたＩＲＬＳ（Iteratively reweighted least squares）法が使用される。
【０１１０】
数式（３７）〜（４０）に示すように表現すると、目的関数を最小化する垂平面と、一定平面とは、数式（４１），（４２）に示す形式となる。ここで、ｗ（ｅ）は、Ｘｋの重み付けを計算するための式を示し、Ｍは、重み付けられた平均値を示し、Ｃは、分散行列を示し、ｓは、重み付けの合計値を示す。また、μは、３×３分散行列の最小の固有値を示す。したがって、Ｎｉは、行列Ｃの固有値に対応する固有ベクトルとなる。
【０１１１】
【数１４】
2011085971.tif 000016
外側に向く法線は、対象点ｉを見ることのできるイメージに返り、そのフレームにおける視点方向と法線とを比較することにより、内側を向く法線と区別することができる。ここで、外側を向く法線が記憶部１５に記憶される。平面Ｏｉの中心は、Ｘｉに最も近い平面上の点に定義される。ＯｉとＸｉとの間の距離が十分に近い場合には、対象点ｉに対する接平面のあてはめプロセスは完了する。
【０１１２】
一方、そうでない場合には、よい適合は見つからないので、第２の方法を試みる。
【０１１３】
この場合には、平面が対象点Ｘｉを含んでいるはずであり、平面の向きだけの変更が行なわれる。言い換えれば、エラー関数は、数式（４３）、（４４）に示すようになる。
【０１１４】
【数１５】
2011085971.tif 000017
Ｎｉの解決法を計算するために、上記同様なＧｅｍａｎ−Ｍｃ  Ｃｌｕｒｅ推定量を用いたＩＲＬＳ法が使用される。上記のＮｉのみを解決するためには、閉形による解決方法が与えられる。もし、平面のあてはめにおいて、近傍の対象点から十分なサポートが得られる場合には、外部を差す法線が記憶される。平面Ｏｉの中心は、対象点Ｘｉとなり、それからＣｉの値を算出することができる。そうでなければ、平面は、対象点ｉにはあてはめられない。このようにして、各対象点に対して推定された法線ＮＬは、例えば、図１２に示すように表される。
【０１１５】
すべての接平面が推定されると、空間のいずれの対象点Ｐ（＝（Ｐｘ、Ｐｙ，Ｐｚ））から表面までの距離を計算する符号付の距離関数を定義することができる。対象点Ｐについて、中心点Ｏｊが対象点Ｐから所定の距離以内にある全ての垂平面ｊが選択される。そのような平面が見つからない場合には、表面からの点Ｐの距離は、無限大であると仮定される。
【０１１６】
法線がＮｋであり、中心がＯｋである平面ｋに対して、平面ｋまでの点Ｐからの距離は、単に数式（４５）で示される。
【０１１７】
【数１６】
2011085971.tif 000018
表面からの点Ｐの推定距離は、各平面に対するそれらの距離の加重平均である。或る平面ｋに対して割り当てられる重みは、数式（４６）に示される。ここで、αは、０によって、除されることを防ぐための減衰定数である。数式（４６）は、点Ｐの平面ｋまでの距離は、点Ｐが中心Ｏｋに近いほどより重く重み付けされることを意味している。
【０１１８】
【数１７】
2011085971.tif 000019
これらの重み付けを利用して、表面からの点Ｐの最終的な推定距離は、数式（４７）に示す式で与えられる。
【０１１９】
【数１８】
2011085971.tif 000020
このようにして、任意の点における表面からの距離を計算することができる。
【０１２０】
次に、３次元座標が検出された対象点に対してメッシュをあてはめる具体的な処理が実行される。
【０１２１】
ここでは、マーチングキューブアルゴリズムが使用される。このマーチングキューブアルゴリズムの基本的な概念は、立方体の８つの角の値によってボクセル（キューブ：立方体）を定義することができるということである。
【０１２２】
立方体の１つ以上の角が所定値β未満の値であり、１つ以上の角の値が、その所定値βより大きな値である場合には、そのボクセルは、表面の或る構成に寄与していることを意味している。
【０１２３】
立方体のどのエッジが表面によって横切られるかを決定することによって、表面の内側の領域と、表面の外側の領域との間で立方体を分けるための三角形のパッチが形成される。
【０１２４】
全ての立方体からのこれらパッチを結合することにより、三角形のパッチにより構成される表面の体裁が形成される。
【０１２５】
ここで、閾値βを０に設定すると、立方体の角の値ｄ（Ｐ）は、数式（４７)で示される。
【０１２６】
マーチングキューブの表現においては、立方体の角の各値と、閾値βとの大きさの組み合わせによって、合計２５６の異なる状況がある。各状況については、回転や対称性を考慮すると１５のファミリーに一般化できる。それぞれのファミリーに対応して、図１３に示すように、三角形のパッチの状態を決定することができる。
【０１２７】
対象点Ｘｉから開始して、表面の内側及び外側の両方の角をもつ（すなわち、閾値βよりも大きい値と、閾値βよりも小さい値の両方の値の角をもつ）マーチングキューブは、Ｏｉに立方体をセンタリングすることにより初期化する。三角形のパッチを立方体内に初期化する。このようにして、表面のための初期の構造を生成する。
【０１２８】
そして、もし、全ての角が表面の内側又は表面の外側である立方体でなければ、立方体を８つの角を取り囲む側に成長させ、各角の値を推定し、三角形のパッチを生成する。
【０１２９】
このように立方体を成長させる処理は、境界上のすべての立方体が、それぞれ全ての角が表面の内側、又は表面の外側となるまで続けられる。
【０１３０】
各三角形のパッチについては、外側に向かう法線とともに記憶される。
【０１３１】
本実施形態のイメージシーケンスにおける復元において含まれている全ての立方体は、図１４に示すようになっている。
【０１３２】
以前に構築したいずれの立方体の内側に存在しない対象点がある場合には、この対象点は、先に構築した表面のいずれにも接続されない表面上にあることを意味する。この場合には、上記したように、立方体を初期化し、立方体を成長させる処理により、表面を復元することができる。このような処理を、未処理の対象点がなくなるまで実行する。これによって、３次元モデルのメッシュを調整する（あてはめる）処理ステップ（ステップＳ６）が終了する。図１５には、３角形のパッチＰＴにより構成されるメッシュが調整された３次元モデルの一例を示す。
【０１３３】
次に、テクスチャー貼付処理(ステップＳ７)が実行される。メッシュが調整された場合には、個々の三角形のフェースの外観は、そのフェースが最も前面に現れているイメージから把握することができる。この処理は、各フェースの法線、各フレームのカメラ位置及びオリエンテーションは既に把握されているので、容易である。従って、画像処理部１３は、イメージから各外観のテクスチャーを把握し、当該テクスチャーを対応するメッシュのフェースに貼り付ける。なお、以上の処理により得られた、対象点の３次元座標、メッシュの構造、フェースに貼り付けるテクスチャーの情報等を含む周囲環境についての３次元（３Ｄ）モデルのデータは、画像処理部１３が記憶部１５に格納する。
【０１３４】
各フェースに対して把握したテクスチャーＴＸが貼り付けられたメッシュ、すなわち、３次元モデルは、図１６に示すようになる。このようにして、テクスチャー貼付処理（ステップＳ７）が終了する。
【０１３５】
このように作成した３次元モデルを図示しない表示画面に表示させる。これに対して、ユーザーは、３次元モデルが満足のいくものであったか否かという処理結果の確認指示を行うことができる。例えば、処理結果がよくなかったとの指示をユーザーから受け付けた場合には、画像処理装置５は、ステップＳ９の新たなイメージを取得する処理を実行させる。このステップにおいて、例えば、対象点が十分でない範囲の画像を追加して取得するようにすることができる。このため、新たに追加した画像を用いてステップＳ３以降の処理を引き続いて実行することにより、より適切に対象点の３次元座標を推定し、３次元モデルをより適切に復元することができる。
【０１３６】
一方、処理結果がよかったとの指示を受け付けた場合には、シミュレーション部１４が記憶部１５に記憶された周囲環境の３次元モデル１８を読み出して（ステップＳ１０）、周囲環境の３次元モデルを利用したシミュレーションを実行する。シミュレーションとしては、例えば、ロボットの動作のティーチングや、ロボットの周囲環境との衝突検出、経路探索等のタスクを現実的に実行することができる。
【０１３７】
本実施形態によると、３次元モデルを作成するタスクを単純化することができ、より適切な周囲環境の対象点の３次元座標を把握することができ、周囲環境を示す３次元モデルを適切に復元することができる。
【０１３８】
以上、本発明を実施形態に基づいて説明したが、本発明は上述した実施の形態に限られず、他の様々な態様に適用可能である。
【０１３９】
例えば、上記実施形態では、周囲環境の３次元モデルを用いて、ロボットのティーチング等のシミュレーション等を例に説明したが、例えば、博物館の３次元モデルを作成することにより、博物館における仮想ツアーを実現することができる。また、作成した３次元モデルは、ゲームにおける仮想空間として利用することもできる。
【符号の説明】
【０１４０】
１  画像処理システム、２  デバイス、３  カメラ、４  デバイスコントローラー、５  画像処理装置、１１  メイン制御部、１２  画像取得部、１３  画像処理部、１４  シミュレーション部、１５  記憶部、１６  カメラ情報、１７  イメージシーケンス、１８  環境３Ｄモデル、１９  デバイス３Ｄモデル、２０  シミュレーション結果。
【図１】
2011085971.tif 000021
【図２】
2011085971.tif 000022
【図３】
2011085971.tif 000023
【図５】
2011085971.tif 000024
【図６】
2011085971.tif 000025
【図８】
2011085971.tif 000026
【図９】
2011085971.tif 000027
【図４】
2011085971.tif 000028
【図７】
2011085971.tif 000029
【図１０】
2011085971.tif 000030
【図１１】
2011085971.tif 000031
【図１２】
2011085971.tif 000032
【図１３】
2011085971.tif 000033
【図１４】
2011085971.tif 000034
【図１５】
2011085971.tif 000035
【図１６】
2011085971.tif 000036