特開 2022-186523 ３次元点群処理装置、３次元点群処理方法、及び３次元点群処理プログラム

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

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

(11)【公開番号】P2022186523

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

(54)【発明の名称】３次元点群処理装置、３次元点群処理方法、及び３次元点群処理プログラム

(51)【国際特許分類】

   G06T 7/50 20170101AFI20221208BHJP

【ＦＩ】

G06T7/50

【審査請求】未請求

【請求項の数】8

【出願形態】ＯＬ

(21)【出願番号】P 2021094793

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

(71)【出願人】

【識別番号】000004226

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

(71)【出願人】

【識別番号】504137912

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

(74)【代理人】

【識別番号】110001519

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

(72)【発明者】

【氏名】八尾 泰洋

(72)【発明者】

【氏名】安藤 慎吾

(72)【発明者】

【氏名】島村 潤

(72)【発明者】

【氏名】石川 涼一

(72)【発明者】

【氏名】大石 岳史

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

5L096

【Ｆターム（参考）】

5L096AA02

5L096AA03

5L096AA06

5L096AA09

5L096CA05

5L096DA05

5L096FA66

5L096FA69

5L096GA07

(57)【要約】

【課題】計測して得られた３次元点群を精度よくアップサンプリングする。
【解決手段】入力処理部２０が、少なくとも撮影位置の関係が予め求められている第１画像及び第２画像と、少なくとも撮影位置と計測位置との関係が予め求められている物体の表面上の３次元点群とを受け付け、３次元点群の３次元点の各々に対応する第１画像上の画素位置を求める。近傍選択部２４が、３次元点群の３次元点の各々に対応する第１画像上の画素位置に基づいて、第１画像上の画素位置の各々について、近傍の複数の画素位置に対応する３次元点の奥行値から、第１画像及び第２画像の間の整合性が高く、かつ近傍の画素位置との距離が近くなるように、奥行値を選択する。
【選択図】図３

【特許請求の範囲】

【請求項1】

少なくとも撮影位置の関係が予め求められている第１画像及び第２画像と、少なくとも前記撮影位置と計測位置との関係が予め求められている物体の表面上の３次元点群とを受け付け、前記３次元点群の３次元点の各々に対応する前記第１画像上の画素位置を求める入力処理部と、
前記３次元点群の３次元点の各々に対応する前記第１画像上の画素位置に基づいて、前記第１画像上の画素位置の各々について、近傍の複数の画素位置に対応する３次元点の奥行値から、前記第１画像及び前記第２画像の間の整合性が高くなるように、前記奥行値を選択する近傍選択部と、
を含む３次元点群処理装置。

【請求項2】

前記近傍選択部は、
前記第１画像上の画素位置の各々について、
前記近傍の複数の画素位置に対応する３次元点の各々に対し、前記第１画像上の画素位置及び前記３次元点の奥行値からなる点を、前記第２画像上に投影し、前記第１画像上の画素位置の画素値と、前記第２画像上に投影した点の画素位置の画素値との距離を求め、
前記近傍の複数の画素位置に対応する３次元点の各々に対し求められた、前記第１画像上の画素位置の画素値と、前記第２画像上に投影した点の画素位置の画素値との距離を用いて表されるコスト関数に基づいて、前記近傍の複数の画素位置に対応する３次元点の奥行値を選択する請求項１記載の３次元点群処理装置。

【請求項3】

前記近傍選択部は、
前記第１画像上の画素位置の各々について、
前記コスト関数及び隣接する画素位置において選択される奥行値との差分を用いて表されるエネルギー関数に基づいて、前記奥行値を選択する請求項２記載の３次元点群処理装置。

【請求項4】

前記第１画像上の画素位置の各々について選択された前記奥行値を平滑化する平滑化部を更に含む請求項１～請求項３の何れか１項記載の３次元点群処理装置。

【請求項5】

前記近傍選択部は、前記３次元点群の３次元点の各々に対応する前記第１画像上の画素位置に基づいて、前記第１画像上の画素位置の各々について、近傍の複数の画素位置に対応する３次元点の奥行値から、前記第１画像及び前記第２画像の間の整合性が高く、かつ前記近傍の画素位置との距離が近くなるように、前記奥行値を選択する請求項１～請求項４の何れか１項記載の３次元点群処理装置。

【請求項6】

