JP7810180B2 - 信号処理装置および信号処理方法 - Google Patents

Description

本開示は、信号処理装置および信号処理方法に関し、特に、dToFセンサの位置情報のずれを補正できるようにした信号処理装置および信号処理方法に関する。

direct ToF方式のToFセンサ（以下、dToFセンサとも称する。）は、受光用の各画素にSPAD（Single Photon Avalanche Diode）と呼ばれる受光素子を用いて、パルス光が物体で反射された反射光を検出する。環境光等によるノイズを抑制するため、パルス光の発光と、その反射光の受光が所定の回数（例えば、数回ないし数百回）繰り返し行われ、dToFセンサは、パルス光の飛行時間のヒストグラムを生成して、ヒストグラムのピークに対応する飛行時間から、物体までの距離を算出する。

低反射率または遠方の被写体の測距や、屋外環境などの外光による外乱影響が強い環境下での測距などにおいては、SN比が低く、ピーク位置の検出が困難であることが知られている。そのため、発光するパルス光をスポット形状とすることでパルス光の到達距離の拡大、換言すれば、反射光の検出数を増加させることが行われている。スポット形状のパルス光は一般的に疎なパルス光となるため、反射光が検出される画素も、スポット径および照射面積に応じて疎となる。

SN比向上のためと、疎な反射光検出環境に合わせた効率的な画素駆動による電力削減を目的に、画素アレイの一部の隣接する複数画素（マルチピクセルと称する。）を１つの大きな画素とみなして、マルチピクセル単位で受光動作を行わせ、ヒストグラムを生成することが行われている。例えば、特許文献１には、隣接する２×３、３×３、３×６、３×９、６×３、６×６、９×９といった任意の画素数でマルチピクセルを形成し、マルチピクセルの信号を用いてヒストグラムを作成して距離を算出することで、空間解像度を下げる代わりにSN比を上げる方法が開示されている。

dToFセンサ等の測距センサは、例えば、多視点で撮影された動画像から被写体の３Dオブジェクトを生成し、任意の視聴位置に応じた３Dオブジェクトの仮想視点画像を生成するボリューメトリックキャプチャ技術に、RGBカメラとともに使用されている。また、自己位置推定と環境地図作成を同時に行うSLAM(Simultaneous Localization and Mapping)などにおいても、dToFセンサ等の測距センサが、RGBカメラとともに使用されている。

特開２０２０－１１２４４３号公報

dToFセンサで取得された測距点の３次元位置情報を、RGBカメラで得られた撮影画像と対応させたときに、キャリブレーション誤差や測距誤差、露光タイミングのずれ等により、dToFセンサで取得された位置情報にずれが発生する場合がある。

本開示は、このような状況に鑑みてなされたものであり、dToFセンサの位置情報のずれを補正できるようにするものである。

本開示の一側面の信号処理装置は、測距センサの所定の測距点における照射光の飛行時間のヒストグラムデータである距離ヒストグラムを取得する取得部と、前記所定の測距点が位置ずれ確認対象の測距点であるか否かを判定する判定部と、前記所定の測距点が位置ずれ確認対象の測距点である場合に、前記所定の測距点の前記距離ヒストグラムと、前記所定の測距点の近傍の測距点の前記距離ヒストグラムとの類似度の判定結果に基づいて、前記距離ヒストグラムから演算された前記所定の測距点の３次元座標を補正する補正処理を実行する補正部とを備える。

本開示の一側面の信号処理方法は、信号処理装置が、測距センサの所定の測距点における照射光の飛行時間のヒストグラムデータである距離ヒストグラムを取得し、前記所定の測距点が位置ずれ確認対象の測距点であるか否かを判定し、前記所定の測距点が位置ずれ確認対象の測距点である場合に、前記所定の測距点の前記距離ヒストグラムと、前記所定の測距点の近傍の測距点の前記距離ヒストグラムとの類似度の判定結果に基づいて、前記距離ヒストグラムから演算された前記所定の測距点の３次元座標を補正する補正処理を実行する。

本開示の一側面においては、測距センサの所定の測距点における照射光の飛行時間のヒストグラムデータである距離ヒストグラムが取得され、前記所定の測距点が位置ずれ確認対象の測距点であるか否かが判定され、前記所定の測距点が位置ずれ確認対象の測距点である場合に、前記所定の測距点の前記距離ヒストグラムと、前記所定の測距点の近傍の測距点の前記距離ヒストグラムとの類似度の判定結果に基づいて、前記距離ヒストグラムから演算された前記所定の測距点の３次元座標を補正する補正処理が実行される。

なお、本開示の一側面の信号処理装置は、コンピュータにプログラムを実行させることにより実現することができる。また、本開示の一側面の信号処理装置を実現するためにコンピュータに実行させるプログラムは、伝送媒体を介して伝送することにより、又は、記録媒体に記録して、提供することができる。

信号処理装置は、独立した装置であっても良いし、他の装置に組み込まれるモジュールであっても良い。

本開示の信号処理システムの第１実施の形態の構成例を示すブロック図である。 図１のRGBカメラとdToFセンサの動作を説明する図である。 dToFセンサを説明する図である。 本開示の位置ずれ補正機能を説明する図である。 距離ヒストグラムのピーク領域を説明する図である。 確認対象判定部による確認対象判定処理を説明する図である。 座標補正部の位置ずれ補正処理を説明する図である。 類似度の算出方法を説明する図である。 類似度の算出方法を説明する図である。 第１実施の形態の信号処理システムによる位置情報算出処理を説明するフローチャートである。 本開示の信号処理システムの第２実施の形態の構成例を示すブロック図である。 確認対象判定部の確認対象判定処理を説明する図である。 確認対象判定部の確認対象判定処理を説明する図である。 本開示の信号処理を実行するコンピュータのハードウエアの構成例を示すブロック図である。

以下、添付図面を参照しながら、本開示の技術を実施するための形態（以下、実施の形態という）について説明する。なお、本明細書及び図面において、実質的に同一の機能構成を有する構成要素については、同一の符号を付することにより重複説明を省略する。説明は以下の順序で行う。
１．信号処理システムの第１実施の形態
２．確認対象判定部の確認対象判定処理
３．座標補正部の位置ずれ補正処理
４．第１実施の形態による位置情報算出処理
５．信号処理システムの第２実施の形態
６．第２実施の形態による位置情報算出処理
７．まとめ
８．コンピュータ構成例

＜１．信号処理システムの第１実施の形態＞
図１は、本開示の信号処理システムの第１実施の形態の構成例を示すブロック図である。

図１の信号処理システム１は、RGBカメラ１１、dToFセンサ１２、および、信号処理装置１３で構成されている。

信号処理装置１３は、データ取得部２１、距離演算部２２、補正処理部２３、記憶部２４、および、出力部２５を有している。補正処理部２３は、確認対象判定部３１、および、座標補正部３２を備える。

RGBカメラ１１は、被写体としての所定の物体を撮影してRGB画像（の動画像）を生成し、信号処理装置１３に供給する。dToFセンサ１２は、direct ToF方式により被写体までの距離情報を取得する測距センサであり、RGBカメラ１１が撮影する物体と同一の物体までの距離情報を取得する。RGBカメラ１１とdToFセンサ１２の相対的な位置関係は固定され、RGBカメラ１１とdToFセンサ１２の撮像範囲はキャリブレーションされている。換言すれば、RGBカメラ１１とdToFセンサ１２の撮像範囲は同一であり、RGBカメラ１１とdToFセンサ１２の各画素の対応関係は既知である。本実施の形態では、説明を簡単にするため、RGBカメラ１１とdToFセンサ１２の位置の違いは無視できるものとして、RGBカメラ１１とdToFセンサ１２のカメラ位置（カメラ姿勢）は同一であるとする。

RGBカメラ１１は、図２に示されるように、時間経過に応じて撮影場所を移動しながら、被写体としての物体OBJを撮影し、RGB画像を生成する。dToFセンサ１２は、RGBカメラ１１とともに移動し、不図示の発光源により照射された複数のスポット光（照射光）が物体OBJで反射されてきた反射光を受光することにより、物体OBJの距離情報としての距離ヒストグラムを取得する。距離ヒストグラムとは、物体OBJまでの距離に対応する、照射光の飛行時間のヒストグラムデータである。

図３を参照して、dToFセンサ１２について簡単に説明する。

dToFセンサ１２は、受光用の各画素にSPAD（Single Photon Avalanche Diode）と呼ばれる受光素子を用いて、照射光としてのパルス光が物体で反射されて戻ってきた反射光を検出する。dToFセンサ１２は、環境光等によるノイズを抑制するため、パルス光の発光と、その反射光の受光とを所定の回数（例えば、数回ないし数百回）繰り返すことによりパルス光の飛行時間のヒストグラムを生成し、距離ヒストグラムを出力する。RGBカメラ１１と同一の撮像範囲について距離ヒストグラムを１回出力する単位を、RGBカメラ１１に倣って１フレームと称する。

