佐藤さとる

梶研 [関節を座標に & 正面を出したい]

2024年05月07日

関節を座標に & 正面を出したい

出席率

3年セミナー：??%

スケジュール

短期的な予定

長期的な予定

~?月シーン検知?をする
~?月論文を書く
~?月論文発表したい

進捗報告

目的

料理中の動作を mocopi を使ってセンシングする。
このデータから最終的に位置推定を行う。

一定の区間でどの動作をしているかを当てる (クラス分類)
料理の手順を元にシーン検知を補正する
- 例) 焼く動作 → 卵割る動作はおかしい
位置とシーンを相補的に補正する
- 例) 冷蔵庫の前で焼く動作はおかしい

前回

骨格を表示したら、はちゃめちゃなった
image.png (80.7 kB)

角度の単位を rad にしてみた
image.png (70.7 kB)
左肘以外は大丈夫ぽい？

blender で確認したら全然違った
スクリーンショット 2024-04-23 11.17.12.png (727.2 kB)

原因

単位は deg(度数法, はちゃめちゃな方) であってた
ベクトルの回転方法が正しくなかった

回転させる関数が、ベクトルのスタート位置(親関節)を考慮していなかった
例)
腰(ROOT) の座標が 0, 0, 0, つま先が 0, -90, 0 としたとき、
つま先を45度曲げても 0, -88, 2(適当) 程度のはずが、
この関数は 76.58131721, -47.27897899, 0 になってしまう

1def rotate(vec, roll, pitch, yaw):
2    r_x = np.array(
3        [[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]]
4    )
5    r_y = np.array(
6        [
7            [np.cos(pitch), 0, np.sin(pitch)],
8            [0, 1, 0],
9            [-np.sin(pitch), 0, np.cos(pitch)],
10        ]
11    )
12    r_z = np.array(
13        [[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]]
14    )
15
16    vec = np.dot(r_x, vec)
17    vec = np.dot(r_y, vec)
18    vec = np.dot(r_z, vec)
19
20    return vec

修正後

1def rotate(origin, end, roll, pitch, yaw):
2    vec = np.array(end) - np.array(origin)
3
4    ...
5
6    return vec + origin

人間が再現できる骨格だが、不思議な姿勢
image.png (65.2 kB)

回転をライブラリに任せる
image.png (64.1 kB)
image.png (71.8 kB)
腕がおかしい

リファクター

ライブラリを使って回転するようにした

1rot = Rotation.from_rotvec(np.array([y_rotate, x_rotate, z_rotate]))
2skeleton_copy[joint]["offset"] = rot.apply(data["offset"])

正解
スクリーンショット 2024-05-06 19.00.14.png (478.9 kB)
肩のあたりが少し違う
デフォルトの姿勢は+90度(?)された状態ぽい
→ 簡単に直せるが、動作認識では問題ない(と考えられる)ため気にしない

正面を出す

考え方

腰や肩の左右を通る面の直角方向を正面とする
全ての点に近似する平面の直角方向を正面とする

1 は簡単だが、捻る動作に弱いため 2 を使う
最小二乗法 というのを使えばいいらしい

1import numpy as np
2
3# y = w[0] + w[1]x[1] + w[2]x[2]型。
4#今回はw = [1, 2, 3]とする。
5X = np.random.random((100, 2)) * 10 # 0から1*10の範囲をとる100*2の行列
6y = 1 + 2 * X[:, 0] + 3 * X[:, 1] + np.random.randn(100)
7#x0とx1の座標からyを作成。randnで本来の値にノイズを加えている。
8
9import matplotlib.pyplot as plt
10from mpl_toolkits.mplot3d import axes3d # 3D表示に使う
11from scipy import linalg # linalg.solve(A, b) 　Ax = bの解を求める関数
12
13Xtil = np.c_[np.ones(X.shape[0]), X] # Xの行列の左端に[1,1,1,...,1]^Tを加える。(7)式を確認しよう
14A = np.dot(Xtil.T, Xtil) # 標準形A,bに当てはめる。
15b = np.dot(Xtil.T, y)
16w = linalg.solve(A, b) # (8)式をwについて解く。
17
18xmesh, ymesh = np.meshgrid(np.linspace(0, 10, 20),
19                            np.linspace(0, 10, 20))
20zmesh = (w[0] + w[1] * xmesh.ravel() +
21        w[2] * ymesh.ravel()).reshape(xmesh.shape)
22
23fig = plt.figure()
24ax = fig.add_subplot(111, projection='3d')
25ax.scatter(X[:, 0], X[:, 1], y, color='k')
26ax.plot_wireframe(xmesh, ymesh, zmesh, color='r')
27plt.show()