JP6001389B2 - 環境復元装置、環境復元方法及び環境復元プログラム - Google Patents

Description

本発明は、対象物の３次元画像を生成する環境復元装置、環境復元方法及び環境復元プログラムに関するものである。

近年、ロボットなどに搭載されたカメラ画像を用いて遠隔操作することが行われている。この場合、カメラ画像が２次元画像だと周辺の環境情報を認識し難いという問題が生じている。

これに対し、例えば、移動体周辺の環境を３次元環境情報として構築し表示する装置が知られている（例えば、特許文献１参照）。

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

しかしながら、単に３次元カメラなどを用いて３次元環境情報を構築する場合、その処理が複雑となり３次元画像を生成するのに時間が掛かることとなる。

本発明は、このような問題点を解決するためになされたものであり、３次元画像をより短時間で生成できる環境復元装置、環境復元方法及び環境復元プログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、対象物の３次元点群データを取得するデータ取得手段と、複数の多角形により球面を近似した測地ドームモデルを設定し、前記データ取得手段により取得された３次元点群データの各点において相互に隣接する近傍点の外積から法線を夫々算出し、該各法線に対して最近傍の前記測地ドームモデルの頂点へ夫々投票したヒストグラムを生成し、該ヒストグラムにおいて最大頻度となる頂点を第１主軸と推定し、該第１主軸に直交する第２主軸及び第３主軸を推定する直交３軸推定手段と、前記直交３軸推定手段により推定された第１主軸、第２主軸、及び第３主軸を主軸とする座標系を定義し、前記データ取得手段により取得された全ての３次元点群データを前記定義した座標系に変換し、前記各主軸に投影された１次元の点群に対して密度がピークとなる位置を面の存在する位置として推定する平面位置推定手段と、前記平面位置推定手段により推定された各面に直交しない前記２つの主軸が成す平面において前記３次元点群が存在する位置に面を生成して、３次元画像を生成する面当てはめ手段と、を備えることを特徴とする環境復元装置である。
この一態様において、前記測地ドームモデルにおいて各頂点に対して直交する各頂点をテーブル情報として記憶する記憶手段を更に備え、前記直交３軸推定手段は、前記記憶手段に記憶された各頂点のテーブル情報を用いて、前記第１主軸に直交する第２主軸及び第３主軸を推定してもよい。
この一態様において、前記データ取得手段は、対象物の３次元距離点群データ及び画像を取得する距離画像センサであり、前記直交３軸推定手段は、前記距離画像センサの設置方向の情報を用いて前記第２主軸及び第３主軸の探索範囲を絞り込み、前記推定を行ってもよい。
この一態様において、前記直交３軸推定手段は、ロボットに設けられたジャイロセンサ又はエンコーダの検出値に基いて前記距離画像センサの設置方向を算出してもよい。
この一態様において、前記面当てはめ手段は、領域４分木を用いて正方形の面断片を面要素として前記面を生成してもよい。
この一態様において、前記面当てはめ手段は、平面位置推定手段により推定された平面に最小単位の大きさで区切ったピクセルを当てはめ、各ピクセルにおける３次元点群の占有状態を算出し、４ピクセルを１組とした解像度の１段低い区切りを作り、４ピクセル全て占有されている場合、該４ピクセルを解像度の低いピクセルとする処理を再帰的に繰り返して面を生成してもよい。
この一態様において、前記データ取得手段は、前記対象物の３次元点群データ及び画像を取得し、前記面当てはめ手段は、前記生成した面に前記データ取得手段により取得した対応する画像を貼付けてもよい。
この一態様において、前記測地ドームモデルは、緯度経度モデルであってもよい。
この一態様において、前記面当てはめ手段により生成された３次元画像を表示する表示手段を更に備えていてもよい。
他方、上記目的を達成するための本発明の一態様は、対象物の３次元点群データを取得するステップと、複数の多角形により球面を近似した測地ドームモデルを設定し、前記取得された３次元点群データの各点において相互に隣接する近傍点の外積から法線を夫々算出し、該各法線に対して最近傍の前記測地ドームモデルの頂点へ夫々投票したヒストグラムを生成し、該ヒストグラムにおいて最大頻度となる頂点を第１主軸と推定し、該第１主軸に直交する第２主軸及び第３主軸を推定するステップと、前記推定された第１主軸、第２主軸、及び第３主軸を主軸とする座標系を定義し、前記取得された全ての３次元点群データを前記定義した座標系に変換し、前記各主軸に投影された１次元の点群に対して密度がピークとなる位置を面の存在する位置として推定するステップと、前記推定された各面に直交しない前記２つの主軸が成す平面において前記３次元点群が存在する位置に面を生成して、３次元画像を生成するステップと、を含むことを特徴とする環境復元方法であってもよい。
また、上記目的を達成するための本発明の一態様は、複数の多角形により球面を近似した測地ドームモデルを設定し、取得された対象物の３次元点群データの各点において相互に隣接する近傍点の外積から法線を夫々算出し、該各法線に対して最近傍の前記測地ドームモデルの頂点へ夫々投票したヒストグラムを生成し、該ヒストグラムにおいて最大頻度となる頂点を第１主軸と推定し、該第１主軸に直交する第２主軸及び第３主軸を推定する処理と、前記推定された第１主軸、第２主軸、及び第３主軸を主軸とする座標系を定義し、前記取得された全ての３次元点群データを前記定義した座標系に変換し、前記各主軸に投影された１次元の点群に対して密度がピークとなる位置を面の存在する位置として推定する処理と、前記推定された各面に直交しない前記２つの主軸が成す平面において前記３次元点群が存在する位置に面を生成して、３次元画像を生成する処理と、をコンピュータに実行させることを特徴とする環境復元プログラムであってもよい。