低反射率または遠方の被写体の測距や、屋外環境などの外光による外乱影響が強い環境下での測距などにおいては、SN比が低く、ピーク位置の検出が困難であることが知られている。そのため、発光するパルス光をスポット形状とすることでパルス光の到達距離の拡大、換言すれば、反射光の検出数を増加させることが行われている。スポット形状のパルス光は一般的に疎なパルス光となるため、反射光が検出される画素も、スポット径および照射面積に応じて疎となる。図３では、RGBカメラ１１と同一の撮像範囲に対して、５ｘ５＝２５個のスポット光SPを照射し、各スポット光SPが物体で反射された反射光を検出する例が示されている。

SN比向上のためと、疎な反射光検出環境に合わせた効率的な画素駆動による電力削減を目的に、dToFセンサ１２は、疎なスポット光SPに応じて、隣接する複数画素をマルチピクセルMPとし、画素アレイの全画素のうちの複数のマルチピクセルMPのみに受光動作を行わせ、マルチピクセルMP単位でヒストグラムを生成する。図３の例では、１つのスポット光SPに対して、３ｘ３の９画素のマルチピクセルMPが設定されている。

なお、図３では、５ｘ５＝２５個のスポット光SPと、３ｘ３の９画素のマルチピクセルMPの例で説明したが、スポット光SPおよびマルチピクセルMPの個数および配置は任意である。以下の説明では、スポット光SPとマルチピクセルMPとを特に区別しない場合、それらに対応する点を測距点とも称する。

図１のRGBカメラ１１は、被写体である所定の物体を時々刻々と撮影して得られるRGB画像（の動画像）を生成し、信号処理装置１３に供給する。dToFセンサ１２は、被写体である所定の物体に照射されたスポット光SPが物体で反射された反射光を受光して得られる距離ヒストグラムと、その距離ヒストグラム取得時のカメラ姿勢とを、信号処理装置１３に供給する。距離ヒストグラムは、dToFセンサ１２で検出されたスポット光SPの中心に対応する画素位置（ｘ、ｙ）とヒストグラムデータとで構成される。カメラ姿勢は、dToFセンサ１２内の慣性計測装置（IMU）で検出された、dToFセンサ１２の外部パラメータの情報である。ただし、dToFセンサ１２は、慣性計測装置（IMU）を備えていなくてもよい。その場合、dToFセンサ１２は、距離ヒストグラムのみを出力し、dToFセンサ１２のカメラ姿勢は、例えば、信号処理装置１３内の距離演算部２２等において、Normal Distribution Transform等の手法により、各フレームで対応する（同一の）測距点の３次元位置情報を計算して算出される。

信号処理装置１３は、RGBカメラ１１が撮影したRGB画像と、dToFセンサ１２が生成した距離ヒストグラムおよびカメラ姿勢とを取得し、被写体である所定の物体のグローバル座標系上の３次元座標を生成して出力する。以下の説明において、単に３次元座標と記述した場合においても、グローバル座標系上の３次元座標を表すものとする。

すなわち、信号処理装置１３は、dToFセンサ１２が生成した距離ヒストグラムと、その時のカメラ姿勢とに基づいて、被写体である所定の物体のグローバル座標系上の３次元座標を算出する処理を行う信号処理装置である。その際、信号処理装置１３は、キャリブレーション誤差や測距誤差、露光タイミングのずれ等により、算出した物体の３次元座標が、RGB画像上の物体の位置に対してずれが発生している場合に、その位置ずれを補正する位置ずれ補正機能を有している。

図４は、dToFセンサ１２から取得した距離ヒストグラムに基づいて算出された位置ずれ補正前の各測距点と、RGBカメラ１１が撮影したRGB画像とを重ねた図を示している。

RGBカメラ１１が撮影したRGB画像５１には、被写体として、車（自動車）４１、人（歩行者）４２、および、木４３が写っている。RGB画像５１に重畳して示されている所定のパターンが付された丸印は、dToFセンサ１２の測距点Kを表す。図４に示される各測距点Kに付されたパターンは、算出された物体までの距離（３次元座標）に応じて、測距点K1、K2、K3の３種類のパターンに分けられている。測距点K1は、車４１に対応する距離（３次元座標）を有する測距点である。測距点K2は、人４２に対応する距離（３次元座標）を有する測距点である。測距点K3は、車４１、人４２、および、木４３以外の背景４４に対応する距離（３次元座標）を有する測距点である。背景４４は、例えば、建物の壁などである。

ここで、dToFセンサ１２の多数の測距点Kのうちの所定の１つの測距点K11に注目する。測距点K11は、距離ヒストグラムに基づいて算出された距離（３次元座標）としては、車４１に対応する距離を有している。しかし、距離ヒストグラムに基づいて算出された測距点K11の３次元座標は、車４１の位置ではなく、背景４４の位置となっている。このような測距点K11の位置ずれは、例えば、dToFセンサ１２のキャリブレーション誤差や測距誤差により発生する。あるいはまた、dToFセンサ１２が１フレームで取得する測距点Kの個数が少なく疎であるために、RGBカメラ１１の撮影範囲全体の距離ヒストグラムを複数フレームに分けて取得するような場合に、距離ヒストグラムの取得タイミング（露光タイミング）のずれや物体の移動などにより、このような位置ずれが発生する。

信号処理装置１３は、測距点K11のような測距点Kの３次元座標の位置ずれを補正する位置ずれ補正処理を実行し、各測距点Kの３次元座標を出力する。測距点K11の例で言えば、図４のRGB画像５１上において、現在の測距点K11を、左下方向の位置に補正し、３次元座標が車４１の位置となるように、３次元座標の位置ずれを補正する。

図１の説明に戻り、信号処理装置１３のデータ取得部２１は、RGBカメラ１１から供給されるRGB画像と、dToFセンサ１２から供給される距離ヒストグラムおよびカメラ姿勢とを取得する。データ取得部２１は、取得したRGB画像を、補正処理部２３へ供給するとともに、取得した距離ヒストグラムおよびカメラ姿勢を、距離演算部２２へ供給する。

距離演算部２２は、データ取得部２１からの距離ヒストグラムおよびカメラ姿勢に基づいて、dToFセンサ１２の測距点ごとに３次元座標（x,y,z）を演算する。より具体的には、距離演算部２２は、スポット光SPに対応するマルチピクセルMPのヒストグラムデータから、カウント値のピーク領域を検出し、検出されたピーク領域とカメラ姿勢とから、３次元座標（x,y,z）を演算する。

ここで、ピーク領域は次のように検出される。例えば、図５に示されるように、カウント値が所定閾値Th以上で、かつ、隣接する複数のビンのなかで最大のカウント値（ピーク）PVを有するビンと、および、その周辺の複数のビンが、ピーク領域として検出される。ピーク周辺の複数のビンは、例えば、ピークPVのカウント値の一定割合（例えば、ピークPVの半分である0.5PV）以上のカウント値を有するピーク周辺のビンと定めてもよいし、ピークPVのビンの前後所定数のビンと定めてもよい。図５の例では、ハッチングが付された３つのビン、すなわち、ピークPVのビンと、そのビン周辺の0.5PV以上のカウント値を有する２つのビンが、ピーク領域として検出される。検出されたピーク領域とカメラ姿勢とから、３次元座標（x,y,z）が算出される。

また、距離演算部２２は、図５の例のように、検出されたピーク領域以外のビンのカウント値の中央値EVCを、外乱光強度として検出することができる。

距離演算部２２は、距離ヒストグラムおよびカメラ姿勢から演算した各測距点の３次元座標（x,y,z）と、距離ヒストグラムとを、補正処理部２３へ供給する。なお、距離演算部２２は、距離ヒストグラムの代わりに、ピーク領域のヒストグラムデータと外乱光強度を、補正処理部２３へ供給してもよい。

補正処理部２３は、データ取得部２１から供給されるRGB画像と、距離演算部２２から供給される各測距点の３次元座標（x,y,z）および距離ヒストグラムとを用いて、演算された各測距点の３次元座標に対して位置ずれの補正が必要か否かを判定する。そして、位置ずれの補正が必要と判定された場合、補正処理部２３は、位置ずれ補正処理を実行し、測距点の３次元座標を移動（補正）する。

具体的には、補正処理部２３の確認対象判定部３１および座標補正部３２が、dToFセンサ１２の１フレームに含まれる複数の測距点それぞれを注目測距点に設定し、以下の処理を行う。

