特開 2022-186522 位置関係導出装置、位置関係導出方法、及び位置関係導出プログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2022186522

(43)【公開日】2022-12-15

(54)【発明の名称】位置関係導出装置、位置関係導出方法、及び位置関係導出プログラム

(51)【国際特許分類】

   G06T 7/70 20170101AFI20221208BHJP

【ＦＩ】

G06T7/70 Z

【審査請求】未請求

【請求項の数】8

【出願形態】ＯＬ

(21)【出願番号】P 2021094792

(22)【出願日】2021-06-04

(71)【出願人】

【識別番号】000004226

【氏名又は名称】日本電信電話株式会社

(71)【出願人】

【識別番号】504137912

【氏名又は名称】国立大学法人 東京大学

(74)【代理人】

【識別番号】110001519

【氏名又は名称】弁理士法人太陽国際特許事務所

(72)【発明者】

【氏名】八尾 泰洋

(72)【発明者】

【氏名】安藤 慎吾

(72)【発明者】

【氏名】島村 潤

(72)【発明者】

【氏名】石川 涼一

(72)【発明者】

【氏名】大石 岳史

【テーマコード（参考）】

5L096

【Ｆターム（参考）】

5L096AA06

5L096CA05

5L096DA02

5L096EA06

5L096EA15

5L096EA16

5L096FA02

5L096FA38

5L096FA66

5L096FA69

5L096GA51

5L096HA01

5L096JA11

(57)【要約】

【課題】キャリブレーションターゲット、又は多数の学習データ等の事前準備を必要とすることなく、カメラと三次元位置を計測する計測装置との位置関係を得る。
【解決手段】投影コスト導出部１０５は、三次元空間の各点の三次元位置を計測する計測装置が計測した三次元点群を、第一カメラと計測装置との位置関係の解候補に基づいて、計測装置が三次元点群を計測した時刻と同一の時刻に第一カメラが撮影した画像である第一カメラ画像に投影した位置における画素情報と、三次元点群を、解候補、及び第一カメラと第二カメラとの位置関係に基づいて、同一の時刻に第二カメラが撮影した画像である第二カメラ画像に投影した位置における画素情報との差分に応じたコストを導出し、最小化部１０７は、コストが最小となる解候補を、第一カメラと計測装置との位置関係の最適解として導出する。
【選択図】図３

【特許請求の範囲】

【請求項1】

三次元空間の各点の三次元位置を計測する計測装置が計測した三次元点群を、第一カメラと前記計測装置との位置関係の解候補に基づいて、前記計測装置が前記三次元点群を計測した時刻と同一の時刻に前記第一カメラが撮影した画像である第一カメラ画像に投影した位置における画素情報と、前記三次元点群を、前記解候補、及び前記第一カメラと第二カメラとの位置関係に基づいて、前記同一の時刻に前記第二カメラが撮影した画像である第二カメラ画像に投影した位置における画素情報との差分に応じたコストを導出する導出部と、
前記コストが最小となる前記解候補を、前記第一カメラと前記計測装置との位置関係の最適解として導出する最小化部と、
を備えた位置関係導出装置。

【請求項2】

三次元空間の各点の三次元位置を計測する計測装置が計測した三次元点群を、第一カメラと前記計測装置との位置関係の解候補に基づいて、前記計測装置が前記三次元点群を計測した時刻と同一の時刻に前記第一カメラが撮影した画像である第一カメラ画像に投影した位置における画素情報と、前記投影した位置の奥行が無限遠であると仮定して、前記三次元点群を、前記解候補、及び前記第一カメラと第二カメラとの位置関係に基づいて、前記同一の時刻に前記第二カメラが撮影した画像である第二カメラ画像に投影した位置における画素情報との差分に応じたコストを導出する導出部と、
前記コストが最小となる前記解候補を、前記第一カメラと前記計測装置との位置関係の最適解として導出する最小化部と、
を備えた位置関係導出装置。

【請求項3】

前記導出部は、前記三次元点群を、前記第一カメラ画像に投影した位置における画素情報と、前記第二カメラ画像に投影した位置における画素情報との差分である投影コストと、前記三次元点群を、前記解候補に基づいて、前記第一カメラ画像に投影した位置における画素情報と、前記投影した位置の奥行が無限遠であると仮定して、前記三次元点群を、前記解候補、及び前記第一カメラと前記第二カメラとの位置関係に基づいて、前記第二カメラ画像に投影した位置における画素情報との差分である背景キャンセル項とに基づく前記コストを導出する請求項１に記載の位置関係導出装置。

【請求項4】

前記第一カメラと前記計測装置との位置関係の解空間における所定範囲に含まれる点に対応する前記解候補を生成する生成部を更に備え、
前記最小化部は、前記生成部が生成した前記解候補のうち、前記コストが最も小さい前記解候補を前記最適解として導出する、
請求項１～請求項３の何れか１項に記載の位置関係導出装置。

【請求項5】

予め定められた条件を満たすまで、前記生成部に、前記最小化部で導出された前記最適解に対応する前記解候補を生成させる処理、及び、前記導出部に、前記コストを導出する処理を繰り返し実行させる終了判定部を、更に備え、
前記生成部は、前記解空間において、前記最小化部で導出された直近の前記最適解を中心とする前記所定範囲を、所定の分割数で分割した領域内の各点から前記最適解に対応する前記解候補を生成する、
請求項４に記載の位置関係導出装置。

【請求項6】

前記生成部は、前記解候補を生成する処理の繰り返し回数が多くなるにしたがって、前記分割数は維持したまま、前記所定範囲を小さくする請求項５に記載の位置関係導出装置。

【請求項7】

導出部が、三次元空間の各点の三次元位置を計測する計測装置が計測した三次元点群を、第一カメラと前記計測装置との位置関係の解候補に基づいて、前記計測装置が前記三次元点群を計測した時刻と同一の時刻に前記第一カメラが撮影した画像である第一カメラ画像に投影した位置における画素情報と、前記三次元点群を、前記解候補、及び前記第一カメラと第二カメラとの位置関係に基づいて、前記同一の時刻に前記第二カメラが撮影した画像である第二カメラ画像に投影した位置における画素情報との差分に応じたコストを導出し、
最小化部が、前記コストが最小となる前記解候補を、前記第一カメラと前記計測装置との位置関係の最適解として導出する、
位置関係導出方法。

【請求項8】

コンピュータを、請求項１～請求項６の何れか１項に記載の位置関係導出装置の各部として機能させるためのプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

開示の技術は、位置関係導出装置、位置関係導出方法、及び位置関係導出プログラムに関する。

【背景技術】

【0002】

ステレオカメラと、三次元空間の各点の三次元位置を計測する計測装置の一例であるＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ）センサとから構成される系において、これらの装置の位置関係を導出する技術が知られている。なお、位置関係とは、２つのセンサ間の三次元空間内での位置の違いを示す関係であり、例えば、三次元の各軸周りの回転角度と、三次元の各軸方向についての距離とからなる関係である。

【0003】