前記入力処理部は、
前記３次元点群の各々の３次元点を、前記第１画像上に投影し、前記第１画像の領域外に投影された３次元点を除去し、除去後の前記３次元点群の３次元点の各々に対応する前記第１画像上の画素位置及び奥行値の組み合わせである要素からなる集合を求め、
前記集合の各要素の点を、前記第２画像上に投影し、前記第２画像の領域外に投影された要素を除去し、除去後の前記集合に更新し、
前記近傍選択部は、
前記集合の要素の前記第１画像上の画素位置に基づいて、前記第１画像上の画素位置の各々について、近傍の複数の画素位置を持つ、前記集合の要素の奥行値から、前記第１画像及び前記第２画像の間の整合性が高く、かつ前記近傍の画素位置との距離が近くなるように、前記集合の要素の奥行値を選択する
請求項１～請求項５の何れか１項記載の３次元点群処理装置。

【請求項7】

入力処理部が、少なくとも撮影位置の関係が予め求められている第１画像及び第２画像と、少なくとも前記撮影位置と計測位置との関係が予め求められている物体の表面上の３次元点群とを受け付け、前記３次元点群の３次元点の各々に対応する前記第１画像上の画素位置を求め、
近傍選択部が、前記３次元点群の３次元点の各々に対応する前記第１画像上の画素位置に基づいて、前記第１画像上の画素位置の各々について、近傍の複数の画素位置に対応する３次元点の奥行値から、前記第１画像及び前記第２画像の間の整合性が高くなるように、前記奥行値を選択する
３次元点群処理方法。

【請求項8】

コンピュータを、請求項１～請求項６の何れか１項に記載の３次元点群処理装置として機能させるための３次元点群処理プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

開示の技術は、３次元点群処理装置、３次元点群処理方法、及び３次元点群処理プログラムに関する。

【背景技術】

【0002】

非特許文献１には、ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ）とステレオ画像により、ＬｉＤＡＲによって疎な点群をステレオ画像の視差推定結果に基づいてアップサンプリングした深度画像を出力する技術が開示されている。

【0003】

非特許文献２には、教師ありの深層学習によってＬｉＤＡＲとステレオ画像からＬｉＤＡＲ点群をアップサンプリングした深度画像を出力する技術が開示されている。

【0004】

非特許文献３には、半教師ありの深層学習によってＬｉＤＡＲとステレオ画像からＬｉＤＡＲ点群をアップサンプリングした深度画像を出力する技術が開示されている。非特許文献３に開示の技術では投影誤りがある場合でもその影響を除去して推論をすることが可能である。

【先行技術文献】

【非特許文献】

【0005】

【非特許文献1】Maddern, Will, and Paul Newman. "Real-time probabilistic fusion of sparse 3D LIDAR and dense stereo." 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2016.

【非特許文献2】Park, Kihong, Seungryong Kim, and Kwanghoon Sohn. "High-precision depth estimation with the 3d lidar and stereo fusion." 2018 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2018.

【非特許文献3】Cheng, Xuelian, et al. "Noise-aware unsupervised deep lidar-stereo fusion." Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition. 2019.

【発明の概要】

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

【0006】

非特許文献１の問題点は、投影されたＬｉＤＡＲ点を正しい奥行値とみなして処理をすることであり、投影誤りがある場合を考慮していないことである。

【0007】

非特許文献２の問題点は、非特許文献１と同様、投影誤りがデータにあることを考慮していないこと、および学習のために高密度な深度画像を必要とするがそのようなデータを得ることが容易ではないことである。

【0008】

非特許文献３の問題点は、学習時に正しくレジストレーションされたＬｉＤＡＲ計測とステレオ画像の組が多量に必要となることであり、そのようなデータを得ることは容易ではない。

【0009】

上述したように、非特許文献１、２の問題は「ＬｉＤＡＲデータの投影誤りを考慮していない」という点である。また、非特許文献３の問題は「多量の学習データを必要とする」という点である。

【0010】

さらに、非特許文献１～３はいずれも「視差推定をした結果によってアップサンプリングしたデータを作成する」という処理になっている。ステレオによる視差推定は長距離での距離計測精度が落ちる。これに対してＬｉＤＡＲは長距離でも距離計測精度が高いが、その特性を生かすことができていない。

【0011】

開示の技術は、上記の点に鑑みてなされたものであり、計測して得られた３次元点群を精度よくアップサンプリングすることができる３次元点群処理装置、３次元点群処理方法、及び３次元点群処理プログラムを提供することを目的とする。

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

【0012】

本開示の第１態様は、３次元点群処理装置であって、少なくとも撮影位置の関係が予め求められている第１画像及び第２画像と、少なくとも前記撮影位置と計測位置との関係が予め求められている物体の表面上の３次元点群とを受け付け、前記３次元点群の３次元点の各々に対応する前記第１画像上の画素位置を求める入力処理部と、前記３次元点群の３次元点の各々に対応する前記第１画像上の画素位置に基づいて、前記第１画像上の画素位置の各々について、近傍の複数の画素位置に対応する３次元点の奥行値から、前記第１画像及び前記第２画像の間の整合性が高くなるように、前記奥行値を選択する近傍選択部と、を含む。