確認対象判定部３１は、注目測距点が位置ずれ確認対象の測距点であるか、換言すれば、位置ずれが起きている可能性の高い測距点であるか否かを判定する。確認対象判定部３１は、注目測距点が位置ずれ確認対象の測距点であると判定した場合、注目測距点の位置ずれ補正処理の実行を座標補正部３２へ指示する。一方、注目測距点が位置ずれ確認対象の測距点ではないと判定した場合、確認対象判定部３１は、距離演算部２２で演算された注目測距点の３次元座標（x,y,z）と距離ヒストグラムを、そのまま（位置ずれ補正処理を行うことなく）、記憶部２４に供給し、記憶させる。

座標補正部３２は、確認対象判定部３１から、注目測距点の位置ずれ補正処理の実行が指示された場合、注目測距点の位置ずれ補正処理を実行する。座標補正部３２は、位置ずれ補正処理として、位置ずれの補正が必要か否かを判定し、必要であると判定した場合に、距離演算部２２で演算された注目測距点の３次元座標（x,y,z）を補正（移動）する処理を実行する。位置ずれ補正処理後の注目測距点の３次元座標（x,y,z）と距離ヒストグラムは、記憶部２４に供給され、記憶される。

また、補正処理部２３は、データ取得部２１から供給されたRGB画像も、所定のタイミングで記憶部２４に供給し、記憶させる。確認対象判定部３１または座標補正部３２のどちらが、RGB画像を記憶部２４へ記憶させてもよい。

補正処理部２３は、dToFセンサ１２から順次供給される複数フレームの全てのフレームに対して処理を終了した場合、終了通知を出力部２５に供給する。

出力部２５は、補正処理部２３から終了通知が供給された場合、記憶部２４に記憶されている全てのフレームの各測距点の３次元座標を、補正処理後の３次元座標として、出力する。

なお、補正処理部２３は、dToFセンサ１２から順次供給される複数フレームに対して、フレーム単位で終了通知を出力部２５に供給してもよい。この場合、出力部２５は、フレーム単位で、記憶部２４から、補正処理後の３次元座標を取得して出力する。

信号処理装置１３は、以上の構成を有する。以下、信号処理装置１３の補正処理部２３の詳細についてさらに説明する。

＜２．確認対象判定部の確認対象判定処理＞
次に、確認対象判定部３１による確認対象判定処理について説明する。

確認対象判定部３１は、注目測距点が確認対象の測距点であるか、換言すれば、位置ずれが起きている可能性の高い測距点であるか否かを判定する。

具体的には、位置ずれは物体の境界付近で起きやすいこと、あるいは、位置ずれが物体の境界付近で発生した場合に、位置ずれの影響が大きいことから、確認対象判定部３１は、RGBカメラ１１で撮影されたRGB画像から、物体境界近傍の領域をエッジ領域として抽出する。そして、確認対象判定部３１は、注目測距点がエッジ領域に含まれる場合に、位置ずれが起きている可能性の高い、確認対象の測距点であると判定する。

図６は、確認対象判定部３１による確認対象判定処理を説明する図である。

まず、確認対象判定部３１は、RGBカメラ１１で撮影されたRGB画像５１に対して、例えばCannyの手法などを用いてエッジを検出する。図６のエッジ画像５１Eは、RGB画像５１から検出されたエッジを黒色の画素値で表現した画像である。

次に、確認対象判定部３１は、エッジが検出されたエッジ画像５１Eに対して、黒色画素を膨張させるフィルタ処理をN回（Nは２以上の整数。）実行することで、エッジから所定幅のエッジ領域を決定する。図６の例では、エッジ領域画像５１Rにおいてドットパターンで示された領域が、エッジ画像５１Eに基づいて決定されたエッジ領域を示している。

なお、エッジ領域は、上述した膨張処理以外の方法で決定してもよい。例えば、過去数フレームから動きが検出された領域をエッジ領域として決定してもよい。より具体的には、記憶部２４に記憶されている過去フレームのRGB画像と現フレームのRGB画像とを入力とし、Gunnar-Farneback法などで求めた密なオプティカルフロー画像中で、動きが大きい部分に関して膨張処理回数を増やした領域を、エッジ領域として決定してもよい。

次に、確認対象判定部３１は、RGBカメラ１１のカメラ内部パラメータを用いて、注目測距点の３次元座標に対応する、RGB画像５１上のスクリーン座標（u,v）を求める。そして、確認対象判定部３１は、注目測距点のRGB画像５１上のスクリーン座標(u,v)が、エッジ領域内に含まれる場合、注目測距点が確認対象の測距点であると判定する。一方、注目測距点のRGB画像５１上のスクリーン座標(u,v)が、エッジ領域外に位置する場合、注目測距点が確認対象の測距点ではないと判定する。なお、注目測距点の３次元座標をスクリーン座標(u,v)へ変換するためのRGBカメラ１１のカメラ内部パラメータは既知である。

図６の例では、注目測距点が測距点g1である場合、測距点g1のRGB画像５１上の対応点gs1のスクリーン座標(u,v)は、エッジ領域外となっているため、測距点g1は確認対象の測距点ではないと判定される。

これに対して、注目測距点が測距点g2である場合、測距点g2のRGB画像５１上の対応点gs2のスクリーン座標(u,v)は、エッジ領域内となっているため、測距点g2は確認対象の測距点であると判定される。

＜３．座標補正部の位置ずれ補正処理＞
次に、注目測距点が確認対象の測距点であると判定された場合に座標補正部３２によって実行される、位置ずれ補正処理について説明する。

図７は、図４に示したRGB画像５１の測距点K11が確認対象の測距点であると判定された場合の、座標補正部３２による位置ずれ補正処理を説明する図である。

図７では、図４に示したRGB画像５１の測距点K11を含む周辺領域を拡大した拡大領域６１が示されている。また、拡大領域６１には、測距点K11のほか、その周辺に位置する８個の測距点K21ないしK28も含まれている。

座標補正部３２は、注目測距点K11を中心にMxMの周辺の近傍測距点を用いて、位置ずれ補正処理を実行する。図７の例では、M＝３として、注目測距点K11の周辺の８個の測距点K21ないしK28を用いて、位置ずれ補正処理が実行される。ここで、拡大領域６１内の測距点K11の位置（位置ベクトル）を、位置ａとする。また、８個の測距点K21ないしK28の位置（位置ベクトル）を、それぞれ、位置ｂ１ないしｂ８とする。

なお、図７において、測距点K11と測距点K21ないしK28の９個の測距点の近傍に表示されたヒストグラムは、各測距点の距離ヒストグラムを概念的に示したものであり、RGB画像５１の一部ではない。

拡大領域６１は、エッジ６２を含み、エッジ６２を境界として、車４１の領域６３と、背景４４の領域６４とに分けられる。また、エッジ６２から所定幅の領域が、エッジ領域６５とされている。

座標補正部３２は、RGB画像５１のスクリーン座標系において、注目測距点K11の位置ａから、エッジ６２の最短位置ｅまでのベクトル（ｅ－ａ）を計算する。

座標補正部３２は、注目測距点K11と、その近傍の複数の測距点との距離ヒストグラムの類似度を計算し、類似度の重心ベクトルを計算する。この例では、座標補正部３２は、注目測距点K11を中心とする３ｘ３の９個の測距点を用いて、注目測距点K11と、その周辺の８個の測距点K21ないしK28との類似度を計算し、類似度の重心ベクトルを計算する例を説明する。

距離ヒストグラムには、測距点の単一の距離情報だけではなく、周囲の測距点との対応関係や、隣接するフレーム間の対応関係を判断する手がかりとなる物体の反射率や外乱光などの付加情報が含まれている。距離ヒストグラムどうしの比較を行うことで、例えば下記のような情報を、類似度の算出に当たり利用可能である。
・距離減衰線に沿った移動（同じ被写体の距離変化）
・物体境界や、ガラス等の透明被写体による他のピークの出現
・内部散乱によるヒストグラム波形のひずみ
・外乱光（光源方向を検出し、影のエッジ情報を利用可能）
・物体境界で発生するヒストグラム波形の入れ替わり

本実施の形態では、距離ヒストグラムどうしの類似度算出の例として、（１）距離ヒストグラムの相関を用いる例と、（２）多様体空間における距離を用いる例を説明する。

初めに、（１）距離ヒストグラムの相関を用いて類似度を算出する場合の、類似度の算出方法を説明する。

注目測距点K11の距離ヒストグラムH_aが、N個のビンで構成され、i番目のビンのカウント値をC_ai(i=1,2,3,・・・,N)とすると、距離ヒストグラムH_aは、H_a＝｛C_a1,C_a2,・・・,C_aN｝で表される。また、注目測距点K11近傍の測距点K21ないしK28の所定の１つの測距点K2bの距離ヒストグラムH_bが、N個のビンで構成され、i番目のビンのカウント値をC_bi(i=1,2,3,・・・,N)とすると、距離ヒストグラムH_bは、H_b＝｛C_b1,C_b2,・・・,C_bN｝で表される。