例えば、非特許文献１には、４つの円形の穴の空いた板であるキャリブレーションターゲットを用いて、ステレオカメラとＬｉＤＡＲセンサとの位置関係を導出する技術が開示されている。

【0004】

非特許文献２には、ステレオ画像によって生成した奥行画像とＬｉＤＡＲセンサによって生成した奥行画像とが一致するように、ステレオカメラとＬｉＤＡＲセンサとの位置関係を導出する技術が開示されている。

【0005】

非特許文献３には、ステレオカメラとＬｉＤＡＲセンサとの位置関係を導出可能なニューラルネットワークを学習によって得る技術が開示されている。

【先行技術文献】

【非特許文献】

【0006】

【非特許文献1】Guindel, Carlos, et al. "Automatic extrinsic calibration for lidar-stereo vehicle sensor setups." 2017 IEEE 20th international conference on intelligent transportation systems (ITSC). IEEE, 2017.

【非特許文献2】John, Vijay, et al. "Automatic calibration and registration of lidar and stereo camera without calibration objects." 2015 IEEE International Conference on Vehicular Electronics and Safety (ICVES). IEEE, 2015.

【非特許文献3】Park, Kihong, Seungryong Kim, and Kwanghoon Sohn. "High-precision depth estimation using uncalibrated LiDAR and stereo fusion." IEEE Transactions on Intelligent Transportation Systems 21.1 (2019): 321-335.

【発明の概要】

【発明が解決しようとする課題】

【0007】

非特許文献１に記載の技術の問題点は、特定の形状のキャリブレーションターゲットがない場合、上記位置関係を導出できない点である。

【0008】

非特許文献２に記載の技術の問題点は、ステレオ画像の奥行画像とマッチングできる程度に解像度が高い奥行画像を、ＬｉＤＡＲセンサが生成する必要がある点である。ＬｉＤＡＲセンサには、低解像度のものから高解像度のものまであるため、解像度が低いＬｉＤＡＲセンサを用いて、非特許文献２に記載の技術を適用した場合、上記位置関係を導出できない。

【0009】

非特許文献３に記載の技術の問題点は、位置関係が既知のステレオカメラとＬｉＤＡＲセンサとを用いて取得した大量の学習データが必要である点である。上記学習データを取得するためには、特定の形状のキャリブレーションターゲットを用いたキャリブレーション、又は手動でのキャリブレーションを実施したステレオカメラ及びＬｉＤＡＲセンサで事前に計測を行う必要がある。

【0010】

開示の技術は、上記の点に鑑みてなされたものであり、キャリブレーションターゲット、又は多数の学習データ等の事前準備を必要とすることなく、カメラと三次元位置を計測する計測装置との位置関係を得ることができる位置関係導出装置、位置関係導出方法、及び位置関係導出プログラムを提供することを目的とする。

【課題を解決するための手段】

【0011】

本開示の第１態様は、位置関係導出装置であって、三次元空間の各点の三次元位置を計測する計測装置が計測した三次元点群を、第一カメラと前記計測装置との位置関係の解候補に基づいて、前記計測装置が前記三次元点群を計測した時刻と同一の時刻に前記第一カメラが撮影した画像である第一カメラ画像に投影した位置における画素情報と、前記三次元点群を、前記解候補、及び前記第一カメラと第二カメラとの位置関係に基づいて、前記同一の時刻に前記第二カメラが撮影した画像である第二カメラ画像に投影した位置における画素情報との差分に応じたコストを導出する導出部と、前記コストが最小となる前記解候補を、前記第一カメラと前記計測装置との位置関係の最適解として導出する最小化部と、を含む。

【0012】

本開示の第２態様は、位置関係導出方法であって、導出部が、三次元空間の各点の三次元位置を計測する計測装置が計測した三次元点群を、第一カメラと前記計測装置との位置関係の解候補に基づいて、前記計測装置が前記三次元点群を計測した時刻と同一の時刻に前記第一カメラが撮影した画像である第一カメラ画像に投影した位置における画素情報と、前記三次元点群を、前記解候補、及び前記第一カメラと第二カメラとの位置関係に基づいて、前記同一の時刻に前記第二カメラが撮影した画像である第二カメラ画像に投影した位置における画素情報との差分に応じたコストを導出し、最小化部が、前記コストが最小となる前記解候補を、前記第一カメラと前記計測装置との位置関係の最適解として導出することを含む。

【0013】

本開示の第３態様は、位置関係導出プログラムであって、コンピュータを、上記第１態様の位置関係導出装置の各部として機能させるためのプログラムである。

【発明の効果】

【0014】

開示の技術によれば、キャリブレーションターゲット、又は多数の学習データ等の事前準備を必要とすることなく、カメラと三次元位置を計測する計測装置との位置関係を得ることができる。

【図面の簡単な説明】

【0015】

【図1】実施形態に係る位置関係導出システムのハードウェア構成の一例を示す模式図である。

【図2】実施形態に係る位置関係導出装置のハードウェア構成の一例を示す模式図である。

【図3】実施形態に係る位置関係導出装置の機能構成の一例を示すブロック図である。

【図4】実施形態に係る撮影シーンの一例を示す模式図である。

【図5】実施形態に係る第一カメラ画像の一例を示す模式図である。

【図6】実施形態に係る第二カメラ画像の一例を示す模式図である。

【図7】実施形態に係る回転行列の初期値及び並進ベクトルの初期値を適用して三次元点群を第一カメラ画像に投影した場合の一例を示す模式図である。

【図8】実施形態に係る解候補の生成方法の一例を示す模式図である。

【図9】実施形態に係る投影コストの導出方法の一例を示す模式図である。

【図10】実施形態に係る背景キャンセル項の導出方法の一例を示す模式図である。

【図11】実施形態に係る最適解に対応する回転行列及び並進ベクトルを適用して三次元点群を第一カメラ画像に投影した場合の一例を示す模式図である。

【図12】実施形態の位置関係導出装置における位置関係導出処理の一例を示すフローチャートである。

【発明を実施するための形態】

【0016】

以下、開示の技術の実施形態の一例を、図面を参照しつつ説明する。なお、各図面において同一又は等価な構成要素及び部分には同一の参照符号を付与している。また、図面の寸法比率は、説明の都合上誇張されており、実際の比率とは異なる場合がある。

【0017】

図１に示すように、本実施形態に係る位置関係導出システム１００は、計測装置２０、第一カメラ３０Ａ、及び第二カメラ３０Ｂを含む。計測装置２０、第一カメラ３０Ａ、及び第二カメラ３０Ｂの各々は、同一の計測対象を計測範囲又は撮影範囲とするように配置されている。なお、位置関係導出システム１００では、第一カメラ３０Ａと第二カメラ３０Ｂとの位置関係は既知であるが、第一カメラ３０Ａと計測装置２０との位置関係、及び第二カメラ３０Ｂと計測装置２０の位置関係はいずれも未知である。ここで、位置関係とは、２つのセンサ間の三次元空間内での位置及び姿勢の違いを示す関係であり、姿勢の違いを表す三次元の各軸周りの回転角度（以下、「回転」ともいう）と、位置の違いを表す三次元の各軸方向の距離（以下、「並進」ともいう）とで表される関係である。