【0013】

本開示の第２態様は、３次元点群処理方法であって、入力処理部が、少なくとも撮影位置の関係が予め求められている第１画像及び第２画像と、少なくとも前記撮影位置と計測位置との関係が予め求められている物体の表面上の３次元点群とを受け付け、前記３次元点群の３次元点の各々に対応する前記第１画像上の画素位置を求め、近傍選択部が、前記３次元点群の３次元点の各々に対応する前記第１画像上の画素位置に基づいて、前記第１画像上の画素位置の各々について、近傍の複数の画素位置に対応する３次元点の奥行値から、前記第１画像及び前記第２画像の間の整合性が高くなるように、前記奥行値を選択する。

【0014】

本開示の第３態様は、３次元点群処理プログラムであって、コンピュータを、上記第１態様の３次元点群処理装置として機能させるためのプログラムである。

【発明の効果】

【0015】

開示の技術によれば、計測して得られた３次元点群を精度よくアップサンプリングすることができる。

【図面の簡単な説明】

【0016】

【図1】本実施形態の３次元点群処理装置として機能するコンピュータの一例の概略ブロック図である。

【図2】ＬｉＤＡＲセンサによる計測点と、第１カメラ及び第２カメラの撮影シーンとの一例を示す図である。

【図3】本実施形態の３次元点群処理装置の構成を示すブロック図である。

【図4】（Ａ）第１画像の一例を示す図、（Ｂ）第２画像の一例を示す図、及び（Ｃ）第１画像に３次元点群を投影した結果の一例を示す図である。

【図5】近傍集合を抽出する方法を説明するための図である。

【図6】本実施形態の３次元点群処理装置の３次元点群処理ルーチンを示すフローチャートである。

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

【0017】

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

【0018】

＜本実施形態の概要＞
本実施形態では、ＬｉＤＡＲによる計測で得られた疎な３次元点群について、カメラにより撮影された第一画像と第二画像を手掛かりとしてアップサンプリングをする。

【0019】

具体的には、ＬｉＤＡＲの投影を正しいとせずに、画素の周囲に投影された３次元点から、コスト関数と平滑化項とからなるエネルギー関数に基づいて最適である奥行値を選択するという手法により、学習データを必要とせず、ＬｉＤＡＲの投影情報に誤りがある場合でも、ＬｉＤＡＲによって計測された３次元点群を正しくアップサンプリングするものである。

【0020】

さらに、本実施形態は、ＬｉＤＡＲで計測した３次元点群を画素に割り当てる形でアップサンプリングをするため、視差推定を実施する手法とは異なり、長距離において計測精度が維持されることを特徴としている。

【0021】

このように、本実施形態では、学習データを用いないこと、ＬｉＤＡＲとカメラ間に、車両の移動やセンサ位置の違いから生じるオクルージョンによる投影誤りがある場合でも正しくアップサンプリングが可能なこと、画像の視差による距離推定をせずＬｉＤＡＲで計測された３次元点群をアップサンプリングするので長距離での精度低下が抑えられることを特徴とする。

【0022】

＜本実施形態に係る３次元点群処理装置の構成＞
図１は、本実施形態の３次元点群処理装置１０のハードウェア構成を示すブロック図である。

【0023】

図１に示すように、３次元点群処理装置１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１１、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）１２、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）１３、ストレージ１４、入力部１５、表示部１６及び通信インタフェース（Ｉ／Ｆ）１７を有する。各構成は、バス１９を介して相互に通信可能に接続されている。

【0024】

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

【0025】

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

【0026】

入力部１５は、マウス等のポインティングデバイス、及びキーボードを含み、少なくとも撮影位置の関係が予め求められている第１画像及び第２画像と、少なくとも撮影位置と計測位置との関係が予め求められている物体の表面上の３次元点群Ｐとを含む各種の入力を行うために使用される。例えば、入力部１５には、図２に示すような、ＬｉＤＡＲセンサ５０によって計測された３次元点群Ｐと、第１カメラ５２によって撮影された第１画像（図４（Ａ）参照）と、第２カメラ５４によって撮影された第２画像（図４（Ｂ）参照）と、が入力される。第１カメラ５２と第２カメラ５４との撮影位置の関係が予め求められており、第１カメラ５２の撮影位置とＬｉＤＡＲセンサ５０の計測位置との関係が予め求められている。

【0027】

