JP6396982B2

JP6396982B2 - 空間モデル処理装置

Info

Publication number: JP6396982B2
Application number: JP2016255908A
Authority: JP
Inventors: 裕子世木
Original assignee: Secom Co Ltd
Current assignee: Secom Co Ltd
Priority date: 2016-12-28
Filing date: 2016-12-28
Publication date: 2018-09-26
Anticipated expiration: 2036-12-28
Also published as: JP2018106643A

Description

本発明は、空間モデルを補正する空間モデル処理装置に関し、特に複数の局所領域を三次元計測した点群を位置合わせすることにより生成された空間モデルを補正する空間モデル処理装置に関する。

広い空間の三次元モデル（空間モデル）を生成する場合には、空間中を移動しながら互いにオーバーラップを持たせて局所領域ごとに三次元計測し、各局所領域の計測で得られた複数のポイントクラウド（点群）を位置合わせして連結していた。

すなわち、オーバーラップ部分の点群が表す形状等を手掛かりに、複数の局所領域をそれぞれのローカル座標系にて三次元計測した複数の点群を共通座標系に変換することで位置合わせし、これらを連結して空間モデルを生成していた。

特開２０１６−１７００３１号公報

しかしながら、局所領域の中には無地の壁のような広い平面が支配的で二次元的にも三次元的にも特徴が少ないものが含まれ得る。また、局所領域の中にはドア越しに三次元計測した隣の部屋を含む局所領域であって当該隣の部屋に関する特徴が少ない局所領域が含まれ得る。そのため、特徴の少ない局所領域に関しては位置合わせ誤差が生じやすく、複数の局所領域を連結した空間モデルには局所的な座標変換のズレが生じていた。局所領域ごとのズレは微小であっても、連結する局所領域が増えるほどズレが蓄積し、空間モデル全体でみると大きなズレとなる場合があった。例えば、本来、平面である壁が屈曲した壁になったり、複数の平面が隙間を有して重なった層状の壁になったりすることが起こる。

このようなズレは、局所領域の特徴の少なさに起因するため、抜本的に減少させるためには情報の追加が必要である。例えば、利用者が空間モデル中で局所領域のローカル座標系を移動させる（回転・並進させる）、または利用者が空間モデル中にマーカーを設定して新たな特徴を加えるなど、利用者が介在しなければ抜本的に減少させることができない。

ところが、利用者が三次元座標系のデータを補正する作業は多大な労力とスキルが必要であった。例えば、壁等が支配的な局所領域については手がかりの少ない大きな平面に対する作業となり、ドア越しの隣の部屋を含む局所領域については計測範囲の狭い隣の部屋の部分に対して行うセンシティブな作業となるため、多くの試行錯誤や経験が必要となる。

本発明は上記問題を鑑みてなされたものであり、多大な労力とスキルを必要とせずに、三次元座標系の空間モデルを補正できる空間モデル処理装置を提供することを目的とする。

（１）本発明に係る空間モデル処理装置は、基準平面とする物体平面と当該基準平面に対し垂直な物体平面とが存在する空間について、三次元計測により得られた点群で構成された空間モデルを補正する装置であって、前記空間内の複数の局所領域それぞれについて当該局所領域のローカル座標系での前記点群で構成された局所空間モデルを前記複数の局所領域に共通する共通座標系にて統合した前記空間モデルを記憶している空間モデル記憶手段と、前記空間モデルにおいて面積が最大である物体平面に対応する部分を検出し、当該部分に沿った平面を前記基準平面として推定する基準平面推定手段と、前記基準平面に対し垂直な物体平面の前記基準平面上での位置を表す線分を利用者に入力させる入力手段と、前記線分を含み前記基準平面推定手段が推定した推定基準平面に垂直な目標面を設定し、前記空間モデルから前記目標面に対応する位置及び形状を有する注目部分を抽出し、前記共通座標系において前記目標面に前記注目部分が整合するように前記局所空間モデルを移動する空間モデル補正手段と、を備える。

（２）上記（１）に記載の空間モデル処理装置は、前記空間モデルを前記推定基準平面に投影した投影像における前記点群の密度に応じて画素値を定めた密度画像を生成する密度画像生成手段と、前記密度画像を表示する表示手段と、をさらに備え、前記入力手段は、前記利用者に当該密度画像上にて前記線分を入力させるものとすることができる。

（３）上記（２）に記載の空間モデル処理装置において、前記密度画像生成手段は、前記空間モデルから、前記推定基準平面に垂直であり且つ互いに直交する２平面を検出し、当該２平面それぞれの前記推定基準平面への投影像である線分の方向を前記密度画像の水平方向及び垂直方向に設定する構成とすることができる。

本発明の空間モデル処理装置によれば、少ない労力とスキルで効果的に三次元座標系の空間モデルを補正することが可能となる。