【0018】

また、本実施形態では、計測装置２０として、ＬｉＤＡＲセンサを適用している。しかし、計測装置２０として、超音波センサ又は赤外線センサ等、三次元空間の各点の三次元位置を計測可能な装置を適用してもよい。また、本実施形態では、第一カメラ３０Ａと第二カメラ３０Ｂとで、ステレオカメラを構成する。また、本実施形態では、第一カメラ３０Ａと第二カメラ３０Ｂは便宜的に番号を付与しているだけである。したがって、第一カメラ３０Ａと第二カメラ３０Ｂを入れ替えても、本実施形態に適用可能である。

【0019】

次に、本実施形態に係る位置関係導出装置１０のハードウェア構成を説明する。図２に示すように、位置関係導出装置１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１１、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）１２、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）１３、ストレージ１４、及び通信Ｉ／Ｆ（Ｉｎｔｅｒｆａｃｅ）１７を有する。各構成は、バス１９を介して相互に通信可能に接続されている。

【0020】

ＣＰＵ１１は、中央演算処理ユニットであり、各種プログラムを実行したり、各部を制御したりする。すなわち、ＣＰＵ１１は、ＲＯＭ１２又はストレージ１４からプログラムを読み出し、ＲＡＭ１３を作業領域としてプログラムを実行する。ＣＰＵ１１は、ＲＯＭ１２又はストレージ１４に記憶されているプログラムに従って、上記各構成の制御及び各種の演算処理を行う。本実施形態では、ＲＯＭ１２又はストレージ１４には、位置関係導出プログラムが格納されている。位置関係導出プログラムは、１つのプログラムであってもよいし、複数のプログラム又はモジュールで構成されるプログラム群であってもよい。

【0021】

ＲＯＭ１２は、各種プログラム及び各種データを格納する。ＲＡＭ１３は、作業領域として一時的にプログラム又はデータを記憶する。ストレージ１４は、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）又はＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）により構成され、オペレーティングシステムを含む各種プログラム、及び各種データを格納する。

【0022】

通信Ｉ／Ｆ１７は、計測装置２０、第一カメラ３０Ａ、及び第二カメラ３０Ｂを含む他の機器と通信するためのインタフェースであり、例えば、イーサネット（登録商標）、ＦＤＤＩ、Ｗｉ－Ｆｉ（登録商標）等の規格が用いられる。

【0023】

次に、位置関係導出装置１０の機能構成について説明する。図３に示すように、位置関係導出装置１０は、イテレーション部１０１、画像平滑化部１０２、生成部１０３、導出部１０４、最小化部１０７、及び終了判定部１０８を備えている。なお、導出部１０４は、投影コスト導出部１０５及び背景キャンセル項導出部１０６を備えている。

【0024】

イテレーション部１０１は、イテレーションのためのデータ準備をする。具体的には、イテレーション部１０１は、後述する各機能部の処理のイテレーション回数ｊのカウント、及びイテレーションごとに更新するパラメータ（後述するｆ、λ等）の設定を行う。なお、イテレーション回数ｊの初期値は０である。

【0025】

画像平滑化部１０２は、計測装置２０が三次元点群Ｐを計測した時刻と同一の時刻に、第一カメラ３０Ａが撮影した第一カメラ画像Ｉ＿１、及び上記同一の時刻に第二カメラ３０Ｂが撮影した画像である第二カメラ画像Ｉ＿２を取得する。図４に、第一カメラ３０Ａ及び第二カメラ３０Ｂが上記同一の時刻に撮影したシーン、すなわち計測対象の被写体を含む風景の一例を概略的に示す。図４中の丸、三角、及び菱形の各々は、被写体を概略的に表したものである。また、図５に、図４に示すシーンを第一カメラ３０Ａで撮影した第一カメラ画像Ｉ＿１の一例、図６に、図４に示すシーンを第二カメラ３０Ｂで撮影した第二カメラ画像Ｉ＿２の一例を示す。なお、第一カメラ画像Ｉ＿１及び第二カメラ画像Ｉ＿２は、例えば、歪み補正されたＲＧＢ又はグレースケールの画像であり、解像度が縦Ｈ×横Ｗピクセルの画像である。

【0026】

画像平滑化部１０２は、第一カメラ画像Ｉ＿１及び第二カメラ画像Ｉ＿２を平滑化した画像を出力する。後述する最小化部１０７が最小化するコストは、解空間上で一般的になめらかではないため、解候補のサンプリングの粒度によっては、真に最適な解候補を見逃す可能性がある。そこで、第一カメラ画像Ｉ＿１及び第二カメラ画像Ｉ＿２の画素値に基づいて算出されるコスト（詳細は後述）が解空間上でなめらかになるように、第一カメラ画像Ｉ＿１及び第二カメラ画像Ｉ＿２を平滑化するものである。後述する生成部１０３は、イテレーション回数ｊが多くなるにしたがって粒度の小さいサンプリングを行うため、それに合わせて画像平滑化部１０２は、イテレーション回数ｊが多くなるにしたがって画像の平滑化の度合いが小さくなるようにする。

【0027】

具体的には、画像平滑化部１０２は、第一カメラ画像Ｉ＿１（図５参照）に標準偏差ｆ（ＳＩＧＭＡ，ｊ）のガウシアンフィルタを適用し、ガウシアンフィルタを適用した第一カメラ画像Ｉ＿１＿Ｇを作成する。また、画像平滑化部１０２は、第二カメラ画像Ｉ＿２（図６参照）に標準偏差ｆ（ＳＩＧＭＡ，ｊ）のガウシアンフィルタを適用し、ガウシアンフィルタを適用した第二カメラ画像Ｉ＿２＿Ｇを作成する。なお、ＳＩＧＭＡは、第一カメラ画像Ｉ＿１及び第二カメラ画像Ｉ＿２に適用するガウシアンフィルタの標準偏差の初期値である。また、ｆはイテレーション回数ｊの増加に従って減衰するような関数であり、一例として、ｆ（ＳＩＧＭＡ，ｊ）＝ＳＩＧＭＡ／（２＾ｊ）としてよい。画像平滑化部１０２は、作成した平滑化後の第一カメラ画像Ｉ＿１＿Ｇ及び第二カメラ画像Ｉ＿２＿Ｇを出力する。

【0028】

生成部１０３は、第一カメラ３０Ａと計測装置２０との位置関係の解空間における所定の探索空間に含まれる点に対応する解候補ＣＡＮＤを生成する。具体的には、生成部１０３は、イテレーションごとに、後述する最小化部１０７で導出された直近の最適解Ａを中心とする探索空間からサンプリングを実施して、解候補ＣＡＮＤを生成する。

【0029】

上述したように、生成部１０３は、イテレーション回数ｊが多くなるにしたがって粒度の小さいサンプリングを行う。例えば、生成部１０３は、探索空間を所定の分割数Ｓで分割した各領域から解候補ＣＡＮＤをサンプリングする場合、イテレーション回数ｊが多くなるにしたがって、分割数Ｓを維持したまま、探索空間を小さくする。これにより、イテレーション回数ｊが多くなるにしたがって、探索の解像度が大きくなる。