第１画像は、歪み補正されたＲＧＢもしくはグレースケールの画像であり、第２画像は、歪み補正されたＲＧＢもしくはグレースケールの画像である。３次元点群Ｐは、ＬｉＤＡＲセンサ５０によって計測された３次元点の集合である。一つ一つの３次元点は、３次元のベクトルであり、３次元点群Ｐに、３次元点がＮ点含まれる場合には、３次元点群ＰはＮ個の要素を持つ３次元のベクトルの集合となる。

【0028】

また、入力部１５には、第１カメラ５２の内部パラメータＫ＿１、第２カメラ５４の内部パラメータＫ＿２、第１カメラ５２と第２カメラ５４間の回転行列Ｒ＿Ｃ、第１カメラ５２と第２カメラ５４間の並進ベクトルＴ＿Ｃ、第１カメラ５２とＬｉＤＡＲセンサ５０間の投影行列Ｒ＿Ｌ、及び第１カメラ５２とＬｉＤＡＲセンサ５０間の並進ベクトルＴ＿Ｌが入力される。

【0029】

第１カメラ５２及び第２カメラ５４の内部パラメータＫ＿１、Ｋ＿２は、３×３のカメラ内部パラメータ行列である。第１カメラ５２と第２カメラ５４間の回転行列Ｒ＿Ｃは、３×３の回転行列である。第１カメラ５２と第２カメラ５４間の並進ベクトルＴ＿Ｃは、３次元のベクトルである。第１カメラ５２とＬｉＤＡＲセンサ５０間の投影行列Ｒ＿Ｌは、３×３の回転行列である。第１カメラ５２とＬｉＤＡＲセンサ５０間の並進ベクトルＴ＿Ｌは、３次元のベクトルである。

【0030】

表示部１６は、例えば、液晶ディスプレイであり、ＬｉＤＡＲセンサ５０によって計測された３次元点群Ｐに対してアップサンプリングした結果を含む各種の情報を表示する。表示部１６は、タッチパネル方式を採用して、入力部１５として機能しても良い。

【0031】

通信インタフェース１７は、他の機器と通信するためのインタフェースであり、例えば、イーサネット（登録商標）、ＦＤＤＩ、Ｗｉ－Ｆｉ（登録商標）等の規格が用いられる。

【0032】

次に、３次元点群処理装置１０の機能構成について説明する。図３は、３次元点群処理装置１０の機能構成の例を示すブロック図である。

【0033】

３次元点群処理装置１０は、機能的には、図３に示すように、入力処理部２０、近傍抽出部２２、近傍選択部２４、及び平滑化部２６を備えている。

【0034】

入力処理部２０は、入力部１５により受け付けた、第１画像及び第２画像と、３次元点群Ｐとに基づいて、３次元点群Ｐの各々の３次元点を第１画像上に投影し、各３次元点に対応する第１画像上の画素位置を求める（図４（Ｃ）参照）。図４（Ｃ）では、ドットで表される物体の表面上の３次元点の一つが、第１画像上の白い物体を表す領域内の画素位置に誤って投影されている例を示している。

【0035】

具体的には、３次元点群Ｐの各々の３次元点を、第１画像上に投影し、第１画像の領域外に投影された３次元点を３次元点群Ｐから除去する。除去後の３次元点群Ｐの各々の３次元点に対応する第１画像上の画素位置及び奥行値の組み合わせである要素からなる集合Ｑ＿１を求める。集合Ｑ＿１の各要素が示す点を、第２画像上に投影し、第２画像の領域外に投影された点を示す要素を集合Ｑ＿１から除去し、除去後の集合Ｑ＿１に更新する。

【0036】

例えば、３次元点群Ｐに含まれる３次元点それぞれについて、以下の式に従って投影行列Ｒ＿Ｌ、並進ベクトルＴ＿Ｌを適用して３次元点群Ｐ＿１を求める。３次元点群Ｐ＿１はＮ個の３次元ベクトルの集合である。

【0037】

Ｐ＿１＝Ｒ＿Ｌ Ｐ＋Ｔ＿Ｌ

【0038】

３次元点群Ｐ＿１を内部パラメータＫ＿１により第１カメラ５２の第１画像上に投影し、第１画像の領域外に投影されたものを除去し、（第１画像中のｘ座標、第１画像中のｙ座標、３次元点の奥行値）からなる要素の集合Ｑ＿１を得る。例えば、３次元点ｐ（∈Ｐ＿１）の各々について以下の式に従って計算される（ｘ，ｙ，ｄ）を集合Ｑ＿１の要素とする。
（Ｘ，Ｙ，ｄ）＝Ｋ＿１ ｐ
ｘ＝Ｘ／ｄ
ｙ＝Ｙ／ｄ
第１画像の領域内に投影された点の数をＭ＿１と表記する。なお、集合Ｑ＿１の各要素と変換前の３次元点群Ｐの各３次元点との対応付けは保持されている。