このとき、距離ヒストグラムH_aと距離ヒストグラムH_bの類似度r_abは、以下の式（１）で表現することができる。
式（１）中のC_{a_av}は、C_{a_av}＝AVE(C_a1,C_a2,・・・,C_aN)、すなわち、距離ヒストグラムH_aのN個のカウント値C_a1,C_a2,・・・,C_aNの平均値を表す。同様に、C_{b_av}は、C_{b_av}＝AVE(C_b1,C_b2,・・・,C_bN)、すなわち、距離ヒストグラムH_bのN個のカウント値C_b1,C_b2,・・・,C_bNの平均値を表す。式（１）のr_abは、距離ヒストグラムH_aと距離ヒストグラムH_bの相関係数に対応する。

注目測距点K11と、その近傍の測距点K2bの類似ベクトルvr_abは、式（１）の類似度r_abを用いて、以下の式（２）で表現することができる。
式（２）中のaは、測距点K11の位置ベクトルであり、bは、測距点K2bの位置ベクトルである。したがって、注目測距点K11と、その近傍の測距点K2bの類似ベクトルvr_abは、注目測距点K11の位置ベクトルａと、測距点K2bの位置ベクトルｂのベクトル差分に、位置ベクトルａと位置ベクトルｂの類似度（相関係数）r_abを乗算して求められる。

注目測距点K11の近傍の８個の測距点K21ないしK28の全てについて、式（２）の類似ベクトルvr_abが計算され、それらを類似ベクトルvr_abi（i=1,2,・・,8）と表す。

注目測距点K11と、近傍の８個の測距点K21ないしK28それぞれの類似ベクトルvr_abi（i=1,2,・・,8）の重心ベクトルrは、以下の式（３）で表現される。
式（３）のMは、図７の例ではM＝３であるので、M^２-1＝８である。

図７には、注目測距点K11の近傍の８個の測距点K21ないしK28の類似ベクトルvr_ab1ないしvr_ab8の重心ベクトルrが示されている。

次に、座標補正部３２は、注目測距点K11の近傍の８個の測距点K21ないしK28の類似ベクトルvr_ab1ないしvr_ab8の重心ベクトルrと、注目測距点K11の位置ａからエッジ６２の最短位置ｅへ向かうベクトル（ｅ－ａ）とがなす角θを計算する。

座標補正部３２は、重心ベクトルrとベクトル（ｅ－ａ）とがなす角θが所定の閾値内である場合、例えば－π/2＜θ＜π/2である場合、２つのベクトルの向きが一致していると判定する。反対に、重心ベクトルrとベクトル（ｅ－ａ）とがなす角θが所定の閾値より大きい場合、座標補正部３２は、２つのベクトルの向きが一致していないと判定する。

座標補正部３２は、２つのベクトルの向きが一致している場合、注目測距点K11の補正が必要であると判定し、注目測距点K11がエッジ６２をまたぐように、注目測距点K11の位置ａを移動（補正）する。例えば、座標補正部３２は、注目測距点K11の位置ａを、a’＝a＋２（ｅ－ａ）で計算される図７の位置a’に移動（補正）する。反対に、２つのベクトルの向きが一致していない場合、注目測距点K11の補正は不要であると判定され、注目測距点K11の位置ａは移動（補正）されない。

以上のように、（１）距離ヒストグラムの相関を用いて算出した類似ベクトルvr_abから、重心ベクトルｒを算出し、注目測距点K11の位置ａを補正するか否かを判定して、注目測距点の位置ずれを補正することができる。

次に、（２）多様体空間における距離を用いて類似度を算出する場合の、類似度の算出方法を説明する。

座標補正部３２は、図８に示されるように、注目測距点K11の距離ヒストグラムH_aのピークカウント値C_aと、外乱光のカウント値（中央値）D_aを計算する。そして、座標補正部３２は、距離ヒストグラムH_aの各ビンのカウント値をカウント合計値で除算することにより正規化し、確率密度関数となるように変換した上で、距離ヒストグラムH_aに対してガウシアンフィッティング（ガウス関数近似）を行う。例えば、注目測距点K11の距離ヒストグラムH_aが、平均μ_a、分散σ_a ²の正規分布N(μ_a，σ_a ²)に近似されたとする。

座標補正部３２は、注目測距点K11近傍の測距点K21ないしK28の所定の１つの測距点K2bの距離ヒストグラムH_bに対しても同様に、ピークカウント値C_bと、外乱光のカウント値D_bを計算するとともに、ガウシアンフィッティングを行う。距離ヒストグラムH_bが、例えば、平均μ_b、分散σ_b ²の正規分布N(μ_b，σ_b ²)に近似されたとする。

次に、座標補正部３２は、図９に示されるように、注目測距点K11と、近傍の測距点K2bを、ｘ軸を平均μ、ｙ軸を分散σ²、ｚ軸をピークカウント値Cとする非ユークリッド空間上にマッピングする。図９の非ユークリッド空間上の点Pは、注目測距点K11に対応し、p（x_p）＝N(μ_a，σ_a ²)、C_p＝C_a、D_p＝D_aである。点Qは、注目測距点K2bに対応し、p（x_q）＝N(μ_b，σ_b ²)、C_q＝C_b、D_q＝D_bである。

次に、座標補正部３２は、注目測距点K11と測距点K2bに対応する非ユークリッド空間上の点Pと点Qの疑似距離Lを以下の式（４）より計算する。
式（４）において、KL(p||q)は、点P，Qの２つの正規分布間のKLダイバージェンスを表す。｜C_p－C_q｜は、L1ノルムによる点P，Qのピークカウント値C_pとC_qの差分であり、｜D_p－D_q｜は、L1ノルムによる点P，Qの外乱光のカウント値D_pとD_qの差分である。また、a,b,cは、それぞれの重みを表す係数であり、０から１の範囲で任意に設定することができる。

すなわち、注目測距点K11と測距点K2bに対応する非ユークリッド空間上の点Pと点Qの疑似距離Lは、点P，Qの２点間のKLダイバージェンスと、ピークカウント値C_pとC_qの差分と、外乱光のカウント値D_pとD_qの差分の重み付き和で算出される。

次に、座標補正部３２は、注目測距点K11と、その近傍の測距点K2bの類似ベクトルvr_abを、疑似距離Lを用いて、以下の式（５）により計算する。
vr_ab＝1-（1/L） ・・・・・・・・・・・・・・・・・（５）

その後の処理は、距離ヒストグラムの相関を用いた場合と同様である。すなわち、注目測距点K11の近傍の８個の測距点K21ないしK28の全てについて、式（５）の類似ベクトルvr_abが計算され、類似ベクトルvr_abi（i=1,2,・・,8）が計算される。そして、重心ベクトルrが、上述の式（３）で計算され、重心ベクトルrとベクトル（ｅ－ａ）とがなす角θが所定の閾値内であるか否かに応じて、注目測距点K11の位置ａが移動（補正）される。

以上の多様体空間における距離を用いて類似度を算出する場合、距離ヒストグラムの相関を用いた場合と比較して、データ量を抑えることができる。

上述した例では、M＝３として、注目測距点K11の近傍の８個の測距点K21ないしK28を用いて、注目測距点K11を補正するか否かを判定したが、近傍の測距点の個数は任意に設定可能であることは言うまでもない。

＜４．第１実施の形態による位置情報算出処理＞
次に、図１０のフローチャートを参照して、第１実施の形態の信号処理システム１による位置情報算出処理について説明する。この処理は、例えば、RGBカメラ１１およびdToFセンサ１２からRGB画像および距離ヒストグラムが供給されたとき開始される。

なお、図１０の位置情報算出処理では、座標補正部３２が距離ヒストグラムどうしの類似度を算出する方法として、（１）距離ヒストグラムの相関を用いた場合の例で説明する。

初めに、ステップＳ１において、データ取得部２１は、RGBカメラ１１から供給されたRGB画像と、dToFセンサ１２から供給された距離ヒストグラムおよびカメラ姿勢とを取得する。データ取得部２１は、取得したRGB画像を、補正処理部２３へ供給するとともに、取得した距離ヒストグラムおよびカメラ姿勢を、距離演算部２２へ供給する。

ステップＳ２において、距離演算部２２は、データ取得部２１からの距離ヒストグラムおよびカメラ姿勢に基づいて、測距点ごとに３次元座標（x,y,z）を演算する。より具体的には、距離演算部２２は、スポット光SPに対応するマルチピクセルMPのヒストグラムデータから、カウント値のピーク領域を検出し、検出されたピーク領域とカメラ姿勢とから、３次元座標（x,y,z）を演算する。演算された各測距点の３次元座標（x,y,z）は、距離ヒストグラムとともに、補正処理部２３へ供給される。