【0030】

なお、探索空間は、回転三次元（ｙａｗ，ｐｉｔｃｈ，ｒｏｌｌ）と並進三次元（ｘ，ｙ，ｚ）とからなる六次元の空間である。生成部１０３は、１回のイテレーションにおいて、六次元の空間で解候補ＣＡＮＤを生成してもよい。また、生成部１０３は、例えば、回転三次元の解候補の探索空間（以下、「回転空間」ともいう）で解候補ＣＡＮＤを生成した後、並進三次元の解候補の探索空間（以下、「並進空間」ともいう）で解候補ＣＡＮＤを生成するなど、交互に解候補ＣＡＮＤを生成してもよい。また、生成部１０３は、回転空間での解候補ＣＡＮＤの探索を複数イテレーションで実施した後に、並進空間での解候補ＣＡＮＤの探索を複数イテレーションで実施してもよい。

【0031】

具体的には、まず生成部１０３は、計測装置２０により計測された三次元点群Ｐ、及び後述する最小化部１０７で導出された直近の最適解Ａを入力として取得する。そして、生成部１０３は、三次元点群Ｐに含まれるそれぞれの三次元点について、式（１）を用いて、計測装置２０の座標系から第一カメラ３０Ａの座標系に変換したＰ＿ＲＴを導出する。

【0032】

【数1】

【0033】

なお、Ｒ＿ＬＡＳＴとは、後述する最小化部１０７が前イテレーションまでに導出した第一カメラ３０Ａと計測装置２０との位置関係の最適解Ａに含まれる３×３の回転行列を示す。また、Ｔ＿ＬＡＳＴとは、前イテレーションまでに導出した第一カメラ３０Ａと計測装置２０との位置関係の最適解Ａに含まれる三次元の並進ベクトル（３×１次元の配列）を示す。ｊ＝０の場合、生成部１０３は、Ｒ＿ＬＡＳＴを、回転行列の初期値Ｒ＿ＩＮＩＴとする。また、生成部１０３は、Ｔ＿ＬＡＳＴを、並進ベクトルの初期値Ｔ＿ＩＮＩＴとする。なお、Ｒ＿ＩＮＩＴは、Ｒ＿ＬＡＳＴと同様、３×３の回転行列であり、Ｔ＿ＩＮＩＴは、Ｔ＿ＬＡＳＴと同様、三次元の並進ベクトル（３×１次元の配列）である。また、Ｒ＿ＩＮＩＴ及びＴ＿ＩＮＩＴは、最適化の過程で初期値として用いるだけであるため、任意の値を設定しておけばよい。また、解の収束を考慮して、所定範囲内の値からＲ＿ＩＮＩＴ及びＴ＿ＩＮＩＴを設定してもよい。また、上記三次元点は三次元のベクトルである。三次元点群Ｐは、上記三次元点がＮ点存在する場合、Ｎ×３×１次元の配列である。また、Ｐ＿ＲＴもＮ×３×１次元の配列である。

【0034】

図７に、後述する生成部１０３が、Ｒ＿ＩＮＩＴ及びＴ＿ＩＮＩＴを適用して三次元点群Ｐを第一カメラ画像Ｉ＿１＿Ｇに投影した場合の一例を示す。図７に示すように、Ｒ＿ＩＮＩＴ及びＴ＿ＩＮＩＴは初期値であるため、これらの移動を適用した場合、三次元点群Ｐは被写体以外にも投影される。

【0035】

なお、本実施形態では、第一カメラ３０Ａと計測装置２０との位置関係における三次元の回転を３×３の回転行列として、三次元の並進を三次元のベクトルとして表現する。しかし、これらの表現方法として他の表現方法もあり、開示の技術は、その表現方法の違いには依存しない。例えば、これらの表現方法として、回転をオイラー角で表す方法、若しくは四元数で表す方法、又は回転と並進とをまとめて六次元のリー代数で表す方法が一般的に用いられる。これらの方法により表現したものを回転行列と並進ベクトルに変換すれば、容易に本実施形態に適用できる。

【0036】

次に、生成部１０３は、Ｐ＿ＲＴのうち、第一カメラ画像Ｉ＿１＿Ｇの撮影範囲（画角内）に存在するものの集合Ｐ＿ＲＥＭを求める。具体的には、生成部１０３は、第一カメラ内部パラメータＫ＿１により、Ｐ＿ＲＴを第一カメラ画像Ｉ＿１＿Ｇに投影した際に画角外に投影されるものをＰ＿ＲＴから除外し、残存する点をＰ＿ＲＥＭとする。上記除外後に残存する点数をＭとすると、Ｐ＿ＲＥＭはＭ×３×１次元の配列となる。なお、Ｐ＿ＲＥＭはＰ＿ＲＴの部分集合である。また、Ｋ＿１は、ワールド座標系の三次元座標と、第一カメラ画像の二次元座標との関係を表す３×３のカメラ内部パラメータ行列である。

【0037】

次に、生成部１０３は、回転に関する解候補の探索範囲である回転探索範囲ＲＡＤ＿ＲＡＮＧＥを直近の最適解Ａを中心とし、ｙａｗ，ｐｉｔｃｈ，ｒｏｌｌ方向それぞれに［－ＧＲ，ＧＲ］の範囲として、回転の解候補ＲＡＤ＿ＣＡＮＤを生成する。また、生成部１０３は、並進に関する解候補の探索範囲である並進探索範囲ＳＨＩＦＴ＿ＲＡＮＧＥを直近の最適解Ａを中心とし、ｘ，ｙ，ｚ方向それぞれに［－ＨＤ，ＨＤ］の範囲として、並進の解候補ＳＨＩＦＴ＿ＣＡＮＤを生成する。

【0038】

ＧＲは、回転探索範囲ＲＡＤ＿ＲＡＮＧＥの大きさを規定するスカラー値である。生成部１０３は、ＧＲを、式（２）を用いて導出する。

【0039】

【数2】

【0040】

ＨＤは、並進探索範囲ＳＨＩＦＴ＿ＲＡＮＧＥの大きさを既定するスカラー値である。生成部１０３は、ＨＤを、式（３）を用いて導出する。

【0041】

【数3】

【0042】

なお、ｌａｍｂｄａ＿ｒ及びｌａｍｂｄａ＿ｄの各々は、１または０の値をとる変数である。そのイテレーションで回転空間を探索する場合はｌａｍｂｄａ＿ｒ＝１とし、回転空間を探索しない場合はｌａｍｂｄａ＿ｒ＝０とする。同様に、そのイテレーションで並進空間を探索する場合はｌａｍｂｄａ＿ｄ＝１とし、並進空間を探索しない場合ｌａｍｂｄａ＿ｄ＝０とする。ｇ及びｈは、それぞれイテレーション回数ｊの増加に従って減衰する関数であり、一例として式（４）及び式（５）を用いてそれぞれ表される。