【0039】

そして、３次元点群Ｐ＿１に含まれる３次元点それぞれについて、以下の式に従って投影行列Ｒ＿Ｃ、並進ベクトルＴ＿Ｃを適用して３次元点群Ｐ＿２を求める。３次元点群Ｐ＿２はＮ個の３次元ベクトルの集合である。

【0040】

Ｐ＿２＝Ｒ＿Ｃ Ｐ＿１＋Ｔ＿Ｃ

【0041】

３次元点群Ｐ＿２を内部パラメータＫ＿２により第２カメラ５４の第２画像上に投影し、３次元点群Ｐ＿２から第２画像の領域外に投影されたものを除去し、（第２画像中のｘ座標、第２画像中のｙ座標、３次元点の奥行値）からなる要素の集合Ｑ＿２を得る。第２画像の領域内に投影された点の数をＭ＿２と表記する。なお、集合Ｑ＿２の各要素と変換前の３次元点群Ｐの各要素との対応付けは保持されている。

【0042】

集合Ｑ＿１のうち、集合Ｑ＿２にも含まれている要素を、３次元点群Ｐとの対応付けを利用して抽出し、集合Ｑ＿１及び集合Ｑ＿２の両方に含まれている要素からなる集合となるように、集合Ｑ＿１を更新する。

【0043】

近傍抽出部２２は、３次元点群Ｐの各々の３次元点に対応する第１画像上の画素位置に基づいて、第１画像上の画素位置の各々について、近傍の複数の画素位置に対応する３次元点の奥行値を抽出する。

【0044】

具体的には、第１画像の画素について、集合Ｑ＿１に含まれる要素のうち、近傍の画素位置を持つ要素を抽出する。これにより、各画素位置（ｘ，ｙ）について近傍集合Ｑ＿１＿ｘｙが抽出される。各画素位置（ｘ，ｙ）の近傍集合Ｑ＿１＿ｘｙは、集合Ｑ＿１の部分集合である。また、近傍の画素位置を持つ要素が抽出されなかった画素位置（ｘ，ｙ）については、近傍集合Ｑ＿１＿ｘｙは空集合とする。

【0045】

なお、近傍の定義は任意であるが、例えば、画素位置からある半径以内に存在すること（図５）、又は画素位置からＫ近傍であることを、近傍としてもよい。図５では、注目点である画素位置から探索半径以内に存在する画素位置を有する要素を、近傍集合Ｑ＿１＿ｘｙとして抽出する例を示している。

【0046】

後述する信念伝播部３２において、画素位置（ｘ，ｙ）には近傍集合Ｑ＿１＿ｘｙに含まれる何れかの要素の奥行値が割り当てられる。

【0047】

近傍選択部２４は、第１画像上の画素位置（ｘ，ｙ）の各々について、近傍集合Ｑ＿１＿ｘｙの要素の奥行値から、第１画像と第２画像との間の整合性が高く、かつ当該画素位置（ｘ，ｙ）と当該要素の画素位置との距離が近くなるように、奥行値を選択する。

【0048】

具体的には、近傍選択部２４は、コスト計算部３０及び信念伝播部３２を備えている。

【0049】

コスト計算部３０は、第１画像上の画素位置（ｘ，ｙ）毎に、近傍集合Ｑ＿１＿ｘｙに含まれる要素の各々について、当該画素位置（ｘ，ｙ）及び当該要素の奥行値を用いて表される点を、第２画像上に投影し、第１画像上の当該画素位置（ｘ，ｙ）の画素値と、第２画像上に点が投影された画素位置の画素値との距離を求める。コスト計算部３０は、第１画像上の画素位置（ｘ，ｙ）毎に、近傍集合Ｑ＿１＿ｘｙに含まれる要素の各々について、第１画像上の画素位置（ｘ，ｙ）と当該要素の画素位置との距離、及び上記で求められた第１画像上の当該画素位置（ｘ，ｙ）の画素値と、第２画像上に点が投影された画素位置の画素値との距離を用いて表されるコスト関数を計算する。

【0050】

具体的には、コスト計算部３０は、近傍集合Ｑ＿１＿ｘｙに含まれる各要素ｑ＝（ｑ＿ｘ，ｑ＿ｙ，ｄ）について、コスト関数を計算する。コスト関数は、画素位置（ｘ，ｙ）が要素ｑの奥行値ｄを取る尤もらしさを評価する。

【0051】