ステップＳ３において、補正処理部２３は、dToFセンサ１２から供給された１フレームに含まれる測距点の所定の一つを注目測距点に決定し、処理をステップＳ４に進める。

ステップＳ４において、補正処理部２３の確認対象判定部３１は、確認対象判定処理を実行する。例えば、確認対象判定部３１は、RGB画像５１のエッジを検出して膨張処理を施すことによりエッジ領域を決定し、注目測距点のRGB画像５１上のスクリーン座標（u,v）が、エッジ領域内に含まれるか否かにより、注目測距点が確認対象の測距点であるか否かを判定する。

ステップＳ５において、確認対象判定部３１は、確認対象判定処理の結果に基づいて、注目測距点が確認対象の測距点であるかを判定する。ステップＳ５で、注目測距点が確認対象の測距点ではないと判定された場合、処理はステップＳ６に進み、注目測距点が確認対象の測距点であると判定された場合、処理はステップＳ７に進む。

注目測距点が確認対象の測距点ではないと判定された場合のステップＳ６では、確認対象判定部３１は、距離演算部２２で演算された注目測距点の３次元座標（x,y,z）と距離ヒストグラムを、そのまま、記憶部２４に供給し、記憶させる。データ取得部２１から供給されたRGB画像も記憶部２４に供給され、記憶される。

一方、注目測距点が確認対象の測距点であると判定された場合のステップＳ７では、座標補正部３２は、RGB画像５１のスクリーン座標系において、注目測距点の位置ａからエッジ６２の最短位置ｅまでのベクトル（ｅ－ａ）を計算する。

ステップＳ８において、座標補正部３２は、注目測距点と、その近傍の複数の測距点それぞれとの距離ヒストグラムの類似度（類似ベクトル）を計算し、類似度の重心ベクトルを計算する。例えば、注目測距点K11を中心とする３ｘ３の９個の測距点を用いて、注目測距点K11と、その周辺の８個の測距点K21ないしK28との距離ヒストグラムの類似ベクトルvr_ab１ないしvr_ab8が計算され、式（３）の類似ベクトルvr_ab１ないしvr_ab8の重心ベクトルrが計算される。

ステップＳ９において、座標補正部３２は、注目測距点近傍の複数の測距点の類似度の重心ベクトルrと、注目測距点からエッジ６２の最短位置ｅへ向かうベクトル（ｅ－ａ）の２つのベクトルの向きが一致しているかを判定する。ステップＳ９では、例えば、重心ベクトルrとベクトル（ｅ－ａ）とがなす角θが所定の閾値内である場合、２つのベクトルの向きが一致していると判定され、所定の閾値以上である場合、２つのベクトルの向きが一致していないと判定される。

ステップＳ９で、２つのベクトルの向きが一致していないと判定された場合、処理は上述したステップＳ６に進む。したがって、この場合、注目測距点の３次元座標（x,y,z）は補正されず、そのまま、記憶部２４に記憶される。

一方、ステップＳ９で、２つのベクトルの向きが一致していると判定された場合、処理はステップＳ１０に進む。ステップＳ１０において、座標補正部３２は、注目測距点がエッジ６２をまたぐように、注目測距点の位置ａを移動し、移動後の３次元座標（x,y,z）を、補正後の３次元座標（x,y,z）として、記憶部２４に記憶させる。注目測距点の距離ヒストグラムと、データ取得部２１から供給されたRGB画像も記憶部２４に供給され、記憶される。

ステップＳ６またはＳ１０の後、ステップＳ１１において、補正処理部２３は、dToFセンサ１２から供給された１フレームの全ての測距点を注目測距点としたかを判定する。ステップＳ１１で、１フレームの全ての測距点をまだ注目測距点としていないと判定された場合、処理はステップＳ３に戻され、上述したステップＳ３ないしＳ１１が繰り返される。すなわち、１フレームの測距点のなかで、まだ注目測距点とされていない測距点が次の注目測距点に設定され、確認対象の測距点か否かが判定され、２つのベクトルの向きが一致していると判定された場合、注目測距点の３次元座標（x,y,z）が補正（移動）される。

一方、ステップＳ１１で、１フレームの全ての測距点を注目測距点としたと判定された場合、処理はステップＳ１２に進み、信号処理装置１３は、処理を終了するか否かを判定する。例えば、dToFセンサ１２から供給された全てのフレームの測距点について、上述したステップＳ１ないしＳ１１の処理を実行し、dToFセンサ１２から次のフレームの距離ヒストグラムが供給されない場合、信号処理装置１３は、処理を終了すると判定する。反対に、dToFセンサ１２から次のフレームの距離ストグラムが供給された場合、信号処理装置１３は、処理を終了しないと判定する。

ステップＳ１２で、まだ処理を終了しないと判定された場合、処理はステップＳ１へ戻され、上述したステップＳ１ないしＳ１２の処理が繰り返される。

一方、ステップＳ１２で、処理を終了すると判定された場合、処理はステップＳ１３に進み、補正処理部２３は、終了通知を出力部２５へ供給する。

ステップＳ１４において、出力部２５は、記憶部２４に記憶されている全てのフレームの各測距点の３次元座標（x,y,z）を出力して、図１０の位置情報算出処理を終了する。出力される各フレームの各測距点の３次元座標（x,y,z）には、補正処理されたものと、補正処理されないものがある。

以上説明した第１実施の形態に係る信号処理システム１の位置情報算出処理によれば、注目測距点がエッジ領域内に含まれるか否かにより、注目測距点が確認対象の測距点であるか否かが判定される。そして、注目測距点が確認対象の測距点であると判定された場合、注目測距点の距離ヒストグラムと、近傍の複数の測距点の距離ヒストグラムとの類似度（類似ベクトル）を計算することにより、注目測距点の位置が、距離ヒストグラムが類似する近傍の測距点の方に補正される。これにより、dToFセンサ１２の位置情報のずれを補正することができる。

＜５．信号処理システムの第２実施の形態＞
図１１は、本開示の信号処理システムの第２実施の形態の構成例を示すブロック図である。

図１１の第２実施の形態において、図１で示した第１実施の形態と対応する部分については同一の符号を付してあり、その部分の説明は適宜省略する。

第２実施の形態に係る信号処理システム１は、RGBカメラ１１、dToFセンサ１２、および、信号処理装置１３で構成されている。信号処理装置１３は、データ取得部２１、距離演算部２２、補正処理部２３、記憶部２４、および、出力部２５を有し、補正処理部２３は、確認対象判定部７１、および、座標補正部３２を備える。

すなわち、第２実施の形態に係る信号処理システム１では、第１実施の形態における補正処理部２３の確認対象判定部３１が、確認対象判定部７１に変更されており、その他の構成は、第１実施の形態と同様である。換言すれば、第２実施の形態は、注目測距点が確認対象の測距点であるか否かを判定する判定方法が、第１実施の形態と異なり、その他の点は第１実施の形態と同様である。

上述した第１実施の形態では、物体境界近傍の領域として、RGB画像からエッジ領域を抽出し、注目測距点がエッジ領域に含まれるか否かによって、位置ずれが起きている可能性の高い測距点であるか否かが判定された。これに対して、第２実施の形態の確認対象判定部７１は、複数フレームにわたる距離ヒストグラムの変化を観測することにより、注目測距点が物体境界近傍の領域であるかを判定する。そして、注目測距点が物体境界近傍の領域であると判定された場合、確認対象判定部７１は、位置ずれが起きている可能性の高い測距点として、注目測距点を確認対象の測距点であると判定する。

例えば、図１２に示されるように、背景側の物体８１と前景側の物体８２が被写体として存在し、物体８１と物体８２の境界部分にスポット光SPが照射される場合について考える。前景側の物体８２が、動被写体であり、矢印で示される左方向へ移動しているとする。

dToFセンサ１２が出力する距離ヒストグラムは、物体８１に対応するピーク領域ｈ１と、物体８２に対応するピーク領域ｈ２の２つのピーク領域を含む。スポット光SPが物体８２に当たる領域が、物体８１に当たる領域よりも大きい場合には、物体８２に対応するピーク領域ｈ２のピークカウント値が、物体８１に対応するピーク領域ｈ１のピークカウント値よりも大きくなる。その後、物体８２の移動により、スポット光SPが物体８１に当たる領域が、物体８２に当たる領域よりも大きくなると、物体８１に対応するピーク領域ｈ１のピークカウント値が、物体８２に対応するピーク領域ｈ２のピークカウント値よりも大きくなる。また、図示は省略するが、スポット光SPが物体８１と物体８２の境界部分ではなく、物体８１または物体８２のいずれか一方のみに照射されている場合には、dToFセンサ１２が出力する距離ヒストグラムは、大きなピークカウント値を有する１つのピーク領域を有するヒストグラムとなる。

