知能システム論 1 (13) 2014.7.2 情報システム学研究科情報メディアシステム学専攻知能システム学講座末廣尚士
16 物体の位置 姿勢計測 ロボットでのハンドリングに不可欠 物差しで測るステレオカメラで測る depthカメラ ( たとえばkinect) で測る depthカメラのデータは座標変換で議論できるイメージカメラの場合は透視投影変換を考える必要がある 物体の位置 姿勢計測は座標系の決定やキャリブレーションの基本でもある 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 2
- 座標系を用いた表現 hand table camera box placea placeb base world 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 3
- 座標系の関係の決まり方 arm T w base world T arm t T arm world T t world table world T c camera w T h wrist t T B t T A t T b 計測 hand ( 計測 ) placeb placea box 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 4
- 座標系の決まり方 t T A arm T w w T h t T B 場所を表すだけの形のない座標系は計測が難しい. 多くの場合, 設計値で決める. 講義 : 座標系の表現 アームやハンドの設計値と運動学で決まる. 講義 : 順運動学, 逆運動学 world T t world T arm world T c t T b t T arm 設備のレイアウト設計値で大まかな値が決まる. 作業を行う場合には, 精密な計測が必要となる. 移動ロボットの場合は移動ベース座標の計測が必要. 講義 : 座標系のキャリブレーション 工場などでは操作対象は設計時に決められた場所に置くのが基本. 日常生活, 未整備環境では, カメラなどによる計測が必要となる. 講義 : 物体座標系の精密計測講義外 : 物体認識, その他 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 5
- 問題 depthカメラで計測して得られたの直方体の3つの面上の計測点群からその直方体の位置姿勢を求めよ. 基本は, 概略位置決め : 計測値と物体の概略の関係性精密位置決め : 誤差の最小化. ニュートン法など やり方を少し考えてみよう 対象の性質 ( 形状, 色, などなど ) や計測系の性質 ( 画像, 距離など ) によりやり方が少しずつ異なる考え方を学んで欲しい 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 6
- 問題のイメージ A B C A B C 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 7
- 問題を詳細化する 何が求められているか? どのような形で解答すれば良いのか? それは求めることが可能か? 前提は何か? 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 8
- 問題の詳細化 (1) 直方体の座標系のカメラからの座標変換を求める 座標系の原点を直方体の中心に置く. 座標軸を各辺の向き ( 各面の法線方向 ) と並行になるようにする ( 直交する3 方向が得られるので, 右手系になるように x,y,z 軸を割り当てる ). 直方体の x,y,z 軸方向の辺の長さを 2*lx, 2*ly, 2*lz と表す. 計測された 3 面は,x=lx, y=ly, z=lz の式で表される 3 面 A,B,C とする. 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 9
- 問題の詳細化 (2) 各計測点がどの面を計測したものであるかは分かっているとする. すなわち,depth カメラによる点の位置の計測値 pai(i=1..na), pbi(i=1..nb), pci(i=1..nc) のが得られているとする. 問題は, 計測 (depth カメラ ) 座標系で表現された各点の位置計測値に基づいて, 計測座標系から見た直方体座標系の位置 姿勢を求めるということになる. 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 10
- どこから手を着けるか? 計測点群から最初に求められそうなものは何か? それを求めるにはどうしたらよいか? 問題の詳細化 それを求めると最終的な解答に近づきそうか? 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 11
- サブゴール 点群から面が求められるのではないか? 3つの面が求められれば, 座標系の原点位置や座標軸 ( 姿勢 ) も決まるのではないか. 点の計測値の誤差を考慮するのか? 誤差がなければ3 点で平面を決定することが出来る. 誤差があった場合は? 最小 2 乗近似? サブゴール ( サブ問題 ): 同一平面上の点の ( 誤差がある ) 計測値から最小 2 乗近似平面を求めよ. 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 12
- 点群の平面フィッティング どのようなやり方があるか? 何を 最小 2 乗 にするのか? 一般の回帰平面を求めるやり方で良いか? 平面を z=ax+by+c として,zに関する誤差を最小化する. これで良い? 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 13
- 慣性モーメントを利用した点群の平面フィッティング 平面と点との距離を最小 2 乗の対象としたとき 点群に対する最小 2 乗フィッティング平面は, 点群の重心 ( 平均値 ) を通り, 慣性モーメントが最大となる慣性主軸を法線とする平面である. ( 各点の質量を 1 とする ) 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 14
- 点群の慣性モーメント p i p av 点群重心として, p i p av =( x i, y i, z i ) T とすると慣性モーメントは =( i I ( y i 2 +z i 2 ) i i i x i y i x i z i i y i x i i (z i 2 +x i 2 ) i i y i z i i z i x i )) z i y i ( x 2 i + y 2 i 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 15
- 慣性モーメントの主軸 一般に慣性行列 I は正規直交行列 R と対角行列を用いて以下のように変形できる. I diag I =R I diag R T ここで, I diag =diag ( I 1, I 2, I 3 ) (I 1 I 2 I 3 0) R=(r 1,r 2,r 3 ) とすると, 法線となる. r 1 は, 点群の最小 2 乗近似平面の 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 16
- 慣性モーメントの主軸の求め方 慣性行列の I =R I diag R T への変形は, 特異値分解 (SVD) で求めることが出来る. A=UWV T で, A=I とすると, U =R W =I diag V T =R T となる. 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 17
- 点群の平面フィッティングの答え 点群の重心を慣性モーメントの最大慣性主軸を n (n=r 1 ) とすると, 最小 2 乗近似平面の方程式は, となる. p av n p d =0 (d =n p av ) 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 18
- 3 つの近似平面から直方体の位置 姿勢を求める どうすればよいか? 詳細化 何か注意すべき点は? 本当にそれで OK? 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 19
- 姿勢 ( 座標軸 ) 立方体の座標軸 x, y, zは面 A, B, Cの法線の向きになっていたはず. では面 A, B, Cのフィッティング面の法線の向きを座標軸 x, y, zとしてよいか? 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 20
- 法線の向きの調整 平面の法線と, その面上の点から他の面上の点の重心へ引いたベクトルとの内積の符号を調べれば平面の法線が直方体の内側に向いているのか外側に向いているのかがわかる. 面をx=lx, y=ly, z=lzで表したということは, 外向きの法線が各軸に対応することになる. よって, 各面の外向きの法線を各座標軸とすれば良い. 本当にそれで良いか?( とりあえず保留 ) 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 21
- 原点の位置? 法線 ( 座標軸 ) の向きも正しく定まった. 面の方程式も与えられている. さて原点の位置はどこだろうか? 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 22
- 原点の位置 各面の重心を座標軸の後ろ向きに,lx, ly, lz もどしたらどうだろうか? それが 1 点に集まるだろうか? そもそも重心は面の中心だろうか? 各面を座標軸の後ろ向きに,lx, ly, lz 平行移動した面を考え, その交点を原点とする. 前の答えの 2 つの問題は解決している. なかなか良さそう ではどうする? 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 23
- 3 平面の交点 3 つの平面の交点 n 1 p d 1 =0 n 2 p d 2 =0 n 3 p d 3 =0 p= d 1(n 2 n 3 )+d 2 (n 3 n 1 )+d 3 (n 1 n 2 ) n 1 (n 2 n 3 ) 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 24
- 原点の位置のまとめ 各面の方程式を法線の向きも考慮して以下とする n a p d a =0 n b p d b =0 n c p d c =0 (d a =n a p a,av,,,) これらを座標軸の後ろ向きに,lx, ly, lz 平行移動した面は, n a p d a l x =0 n b p d b l y =0 n c p d c l z =0 これらの交点を求めて座標系の原点とする. 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 25
- 位置 姿勢のまとめ? フィッティングした平面の法線の向きを考慮して各面の法線 n a, n b, n c を求める. 直方体の大きさを考慮して原点を通る ( と考えられる )3つの面の交点を p 0 求める. n a p d a l x =0 n b p d b l y =0 (d a =n a p a,av,,,) n c p d c l z =0 p 直方体座標系の原点 0 (n, 姿勢行列 a,n b, n c ) これで問題はないか? 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 26
- まとめの問題点 大きな問題点は得られた各面の法線が直交している保証がないこと. すなわち (n a, n b, n c ) が座標系の姿勢行列にならない. ではどうするか? 法線を利用して直交な座標軸を定める? 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 27
- まとめの修正 まず n a をx 軸ベクトル x 0 とする. 次に, なるべく n b に近い直交ベクトルを求め, 正規化してy 軸ベクトル y 0 とする. y 0 =normalize(n b (n a n b )n a ) 最後に, 双方に直交するz 軸ベクトル を定 める. z 0 =x 0 y 0 これで本当によいか? z 0 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 28
- 修正の問題点 小さな悩み : 原点位置も修正するか? 中くらいの悩み : 各軸の扱いが 非対称 であること 大きな問題 : 最小 2 乗であることが保証されなくなったこと ではどうする? 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 29
- ニュートン法の出番 計測点が面上にあるという条件を直方体座標系の位置 姿勢パラメタに関する ( 非線形 ) 方程式として表す. それをすべての計測点に関して連立させ, 非線形多変数方程式としてニュートン法を使って収束解を求める. ニュートン法で求めた解が最小 2 乗解になっていることについては別途検討. 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 30
- box_fit.py (1) 利用するモジュール from geo import * import numpy import numpy.linalg numpy の行列と geo の行列は互換性がないので変換する必要があることに注意すること 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 31
- box_fit.py (2) 計測点列の読み込み import box_points1 as points nx=points.n_d_pl_list[0][0] ny=points.n_d_pl_list[1][0] nz=points.n_d_pl_list[1][0] lx=points.n_d_pl_list[0][1] ly=points.n_d_pl_list[1][1] lz=points.n_d_pl_list[2][1] pos_a=points.n_d_pl_list[0][2] pos_b=points.n_d_pl_list[1][2] pos_c=points.n_d_pl_list[2][2] 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 32
- box_fit.py (3) 面のフィッティング def calc_tensor(pos) :... tens_a=calc_tensor(pos_a) tens_b=calc_tensor(pos_b) tens_c=calc_tensor(pos_c) u_a,w_a,v_a=numpy.linalg.svd(tens_a[1],compute_uv=1) u_b,w_b,v_b=numpy.linalg.svd(tens_b[1],compute_uv=1) u_c,w_c,v_c=numpy.linalg.svd(tens_c[1],compute_uv=1) na=vector(u_a[0][0],u_a[1][0],u_a[2][0]) da=na.dot(tens_a[0]) nb=vector(u_b[0][0],u_b[1][0],u_b[2][0]) db=nb.dot(tens_b[0]) nc=vector(u_c[0][0],u_c[1][0],u_c[2][0]) dc=nc.dot(tens_c[0]) 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 33
- box_fit.py (4) 面の交点と方向チェック def cross_point(a,b,c) : return (1.0/a[0].dot(b[0]*c[0]))*(a[1]*(b[0]*c[0])+b[1]*(c[0]*a[0]) +c[1]*(a[0]*b[0])) def check_direction(n,ori,ref) : tmp=n.dot(ref-ori) if tmp >= 0.0 : return -1.0 #inside else : return 1.0 #outside 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 34
- box_fit.py (5) 方向チェックと原点の計算 pc=cross_point([na,da],[nb,db],[nc,dc]) tmp = check_direction(na,pc,tens_b[0]) na = tmp*na da = tmp*da tmp = check_direction(nb,pc,tens_c[0]) nb = tmp*nb db = tmp*db tmp = check_direction(nc,pc,tens_a[0]) nc = tmp*nc dc = tmp*dc p0=cross_point([na,da-lx],[nb,db-ly],[nc,dc-lz]) 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 35
- box_fit.py (6) 法線の直交化と座標系 T0 の計算 x0=na tmp=nb-nb.dot(na)*na y0=(1.0/abs(tmp))*tmp z0=x0*y0 A0 = MATRIX([x0,y0,z0]).trans() T0=FRAME(mat=A0,vec=p0) print 'T0= ', T0 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 36
- box_fit.py (7) 残差の計算 def calc_f(pos_list,n_vec,dist,trans) : rslt = [] t_inv=-trans for pp in pos_list : tmp = n_vec.dot(t_inv*pp) - dist rslt.append(tmp) return rslt 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 37
- box_fit.py (8) T0 の残差の計算 f0 = (calc_f(pos_a,vector(1,0,0),lx,t0) +calc_f(pos_b,vector(0,1,0),ly,t0) + calc_f(pos_c,vector(0,0,1),lz,t0) ) f0_v = 0.0 for ff in f0 : f0_v += ff*ff f0_v /= len(f0) print "f0_v = ",f0_v 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 38
- box_fit.py (10) 直交化後の原点の再計算と座標系 T00 と残差 dx=x0.dot(tens_a[0]) dy=y0.dot(tens_b[0]) dz=z0.dot(tens_c[0]) p00=cross_point([x0,dx-lx],[y0,dy-ly],[z0,dz-lz]) T00=FRAME(mat=A0,vec=p00) print 'T00= ', T00 f00=( calc_f(pos_a,vector(1,0,0),lx,t00) +calc_f(pos_b,vector(0,1,0),ly,t00) +calc_f(pos_c,vector(0,0,1),lz,t00)) f00_v = 0.0 for ff in f00 : f00_v += ff*ff f00_v /= len(f00) print "f00_v = ",f00_v 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 39
- 次回予告 物体の位置 姿勢計測の一般論な多様面形状を用いたフィッティング座標系のキャリブレーションハンドアイキャリブレーションを中心に 2014 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 40