まず、第１画像の画素位置（ｘ，ｙ）がｄの奥行値を持っていたときに第２画像上のどこに投影されるかを導出する。これは、第１画像の画素位置（ｘ，ｙ）と奥行値ｄとを用いて表される点（ｘ，ｙ，ｄ）を内部パラメータＫ＿１の逆行列によって逆投影し、投影行列Ｒ＿Ｃと並進ベクトルＴ＿Ｃを適用して第２カメラ５４の第２画像の座標系に座標変換をしたのちに、内部パラメータＫ＿２によって第２画像平面に投影することによって得ることができる。これによって得た第２画像上での画素位置を（ｘ’，ｙ’）とする。例えば、以下の式に従って、画素位置（ｘ，ｙ）を、奥行値ｄを用いて第２画像に投影した座標（ｘ’，ｙ’）を計算する。
（Ｘ’，Ｙ’，ｄ’）＝Ｋ＿２（Ｒ＿Ｃ Ｋ＿１^－１（ｄ×ｘ，ｄ×ｙ，ｄ）＋Ｔ＿Ｃ）
ｘ’＝Ｘ’／ｄ’
ｙ’＝Ｙ’／ｄ’
これによって、画素位置（ｘ，ｙ）がｑの奥行値を取った際のコスト関数ｃ＿ｘｙｑは以下のように求められる。

【0052】

ｃ＿ｘｙｑ＝ｗ＿Ｓ Ｓｔｅｒｅｏ（Ｉ＿１，Ｉ＿２，ｘ，ｙ，ｘ’，ｙ’）＋ｗ＿Ｄ Ｄｉｓｔａｎｃｅ（ｘ，ｙ，ｑ＿ｘ，ｑ＿ｙ）

【0053】

Ｓｔｅｒｅｏは、第１画像の画素位置（ｘ，ｙ）と第２画像の画素位置（ｘ’，ｙ’）における画素値の違いを評価する関数であり、画素値の差異、センサス変換をした画像間のハミング距離、画素値の勾配の差異を取る方法などがあり（非特許文献３参照）、その重みづけやウィンドウサイズは任意である。

【0054】

Ｄｉｓｔａｎｃｅは、第１画像の画素位置（ｘ，ｙ）と要素の画素位置（ｑ＿ｘ，ｑ＿ｙ）の距離であり、ｌ１距離、ｌ２距離、もしくはＨｕｂｅｒ距離などであり、距離をトランケーションした値を用いてもよい。

【0055】

ｗ＿Ｓ，ｗ＿Ｄはそれぞれの重みである。ｃ＿ｘｙｑの意味するところは、画素位置（ｘ，ｙ）に奥行値ｄを割り当てた際に第１画像と第２画像の整合性が高く、かつ当該画素位置（ｘ，ｙ）の近くに投影された３次元点の奥行値が割り当てやすくなるということを意味する。

【0056】

信念伝播部３２は、第１画像上の画素位置（ｘ，ｙ）毎に、コスト関数及び隣接する画素において選択される奥行値との差分を用いて表されるエネルギー関数に基づいて、３次元点の奥行値を選択する。

【0057】

具体的には、信念伝播部３２は、前述のコスト関数に加えて、平滑化項を考慮したエネルギー関数を定義し、そのエネルギー関数を最小化する奥行値ｄ（ｘ，ｙ）を、第１画像の各画素位置（ｘ，ｙ）のそれぞれについて、信念伝播によって選択する。エネルギー関数Ｅは以下の式で定義される。

【0058】

【0059】

ｎｏｒｍ（∇ｄ（ｘ，ｙ））は隣接する奥行値ｄ（ｘ，ｙ）の差分のノルム（ｌ１距離、ｌ２距離、もしくはＨｕｂｅｒ距離など）を意味し、ノルムをトランケーションした値を用いてもよい。ｎｏｒｍ（∇ｄ（ｘ，ｙ））は隣接する画素間での奥行値の差分が小さいほど値が小さくなる。
例えば、ｎｏｒｍ（∇ｄ（ｘ，ｙ））は、注目画素位置から右と下に隣接する画素位置に対して計算する、選択した奥行値の差分のノルム（ｌ１距離、ｌ２距離、もしくはＨｕｂｅｒ距離など）の和である。右に隣接する画素位置との差分をどのように計算するかを具体的に示す。下に隣接する画素位置との差分の計算も同様である。
第１画像での注目画素位置（ｘ，ｙ）において、奥行値ｄをＱ＿１＿ｘｙから選択しており、隣接する画素位置（ｘ＋１，ｙ）において奥行値ｄ’をＱ＿１＿ｘ＋１＿ｙから選択しているとすると、それらの差分（ｄ－ｄ’）を導出し、そのノルムを求める。

【0060】

エネルギー関数Ｅを最小化することにより、ステレオ誤差、投影位置の近さ、各画素位置の奥行値の滑らかさを考慮した上で最適な奥行値ｄ（ｘ，ｙ）を各画素位置（ｘ，ｙ）について定めることができる。