本発明によれば、３次元画像をより短時間で生成できる環境復元装置、環境復元方法及び環境復元プログラムを提供することができる。

本発明の一実施の形態に係る環境復元装置の概略的なシステム構成を示すブロック図である。 法線を法線マップ上にプロットした一例を示す図である。 測地ドームモデルの一例を示す図である。 第１主軸に直交する頂点を示す図である。 第２主軸及び第３主軸の推定方法を説明するための図である。 各主軸に投影された１次元の点群に対して密度がピークとなる位置の一例を示す図である。 ４分木におけるピクセルリダクションを説明するための図である。 ４分木におけるピクセルリダクションにより生成された面の一例を示す図である。 画像を貼付けて生成された面の一例を示す図である。 本発明の一実施の形態に係る環境復元方法のフローの一例を示すフローチャートである。

以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明の一実施の形態に係る環境復元装置の概略的なシステム構成を示すブロック図である。本実施の形態に係る環境復元装置１は、対象物の３次元点群データを取得する距離画像センサ２と、相互に直交する３軸を推定する直交３軸推定部３と、３次元点群を含む平面が存在する位置を推定する平面位置推定部４と、推定された平面に面を当てはめ３次元画像を生成する面当てはめ部５と、を備えている。

なお、環境復元装置１は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）、などからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ、メモリ、及びインターフェイス部は、データバスなどを介して相互に接続されている。

距離画像センサ２は、データ取得手段の一具体例であり、例えば、Ｋｉｎｅｃｔ（登録商標）などの３次元カメラ（３０ｆｐｓでＶＧＡ（６４０×４８０）解像度の３次元点群を測定可能）を用いて周辺環境を測定し、対象物の３次元点群及び画像を取得する。なお、距離画像センサ２としては、高速に対象物の３次元点群および画像が取得できれば任意のセンサを用いることができる。

ここで、全ての点の法線は直交する３軸のいずれかに属するというマンハッタンワールド（Manhattan-world）仮説が知られている。そこで、直交３軸推定部３は、距離画像センサ２により取得された３次元点群に基いて主軸となる直交３軸（第１主軸、第２主軸、第３主軸）を推定する。