【0043】

【数4】

【0044】

ＧＲが０でない場合、生成部１０３は、回転探索範囲ＲＡＤ＿ＲＡＮＧＥのｙａｗ，ｐｉｔｃｈ，ｒｏｌｌ方向それぞれについて等間隔にＳ個サンプリングし、回転の解候補ＲＡＤ＿ＣＡＮＤを生成する。これにより、生成部１０３は、イテレーションごとにＳ＾３個の回転の解候補ＲＡＤ＿ＣＡＮＤを生成する。なお、ＧＲが０である場合、生成部１０３は、ＲＡＤ＿ＣＡＮＤを生成しない。

【0045】

また、ＨＤが０でない場合、生成部１０３は、並進探索範囲ＳＨＩＦＴ＿ＲＡＮＧＥのｘ，ｙ，ｚ方向それぞれについて等間隔にＳ個サンプリングし、並進の解候補ＳＨＩＦＴ＿ＣＡＮＤを生成する。これにより、生成部１０３は、イテレーションごとにＳ＾３個の並進の解候補ＳＨＩＦＴ＿ＣＡＮＤを生成する。なお、ＨＤが０である場合、生成部１０３は、ＳＨＩＦＴ＿ＣＡＮＤを生成しない。

【0046】

図８に、生成部１０３が、直近の最適解Ａを中心として、±ＧＲ（±ＨＤ）の範囲を回転探索範囲ＲＡＤ＿ＲＡＮＧＥ（並進探索範囲ＳＨＩＦＴ＿ＲＡＮＧＥ）とした例を示す。また、図８は、Ｓ＝３とし、ｙａｗ，ｐｉｔｃｈ，ｒｏｌｌ方向（ｘ，ｙ，ｚ方向）それぞれについて等間隔に３個ずつサンプリングし、２７個の回転の解候補ＲＡＤ＿ＣＡＮＤ（並進の解候補ＳＨＩＦＴ＿ＣＡＮＤ）を生成した例である。なお、図８では、丸印が解候補ＲＡＤ＿ＣＡＮＤ（並進の解候補ＳＨＩＦＴ＿ＣＡＮＤ）であり、ｐｉｔｃｈ方向（ｙ方向）のサンプリングの間隔毎に丸印内の色（ハッチング）を異ならせて表現している。

【0047】

生成部１０３は、ＲＡＤ＿ＣＡＮＤとＳＨＩＦＴ＿ＣＡＮＤとの全ての組み合わせを本イテレーションの解候補とする。すなわち、生成部１０３は、解候補ＣＡＮＤ＝ＲＡＤ＿ＣＡＮＤ×ＳＨＩＦＴ＿ＣＡＮＤを本イテレーションの解候補とする。ＣＡＮＤのそれぞれは（ｙａｗ，ｐｉｔｃｈ，ｒｏｌｌ，ｘ，ｙ，ｚ）の値からなる。以下では、生成した解候補ＣＡＮＤの要素数をＳ＿Ｃと記載する。

【0048】

次に、生成部１０３は、Ｐ＿ＲＥＭを解候補ＣＡＮＤに含まれる回転行列及び並進ベクトルによって移動した三次元座標の集合であるＰ＿ＣＡＮＤを導出する。Ｐ＿ＣＡＮＤは、Ｍ×Ｓ＿Ｃ×３×１次元の配列である。

【0049】

次に、生成部１０３は、Ｐ＿ＣＡＮＤを第一カメラ内部パラメータＫ＿１により第一カメラ画像Ｉ＿１＿Ｇに投影した二次元点の画像上での二次元座標の集合Ｐ＿ＣＡＮＤ＿１（Ｍ×Ｓ＿Ｃ×２次元の配列）を導出する。

【0050】

次に、生成部１０３は、Ｐ＿ＣＡＮＤを第二カメラ画像Ｉ＿２＿Ｇに投影した二次元点の画像上での二次元座標の集合Ｐ＿ＣＡＮＤ＿２（Ｍ×Ｓ＿Ｃ×２の配列）を導出する。具体的には、まず、生成部１０３は、Ｐ＿ＣＡＮＤを、第一カメラ３０Ａと第二カメラ３０Ｂ間の回転行列Ｒ＿Ｃ、及び第一カメラ３０Ａと第二カメラ３０Ｂ間の並進ベクトルＴ＿Ｃによって第二カメラ３０Ｂの座標系に変換する。その後、生成部１０３は、座標変換したＰ＿ＣＡＮＤを、第二カメラ内部パラメータＫ＿２によって第二カメラ画像Ｉ＿２＿Ｇに投影することでＰ＿ＣＡＮＤ＿２を導出する。ここで、Ｐ＿ＣＡＮＤ＿１とＰ＿ＣＡＮＤ＿２との２つの配列の並び、すなわち、三次元点を各画像に投影した二次元点の組み合わせの対応はついている。なお、Ｋ＿２は、３×３のカメラ内部パラメータ行列である。また、Ｒ＿Ｃは３×３の回転行列であり、Ｔ＿Ｃは三次元の並進ベクトルである。

【0051】

最後に、生成部１０３は、Ｐ＿ＣＡＮＤ＿１とＰ＿ＣＡＮＤ＿２とを出力する。

【0052】

投影コスト導出部１０５は、生成部１０３が生成した全ての解候補ＣＡＮＤについての投影コストＣを導出する。投影コストＣとは、三次元点群Ｐに解候補ＣＡＮＤによる三次元の回転及び三次元の並進を適用することで、第一カメラ画像Ｉ＿１＿Ｇ及び第二カメラ画像Ｉ＿２＿Ｇに投影し、投影した位置及びその周辺における画素がどの程度異なるかを評価するコストである。解候補ＣＡＮＤが、第一カメラ３０Ａと計測装置２０との真の位置関係に近くなる場合、解候補ＣＡＮＤに基づいて移動させた三次元点群Ｐが第一カメラ画像Ｉ＿１＿Ｇ及び第二カメラ画像Ｉ＿２＿Ｇの各々に投影される点は同じような画像になるべきであるという考えに基づくものである。投影コストＣを用いることで、キャリブレーションターゲットを検出する事前処理を経ずに、かつ三次元点群Ｐの解像度が低い場合でも、第一カメラ３０Ａと計測装置２０との位置関係を評価することができる。

【0053】

投影コスト導出部１０５は、投影コストＣとして、Ｐ＿ＣＡＮＤ＿１とＰ＿ＣＡＮＤ＿２とで、二次元点の組それぞれに対して値を求めたのちに、Ｍ個の三次元点（二次元点の組）全てについての和を取る。したがって、投影コストＣは、Ｓ＿Ｃの要素を持つ配列となる。具体的には、投影コスト導出部１０５は、三次元点を第一カメラ画像Ｉ＿１＿Ｇに投影した二次元点をｐ＿１（ｐ＿１∈Ｐ＿ＣＡＮＤ＿１）、第二カメラ画像Ｉ＿２＿Ｇに投影した二次元点をｐ＿２（ｐ＿２∈Ｐ＿ＣＡＮＤ＿２）とし、式（６）によって投影コストＣを導出する。