【0061】

エネルギー関数Ｅの最小化は、信念伝播（Ｂｅｌｉｅｆ Ｐｒｏｐａｇａｔｉｏｎ）という手法、より具体的にはループありの信念伝播（Ｌｏｏｐｙ Ｂｅｌｉｅｆ Ｐｒｏｐａｇａｔｉｏｎ）によって行うことができる。

【0062】

以上のように、信念伝播部３２により、以下の式に従って、エネルギー関数Ｅの最小化によって、全ての画素位置（ｘ，ｙ）の各々について近傍集合Ｑ＿１＿ｘｙから選択される奥行値ｄ（ｘ，ｙ）の組み合わせが求められる。

【0063】

ｄ（ｘ，ｙ）＝ａｒｇｍｉｎ Ｅ

【0064】

平滑化部２６は、第１画像上の画素位置（ｘ，ｙ）の各々について選択された奥行値ｄ（ｘ，ｙ）を平滑化する。

【0065】

ここで、奥行値ｄ（ｘ，ｙ）は、信念伝播部３２により近傍の画素位置に投影される３次元点の奥行値の割り当てによって導出されるので、不連続なマップとなっている。そこで、本実施形態に係る平滑化部２６は、連続関数の平滑化法であるＶａｒｉａｔｉｏｎａｌ法によって平滑化をし、平滑化後の奥行値ｄ＿ｖ（ｘ，ｙ）を生成し、深度画像として表示部１６により出力する。

【0066】

具体的には、Ｖａｒｉａｔｉｏｎａｌ法により、以下のエネルギー関数Ｅ＿Ｖを最小化する。

【0067】

【0068】

ｎｏｒｍ１，ｎｏｒｍ２はｌ１距離、ｌ２距離、もしくはＨｕｂｅｒ距離などの距離である。また、Ｇは非特許文献４記載のＡＤＴ（Ａｎｉｓｏｔｒｏｐｉｃ Ｄｉｆｆｕｓｉｏｎ Ｔｅｎｓｏｒ）もしくはＢ－ＡＤＴ（Ｂｉｎａｒｙ Ａｎｉｓｏｔｒｏｐｉｃ Ｄｉｆｆｕｓｉｏｎ Ｔｅｎｓｏｒ）であり、画像の位置ごとに平滑化項ｎｏｒｍ２（∇ｄ＿Ｖ（ｘ，ｙ））を重みづけするものである。

【0069】

エネルギー関数Ｅ＿Ｖの最小化は非特許文献４に記載のようにｆｉｒｓｔ ｏｒｄｅｒ ｐｒｉｍａｌ ｄｕａｌ ａｌｇｏｒｉｔｈｍによって実施できる。これにより、以下の式に従って、平滑化後の、全ての画素位置（ｘ，ｙ）の各々についての奥行値ｄ＿Ｖ（ｘ，ｙ）の組み合わせが求められる。

【0070】

ｄ＿Ｖ（ｘ，ｙ）＝ａｒｇｍｉｎ Ｅ＿Ｖ

【0071】

［非特許文献４］Yao, Yasuhiro, et al. "Discontinuous and Smooth Depth Completion with Binary Anisotropic Diffusion Tensor." IEEE Robotics and Automation Letters 5.4 (2020): 5128-5135.

【0072】

＜本実施形態に係る３次元点群処理装置の作用＞
次に、３次元点群処理装置１０の作用について説明する。

【0073】

図６は、３次元点群処理装置１０による３次元点群処理の流れを示すフローチャートである。ＣＰＵ１１がＲＯＭ１２又はストレージ１４から３次元点群処理プログラムを読み出して、ＲＡＭ１３に展開して実行することにより、３次元点群処理が行なわれる。また、３次元点群処理装置１０に、ＬｉＤＡＲセンサ５０によって計測された３次元点群Ｐと、第１カメラ５２によって撮影された第１画像と、第２カメラ５４によって撮影された第２画像と、が入力される。また、３次元点群処理装置１０に、第１カメラ５２の内部パラメータＫ＿１、第２カメラ５４の内部パラメータＫ＿２、第１カメラ５２と第２カメラ５４間の回転行列Ｒ＿Ｃ、第１カメラ５２と第２カメラ５４間の並進ベクトルＴ＿Ｃ、第１カメラ５２とＬｉＤＡＲセンサ５０間の投影行列Ｒ＿Ｌ、及び第１カメラ５２とＬｉＤＡＲセンサ５０間の並進ベクトルＴ＿Ｌが入力されているものとする。

【0074】

ステップＳ１００で、ＣＰＵ１１は、入力処理部２０として、入力部１５により受け付けた、第１画像及び第２画像と、３次元点群とを取得する。