直交３軸推定部３は、直交３軸推定手段の一具体例であり、３次元点群の各点において、相互に隣接する近傍点の外積から法線を算出する。ここで、算出した法線を法線マップ上に夫々プロットすると、例えば、図２にようになる。直交３軸推定部３は、これらプロットされた法線の中から相互に直交する位置にある３つの極（丸印）を、直交３軸として推定する。

直交３軸推定部３は、例えば、正１２面体、正２０面体などを再帰的に分割して、多数の３角形により球面を近似した測地ドームモデル（図３）を設定する。この測地ドームモデルは、密度が均質（頂点間の距離が同一）であることから、各法線の向きによる精度のバラツキが少ない。なお、測地ドームモデルとして、例えば、緯度経度モデルを用いることもできる。この緯度経度モデルは特異点が存在しないためより正確に後述の各主軸を推定することが可能となり、計算もより簡素化される。

直交３軸推定部３は、例えば、半径１．０の測地ドームモデルに各頂点を設定し、設定した各頂点の位置情報をテーブル情報などに保持する。そして、直交３軸推定部３は、各法線に対して最近傍の頂点へ夫々投票したヒストグラムを生成する。

直交３軸推定部３は、例えば、ｋｄ−ｔｒｅｅによる上記各法線に対する最近傍探索を行う。直交３軸推定部３は、近似の最近傍探索を行うために全頂点を探索木に登録し、各法線がどの頂点に含まれているかを高速に探索する。直交３軸推定部３は、生成したヒストグラムにおいて、最大頻度となる頂点を第１主軸として推定する。

ここで、第２主軸および第３主軸は、第１主軸に対して直交しているため、第１主軸に直交する頂点だけを探索すればよいこととなる（図４）。直交３軸推定部３は、第１主軸の頂点から作られる法線との内積が０となる（直交する）法線を探索し、探索して得られた法線の頂点を第２主軸および第３主軸と推定する（図５）。

なお、直交３軸推定部３は、各頂点に対して直交する各頂点を予め算出し、テーブル情報として記憶部（記憶手段の一具体例）７に記憶させてもよい。直交３軸推定部３は、記憶部７に記憶された頂点のテーブル情報を用いて、第２主軸および第３主軸の探索を行う。これにより、高速に第２主軸および第３主軸を推定することができる。記憶部７は、記憶手段の一具体例であり、例えば、ＲＡＭ、ＲＯＭ、磁気ディスク装置、光ディスク装置などで構成されている。

さらに、直交３軸推定部３は、より高速に推定を行うために、距離画像センサ２が設置されている方向の情報を用いて、上記３主軸推定の探索範囲を絞り込んでも良い。例えば、ロボットなどにおいて距離画像センサ２と同一系に取付けられたジャイロセンサの検出値に基いて距離画像センサ２が設置されている方向を算出して、上記３主軸推定の探索範囲を絞り込むことができる。また、ロボットなどの設計値や各軸のエンコーダにより検出されたエンコーダ値に基いて距離画像センサ２が設置されている方向を算出し、上記３主軸推定の探索範囲を絞り込むことができる。

なお、距離画像センサ２の測定範囲内において直交３軸を推定するための平面を測定できない場合（例えば、物体に接近し過ぎていて平面が見えない場合、一つの平面しか見えない場合、斜面が見える場合等）、直交３軸推定部３は、上記のように算出した距離画像センサ２が設置されている方向の情報を用いて、上記３主軸推定の探索範囲を絞り込んでも良い。

平面位置推定部４は、平面位置推定手段の一具体例であり、直交３軸推定部３により推定された直交３軸（第１主軸、第２主軸、第３主軸）を主軸とする座標系Ａを定義する。そして、平面位置推定部４は、距離画像センサ２により取得された全ての３次元点群を、定義した座標系Ａに変換する。さらに、平面位置推定部４は、座標系Ａ上に変換した３次元点群を各主軸に投影する。平面位置推定部４は、各主軸に投影された１次元の点群に対して、その密度がピークとなる位置を面の存在する位置として推定する（図６）。