したがって、確認対象判定部７１は、複数フレームにわたる距離ヒストグラムを観測し、距離ヒストグラムのピーク領域数が「1→２→１」と変化する測距点を、確認対象の測距点であると判定する。なお、ピーク領域数が「1→２→１」と変化する測距点には、「背景→前景と背景の境界部分→前景」のように変化する場合と、「前景→前景と背景の境界部分→背景」のように変化する場合とがある。

図１３は、確認対象判定部７１による確認対象判定処理を説明する図である。

まず、確認対象判定部７１は、現在の時刻tのフレームFtにおける注目測距点Kxの距離ヒストグラムのピーク領域数が１つであるか、または２つ以上であるかを判定する。注目測距点Kxの距離ヒストグラムのピーク領域数が２つ以上である場合、その測距点は物体境界の可能性があり、位置ずれが起きている可能性の高い測距点として、注目測距点を確認対象の測距点であると判定する。

一方、注目測距点Kxの距離ヒストグラムのピーク領域数が１つである場合には、記憶部２４から過去Wフレーム（Wは２以上の整数。）の同一位置の測距点Kxの距離ヒストグラムを取得し、過去Wフレームにおける距離ヒストグラムのピーク領域数と、カウント値が最大のビン位置を確認する。過去Wフレームにおける距離ヒストグラムのピーク領域数が１つで、最大カウント値のビン位置も同じである場合には、確認対象判定部７１は、位置ずれが起きている可能性の低い測距点であり、注目測距点は確認対象の測距点ではないと判定する。

一方、過去Wフレームにおける距離ヒストグラムのピーク領域数が２つ以上で、最大カウント値のビン位置が変化している場合には、確認対象判定部７１は、位置ずれが起きている可能性の高い測距点として、注目測距点を確認対象の測距点であると判定する。

図１３の例では、現在の時刻tのフレームFtにおける注目測距点Kxの距離ヒストグラムのピーク領域数は１つであるため、記憶部２４から過去Wフレームの同一位置の測距点Kxの距離ヒストグラムが取得され、過去Wフレームにおける距離ヒストグラムのピーク領域数と、最大カウント値のビン位置が確認される。

いまW＝３とされ、過去３フレームにおける距離ヒストグラムのピーク領域数と、最大カウント値のビン位置が確認されることとすると、時刻ｔ－１のフレームFt-1および時刻ｔ－２のフレームFt-2のピーク領域数は２つであり、最大カウント値のビン位置が、時刻ｔ－２のフレームFt-2と時刻ｔ－１のフレームFt-1とで変化している。図１３に示される各フレームの距離ヒストグラムにおいて、最大カウント値のピークには丸印が付されている。

したがって、確認対象判定部７１は、現在の時刻tのフレームFtにおける注目測距点Kxを、位置ずれが起きている可能性の高い測距点であり、確認対象の測距点であると判定する。

なお、第２実施の形態においては、上述したように、過去Wフレームにわたる距離ヒストグラムの変化を観測することにより、注目測距点が物体境界近傍の領域であるかを判定し、確認対象の測距点であるか否かを判定するので、dToFセンサ１２の各マルチピクセルMPの位置は、撮影期間中の全フレームにおいて変化せず、各フレームで同一の位置をサンプリングする必要がある。

＜６．第２実施の形態による位置情報算出処理＞
第２実施の形態の信号処理システム１による位置情報算出処理は、第１実施の形態で説明した図１０のフローチャートのステップＳ４で行われる確認対象判定処理の判定方法が異なり、その他のステップＳ１ないしＳ３およびステップＳ５ないしＳ１４の処理は同一である。

図１０のフローチャートを参照して説明すると、図１０のステップＳ４において、確認対象判定部７１は、現在の時刻tのフレームFtにおける注目測距点Kxの距離ヒストグラムのピーク領域数が１つであるか、または２つ以上であるかを判定する。そして、注目測距点Kxの距離ヒストグラムのピーク領域数が２つ以上である場合、確認対象判定部７１は、注目測距点を確認対象の測距点であると判定する。

一方、現在の時刻tのフレームFtにおける注目測距点Kxの距離ヒストグラムのピーク領域数が１つである場合、確認対象判定部７１は、さらに、過去Wフレームにおける距離ヒストグラムのピーク領域数と、カウント値が最大のビン位置を確認する。過去Wフレームにおける距離ヒストグラムのピーク領域数が１つで、最大カウント値のビン位置も同じである場合には、確認対象判定部７１は、注目測距点Kxは確認対象の測距点ではないと判定する。一方、過去Wフレームにおける距離ヒストグラムのピーク領域数が２つで、最大カウント値のビン位置も入れ替わっている場合には、確認対象判定部７１は、注目測距点Kxは確認対象の測距点であると判定する。

以上説明した第２実施の形態に係る信号処理システム１の位置情報算出処理によれば、注目測距点の距離ヒストグラムのピーク領域数と、最大カウント値のビン位置が、過去Wフレームにおいて変化しているか否かにより、注目測距点が確認対象の測距点であるか否かが判定される。そして、注目測距点が確認対象の測距点であると判定された場合、注目測距点の距離ヒストグラムと、近傍の複数の測距点の距離ヒストグラムとの類似度（類似ベクトル）を計算することにより、注目測距点の位置が、距離ヒストグラムが類似する近傍の測距点の方に補正される。これにより、dToFセンサ１２の位置情報のずれを補正することができる。

なお、第２実施の形態において、RGBカメラ１１およびdToFセンサ１２が、位置を固定して被写体を撮影している場合、距離ヒストグラムのピーク領域数が「1→２→１」のように変化する測距点は動被写体を撮影した点であり、計算された３次元座標の信頼度が低いと予想される。そこで、RGBカメラ１１およびdToFセンサ１２の位置が固定の場合であって、距離ヒストグラムのピーク領域数が「1→２→１」のように変化する測距点については、３次元座標の測定値を削除し、信号処理装置１３から出力しないようにしてもよい。

あるいはまた、距離ヒストグラムのピーク領域数が「1→２→１」のように変化する測距点に信頼度を付加して出力するようにして、低信頼度の測距点であることが後段の装置で識別できるようにしてもよい。測距点の信頼度は、例えば、距離ヒストグラムのピーク領域数が「1→２→１」や、「1→２→２→１」、「1→２→２→２→１」のように、距離ヒストグラムのピークの入れ替わりにかかるフレーム数の逆数を１から引いた値で求めることができる。具体的には、例えば、距離ヒストグラムのピーク領域数が「1→２→１」と変化する測距点の信頼度は、「1-1/3＝0.6」と算出することができる。また例えば、距離ヒストグラムのピーク領域数が「1→２→２→１」と変化する測距点の信頼度は、「1-1/4＝0.75」と算出することができる。また例えば、距離ヒストグラムのピーク領域数が「1→２→２→２→１」と変化する測距点の信頼度は、「1-1/5＝0.8」と算出することができる。

信頼度が付加された測距点を取得した後段の装置では、低信頼度の測距点として認識することで、例えば、残像を防ぐような処理を行うことができる。

＜７．まとめ＞
信号処理装置１３は、測距センサの所定の測距点における照射光の飛行時間のヒストグラムデータである距離ヒストグラムを取得するデータ取得部２１と、所定の測距点の距離ヒストグラムから、所定の測距点の３次元座標を演算する距離演算部２２と、所定の測距点が位置ずれ確認対象の測距点であるか否かを判定する確認対象判定部３１と、所定の測距点が位置ずれ確認対象の測距点である場合に、所定の測距点の距離ヒストグラムと、所定の測距点の近傍の測距点の距離ヒストグラムとの類似度の判定結果に基づいて、距離演算部２２で演算された所定の測距点の３次元座標を補正する位置ずれ補正処理を実行する座標補正部３２とを備える。

第１実施の形態の確認対象判定部３１は、RGBカメラ１１が被写体を撮影したRGB画像（撮影画像）から物体のエッジ領域を検出し、所定の測距点がエッジ領域に含まれる場合に、位置ずれが起きている可能性の高い、確認対象の測距点であると判定する。

一方、第２実施の形態の確認対象判定部３１は、複数フレームにわたる測距点の距離ヒストグラムの変化を観測することにより、所定の測距点が位置ずれ確認対象の測距点であるか否かを判定する。例えば、確認対象判定部３１は、距離ヒストグラムのピーク領域数が「1→２→１」と変化し、カウント値が最大のビン位置が変化している場合に、確認対象の測距点であると判定する。

座標補正部３２は、確認対象の測距点の距離ヒストグラムと、その測距点の近傍の測距点の距離ヒストグラムとの類似度の重心ベクトルが、物体境界の方向と一致している場合に、測距点の３次元座標の補正が必要であると判定し、測距点の３次元座標を補正（移動）する。