【0075】

ステップＳ１０２で、ＣＰＵ１１は、入力処理部２０として、第１画像及び第２画像と、３次元点群とに基づいて、集合Ｑ＿１、Ｑ＿２を計算する。

【0076】

ステップＳ１０４では、ＣＰＵ１１は、近傍抽出部２２として、３次元点群Ｐの各々の３次元点に対応する第１画像上の画素位置に基づいて、第１画像上の各画素位置（ｘ，ｙ）について、近傍集合Ｑ＿１＿ｘｙを抽出する。

【0077】

ステップＳ１０６では、ＣＰＵ１１は、コスト計算部３０として、第１画像上の画素位置（ｘ，ｙ）毎に、近傍集合Ｑ＿１＿ｘｙに含まれる各要素ｑ＝（ｑ＿ｘ，ｑ＿ｙ，ｄ）について、コスト関数を計算する。

【0078】

ステップＳ１０８では、ＣＰＵ１１は、信念伝播部３２として、第１画像上の画素位置（ｘ，ｙ）毎に、コスト関数及び隣接する画素において選択される奥行値との差分を用いて表されるエネルギー関数に基づいて、近傍集合Ｑ＿１＿ｘｙに含まれる各要素の奥行値の何れかを選択する。

【0079】

ステップＳ１１０では、ＣＰＵ１１は、平滑化部２６として、第１画像上の画素位置の各々について選択された奥行値を平滑化し、平滑化された奥行値を深度画像として表示部１６により表示して、３次元点群処理ルーチンを終了する。

【0080】

以上説明したように、本実施形態に係る３次元点群処理装置は、ＬｉＤＡＲセンサにより計測された３次元点群の３次元点の各々が投影される第１画像上の画素位置に基づいて、第１画像上の画素位置の各々について、近傍の複数の画素位置に対応する３次元点の奥行値から、第１画像及び第２画像の間の整合性が高く、かつ近傍の画素位置との距離が近くなるように、奥行値を選択する。これにより、計測して得られた３次元点群を精度よくアップサンプリングすることができる。

【0081】

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

【0082】

例えば、ＬｉＤＡＲセンサによる計測で、３次元点群を取得する場合を例に説明したが、これに限定されるものではない。ＬｉＤＡＲセンサ以外のセンサを用いて、３次元点群を計測するようにしてもよい。

【0083】

また、第１画像と第２画像とが異なるカメラによって撮影された場合を例に説明したが、これに限定されるものではない。撮影位置の関係が予め求められていれば、第１画像と第２画像とが同じカメラによって撮影されたものでもよい。

【0084】

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

【0085】

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

【0086】

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

【0087】

（付記項１）
３次元点群処理装置であって、
メモリと、
前記メモリに接続された少なくとも１つのプロセッサと、
を含み、
前記プロセッサは、
少なくとも撮影位置の関係が予め求められている第１画像及び第２画像と、少なくとも前記撮影位置と計測位置との関係が予め求められている物体の表面上の３次元点群とを受け付け、前記３次元点群の３次元点の各々に対応する前記第１画像上の画素位置を求め、
前記３次元点群の３次元点の各々に対応する前記第１画像上の画素位置に基づいて、前記第１画像上の画素位置の各々について、近傍の複数の画素位置に対応する３次元点の奥行値から、前記第１画像及び前記第２画像の間の整合性が高くなるように、前記奥行値を選択する
ように構成される３次元点群処理装置。

【0088】

（付記項２）
３次元点群処理を実行するようにコンピュータによって実行可能なプログラムを記憶した非一時的記憶媒体であって、
前記３次元点群処理は、
少なくとも撮影位置の関係が予め求められている第１画像及び第２画像と、少なくとも前記撮影位置と計測位置との関係が予め求められている物体の表面上の３次元点群とを受け付け、前記３次元点群の３次元点の各々に対応する前記第１画像上の画素位置を求め、
前記３次元点群の３次元点の各々に対応する前記第１画像上の画素位置に基づいて、前記第１画像上の画素位置の各々について、近傍の複数の画素位置に対応する３次元点の奥行値から、前記第１画像及び前記第２画像の間の整合性が高くなるように、前記奥行値を選択する
非一時的記憶媒体。

【符号の説明】

【0089】

１０ ３次元点群処理装置
１４ ストレージ
１５ 入力部
１６ 表示部
２０ 入力処理部
２２ 近傍抽出部
２４ 近傍選択部
２６ 平滑化部
３０ コスト計算部
３２ 信念伝播部
５０ ＬｉＤＡＲセンサ
５２ 第１カメラ
５４ 第２カメラ

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】