ここで、上記平面位置推定部４により推定される各面は、第１主軸、第２主軸、第３主軸のうちいずれかの主軸に直交するため、直交しない２主軸で成す２次元平面内で面の当てはめを考える。平面位置推定部４により推定された平面式は無限平面であるため、面当てはめ部５は、当てはめる実際の面の面積を推定すべく、上記各２次元平面内で３次元点群が存在する位置に面を生成する。

面当てはめ部５は、面当てはめ手段の一例であり、例えば、領域４分木を用いて正方形の面断片を面要素として面を生成する。これにより、距離画像センサ２による３次元点群に欠けが生じている場合、その位置を無闇に補完することを避けつつ、出来るだけ面を拡大して情報量を減少させることができる。

面当てはめ部５は、平面位置推定部４により推定された平面に最小単位の大きさで区切ったピクセルを当てはめ、各ピクセルに含まれる３次元点群をカウントすることで各ピクセルの占有状態を算出する。面当てはめ部５は、例えば、各ピクセルの占有率が閾値以上のとき、そのピクセルが３次元点群で占有されていると判断する。

次に、面当てはめ部５は、４ピクセルを１組とした解像度の１段低い区切りを作る（図７）。そして、面当てはめ部５は、その４ピクセル全て占有されている場合、その４ピクセルをその解像度の低いピクセルとして処理する。面当てはめ部５は、このように部分的に解像度を下げる処理（４分木によるピクセルリダクション）を再帰的に繰り返し、面を生成する（図８）。これにより、より大きな面要素で面を表現できる。このように、４分木を用いることで、面群表現を軽量化することができ、処理効率を向上させることができる。

最後に、面当てはめ部５は、生成した面に距離画像センサ２により取得した対応する画像（テクスチャー）を貼付けて３次元画像を生成する（図９）。面当てはめ部５により生成された３次元画像は、例えば、液晶ディスプレイ装置、有機ＥＬディスプレイ装置などの表示装置６に出力される。表示装置６は、表示手段の一具体例であり、表示画面上に生成された３次元画像を表示する。これにより、ユーザは３次元カメラなどにより撮影された対象物の３次元画像を迅速に見ることができる。

次に、環境復元装置による環境復元方法について詳細に説明する。図１０は、本実施の形態に係る環境復元方法のフローの一例を示すフローチャートである。

距離画像センサ２は、３次元カメラを用いて周辺環境を測定し対象物の３次元点群及び画像を取得する（ステップＳ１０１）。

直交３軸推定部３は、距離画像センサ２により取得された３次元点群の各点において、相互に隣接する近傍点の外積から各法線を算出する（ステップＳ１０２）。

直交３軸推定部３は、測地ドームモデルに各頂点を設定し、算出した各法線に対して最近傍の各頂点へ夫々投票したヒストグラムを生成する（ステップＳ１０３）。

直交３軸推定部３は、生成したヒストグラムにおいて、最大頻度となる頂点を第１主軸として推定する（ステップＳ１０４）。

直交３軸推定部３は、上記推定した第１主軸の頂点から作られる法線との内積が０となる法線を探索し、探索して得られた法線の頂点を第２主軸および第３主軸と推定する（ステップＳ１０５）。

平面位置推定部４は、直交３軸推定部３により推定された第１主軸、第２主軸、及び第３主軸を主軸とする座標系Ａを定義する（ステップＳ１０６）。

平面位置推定部４は、距離画像センサ２により取得された全ての３次元点群を、定義した座標系Ａに変換する（ステップＳ１０７）。平面位置推定部４は、第１主軸、第２主軸、及び第３主軸に夫々投影された１次元の点群に対して、その密度がピークとなる位置を面の存在する位置として推定する（ステップＳ１０８）。