信号処理装置１３の上述した確認対象判定処理および位置ずれ補正処理によれば、dToFセンサ１２の位置情報のずれを正しい方向に補正することができる。

信号処理装置１３は、上述した第１実施の形態または第２実施の形態のいずれか一方のみの構成および機能を備えるものでもよいし、両方の構成および機能を備え、例えば、第１実施の形態に対応した第１の動作モードと、第２実施の形態に対応した第２の動作モードとを切り替えることにより、いずれか一方の処理を選択的に行うものでもよい。

＜本技術の応用例＞
dToFセンサ１２から取得したヒストグラムデータを用いて３次元座標を補正して出力することが可能な本開示の位置情報算出処理は、自己位置推定と環境地図作成を同時に行うSLAM(Simultaneous Localization and Mapping)、物体を掴んで移動や作業を行うロボット動作、仮想のシーンや物体をCG（computer graphics）で生成する場合のCGモデリング、物体認識処理や物体分類処理、などの様々なアプリケーションの３次元測定に適用することができる。本開示の補正処理を適用することにより、物体の３次元座標の測定精度を向上させることができる。

＜８．コンピュータ構成例＞
上述した一連の処理は、ハードウエアにより実行することもできるし、ソフトウエアにより実行することもできる。一連の処理をソフトウエアにより実行する場合には、そのソフトウエアを構成するプログラムが、コンピュータにインストールされる。ここで、コンピュータには、専用のハードウエアに組み込まれているマイクロコンピュータや、各種のプログラムをインストールすることで、各種の機能を実行することが可能な、例えば汎用のパーソナルコンピュータなどが含まれる。

図１４は、上述した一連の処理をプログラムにより実行するコンピュータのハードウエアの構成例を示すブロック図である。

コンピュータにおいて、CPU（Central Processing Unit）１０１，ROM（Read Only Memory）１０２，RAM（Random Access Memory）１０３は、バス１０４により相互に接続されている。

バス１０４には、さらに、入出力インタフェース１０５が接続されている。入出力インタフェース１０５には、入力部１０６、出力部１０７、記憶部１０８、通信部１０９、及びドライブ１１０が接続されている。

入力部１０６は、キーボード、マウス、マイクロホン、タッチパネル、入力端子などよりなる。出力部１０７は、ディスプレイ、スピーカ、出力端子などよりなる。記憶部１０８は、ハードディスク、RAMディスク、不揮発性のメモリなどよりなる。通信部１０９は、ネットワークインタフェースなどよりなる。ドライブ１１０は、磁気ディスク、光ディスク、光磁気ディスク、或いは半導体メモリなどのリムーバブル記録媒体１１１を駆動する。

以上のように構成されるコンピュータでは、CPU１０１が、例えば、記憶部１０８に記憶されているプログラムを、入出力インタフェース１０５及びバス１０４を介して、RAM１０３にロードして実行することにより、上述した一連の位置情報算出処理が行われる。RAM１０３にはまた、CPU１０１が各種の処理を実行する上において必要なデータなども適宜記憶される。

コンピュータ（CPU１０１）が実行するプログラムは、例えば、パッケージメディア等としてのリムーバブル記録媒体１１１に記録して提供することができる。また、プログラムは、ローカルエリアネットワーク、インターネット、デジタル衛星放送といった、有線または無線の伝送媒体を介して提供することができる。

コンピュータでは、プログラムは、リムーバブル記録媒体１１１をドライブ１１０に装着することにより、入出力インタフェース１０５を介して、記憶部１０８にインストールすることができる。また、プログラムは、有線または無線の伝送媒体を介して、通信部１０９で受信し、記憶部１０８にインストールすることができる。その他、プログラムは、ROM１０２や記憶部１０８に、あらかじめインストールしておくことができる。

なお、コンピュータが実行するプログラムは、本明細書で説明する順序に沿って時系列に処理が行われるプログラムであっても良いし、並列に、あるいは呼び出しが行われたとき等の必要なタイミングで処理が行われるプログラムであっても良い。

本明細書において、システムとは、複数の構成要素（装置、モジュール（部品）等）の集合を意味し、すべての構成要素が同一筐体中にあるか否かは問わない。したがって、別個の筐体に収納され、ネットワークを介して接続されている複数の装置、及び、１つの筐体の中に複数のモジュールが収納されている１つの装置は、いずれも、システムである。

また、本開示の実施の形態は、上述した実施の形態に限定されるものではなく、本開示の技術の要旨を逸脱しない範囲において種々の変更が可能である。

なお、本明細書に記載された効果はあくまで例示であって限定されるものではなく、本明細書に記載されたもの以外の効果があってもよい。

なお、本開示の技術は、以下の構成を取ることができる。
（１）
測距センサの所定の測距点における照射光の飛行時間のヒストグラムデータである距離ヒストグラムを取得する取得部と、
前記所定の測距点が位置ずれ確認対象の測距点であるか否かを判定する判定部と、
前記所定の測距点が位置ずれ確認対象の測距点である場合に、前記所定の測距点の前記距離ヒストグラムと、前記所定の測距点の近傍の測距点の前記距離ヒストグラムとの類似度の判定結果に基づいて、前記距離ヒストグラムから演算された前記所定の測距点の３次元座標を補正する補正処理を実行する補正部と
を備える信号処理装置。
（２）
前記補正部は、前記所定の測距点の前記距離ヒストグラムと、前記所定の測距点の近傍の測距点の前記距離ヒストグラムとの類似度の重心ベクトルが、物体境界の方向と一致していると判定した場合に、前記補正処理を実行する
前記（１）に記載の信号処理装置。
（３）
前記補正部は、前記類似度の重心ベクトルが、物体境界の方向と一致していると判定した場合に、前記物体境界をまたぐ位置に、前記所定の測距点の３次元座標を補正する
前記（２）に記載の信号処理装置。
（４）
前記補正部は、前記所定の測距点の前記距離ヒストグラムと、前記所定の測距点の近傍の複数の測距点の前記距離ヒストグラムそれぞれとの前記類似度とから、前記類似度の重心ベクトルを計算する
前記（２）に記載の信号処理装置。
（５）
前記補正部は、前記所定の測距点の前記距離ヒストグラムと、前記近傍の１つの測距点の前記距離ヒストグラムとの類似度を、前記所定の測距点の位置ベクトルと前記近傍の１つの測距点の位置ベクトルの差分と、前記所定の測距点の前記距離ヒストグラムと前記近傍の１つの測距点の前記距離ヒストグラムとの相関係数とから計算する
前記（４）に記載の信号処理装置。
（６）
前記補正部は、前記所定の測距点の前記距離ヒストグラムと、前記近傍の１つの測距点の前記距離ヒストグラムとの類似度を、前記所定の測距点の前記距離ヒストグラムと、前記近傍の１つの測距点の前記距離ヒストグラムのそれぞれを近似した確率密度関数どうしの距離を用いて計算する
前記（４）に記載の信号処理装置。
（７）
前記判定部は、前記距離ヒストグラムが示す前記所定の測距点の３次元座標が、物体境界近傍の領域か否かを判定し、前記所定の測距点の３次元座標が物体境界近傍の領域である場合に、前記所定の測距点が位置ずれ確認対象の測距点であると判定する
前記（１）乃至（６）のいずれかに記載の信号処理装置。
（８）
前記判定部は、被写体を撮影した撮影画像に含まれる物体のエッジ領域を検出し、前記所定の測距点の３次元座標が前記エッジ領域に含まれる場合に、前記所定の測距点の３次元座標が前記物体境界近傍の領域であると判定する
前記（７）に記載の信号処理装置。
（９）
前記取得部は、前記測距センサと同一範囲を撮影した前記撮影画像も取得し、
前記判定部は、前記撮影画像に含まれる物体のエッジ領域を検出する
前記（８）に記載の信号処理装置。
（１０）
前記判定部は、被写体を撮影した撮影画像で動きが検出された領域をエッジ領域として検出し、前記所定の測距点の３次元座標が前記エッジ領域に含まれる場合に、前記所定の測距点の３次元座標が前記物体境界近傍の領域であると判定する
前記（７）に記載の信号処理装置。
（１１）
前記判定部は、複数フレームにわたる前記距離ヒストグラムの変化を観測することにより、前記所定の測距点の３次元座標が前記物体境界近傍の領域か否かを判定する
前記（７）に記載の信号処理装置。
（１２）
前記判定部は、前記複数フレームにおいて前記距離ヒストグラムのピーク領域数が変化する場合に、前記所定の測距点の３次元座標が前記物体境界近傍の領域であると判定する
前記（１１）に記載の信号処理装置。
（１３）
前記判定部は、前記複数フレームにおいて前記距離ヒストグラムの最大カウント値のビン位置が変化している場合に、前記所定の測距点の３次元座標が前記物体境界近傍の領域であると判定する
前記（１１）または（１２）に記載の信号処理装置。
（１４）
前記所定の測距点の前記距離ヒストグラムから、前記所定の測距点の３次元座標を演算する演算部をさらに備える
前記（１）乃至（１３）のいずれかに記載の信号処理装置。
（１５）
信号処理装置が、
測距センサの所定の測距点における照射光の飛行時間のヒストグラムデータである距離ヒストグラムを取得し、
前記所定の測距点が位置ずれ確認対象の測距点であるか否かを判定し、
前記所定の測距点が位置ずれ確認対象の測距点である場合に、前記所定の測距点の前記距離ヒストグラムと、前記所定の測距点の近傍の測距点の前記距離ヒストグラムとの類似度の判定結果に基づいて、前記距離ヒストグラムから演算された前記所定の測距点の３次元座標を補正する補正処理を実行する
信号処理方法。