【0054】

【数5】

【0055】

ここで、式（６）におけるＳｔｅｒｅｏは第一カメラ画像Ｉ＿１＿Ｇと第二カメラ画像Ｉ＿２＿Ｇとに基づく、任意のステレオマッチングコストである。例えば、Ｓｔｅｒｅｏは、画素値の差異を直接用いるフォトメトリックコストでもよいし、Ｃｅｎｓｕｓコストでもよいし、勾配の差分によるコストでもよいし、それらの任意の組み合わせの任意の重みづけ和でもよい。また、それぞれのコストを導出する際のウィンドウサイズなどのパラメータも任意でよい。

【0056】

例えば、Ｓｔｅｒｅｏとして、フォトメトリックコストのウィンドウサイズをｗ＿ｐ、Ｃｅｎｓｕｓコストのカーネルサイズをｄ＿ｃ、Ｃｅｎｓｕｓコストのウィンドウサイズをｗ＿ｃ、勾配の差分によるコストのウィンドウサイズをｗ＿ｇ、フォトメトリックコストの重みをα＿ｐ、Ｃｅｎｓｕｓコストの重みをα＿ｃ、及び勾配の差分によるコストの重みをα＿ｇとし、式（７）で表されるステレオマッチングコストを用いてもよい。

【0057】

【数6】

【0058】

ここで、図９を用いて、２つの解候補（ＣＡＮＤ１及びＣＡＮＤ２）の各々の投影コストＣを導出する場合の具体例を説明する。

【0059】

図９に示すように、ＣＡＮＤ１の移動を適用した三次元点Ａ１を第一カメラ画像Ｉ＿１＿Ｇに投影した座標ＰＡ１１における画素は背景を表す画素であり、第二カメラ画像Ｉ＿２＿Ｇに投影した座標ＰＡ１２における画素は三角形を表す画素である。また、ＣＡＮＤ１の移動を適用した三次元点Ｂ１を第一カメラ画像Ｉ＿１＿Ｇに投影した座標ＰＢ１１における画素は菱形を表す画素であり、第二カメラ画像Ｉ＿２＿Ｇに投影した座標ＰＢ１２における画素は背景を表す画素である。

【0060】

一方、ＣＡＮＤ２の移動を適用した三次元点Ａ２を第一カメラ画像Ｉ＿１＿Ｇに投影した座標ＰＡ２１における画素は円を表す画素であり、第二カメラ画像Ｉ＿２＿Ｇに投影した座標ＰＡ２２における画素も円を表す画素である。また、ＣＡＮＤ２の移動を適用した三次元点Ｂ２を第一カメラ画像Ｉ＿１＿Ｇに投影した座標ＰＢ２１における画素は菱形を表す画素であり、第二カメラ画像Ｉ＿２＿Ｇに投影した座標ＰＢ２２における画素も菱形を表す画素である。

【0061】

したがって、ＣＡＮＤ１の移動を適用した三次元点をそれぞれの画像に投影した位置におけるステレオマッチングコストは、ＣＡＮＤ２の移動を適用した三次元点をそれぞれの画像に投影した位置におけるステレオマッチングコストより大きい。言い換えると、ＣＡＮＤ１の投影コストＣは、ＣＡＮＤ２の投影コストＣより大きい。上述した投影コストＣを用いた場合、投影コストＣの値が大きいほど、投影した位置の画素間のマッチング度合いが低いことを表す。

【0062】

背景キャンセル項導出部１０６は、三次元点群Ｐが背景領域に投影されている度合いに応じて、投影コストＣを低下させるための項である背景キャンセル項Ｄを導出する。第一カメラ画像Ｉ＿１＿Ｇ及び第二カメラ画像Ｉ＿２＿Ｇの背景領域に三次元点が投影されていた場合、どのような視差値でもステレオマッチングコストが小さくなるため、対応する投影コストＣも小さくなる。その結果、投影コストＣだけでコスト関数を作ると、後述する最小化部１０７は、最適解Ａとして、背景領域に多くの三次元点を投影する解候補を選ぶ可能性がある。そのため、三次元点群Ｐが背景領域に投影されている度合いに応じて、投影コストＣを低下させる必要がある。

【0063】

具体的には、背景キャンセル項導出部１０６は、生成部１０３が出力したＰ＿ＣＡＮＤ＿１に含まれる全ての二次元点の奥行値を無限大とし、Ｋ＿１、Ｒ＿Ｃ、Ｔ＿Ｃ、及びＫ＿２によって第二カメラ画像Ｉ＿２＿Ｇに投影した二次元座標の集合Ｐ＿ＣＡＮＤ＿２＿ＩＮＦを導出する。なお、Ｐ＿ＣＡＮＤ＿２＿ＩＮＦはＭ×Ｓ＿Ｃ×２の配列である。また、点までの距離を無限遠としているため、第一カメラ３０Ａと第二カメラ３０Ｂ間の並進ベクトルＴ＿Ｃによる並進の効果は無視できる。したがって、背景キャンセル項導出部１０６は、Ｋ＿１、Ｒ＿Ｃ、及びＫ＿２のみによってＰ＿ＣＡＮＤ＿２＿ＩＮＦを導出する。

【0064】

背景キャンセル項導出部１０６は、背景キャンセル項Ｄとして、Ｐ＿ＣＡＮＤ＿１とＰ＿ＣＡＮＤ＿２＿ＩＮＦとで、二次元点の組それぞれに対して値を求めたのちに、Ｍ個の二次元点全てについての和を取る。したがって、背景キャンセル項Ｄは、Ｓ＿Ｃの要素を持つ配列となる。具体的には、背景キャンセル項導出部１０６は、三次元点を第一カメラ画像Ｉ＿１＿Ｇに投影した二次元点をｐ＿１（ｐ＿１∈Ｐ＿ＣＡＮＤ＿１）、奥行き値を無限大として第二カメラ画像Ｉ＿２＿Ｇに投影した二次元点をｐ＿２＿ｉｎｆ（ｐ＿２＿ｉｎｆ∈Ｐ＿ＣＡＮＤ＿２＿ＩＮＦとし、式（８）によって背景キャンセル項Ｄを導出する。

【0065】

【数7】

【0066】

ここで、式（８）におけるＳｔｅｒｅｏは、投影コストＣを導出した際のステレオマッチングコストと同様である。ただし、式（８）におけるＳｔｅｒｅｏが、式（６）におけるステレオマッチングコスト（例えば、式（７））と必ずしも一致していなくてもよい。すなわち、Ｓｔｅｒｅｏを構成するコストの要素の種類、数、重み、及びウィンドウサイズなどのパラメータは任意である。

【0067】

ここで、図１０を用いて、２つの解候補（ＣＡＮＤ１及びＣＡＮＤ２）の各々の背景キャンセル項Ｄを導出する場合の具体例を説明する。

【0068】