面当てはめ部５は、平面位置推定部４により推定された平面に最小単位の大きさで区切ったピクセルを当てはめ、各ピクセルに含まれる３次元点群をカウントすることで各ピクセルの占有状態を算出する（ステップＳ１０９）。

面当てはめ部５は、４ピクセルを１組とした解像度の１段低い区切りを作り、その４ピクセル全て占有されている場合、その４ピクセルをその解像度の低いピクセルとして処理する。面当てはめ部は、このように部分的に解像度を下げる処理を再帰的に繰り返す面を生成する（ステップＳ１１０）。

面当てはめ部５は、生成した面に距離画像センサ２により取得した画像を貼り付ける（ステップＳ１１１）。

以上、本実施の形態に係る環境復元装置１において、取得した対象物の３次元点群データの各点において相互に隣接する近傍点の外積から法線を夫々算出し、その各法線に対して最近傍の前記測地ドームモデルの頂点へ夫々投票したヒストグラムを生成し、ヒストグラムにおいて最大頻度となる頂点を第１主軸と推定し、第１主軸に直交する第２主軸及び第３主軸を推定する。そして、推定された第１主軸、第２主軸、及び第３主軸を主軸とする座標系を定義し、取得された全ての３次元点群データを定義した座標系に変換し、各主軸に投影された１次元の点群に対して密度がピークとなる位置を面の存在する位置として推定する。さらに、推定された各面に直交しない２つの主軸が成す平面において３次元点群が存在する位置に面を生成して３次元画像を生成する。これにより、対象物の３次元画像を短時間かつ高精度で生成できる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。

また、本発明は、例えば、図１０に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１ 環境復元装置
２ 距離画像センサ
３ 直交３軸推定部
４ 平面位置推定部
５ 面当てはめ部
６ 表示装置
７ 記憶部

Claims (10)

1. 対象物の３次元点群データを取得するデータ取得手段と、
   複数の多角形により球面を近似した測地ドームモデルを設定し、前記データ取得手段により取得された３次元点群データの各点において相互に隣接する近傍点の外積から法線を夫々算出し、該各法線に対して最近傍の前記測地ドームモデルの頂点へ夫々投票したヒストグラムを生成し、該ヒストグラムにおいて最大頻度となる頂点を第１主軸と推定し、該第１主軸に直交する第２主軸及び第３主軸を推定する直交３軸推定手段と、
   前記直交３軸推定手段により推定された第１主軸、第２主軸、及び第３主軸を主軸とする座標系を定義し、前記データ取得手段により取得された全ての３次元点群データを前記定義した座標系に変換し、前記各主軸に投影された１次元の点群に対して密度がピークとなる位置を面の存在する位置として推定する平面位置推定手段と、
   前記平面位置推定手段により推定された各面に直交しない前記２つの主軸が成す平面において前記３次元点群が存在する位置に面を生成して、３次元画像を生成する面当てはめ手段と、
   を備え、
   前記データ取得手段は、対象物の３次元距離点群データ及び画像を取得する距離画像センサであり、
   前記直交３軸推定手段は、前記距離画像センサの設置方向の情報を用いて前記第２主軸及び第３主軸の探索範囲を絞り込み、前記推定を行う
   ことを特徴とする環境復元装置。
2. 請求項１記載の環境復元装置であって、
   前記測地ドームモデルにおいて各頂点に対して直交する各頂点をテーブル情報として記憶する記憶手段を更に備え、
   前記直交３軸推定手段は、前記記憶手段に記憶された各頂点のテーブル情報を用いて、前記第１主軸に直交する第２主軸及び第３主軸を推定することを特徴とする環境復元装置。
3. 請求項１記載の環境復元装置であって、
   前記直交３軸推定手段は、ロボットに設けられたジャイロセンサ又はエンコーダの検出値に基いて前記距離画像センサの設置方向を算出することを特徴とする環境復元装置。