１ 信号処理システム， １１ RGBカメラ， １２ dToFセンサ， １３ 信号処理装置， ２１ データ取得部， ２２ 距離演算部， ２３ 補正処理部， ２４ 記憶部， ２５ 出力部， ３１ 確認対象判定部， ３２ 座標補正部， ７１ 確認対象判定部， １０１ CPU， １０２ ROM， １０４ バス， １０５ 入出力インタフェース， １０６ 入力部， １０７ 出力部， １０８ 記憶部， １０９ 通信部， １１０ ドライブ， １１１ リムーバブル記録媒体

Claims (10)

1. 測距センサの所定の測距点における照射光の飛行時間のヒストグラムデータである距離ヒストグラムと、前記測距センサと同一範囲を撮影した撮影画像を取得する取得部と、
   前記所定の測距点が前記撮影画像の物体のエッジから所定幅のエッジ領域内であるかを判定することにより、前記所定の測距点が位置ずれ確認対象の測距点であるか否かを判定する第１判定部と、
   前記所定の測距点が位置ずれ確認対象の測距点である場合に、前記所定の測距点の前記距離ヒストグラムと、前記所定の測距点の近傍の測距点の前記距離ヒストグラムとの類似度を求め、前記所定の測距点の位置ベクトルと前記近傍の測距点の位置ベクトルとの差分に前記類似度を乗算して類似ベクトルを求める演算を、前記所定の測距点の近傍の複数の測距点について行い、前記複数の測距点の前記類似ベクトルの重心ベクトルを求め、前記重心ベクトルと、前記所定の測距点の位置から前記エッジの最短位置へ向かうベクトルとがなす角度を計算し、前記角度が所定の閾値より大きいか否かを判定する第２判定部と、
   前記角度が所定の閾値内である場合に、前記所定の測距点が前記エッジをまたぐように前記所定の測距点の３次元座標を補正する補正処理を実行する補正部と
   を備える信号処理装置。
2. 測距センサの所定の測距点における照射光の飛行時間のヒストグラムデータである距離ヒストグラムと、前記測距センサと同一範囲を撮影した撮影画像を取得する取得部と、
   前記所定の測距点が測距対象物体のエッジから所定幅のエッジ領域内であるかを判定することにより、前記所定の測距点が位置ずれ確認対象の測距点であるか否かを判定する第１判定部と、
   前記所定の測距点が位置ずれ確認対象の測距点である場合に、前記所定の測距点の前記距離ヒストグラムと、前記所定の測距点の近傍の測距点の前記距離ヒストグラムを確率密度関数に近似し、前記所定の測距点と前記近傍の測距点のそれぞれを、ｘ軸を前記確率密度関数の平均、ｙ軸を前記確率密度関数の分散、ｚ軸を前記距離ヒストグラムのピークカウント値として非ユークリッド空間にマッピングして前記非ユークリッド空間上の疑似距離を計算し、前記疑似距離を用いて前記所定の測距点と前記近傍の測距点の類似ベクトルを計算することを前記所定の測距点の近傍の複数の測距点について行い、前記複数の測距点の前記類似ベクトルの重心ベクトルを求め、前記重心ベクトルと、前記所定の測距点の位置から前記エッジの最短位置へ向かうベクトルとがなす角度を計算し、前記角度が所定の閾値より大きいか否かを判定する第２判定部と、
   前記角度が所定の閾値内である場合に、前記所定の測距点が前記エッジをまたぐように前記所定の測距点の３次元座標を補正する補正処理を実行する補正部と
   を備える信号処理装置。
3. 前記第１判定部は、前記撮影画像に含まれる前記物体のエッジを検出し、前記エッジ領域を決定する
   請求項１または２に記載の信号処理装置。
4. 前記第１判定部は、前記撮影画像で動きが検出された領域を前記エッジ領域として検出する
   請求項１または２に記載の信号処理装置。
5. 前記第１判定部は、前記エッジ領域内であるかの判定に代えて、複数フレームにわたる前記距離ヒストグラムの変化を観測して前記所定の測距点の３次元座標が前記物体境界近傍の領域か否かを判定することにより、前記所定の測距点が位置ずれ確認対象の測距点であるか否かを判定する
   請求項１または２に記載の信号処理装置。
6. 前記第１判定部は、前記複数フレームにおいて前記距離ヒストグラムのピーク領域数が変化する場合に、前記所定の測距点の３次元座標が前記物体境界近傍の領域であると判定する
   請求項５に記載の信号処理装置。
7. 前記第１判定部は、前記複数フレームにおいて前記距離ヒストグラムの最大カウント値のビン位置が変化している場合に、前記所定の測距点の３次元座標が前記物体境界近傍の領域であると判定する
   請求項５に記載の信号処理装置。
8. 前記所定の測距点の前記距離ヒストグラムから、前記所定の測距点の３次元座標を演算する演算部をさらに備える
   請求項１または２に記載の信号処理装置。
9. 信号処理装置が、
   測距センサの所定の測距点における照射光の飛行時間のヒストグラムデータである距離ヒストグラムと、前記測距センサと同一範囲を撮影した撮影画像を取得することと、
   前記所定の測距点が前記撮影画像の物体のエッジから所定幅のエッジ領域内であるかを判定することにより、前記所定の測距点が位置ずれ確認対象の測距点であるか否かを判定することと、
   前記所定の測距点が位置ずれ確認対象の測距点である場合に、前記所定の測距点の前記距離ヒストグラムと、前記所定の測距点の近傍の測距点の前記距離ヒストグラムとの類似度を求め、前記所定の測距点の位置ベクトルと前記近傍の測距点の位置ベクトルとの差分に前記類似度を乗算して類似ベクトルを求める演算を、前記所定の測距点の近傍の複数の測距点について行い、前記複数の測距点の前記類似ベクトルの重心ベクトルを求め、前記重心ベクトルと、前記所定の測距点の位置から前記エッジの最短位置へ向かうベクトルとがなす角度を計算し、前記角度が所定の閾値より大きいか否かを判定することと、
   前記角度が所定の閾値内である場合に、前記所定の測距点が前記エッジをまたぐように前記所定の測距点の３次元座標を補正する補正処理を実行することと
   を含む信号処理方法。
10. 信号処理装置が、
    測距センサの所定の測距点における照射光の飛行時間のヒストグラムデータである距離ヒストグラムと、前記測距センサと同一範囲を撮影した撮影画像を取得するすることと、
    前記所定の測距点が測距対象物体のエッジから所定幅のエッジ領域内であるかを判定することにより、前記所定の測距点が位置ずれ確認対象の測距点であるか否かを判定するすることと、
    前記所定の測距点が位置ずれ確認対象の測距点である場合に、前記所定の測距点の前記距離ヒストグラムと、前記所定の測距点の近傍の測距点の前記距離ヒストグラムを確率密度関数に近似し、前記所定の測距点と前記近傍の測距点のそれぞれを、ｘ軸を前記確率密度関数の平均、ｙ軸を前記確率密度関数の分散、ｚ軸を前記距離ヒストグラムのピークカウント値として非ユークリッド空間にマッピングして前記非ユークリッド空間上の疑似距離を計算し、前記疑似距離を用いて前記所定の測距点と前記近傍の測距点の類似ベクトルを計算することを前記所定の測距点の近傍の複数の測距点について行い、前記複数の測距点の前記類似ベクトルの重心ベクトルを求め、前記重心ベクトルと、前記所定の測距点の位置から前記エッジの最短位置へ向かうベクトルとがなす角度を計算し、前記角度が所定の閾値より大きいか否かを判定するすることと、
    前記角度が所定の閾値内である場合に、前記所定の測距点が前記エッジをまたぐように前記所定の測距点の３次元座標を補正する補正処理を実行するすることと
    を含む信号処理方法。