図１０に示すように、ＣＡＮＤ１の移動を適用した三次元点Ｃ１を第一カメラ画像Ｉ＿１＿Ｇに投影した座標ＰＣ１１における画素は三角形を表す画素である。一方、三次元点Ｃ１を座標ＰＣ１１の奥行が無限遠であると仮定して第二カメラ画像Ｉ＿２＿Ｇに投影した座標ＰＣ１２における画素は背景を表す画素である。また、ＣＡＮＤ１の移動を適用した三次元点Ｄ１を第一カメラ画像Ｉ＿１＿Ｇに投影した座標ＰＤ１１における画素は円を表す画素である。一方、三次元点Ｄ１を座標ＰＤ１１の奥行が無限遠であると仮定して第二カメラ画像Ｉ＿２＿Ｇに投影した座標ＰＤ１２における画素は背景を表す画素である。

【0069】

また、ＣＡＮＤ２の移動を適用した三次元点Ｃ２を第一カメラ画像Ｉ＿１＿Ｇに投影した座標ＰＣ２１における画素は背景を表す画素である。一方、三次元点Ｃ２を座標ＰＣ２１の奥行が無限遠であると仮定して第二カメラ画像Ｉ＿２＿Ｇに投影した座標ＰＣ２２における画素も背景を表す画素である。また、ＣＡＮＤ２の移動を適用した三次元点Ｄ２を第一カメラ画像Ｉ＿１＿Ｇに投影した座標ＰＤ２１における画素は背景を表す画素である。一方、三次元点Ｄ２を座標ＰＤ２１の奥行が無限遠であると仮定して第二カメラ画像Ｉ＿２＿Ｇに投影した座標ＰＤ２２における画素も背景を表す画素である。

【0070】

したがって、ＣＡＮＤ１の移動を適用した三次元点のステレオマッチングコストは、ＣＡＮＤ２の移動を適用した三次元点のステレオマッチングコストより大きい。言い換えると、ＣＡＮＤ１の移動を適用した場合、三次元点が第一カメラ画像Ｉ＿１＿Ｇの背景に投影されている可能性が低いため、ＣＡＮＤ１の背景キャンセル項Ｄは、ＣＡＮＤ２の背景キャンセル項Ｄより大きい。

【0071】

最後に、導出部１０４は、全ての解候補ＣＡＮＤに対応する、投影コスト導出部１０５が導出した投影コストＣと、背景キャンセル項導出部１０６が導出した背景キャンセル項Ｄに変数λによって重みづけをした項との差分であるコストを導出する。なお、導出部１０４は、イテレーション回数ｊの増加にしたがって変数λを小さくすることで、探索範囲が小さくなるにしたがって、背景キャンセル項Ｄにより投影コストＣが低下しなくなるようにしてもよい。また、導出部１０４は、例えば、ｅｘｐ（－Ｄ）Ｃのように、投影コストＣを背景キャンセル項Ｄによって重みづけをし、正規化したコストを導出してもよい。

【0072】

最小化部１０７は、全ての解候補ＣＡＮＤの中から、導出部１０４が導出したコストが最小となる（ｙａｗ，ｐｉｔｃｈ，ｒｏｌｌ，ｘ，ｙ，ｚ）の組を最適解Ａとして導出する。具体的には、最小化部１０７は、式（９）を用いて最適解Ａを導出する。

【0073】

【数8】

【0074】

また、最小化部１０７は、導出部１０４により導出されたコストが、投影コストＣを背景キャンセル項Ｄによって重みづけをし、正規化したコストの場合、式（１０）を用いて最適解Ａを導出してもよい。

【0075】

【数9】

【0076】

最小化部１０７は、導出した最適解Ａを回転行列Ｒ＿ＣＵＲＲＥＮＴと並進ベクトルＴ＿ＣＵＲＲＥＮＴとに変換する。そして、最小化部１０７は、Ｒ＿ＬＡＳＴ及びＴ＿ＬＡＳＴとＲ＿ＣＵＲＲＥＮＴ及びＴ＿ＣＵＲＲＥＮＴとをアキュムレーションすることで、第一カメラ３０Ａと計測装置２０との位置関係の回転行列Ｒ、及び第一カメラ３０Ａと計測装置２０との位置関係の並進ベクトルＴを導出する。具体的には、最小化部１０７は、式（１１）及び式（１２）を用いて回転行列Ｒと並進ベクトルＴを導出する。なお、回転行列Ｒは３×３の回転行列であり、並進ベクトルＴは三次元のベクトル（３×１次元の配列）である。

【0077】

【数10】

【0078】

図１１に、生成部１０３が、回転行列Ｒ及び並進ベクトルＴを適用して三次元点群Ｐを第一カメラ画像Ｉ＿１＿Ｇに投影した場合の一例を示す。図１１に示すように、Ｒ及びＴは最適解Ａに対応しているため、これらの移動を適用した場合、図７に示すように初期値を適用した場合に比べ、三次元点群Ｐが適切な位置（被写体上）に投影される。

【0079】

終了判定部１０８は、予め定められた終了条件を満たす場合、最小化部１０７が導出した回転行列Ｒ及び並進ベクトルＴを出力する。一方、終了判定部１０８は、予め定められた終了条件を満たさない場合、生成部１０３に、最小化部１０７が導出した最適解Ａに対応する解候補を生成させ、かつ、導出部１０４に、コストを繰り返し導出させる。なお、上記終了条件は任意の条件である。例えば、上記終了条件として、既定回数のイテレーションを実施したとの条件、又はＲ及びＴの値が前回イテレーションの結果と比較して差分が閾値よりも小さいとの条件を適用してもよい。

【0080】

次に、位置関係導出装置１０の作用について説明する。

【0081】

図１２は、位置関係導出装置１０による位置関係導出処理の流れを示すフローチャートである。ＣＰＵ１１がＲＯＭ１２又はストレージ１４から位置関係導出プログラムを読み出して、ＲＡＭ１３に展開して実行することにより、位置関係導出処理が行なわれる。

【0082】

ステップＳ１０１で、ＣＰＵ１１は、イテレーション部１０１として、イテレーション回数ｊの初期値を０とする。

【0083】

ステップＳ１０２で、ＣＰＵ１１は、画像平滑化部１０２として、計測装置２０が三次元点群Ｐを計測した時刻と同一の時刻に、第一カメラ３０Ａが撮影した第一カメラ画像Ｉ＿１、及び上記同一の時刻に第二カメラ３０Ｂが撮影した画像である第二カメラ画像Ｉ＿２を入力として取得する。そして、ＣＰＵ１１は、画像平滑化部１０２として、取得した第一カメラ画像Ｉ＿１及び第二カメラ画像Ｉ＿２を平滑化した第一カメラ画像Ｉ＿１＿Ｇ及び第二カメラ画像Ｉ＿２＿Ｇを作成し、出力する。

【0084】