本発明の実施形態に係る空間モデル処理装置の概略の構成を示すブロック図である。本発明の実施形態に係る空間モデル処理装置の概略の機能ブロック図である。補正処理前の空間モデルである点群の一例を示す図である。最大平面およびこれに垂直で互いに直角をなす２平面を用いた空間モデルの座標変換を説明する模式図である。図３で例示した空間モデルから生成した密度画像と当該密度画像上に入力された線分群の例を示す模式図である。図５で例示した線分群から算出された目標面群の例を示す模式図である。図３で例示した空間モデルを図６で示した目標面群に基づいて補正した空間モデルを示す模式図である。空間モデル処理装置の動作の概略のフロー図である。空間モデル処理装置の動作の概略のフロー図である。

以下、本発明の実施の形態（以下実施形態という）である空間モデル処理装置１について、図面に基づいて説明する。空間モデル処理装置１は、基準平面に対し平行な物体平面と垂直な物体平面とが存在する空間について、三次元計測により得られた点群で構成された空間モデルを補正する。

例えば、三次元計測はＲＧＢＤカメラなどの三次元計測装置を用いて行うことができる。ＲＧＢＤカメラは、投射したレーザー光が物体表面との往復にかかる時間から距離を計測するＴＯＦ（Time Of Flight）方式の距離センサ、および当該距離センサと同期して同一視野のＲＧＢ画像を撮影するカラーカメラを組み合わせた装置である。ここで、距離センサはレーザー光を二次元的に走査し、走査範囲内の複数の反射点の方向および距離を計測する。そして、当該方向および距離に基づいて、反射点からなる点群の三次元座標が、三次元計測装置の位置姿勢を基準としたローカル座標系にて決定される。なお、三次元計測装置は、距離画像を取得する距離センサと濃淡画像を取得するモノクロカメラとを組み合わせたものであってもよい。また距離センサはＴＯＦ方式に限らず、パターン光投影方式など他の方式を採るものであってもよい。

例えば、建物の１フロアには、水平面を基準平面として、これに平行な物体平面として床や天井が存在し、一方、複数の部屋や区画の間に垂直な物体平面として壁などが存在する。このような複数の部屋、区画などが並んだ空間全体の三次元計測は上述したようにそれぞれ当該空間の一部である複数の局所領域での三次元計測に分けて行われる。具体的には、ＲＧＢＤカメラのような三次元計測装置を各局所領域に設置して計測する。そのため、各局所領域にて物体を三次元計測して得られる点群は、局所領域ごとに設定された座標系（ローカル座標系）での三次元座標データで表される。そして各局所領域にて得られた点群で構成される局所空間モデルを、複数の局所領域に共通する共通座標系にて統合して全体的な空間モデルが生成される。本発明の空間モデル処理装置１は、空間モデルを構成する複数の局所空間モデルの間の位置関係のズレを補正する処理を行う。

図１は空間モデル処理装置１の概略の構成を示すブロック図である。空間モデル処理装置１は記憶部２、制御部３およびユーザーインターフェース部４を含んで構成される。

記憶部２はＲＯＭ(Read Only Memory)、ＲＡＭ(Random Access Memory)等の記憶装置である。記憶部２は、各種プログラムや各種データを記憶し、制御部３との間でこれらの情報を入出力する。

制御部３はＣＰＵ(Central Processing Unit)、ＤＳＰ(Digital Signal Processor)、ＭＣＵ(Micro Control Unit)等の演算装置を用いて構成される。制御部３は記憶部２からプログラムを読み出して実行することで、後述する各手段として機能する。また、制御部３は各種データを記憶部２に記憶させ、各種データを記憶部２から読み出す。制御部３はユーザーインターフェース部４とも接続され、ユーザーインターフェース部４に画像等を出力して表示させ、また、利用者がユーザーインターフェース部４を用いて入力した座標値等を取得する。

ユーザーインターフェース部４は制御部３から入力された画像等を利用者に対して表示し、また、利用者が操作して入力した座標値等を制御部３に出力する装置である。具体的には、ユーザーインターフェース部４は、液晶ディスプレイまたはＣＲＴ（Cathode Ray Tube）等の表示装置、およびマウスなどのポインティングデバイスまたはキーボード等の操作入力装置からなる。

図２は空間モデル処理装置１の概略の機能ブロック図である。記憶部２は空間モデル記憶手段２０などとして機能する。また、制御部３は基準平面推定手段３０、密度画像生成手段３１および空間モデル補正手段３２などとして機能する。ユーザーインターフェース部４は表示手段４０および入力手段４１として機能する。