4. 請求項１乃至３のうちいずれか１項記載の環境復元装置であって、
   前記面当てはめ手段は、領域４分木を用いて正方形の面断片を面要素として前記面を生成することを特徴とする環境復元装置。
5. 請求項１乃至４のうちいずれか１項記載の環境復元装置であって、
   前記面当てはめ手段は、平面位置推定手段により推定された平面に最小単位の大きさで区切ったピクセルを当てはめ、各ピクセルにおける３次元点群の占有状態を算出し、４ピクセルを１組とした解像度の１段低い区切りを作り、４ピクセル全て占有されている場合、該４ピクセルを解像度の低いピクセルとする処理を再帰的に繰り返して面を生成することを特徴とする環境復元装置。
6. 請求項１乃至５のうちいずれか１項記載の環境復元装置であって、
   前記データ取得手段は、前記対象物の３次元点群データ及び画像を取得し、
   前記面当てはめ手段は、前記生成した面に前記データ取得手段により取得した対応する画像を貼付けることを特徴する環境復元装置。
7. 請求項１乃至６のうちいずれか１項記載の環境復元装置であって、
   前記測地ドームモデルは、緯度経度モデルであることを特徴とする環境復元装置。
8. 請求項１乃至７のうちいずれか１項記載の環境復元装置であって、
   前記面当てはめ手段により生成された３次元画像を表示する表示手段を更に備えることを特徴とする環境復元装置。
9. 対象物の３次元点群データを取得するステップと、
   複数の多角形により球面を近似した測地ドームモデルを設定し、前記取得された３次元点群データの各点において相互に隣接する近傍点の外積から法線を夫々算出し、該各法線に対して最近傍の前記測地ドームモデルの頂点へ夫々投票したヒストグラムを生成し、該ヒストグラムにおいて最大頻度となる頂点を第１主軸と推定し、該第１主軸に直交する第２主軸及び第３主軸を推定するステップと、
   前記推定された第１主軸、第２主軸、及び第３主軸を主軸とする座標系を定義し、前記取得された全ての３次元点群データを前記定義した座標系に変換し、前記各主軸に投影された１次元の点群に対して密度がピークとなる位置を面の存在する位置として推定するステップと、
   前記推定された各面に直交しない前記２つの主軸が成す平面において前記３次元点群が存在する位置に面を生成して、３次元画像を生成するステップと、
   を含み、
   距離画像センサにより前記対象物の３次元距離点群データ及び画像を取得し、
   前記距離画像センサの設置方向の情報を用いて前記第２主軸及び第３主軸の探索範囲を絞り込み、前記推定を行う
   ことを特徴とする環境復元方法。
10. 複数の多角形により球面を近似した測地ドームモデルを設定し、取得された対象物の３次元点群データの各点において相互に隣接する近傍点の外積から法線を夫々算出し、該各法線に対して最近傍の前記測地ドームモデルの頂点へ夫々投票したヒストグラムを生成し、該ヒストグラムにおいて最大頻度となる頂点を第１主軸と推定し、該第１主軸に直交する第２主軸及び第３主軸を推定する処理と、
    前記推定された第１主軸、第２主軸、及び第３主軸を主軸とする座標系を定義し、前記取得された全ての３次元点群データを前記定義した座標系に変換し、前記各主軸に投影された１次元の点群に対して密度がピークとなる位置を面の存在する位置として推定する処理と、
    前記推定された各面に直交しない前記２つの主軸が成す平面において前記３次元点群が存在する位置に面を生成して、３次元画像を生成する処理と、
    をコンピュータに実行させ、
    距離画像センサにより前記対象物の３次元距離点群データ及び画像が取得され、
    前記距離画像センサの設置方向の情報を用いて前記第２主軸及び第３主軸の探索範囲を絞り込み、前記推定を行う
    ことを特徴とする環境復元プログラム。