ステップＳ１０３で、ＣＰＵ１１は、生成部１０３として、前のイテレーションにおいて、後述するステップＳ１０８で導出された回転行列Ｒ及び並進ベクトルＴ、又は回転行列の初期値Ｒ＿ＩＮＩＴ及び並進ベクトルの初期値Ｔ＿ＩＮＩＴを取得する。そして、ＣＰＵ１１は、生成部１０３として、解空間において、Ｒ及びＴ又はＲ＿ＩＮＩＴ及びＴ＿ＩＮＩＴに対応する最適解Ａを中心とした所定の探索範囲からサンプリングすることにより、第一カメラ３０Ａと計測装置２０との位置関係の解候補ＣＡＮＤを生成する。

【0085】

ステップＳ１０４で、ＣＰＵ１１は、生成部１０３として、計測装置２０により計測された三次元点群Ｐを取得する。そして、ＣＰＵ１１は、生成部１０３として、ステップＳ１０３で生成した解候補ＣＡＮＤと、カメラ内部パラメータＫ＿１及びＫ＿２とに基づいて、ステップＳ１０２で平滑化した第一カメラ画像Ｉ＿１＿Ｇ及び第二カメラ画像Ｉ＿２＿Ｇの各々に、三次元点群Ｐを投影する。

【0086】

ステップＳ１０５で、ＣＰＵ１１は、投影コスト導出部１０５として、三次元点群Ｐが投影された二次元点の位置の画素値に基づいて、ステップＳ１０３で生成した解候補ＣＡＮＤに対応する全ての投影コストＣを導出する。

【0087】

ステップＳ１０６で、ＣＰＵ１１は、背景キャンセル項導出部１０６として、三次元点群Ｐが投影された二次元点の位置の画素値、及び第一カメラ画像Ｉ＿１＿Ｇに投影された二次元点の奥行きが無限遠であるとの仮定に基づいて、ステップＳ１０３で生成した解候補ＣＡＮＤに対応する全ての背景キャンセル項Ｄを導出する。

【0088】

ステップＳ１０７で、ＣＰＵ１１は、導出部１０４として、上記ステップＳ１０５で導出した投影コストＣと、上記ステップＳ１０６で導出した背景キャンセル項Ｄとに基づいて、ステップＳ１０３で生成した解候補ＣＡＮＤに対応する全てのコストを導出する。

【0089】

ステップＳ１０８で、ＣＰＵ１１は、最小化部１０７として、ステップＳ１０３で生成した解候補ＣＡＮＤの中から、ステップＳ１０７で導出したコストが最小となる解候補を最適解Ａとして導出し、対応する回転行列Ｒと並進ベクトルＴを導出する。

【0090】

ステップＳ１０９で、ＣＰＵ１１は、終了判定部１０８として、終了条件を満たすか否かを判定する。ＣＰＵ１１は、ステップＳ１０９で肯定判定となった場合は本位置関係導出処理を終了し、否定判定となった場合はステップＳ１１０へ移行する。

【0091】

ステップＳ１１０で、ＣＰＵ１１は、イテレーション部１０１として、イテレーション回数ｊをインクリメントし、ステップＳ１０３に戻る。

【0092】

以上説明したように、本実施形態に係る位置関係導出装置は、三次元空間の各点の三次元位置を計測する計測装置が計測した三次元点群を、第一カメラと計測装置との位置関係の解候補に基づいて、計測装置が三次元点群を計測した時刻と同一の時刻に第一カメラが撮影した画像である第一カメラ画像に投影した位置における画素情報と、三次元点群を、解候補、及び第一カメラと第二カメラとの位置関係に基づいて、同一の時刻に第二カメラが撮影した画像である第二カメラ画像に投影した位置における画素情報との差分に応じたコストを導出する。位置関係導出装置は、コストが最小となる解候補を、第一カメラと計測装置との位置関係の最適解として導出する。これにより、キャリブレーションターゲット、又は多数の学習データ等の事前準備を必要とすることなく、カメラと三次元位置を計測する計測装置との位置関係を得ることができる。

【0093】

＜変形例＞
なお、本発明は、上述した実施形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。

【0094】

例えば、投影コスト導出部１０５の処理を省略してもよい。この場合、最小化部１０７は、生成部１０３が生成した解候補ＣＡＮＤの中から、背景キャンセル項Ｄが最小となる解候補を、最適解Ａとして導出する。

【0095】

また、背景キャンセル項導出部１０６の処理を省略してもよい。この場合、最小化部１０７は、生成部１０３が生成した解候補ＣＡＮＤの中から、投影コストＣが最小となる解候補を、最適解Ａとして導出する。

【0096】

また、上記各実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行した各種処理を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）等の製造後に回路構成を変更可能なＰＬＤ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ）、及びＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、位置関係導出処理を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。

【0097】

また、上記各実施形態では、位置関係導出プログラムがストレージ１４に予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ－ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｋ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＤＶＤ－ＲＯＭ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、及びＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）メモリ等の非一時的（ｎｏｎ－ｔｒａｎｓｉｔｏｒｙ）記憶媒体に記憶された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。

【0098】

以上の実施形態に関し、更に以下の付記を開示する。

【0099】

（付記項１）
位置関係導出装置であって、
メモリと、
前記メモリに接続された少なくとも１つのプロセッサと、
を含み、
前記プロセッサは、
三次元空間の各点の三次元位置を計測する計測装置が計測した三次元点群を、第一カメラと前記計測装置との位置関係の解候補に基づいて、前記計測装置が前記三次元点群を計測した時刻と同一の時刻に前記第一カメラが撮影した画像である第一カメラ画像に投影した位置における画素情報と、前記三次元点群を、前記解候補、及び前記第一カメラと第二カメラとの位置関係に基づいて、前記同一の時刻に前記第二カメラが撮影した画像である第二カメラ画像に投影した位置における画素情報との差分に応じたコストを導出し、
前記コストが最小となる前記解候補を、前記第一カメラと前記計測装置との位置関係の最適解として導出する
ように構成される位置関係導出装置。

【0100】

（付記項２）
位置関係導出処理を実行するようにコンピュータによって実行可能なプログラムを記憶した非一時的記憶媒体であって、
前記位置関係導出処理は、
三次元空間の各点の三次元位置を計測する計測装置が計測した三次元点群を、第一カメラと前記計測装置との位置関係の解候補に基づいて、前記計測装置が前記三次元点群を計測した時刻と同一の時刻に前記第一カメラが撮影した画像である第一カメラ画像に投影した位置における画素情報と、前記三次元点群を、前記解候補、及び前記第一カメラと第二カメラとの位置関係に基づいて、前記同一の時刻に前記第二カメラが撮影した画像である第二カメラ画像に投影した位置における画素情報との差分に応じたコストを導出し、
前記コストが最小となる前記解候補を、前記第一カメラと前記計測装置との位置関係の最適解として導出する
非一時的記憶媒体。

【符号の説明】

【0101】

１０ 位置関係導出装置
１１ ＣＰＵ
１２ ＲＯＭ
１３ ＲＡＭ
１４ ストレージ
１７ 通信Ｉ／Ｆ
１９ バス
１０１ イテレーション部
１０２ 画像平滑化部
１０３ 生成部
１０４ 導出部
１０５ 投影コスト導出部
１０６ 背景キャンセル項導出部
１０７ 最小化部
１０８ 終了判定部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】