空間モデル記憶手段２０は、複数の局所領域ごとの三次元点群（局所空間モデル）と、それらの三次元点群を統合した空間モデルとを記憶する。ここで、上述したように各局所領域に対応して三次元計測が行われるが、各局所領域の具体的な範囲は実質的に三次元計測装置の各計測位置での空間的な計測範囲によって定まる。また、既に述べたように、各局所領域を計測した三次元点群は局所領域ごとのローカル座標系で表されている。一方、全局所領域の三次元点群を統合した空間モデルは、全局所領域にて共通に用いる共通座標系で表されている。

各局所空間モデルを共通座標系に統合するに際しては、隣り合う局所領域の重複部分での整合性が得られるようにローカル座標系を共通座標系にて回転・並進させることによって（移動させることによって）局所空間モデルを位置合わせしている。具体的には、隣り合う局所領域の三次元点群が表す表面形状どうしの重複部分での相違（誤差）の極小化が図られる。また、この重複部分での三次元点群が表す表面形状の相違と共に、当該三次元点群のＲＧＢ値の相違または濃淡値の相違の極小化を図るように位置合わせを行ってもよい。このように位置合わせをして生成された空間モデルが空間モデル補正手段３２による補正処理の対象データとして予め記憶部２に記憶される。また、記憶部２には、当該補正処理後の空間モデルを記憶することができる。

図３は補正処理前の空間モデルである点群の一例を示す図であり、当該空間モデルは２つの部屋を含んでいる。図３（ａ）に示す空間モデル１００は斜め上方から部屋を眺めた斜視図に当たり、図３（ｂ）に示す空間モデル１１０は図３（ａ）に示す点群を部屋の真上から眺めた平面図に当たる。この空間モデルは２つの部屋それぞれの中で三次元計測して得られた２つの局所空間モデルを位置合わせして統合している。

図３（ｂ）において、２つの部屋は中央部とその左側とに位置する。三次元点群１１１および三次元点群１１３は左側の部屋に対応する局所領域にて計測された点群であり、三次元点群１１２および三次元点群１１４は中央部の部屋に対応する局所領域にて計測された点群である。三次元点群１１１および三次元点群１１２は２つの部屋を仕切る同じ壁に対応し、本来１つの平面をなすはずであるが、壁の特徴の少なさが原因で位置合わせ誤差が生じたため、両点群１１１，１１２間にズレが存在する。また、三次元点群１１３および三次元点群１１４は本来直線上に並ぶ壁であるが、部屋間を連結するための特徴がドア越しの計測でしか得られず、部屋間の連結部の特徴の少なさが原因で位置合わせ誤差が生じ、その誤差が波及して両点群１１３，１１４は直線上に整列していない。

基準平面推定手段３０は、空間モデル記憶手段２０から空間モデルを読み出し、読み出した空間モデルから面積が最大の物体平面に対応する部分を検出し、当該部分に沿った平面を基準平面として推定する。そして、基準平面推定手段３０は検出した基準平面の情報を密度画像生成手段３１および空間モデル補正手段３２に出力する。具体的には、例えば、基準平面推定手段３０は、空間モデルを構成する点群を解析して複数の平面を検出し、検出した複数の平面の中から面積が最大である平面を決定し、決定した平面の共通座標系における法線ベクトルと原点からの距離とを算出する。基準平面の向きは当該法線ベクトルにより決定される。また、共通座標系の原点と基準平面との距離は任意に設定でき、例えば、最大平面と同じ距離とすることができる。

なお、基準平面推定手段３０は空間モデルから面積が最大の物体平面に対応する部分として点群を抽出した後、当該点群から基準平面を定める処理とすることもできる。この場合、当該点群に対応する平面の面積は点群の分布範囲に基づいて判断することができる。また、点群が物体表面にて密度一定になるようにダウンサンプリング処理などがされている場合には、点群の構成点の数に基づいて平面の面積を判断することもできる。

ここで、典型的には、室内空間を計測した空間モデルからは床面が最大の物体平面として検出され、屋外空間を計測した空間モデルからは地面が最大の物体平面として検出されるので、基準平面として空間モデルにおける水平面が推定される。

密度画像生成手段３１は空間モデル記憶手段２０から空間モデルを読み出し、読み出した空間モデルを構成する点群を、基準平面推定手段３０から入力された基準平面（推定基準平面）に投影する。そして、密度画像生成手段３１はその投影像における点群の密度に応じて画素値を定めた密度画像を生成する。例えば、密度画像生成手段３１は点群の密度に応じて輝度値を設定した密度画像を生成する。

