Blender:深度センサのシミュレーションをする割りと簡単な方法

今回のファイルはこちら。

git@bitbucket.org:matoge/blendersandbox.git

深度センサは一般的に、何らかのビームを照射して対象物までの点を求める。十分に多様性のある実データを集めるのが難しい場合 and/or アルコリズムの定性的、定量的な評価をしたい場合はシミュレーションを行うが、その際、点（カメラ、センサ）から対象物までの距離を求めるのが基本的な手法になる。

しかし、オブジェクトを何処かで定義して、描画して、アルゴリズムを書いて・・・となると、割りとハードルが高い。Boost.geometry や Point Cloud Library, CGAL 等、C++ で書かれたライブラリはあるにはあるが、使いこなすのにかなり時間がかかる。Blender ならもう少し簡単にできるんじゃないかと思った。

やりたいのは

1. Define a scene with an object on a background
2. Define a sensor that emits some laser pattern and calculate distances from the sensor to the scene
3. Perform some pattern recognition
4. Visualize

これを Blender なら割りと簡単に出来る。

2.72 以降のBlender には bpy.ops.mesh.intersect() というオペレータがあって、２つのオブジェクトがあれば交点を計算してくれる。しかし、出来上がったオブジェクトは、元々の点も交差点もごっちゃになっている上、交差点は同じ点が複数生成される（オブジェクトが分割されるので）、実際は点からの距離のみを知りたい場合は、自分で書いたほうがいい・・ような気がした。

intersect_line_plane

関数は、点と平面から交点を求める関数。点がメッシュの中にはいっているかどうかも同時にチェックしてくれない。チェックするには

intersect_point_tri

という関数を使う。

これらは、tri (三点メッシュ) を前提としているので、まずは全部の quad (4点メッシュ) を tri に変換する。

1. Go into Edit Mode
2. Press A to select all
3. Press Ctrl+T to convert Quads to Triangles
4. Re-export mesh with triangles

そして、以下の様な感じで点を用意して関数に放り込む。グローバル座標で処理しているのに注意。

---

import bpy
import mathutils.geometry as geo

cone = bpy.data.objects['Cone']
cube = bpy.data.objects['Cube']
idx=13
cone.data.vertices[idx].select=True
idxtop=9
vtop = cone.data.vertices[idxtop]
vbottom = cone.data.vertices[idx]

p1 = cone.location + vtop.co
p2 = cone.location + vbottom.co
p3 = cube.data.vertices[4].co + cube.location
p4 = cube.data.polygons[1].normal

rel = geo.intersect_line_plane(p1,p2,p3,p4)
print("Intersection:")
print(rel)
# Move cursor to the intersection
bpy.context.scene.cursor_location = rel

次のような感じになる。

この点がメッシュの中にはいっているかどうかを調べるためには

polygon=cube.data.polygons[1]
# check if the intersection is inside the plane
p1 = cube.data.vertices[polygon.vertices[0]].co+ cube.location
p2 = cube.data.vertices[polygon.vertices[1]].co+ cube.location
p3 = cube.data.vertices[polygon.vertices[2]].co+ cube.location
rel2 = geo.intersect_point_tri( rel, p1, p2, p3)
print( rel2 ) 

メッシュと交叉している場合は、rel2 には Vector が、交叉していない場合は None が入る。コーンの vertices の順番が少し特徴的で index 0 や –1 にはないので、top の頂点を取ってくるには一度 index を確認するとよい。

実際は、ある点に対して「すべての」メッシュに対して同様の計算を行う必要がある。また、レーザーを照射しているので、複数の点が見つかった場合、一番距離の短い点を結果として出す等の処理をするとよい。

例：コーン状のレーザー

こんな感じのレーザーが、近くの壁にあたった場合。

距離は、こんな感じになる。レーザーが壁にあたった場合、距離は局所的には線型で近似出来る。

Change Point Detection

上のモデルで、壁と地面の交点を求めたい。

1. 距離には sd= σ の誤差


2. 計算量はできるだけ小さく


3. 壁の高さ h  として、hが低い場合、どのくらい認識出来るのかを知りたい。


4. センサからの距離 d をパラメータとして、できれば d を求めたい。直感的に、「壁が垂直」という assumption を入れれば、壁の距離は簡単にわかるはず。


5. できれば、一旦 xyz に変換すること無く、距離関数のみでパラメータを推定したい。xyz の変換には時間がかかるので。

こういう問題は Change Point Detection という。基本的に、役に立つのは以下の様な知識

1. 部分的に regression をすればノイズに頑健になる。


2. 単純に移動平均等を取ると sd の影響が大きい。


3. Regression を近似的に行うには Sequential Regression をする。計算量 O(n^3) が O(nk) (k:は精度に応じて） になる。