座標変換などで手計算をあまりしたくないのでsymsをうまく使ってこれを回避する方法をメモします。
Image Jacobianの導出
カメラの座標系から見て位置（X,Y,Z）にある物体を考えます。焦点距離fとすると，以下の関係が成り立ちます[1]。
カメラが6軸の速度・加速度を持つときに画像内の点がどのように動くのか，その関係性を表す行列を導出します。
時間変化する変数を定義
時間tに関する関数として位置の各変数を定義できます。[2]の橋本先生の論文を例にとって見てみましょう。
syms t X(t) Y(t) Z(t) image_p = [X(t); Y(t)]/Z(t)
計算結果：
image_p = (X(t))/Z(t) (Y(t))/Z(t)
時間で偏微分
tで偏微分します。
diff_p = diff(image_p,t)
こうなります。
diff_p = diff(X(t), t)/Z(t) - (X(t)*diff(Z(t), t))/Z(t)^2 diff(Y(t), t)/Z(t) - (Y(t)*diff(Z(t), t))/Z(t)^2
特定の数式の代入による式整理
先程の式のXdotなどをvxに変換していくために以下の数式を代入します。
subs(対象式, 代入対象, 代入する式)という構文を用いて代入。
diff(X(t), t)という式を消していきます。
syms vx vy vz wx wy wz
before={diff(X(t), t),diff(Y(t), t),diff(Z(t), t)};
after = { -vx-wy*Z(t)+wz*Y(t), -vy-wz*X(t)+wx*Z(t), -vz-wx*Y(t)+wy*X(t)};
diff_p_XYZ = subs(diff_p,before,after)
結果，以下のようになります。
(X(t)*(vz - wy*X(t) + wx*Y(t)))/Z(t)^2 - (vx - wz*Y(t) + wy*Z(t))/Z(t) (Y(t)*(vz - wy*X(t) + wx*Y(t)))/Z(t)^2 - (vy + wz*X(t) - wx*Z(t))/Z(t)
小手先の代入テク
ただ，元の式展開では，[x,y]=[X/Z,Y/Z] の関係を用いてX,Yの変数を消去しています。これもsubsで計算可能です。
syms x y
before={X(t), Y(t)};
after = { x*Z(t), y*Z(t) };
diff_p_xy = subs(diff_p_XYZ ,before,after)
(x*(vz - wy*x*Z(t) + wx*y*Z(t)))/Z(t) - (vx + wy*Z(t) - wz*y*Z(t))/Z(t) (y*(vz - wy*x*Z(t) + wx*y*Z(t)))/Z(t) - (vy - wx*Z(t) + wz*x*Z(t))/Z(t)
上では明らかにZで約分できるにもかかわらず約分されていないのが気になります。
小手先テクですがsimplify関数を適用することで整理が可能です。
diff_p_xy = simplify(subs(diff_p_XYZ ,before,after)) diff_p_xy = wz*y - vx/Z(t) - wy - wy*x^2 + (vz*x)/Z(t) + wx*x*y wx - vy/Z(t) - wz*x + wx*y^2 + (vz*y)/Z(t) - wy*x*y
多項式から 行列形式への変形
最終的には画像内の点の動きd[u,v]/dtとカメラの動きの対応を以下のように行列形式で扱います。その時の変換行列が画像ヤコビ行列というやつです。
先程求めたのは多項式ですので，equationToMatrixという関数を使います。
% 便宜上の数式変換なのでぶっちゃけ右辺は何でも良い eqns = [diff_p_xy == [0;0]]; % 抜き出す変数 vars = [vx vy vz wx wy wz]; % 係数行列（ヤコビ行列の抽出） J = equationsToMatrix(eqns,vars)
結果がこちら。計算ミスなく簡単に導出できますね。
J = [ -1/Z(t), 0, x/Z(t), x*y, - x^2 - 1, y] [ 0, -1/Z(t), y/Z(t), y^2 + 1, -x*y, -x]
まとめ
このように式をこねくり回すのもMATLABに任せれば良いと言いたいところですが，結局計算ミスの代わりにプログラムのデバッグが必要になります。 特に添字などの間違いは見逃しやすいのでうまいこと定義した数式を見やすくするなどしてデバッグのコストを下げたいところです。
symbolicを直接latexに変換するといったプラグインも開発されてたりするそうなのでうまいこと活用して楽ができればいいですね。
prettyコマンドの活用
数式を少し見やすくするためのコマンドが一応用意されています。
これ本当に見やすいかな？？？
>> pretty(diff_p) / d d \ | -- X(t) X(t) -- Z(t) | | dt dt | | ------- - ------------ | | Z(t) 2 | | Z(t) | | | | d d | | -- Y(t) Y(t) -- Z(t) | | dt dt | | ------- - ------------ | | Z(t) 2 | \ Z(t) /
参考文献
[1] P.I.Corke先生のYotube解説より抜粋 url
[2] 橋本先生の「Visual Servo - IV - Feature Based Visualservo（ビジュアルサーボ：特徴ベースビジュアルサーボ）」より抜粋