具体的には、まず、密度画像生成手段３１は、空間モデルを構成する全ての点を推定基準平面に投影するとともに推定基準平面の投影領域を密度画像の各画素に対応するブロックに等分割し、ブロックごとに投影点を計数する。次に、密度画像生成手段３１は、ブロックの投影点数が多いほど当該ブロックと対応する画素に高い輝度値を設定して密度画像を生成する。例えば、投影点数が０個の輝度値を０、全ブロック中の最大投影点数の輝度値を２５５と定め、各画素に対応するブロックの投影点数と比例した輝度値を設定することができる。この場合、推定基準平面に垂直な物体平面の位置に対応する画素の輝度値が高輝度となり、典型的には、壁と対応する直線的な領域が特に高輝度となる。そのため密度画像は利用者が壁の位置を視認し易い画像となる。

なお、輝度値の高低は逆にしてもよい。つまりブロックの投影点数が多いほど当該ブロックと対応する画素に低い輝度値を設定して密度画像を生成してもよい。その場合は、典型的には、壁と対応する直線的な領域が特に低輝度となり、やはり密度画像は利用者が壁の位置を視認し易い画像となる。

また、密度画像生成手段３１は、空間モデルから推定基準平面に垂直であり且つ互いに直交する２平面を検出し、当該２平面それぞれの推定基準平面への投影像である線分の方向を密度画像の水平方向（Ｘ軸方向）および垂直方向（Ｙ軸方向）に設定する。その際、当該２平面として、推定基準平面に垂直であり且つ互いに直交するという条件を満たす平面のペアのうち面積が大きなペアを優先して検出するのがよい。例えば、密度画像生成手段３１は上記平面のペアのうち、一方の面積が最大のペア、或いは合計面積が最大のペアを検出することができる。多くの場合、部屋などの互いに交わる壁の角度は直角であるので、このように密度画像の座標軸を設定することで、後述する利用者による入力線分が密度画像のＸ軸方向またはＹ軸方向に平行な線分となる可能性が高くなり、利用者による容易かつ正確な入力を得やすくなる。

推定基準平面に沿ってＸ軸、Ｙ軸を設定することは、推定基準平面の法線方向をＺ軸として、空間モデルについて直交座標系ＸＹＺを定義することに相当する。つまり、基準平面推定手段３０および密度画像生成手段３１によりそれぞれ検出される最大平面およびこれに垂直で互いに直角をなす２平面を用いて空間モデルの座標変換がなされる。図４は当該座標変換を説明する模式図である。空間モデル２００は空間モデル記憶手段２０に予め記憶される空間モデルであり、図３に例示した空間モデル１００と同じである。空間モデル２００の座標系は任意に設定され得る。例えば、或る１つの局所領域に設置されたＲＧＢＤカメラの撮影面の直交する２つの座標軸がＸ軸、Ｙ軸となり、カメラの光軸方向にＺ軸が設定され得る。基準平面推定手段３０は空間モデル２００から最大平面に対応する部分として例えば、床面に対応する点群２１０を検出し、これに沿った水平面を基準平面として推定する。また、例えば、密度画像生成手段３１は基準平面に垂直な面として、空間モデル２００にて床面に立設された壁のうち、最大平面である床面に次いで大きい面積を有する壁に対応する点群２２０と、当該壁に直交する別の壁に対応する点群２３０とを検出する。そして、推定基準平面の法線方向をＺ軸、点群２２０により定義される平面を推定基準平面に投影して得られる線分の方向をＹ軸、また点群２３０により定義される平面を推定基準平面に投影して得られる線分の方向をＸ軸とする直交座標系ＸＹＺが定義され、これに対応して点群の座標が変換された空間モデル２４０が生成される。

表示手段４０は密度画像生成手段３１で生成した密度画像を表示する。具体的には、ユーザーインターフェース部４の表示装置が表示手段４０として機能する。

入力手段４１は基準平面に対し垂直な物体平面の基準平面上での位置を表す線分を利用者に入力させる。具体的には、ユーザーインターフェース部４の入力装置が入力手段４１として機能する。入力手段４１は密度画像上で利用者に線分の位置を指定させるように構成するのが好適である。典型的には、室内であれば部屋を構成する壁、屋外であれば建物などを構成する壁を表す線分が入力される。なお、入力手段４１により利用者が入力した線分は、後述する空間モデル補正手段３２にて、局所空間モデルの位置補正に際して目標となる垂直平面の位置を指定するものとして用いられる。

図５は図３で例示した空間モデル１００，１１０から生成した密度画像３００（図５（ａ））と密度画像３００上に入力された線分群３１０の例（図５（ｂ））を示す模式図である。

空間モデル補正手段３２は、入力手段４１で入力された線分を含む平面であり、且つ基準平面推定手段３０で推定した基準平面（推定基準平面）に垂直である平面を補正処理における目標面として設定する。そして空間モデル補正手段３２は、空間モデル記憶手段２０に記憶した空間モデルから目標面に対応する位置および形状を有する注目部分を抽出し、共通座標系において目標面に注目部分が整合するように局所空間モデルを回転・並進させる移動操作を行う。具体的には、空間モデル補正手段３２により抽出される注目部分は目標面に対応した位置および形状に分布した点群であり、空間モデル補正手段３２は当該点群の分布領域が目標面に略重複するように局所空間モデルを移動し、これにより座標が変換された点群からなる補正後の空間モデルが生成される。補正された空間モデルは空間モデル記憶手段２０に記憶される。この記憶処理は、補正後の空間モデルで補正前の空間モデルを更新する処理としてもよい。

図６は図５で例示した線分群３１０から算出された目標面群４００の例を示す模式図であり、目標面群４００が配置された空間を斜め上方から眺めた斜視図に当たる。

図７は図３で例示した空間モデル１００，１１０を図６で示した目標面群４００に基づいて補正した空間モデル５００を示す模式図であり、部屋の真上方向から見た平面図に当たる。すなわち、図３（ｂ）が補正前の空間モデルを上から見た図であり、図７は補正後の空間モデルを上から見た図である。図７に示す壁の三次元点群５０１，５０２それぞれは、図３で例示した壁の三次元点群１１１，１１２に対応する。図７には、三次元点群１１１，１１２に生じていたズレが三次元点群５０１，５０２では補正されて、両点群５０１，５０２はほぼ一直線に並んでいることが示されている。同様に、図３で例示した壁の三次元点群１１３，１１４のそれぞれに対応する壁の三次元点群５０３，５０４についてもズレが補正されてほぼ一直線に並んでいる。

次に空間モデル処理装置１の動作について説明する。図８および図９は空間モデル処理装置１の動作の概略のフロー図である。

制御部３は空間モデル記憶手段２０から空間モデルを読み出すとともに（ステップＳ１）、基準平面推定手段３０として動作する。基準平面推定手段３０は、空間モデルを構成する三次元点群が表す空間形状の中から平面を検出する（ステップＳ２）。具体的には、基準平面推定手段３０は三次元点群に含まれる点ごとに共通座標系における法線ベクトルおよび原点からの距離を定義し、三次元点群に含まれる点を法線ベクトルの方向および原点からの距離についてクラスタリングすることにより各点を平面ごとのクラスタに分ける。なお、例えば、各点での法線ベクトルは、当該点の近傍の点を用いて推定される微小面の向きに基づいて定義することができ、原点からの距離は当該微小推定面を含む平面に原点から下ろした垂線の長さに基づいて定義することができる。また、基準平面推定手段３０は、後のステップで再利用するためにクラスタリング結果を記憶部２に記憶させる。

基準平面推定手段３０はステップＳ２で検出した平面の中から最大平面を検出し、基準平面を推定する（ステップＳ３）。例えば、点群が物体表面にて密度一定になるようにダウンサンプリングされている場合には、基準平面推定手段３０は平面ごとのクラスタのうち帰属する点の数が最も多いものを最大平面のクラスタと決定することができる。基準平面推定手段３０は共通座標系における最大平面の法線ベクトル（ａ，ｂ，ｃ）と原点からの距離ｄとを算出し、その結果に基づいて推定基準平面の平面方程式ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０を導出する。

制御部３は密度画像生成手段３１として動作し、読み出した空間モデルが密度画像生成手段３１に入力される。密度画像生成手段３１は、空間モデルの三次元点群に対して推定基準平面を共通座標系にてＺ＝０の平面とする座標変換を行う（ステップＳ４）。具体的には、密度画像生成手段３１は共通座標系において、ステップＳ３で算出した推定基準平面に関し原点からの距離を０とし法線ベクトルのＸ成分およびＹ成分をそれぞれ０とするような座標変換式を導出し、算出した座標変換式により三次元点群に含まれる各点の座標を変換する。ちなみに推定基準平面の平面方程式はｃｚ＝０となる。

密度画像生成手段３１は、空間モデルを構成する三次元点群が表す空間形状の中から推定基準平面に略垂直であり、且つ互いに略垂直な２平面を検出する（ステップＳ５）。例えば、平面ごとの上述のクラスタについて、面積の大きい順に、共通座標系における法線ベクトルを算出し、推定基準平面の法線ベクトルとなす角度を算出する。なお、面積の大きい順とする代わりに、帰属する点の数の多い順で近似してもよい。算出した角度が８５度〜９５度であれば垂直とみなして当該平面を第一平面の候補として選出する。さらに平面ごとの上述のクラスタについて、選出した第一平面の候補に次いで面積の大きい順に、法線ベクトルを算出して、選出した第一平面の候補の法線ベクトルとなす角度を算出する。算出した角度が８５度〜９５度であれば垂直とみなし、当該平面と第一平面の候補とをそれぞれ第二平面、第一平面と決定する。第二平面が決定できなかった場合はこれまでに選出した第一平面の候補のクラスタを除いて再度、第一平面の候補の選出からやり直す。なお、ステップＳ２でのクラスタリング結果を流用する代わりに密度画像生成手段３１がステップＳ２の説明で述べたクラスタリングを再度行ってもよい。

密度画像生成手段３１は、ステップＳ５で決定した２平面のそれぞれが共通座標系ＸＹＺにおけるＸ＝０の平面、Ｙ＝０の平面となるように座標変換する（ステップＳ６）。さらに、密度画像生成手段３１は、密度画像の生成の便宜上、空間モデルを構成する三次元点群のＸ座標の最小値Ｘ_ＭＩＮおよびＹ座標の最小値Ｙ_ＭＩＮを求め、点（Ｘ_ＭＩＮ，Ｙ_ＭＩＮ，０）に共通座標系の原点を移動させる座標変換を行う（ステップＳ７）。このように原点を移動させることで、空間モデルを構成する全ての点群のＸ座標、Ｙ座標が非負値となり、表示装置の画像における一般的な水平座標および垂直座標の定義に合わせることができる。

密度画像生成手段３１は、空間モデルを構成する全ての三次元点群を推定基準平面であるＺ＝０へ投影し（ステップＳ８）、投影点の密度に応じた輝度値を設定して、密度画像を生成する（ステップＳ９）。密度画像生成手段３１は生成した密度画像をユーザーインターフェース部４に出力する。

ユーザーインターフェース部４は表示手段４０および入力手段４１として機能し、密度画像を表示装置に表示し（ステップＳ１０）、利用者からの補正完了の旨の入力有無を確認する（ステップＳ１１）。ステップＳ１１にて補正完了の旨があるまで後述する空間モデルの補正処理がループ処理で実行され、一方、補正完了の旨が入力されると（ステップＳ１１にてＹＥＳの場合）、当該補正処理は終了される。なお、ステップＳ１０にて、既にループ処理が開始されている場合には、目標面を指定する線分として既に入力されているものを密度画像に重畳して表示する。

ステップＳ１１にて補正完了の旨の入力がなければ（ステップＳ１１にてＮＯの場合）、空間モデルを補正する一連の処理Ｓ１２〜Ｓ１８が実行される。まず、制御部３が利用者による線分入力を受け付ける（ステップＳ１２）。具体的には、入力手段４１をなすマウス等が利用者により操作されて密度画像上に線分が入力されると、当該線分の座標が入力手段４１から制御部３に出力される。また、入力された線分は記憶部２に記憶され、一方、制御部３は既に入力されている線分を記憶部２から読み出す。

制御部３は線分を入力されると空間モデル補正手段３２として動作する。空間モデル補正手段３２は、共通座標系において、利用者により入力された各線分を含み、推定基準平面に垂直な目標面を算出する（ステップＳ１３）。各目標面はその算出の基礎となった線分の両端点それぞれにおける推定基準平面に対する垂線である２つの側辺と、当該２側辺の間を横方向に渡された上辺および下辺とで囲まれた四角形で規定する。目標面の下辺は２側辺それぞれと最大平面との交点の間を結ぶ線分で規定される。目標面の上辺は空間モデルにおける高さの最大値を有し最大平面に水平な線分とすればよい。

空間モデル補正手段３２は、空間モデルを構成する三次元点群が表す空間形状の中から目標面と対応する平面を検出する（ステップＳ１４）。具体的には、各平面のクラスタから平面の法線を算出するとともに各目標面の法線を算出し、各平面と各目標面との間で法線がなす角度および面間の距離を比較し、各目標面との法線の向きの差および距離が所定値未満である平面を当該目標面と対応する平面として検出する。ちなみに、面間の距離は例えば、平面に対応する三次元点群の中央点から目標面へ下ろした垂線の長さとすることができる。また上記所定値は予めの実験を通じて見積もった位置合わせ誤差の上限値とすることができる（例えば、実空間での１０ｃｍに相当する値）。また空間モデル補正手段３２は、対応する平面が検出されない目標面を以降の処理対象から除外する。なお、ステップＳ２でのクラスタリング結果を流用する代わりに空間モデル補正手段３２がステップＳ１４で改めてクラスタリングを行ってもよい。

空間モデル補正手段３２は、空間モデルを構成する三次元点群が表す平面のそれぞれがどの局所領域に含まれているかを特定する（ステップＳ１５）。この処理では、最大平面のクラスタを除く各平面のクラスタが帰属する局所領域を特定する。

空間モデル補正手段３２は、ステップＳ１５で平面を含むと特定された局所領域のそれぞれについて、当該平面とステップＳ１４にて当該平面との対応が検出された目標面との回転・並進に関する位置の差を算出する（ステップＳ１６）。この差は両者の位置姿勢が似ているほど０に近くなり、ずれているほど大きくなる。

空間モデル補正手段３２は、ステップＳ１６にて算出した差を空間モデル全体にて最小化する各局所領域の補正量を算出する（ステップＳ１７）。局所領域の三次元点群は、共通座標系において既に位置合わせされているため、隣り合う局所領域の三次元点群間の回転・並進量の差は０（つまり回転・並進を表す同次変換行列が単位行列）に近い値になっている。この状況において、目標面と局所領域にて当該目標面に対応する平面との回転・並進量の差を小さくするという追加条件を課し、局所領域の対応平面の位置姿勢を目標面に近づけ、且つ共通座標系における局所領域の三次元点群の位置姿勢ができるだけそのまま保たれるように最適化を行う。

この最適化処理の手法の一例を説明する。空間モデルを構成する局所領域（または局所空間モデル）の個数をＮとし、第ｉ局所空間モデル（１≦ｉ≦Ｎ）をＰ_ｉとする。

重複領域を有する任意の２つのＰ_ｉとＰ_ｊとについて、重複領域での局所空間モデルの対応付けを行って、互いに対応付けられたＰ_ｉの部分とＰ_ｊの部分との間の回転並進パラメータＲｔ_ｉ＿ｊを求める。つまり、互いに対応付けられたＰ_ｉの部分とＰ_ｊの部分との一方にＲｔ_ｉ＿ｊに相当する座標変換を行うと、他方に好適に整合する。Ｒｔ_ｉ＿ｊは並進の３自由度（ｘ，ｙ，ｚ）および回転の３自由度（ρ，θ，φ）からなるベクトルで表現できる。具体的には、
Ｒｔ_ｉ＿ｊ＝（ρ_ｉ＿ｊ，θ_ｉ＿ｊ，φ_ｉ＿ｊ，ｘ_ｉ＿ｊ，ｙ_ｉ＿ｊ，ｚ_ｉ＿ｊ）
という形式となる。

また共通座標系にて位置合わせされたＰ_ｉの並進の３自由度（ｘ，ｙ，ｚ）および回転の３自由度（ρ，θ，φ）からなる位置姿勢パラメータをベクトルＸ_ｉで表す。具体的には、
Ｘ_ｉ＝（ρ_ｉ，θ_ｉ，φ_ｉ，ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）
という形式となる。

重複領域を有する２つのＰ_ｉ，Ｐ_ｊについての位置姿勢の差（Ｘ_ｉ−Ｘ_ｊ）がＲｔ_ｉ＿ｊに相当するものであれば、Ｒｔ_ｉ＿ｊに基づく座標変換によりＰ_ｉ，Ｐ_ｊの位置姿勢は整合するが、一般には（Ｘ_ｉ−Ｘ_ｊ）はＲｔ_ｉ＿ｊによる座標変換で相殺できるとは限らず、この場合、Ｐ_ｉ，Ｐ_ｊの位置姿勢は完全には整合せず、（Ｘ_ｉ−Ｘ_ｊ）−Ｒｔ_ｉ＿ｊが誤差として残る。

空間モデル処理装置１へ補正対象として入力される空間モデルは例えば、重複領域を有する２つのＰ_ｉ，Ｐ_ｊについての（Ｘ_ｉ−Ｘ_ｊ）とＲｔ_ｉ＿ｊとの差（マハラノビス距離）を空間モデル全体で最小化するように局所空間モデルを位置合わせされている。具体的には、重複領域を有する２つのＰ_ｉ，Ｐ_ｊの全ての組み合わせについての上記マハラノビス距離の総和として次式で定義する誤差ε_Ａを最小化するようにＸ_ｉが定められる。ここで、Ｃ_ｉ＿ｊはベクトルＸ_ｉの６つの要素に対応する共分散行列である。

このように位置合わせされた空間モデルは上述したように、局所空間モデルの重複部分の本来同じ平面間にズレが生じ得る。これに対して空間モデル補正手段３２は上述の追加条件により、利用者の指示に基づく目標面に局所空間モデルの対応平面の位置姿勢が近づくように誘導して、位置合わせされた空間モデルを上述のズレが縮小・解消するように修正する。

具体的には、上述した空間モデルにおける局所空間モデルの位置合わせと同様、マハラノビス距離を用いて定義した誤差の最小化により当該修正を行うことができる。ここで、目標面に対応する平面を有する任意のＰ_ｉについて、当該平面と目標面との間の回転並進パラメータをＲｔ_{ｉ＿ｒｅｆ}と表す。Ｒｔ_{ｉ＿ｒｅｆ}はＲｔ_ｉ＿ｊと同様の６成分のベクトルで表現できる。また、目標面の位置姿勢パラメータをＸ_ｉと同様の６成分のベクトルＸ_ｒｅｆで表す。

空間モデル補正手段３２はＰ_ｉと目標面との位置姿勢パラメータの差分（Ｘ_ｉ−Ｘ_ｒｅｆ）とＲｔ_{ｉ＿ｒｅｆ}とに関するマハラノビス距離を空間モデル全体で最小化するようにＸ_ｉを修正する。具体的には、ステップＳ１７にて、目標面に対応する平面を有する全てのＰ_ｉについての上記マハラノビス距離の総和として次式で定義する誤差ε_Ｒを用い、当該誤差ε_Ｒを最小化するＸ_ｉが求められる。そして、当該Ｘ_ｉの補正前のＸ_ｉに対する差が位置姿勢の補正量として算出される。ここで、Ｃ_{ｉ＿ｒｅｆ}はＣ_ｉ＿ｊと同様、共分散行列である。

つまり、空間モデル補正手段３２は、既に行われている位置合わせに加えて、目標面への整合を図る修正を行うことで、例えばε＝ε_Ａ＋ε_Ｒで定義される誤差の和εが最小化されるＸ_ｉが最適な局所領域の配置として与えられる。なお、εにおけるε_Ａとε_Ｒとの重みは変更することができ、例えば、定数λ（但しλ＞０）を用いて、εは一般的にε＝λ・ε_Ａ＋（１−λ）・ε_Ｒとすることができる。

空間モデル補正手段３２は、各局所領域の局所空間モデルを、ステップＳ１７にて算出した当該局所領域についての補正量だけ回転・並進させて空間モデルを補正し、補正した空間モデルを空間モデル記憶手段２０に記憶させる（ステップＳ１８）。つまり、各局所領域のローカル座標を変換する補正が行われ、各局所領域の三次元点群に含まれる点のそれぞれは、補正量だけ回転・並進される。補正の後、処理はステップＳ１０に戻される。

［変形例］
上記実施形態においては、入力手段４１はユーザ操作に応じて線分を入力する例を示したが、入力手段４１はＵＳＢインターフェースなどの外部からのファイル入力手段であってもよい。すなわち、入力手段４１は床面など基準平面に対応する平面上での１または複数の線分が描かれた画像ファイルを読み込み、当該ファイルの線分を目標面の位置を表す線分情報として入力する。

なお、その場合、密度画像生成手段３１による密度画像の生成および表示手段４０による密度画像の表示は省略することもできる。

あるいは、その場合、密度画像を利用して、ユーザに画像ファイルに描かれた画像の回転・並進を指示入力させてもよい。例えば、表示手段４０は読み込んだファイルの画像を密度画像に重畳して表示し、ユーザから密度画像上の２点とそれらの点に対応する画像ファイル中の線分上の２点とをマウスで入力させ、線分上の２点を密度画像上の２点に一致させる二次元の回転・並進を画像ファイルに対して行い、回転・並進後の線分を空間モデル補正手段３２に出力する。

１空間モデル処理装置、２記憶部、３制御部、４ユーザーインターフェース部、２０空間モデル記憶手段、３０基準平面推定手段、３１密度画像生成手段、３２空間モデル補正手段、４０表示手段、４１入力手段。

Claims

基準平面とする物体平面と当該基準平面に対し垂直な物体平面とが存在する空間について、三次元計測により得られた点群で構成された空間モデルを補正する空間モデル処理装置であって、
前記空間内の複数の局所領域それぞれについて当該局所領域のローカル座標系での前記点群で構成された局所空間モデルを前記複数の局所領域に共通する共通座標系にて統合した前記空間モデルを記憶している空間モデル記憶手段と、
前記空間モデルにおいて面積が最大である物体平面に対応する部分を検出し、当該部分に沿った平面を前記基準平面として推定する基準平面推定手段と、
前記基準平面に対し垂直な物体平面の前記基準平面上での位置を表す線分を利用者に入力させる入力手段と、
前記線分を含み前記基準平面推定手段が推定した推定基準平面に垂直な目標面を設定し、前記空間モデルから前記目標面に対応する位置及び形状を有する注目部分を抽出し、前記共通座標系において前記目標面に前記注目部分が整合するように前記局所空間モデルを移動する空間モデル補正手段と、
を備えたことを特徴とする空間モデル処理装置。
前記空間モデルを前記推定基準平面に投影した投影像における前記点群の密度に応じて画素値を定めた密度画像を生成する密度画像生成手段と、
前記密度画像を表示する表示手段と、をさらに備え、
前記入力手段は、前記利用者に当該密度画像上にて前記線分を入力させること、
を特徴とする請求項１に記載の空間モデル処理装置。
前記密度画像生成手段は、前記空間モデルから、前記推定基準平面に垂直であり且つ互いに直交する２平面を検出し、当該２平面それぞれの前記推定基準平面への投影像である線分の方向を前記密度画像の水平方向及び垂直方向に設定すること、を特徴とする請求項２に記載の空間モデル処理装置。