2つの3次元ベクトルからBVHファイルのrotationを算出したい

実現したいこと

pythonで2つのxyzの3次元ベクトルbvhファイルを生成したいので、
角度を計算してMOTIONのXrotation,Yrotation,Zrotationに適切な値を設定したいです。

発生している問題・分からないこと

動かした時におかしな挙動になってしまいます。
MOTIONを設定せず、OFFSETのみ設定した時の初期ポーズは適切に出力できるため事前の座標取得はうまくいっていると思うのですが、MOTIONの角度を設定した時に動かした時にうまくいきません。どうすれば良いでしょうか

こちらの動画の腕の動きを取得したいです
動画
#動画の動きはmediapipeを使って取得しており、理想としてはこのような動きを取得したいです。
出力したい動き

bvhのMOTION部分には各要素の角度が100フレーム分ありますが、文字数制限のため6フレームのみ記入しています、

該当のソースコード

python
1import numpy as np
2import math
3
4#numpy配列をスペースありのstring文字列に変換
5def np2str(array):
6    for i in range(len(array)):
7        array[i]=round(array[i], 3)
8        
9    #array[1],array[2]=array[2],array[1]
10
11    out=" ".join(map(str,array))
12    
13    return out
14
15#numpy配列をスペースありのstring文字列に変換
16def np2strForList(array):
17    out=[]
18    for arr in array:
19        out.append(" ".join(map(str,arr)))
20    
21    
22    out=" ".join(map(str,out))
23
24   
25    return out
26
27
28#xyzの角度を取得
29def rotxyz(vec1,vec2):
30    
31    #各2次元ベクトル
32    vec1xy=np.array([vec1[0],vec1[1],0])
33    vec1yz=np.array([0,vec1[1],vec1[2]])
34    vec1zx=np.array([vec1[0],0,vec1[2]])
35    vec2xy=np.array([vec2[0],vec2[1],0])
36    vec2yz=np.array([0,vec2[1],vec2[2]])
37    vec2zx=np.array([vec2[0],0,vec2[2]])
38    #各長さ
39    vec1xyLen=np.linalg.norm(vec1xy)
40    vec1yzLen=np.linalg.norm(vec1yz)
41    vec1zxLen=np.linalg.norm(vec1zx)
42    vec2xyLen=np.linalg.norm(vec2xy)
43    vec2yzLen=np.linalg.norm(vec2yz)
44    vec2zxLen=np.linalg.norm(vec2zx)
45    #各内積
46    xInner=np.dot(vec1yz,vec2yz)
47    yInner=np.dot(vec1zx,vec2zx)
48    zInner=np.dot(vec1xy,vec2xy)
49
50    #各角度
51    #xrot=np.arccos(xInner/(vec1yzLen*vec2yzLen))
52    #yrot=np.arccos(yInner/(vec1zxLen*vec2zxLen))
53    #zrot=np.arccos(zInner/(vec1xyLen*vec2xyLen))
54
55    xrot=(xInner/(vec1yzLen*vec2yzLen))
56    yrot=(yInner/(vec1zxLen*vec2zxLen))
57    zrot=(zInner/(vec1xyLen*vec2xyLen))
58
59    if math.isnan(xrot):
60        xrot=0
61    if math.isnan(yrot):
62        yrot=0
63    if math.isnan(zrot):
64        zrot=0
65    
66    xDeg=math.degrees(xrot)
67    yDeg=math.degrees(yrot)
68    zDeg=math.degrees(zrot)
69    xDeg=round(xDeg, 3)
70    yDeg=round(yDeg, 3)
71    zDeg=round(zDeg, 3)
72    #帰り値は全てfloat型
73    return xDeg,yDeg,zDeg
74
75#フレーム数
76frames=[[[0.167, -0.453, 0.051], [-0.128, -0.474, 0.047], [0.224, -0.255, -0.007], [-0.256, -0.29, 0.014], [0.35, -0.366, -0.179], [-0.358, -0.388, -0.155]], [[0.17, -0.453, 0.049], [-0.128, -0.475, 0.044], [0.225, -0.26, -0.018], [-0.256, -0.294, 0.006], [0.347, -0.372, -0.194], [-0.358, -0.404, -0.163]], 
77        [[0.172, -0.453, 0.049], [-0.128, -0.475, 0.044], [0.229, -0.261, -0.019], [-0.26, -0.299, 0.006], [0.347, -0.374, -0.196], [-0.358, -0.416, -0.163]], [[0.177, -0.453, 0.054], [-0.126, -0.475, 0.052], [0.235, -0.261, -0.015], [-0.262, -0.306, 0.022], [0.352, -0.373, -0.191], [-0.359, -0.426, -0.129]],
78          [[0.181, -0.453, 0.051], [-0.124, -0.475, 0.047], [0.243, -0.261, -0.016], [-0.262, -0.312, 0.012], [0.354, -0.374, -0.191], [-0.355, -0.432, -0.137]], [[0.184, -0.453, 0.051], [-0.122, -0.476, 0.046], [0.248, -0.261, -0.016], [-0.263, -0.318, 0.014], [0.362, -0.374, -0.191], [-0.347, -0.437, -0.129]]]
79
80
81for frame in range(len(frames)):
82    for i1 in range(6):
83        for i2 in range(3):
84             frames[frame][i1][i2]=round(frames[frame][i1][i2],3)
85print((frames))
86
87
88
89
90
91
92VectorUp=np.array([1,0,0])
93Motions=[]
94#フレームごとに取得
95for index, landmarks in enumerate(frames):
96
97    
98    #位置の設定。（L,Rは絶対位置、Left,Rightは相対位置）
99    #部位ごとに取得＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝
100    Base=np.array([0,0,0])
101    #上半身ーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーー
102    LShoulder=np.array(landmarks[0])# 左肩
103    RShoulder=np.array(landmarks[1])# 右肩
104    LElbow=np.array(landmarks[2])# 左肘
105    RElbow=np.array(landmarks[3])# 右肘
106    LWrist=np.array(landmarks[4])# 左手首
107    RWrist=np.array(landmarks[5])# 右手首
108    HeartAbsol=((LShoulder+RShoulder)/2)#胸（左右の肩の中間点）
109
110
111    if index==0:#初期オフセット設定
112        
113       
114        #体の初期位置================================================================
115        LeftShoulder_origin=LShoulder-HeartAbsol#左腰
116        RightShoulder_origin=RShoulder-HeartAbsol#左腰
117        LeftElbow_origin=LElbow-LShoulder#右ひじ
118        RightElbow_origin=RElbow-RShoulder#左ひじ
119        LeftWrist_origin=LWrist-LElbow#右手首
120        RightWrist_origin=RWrist-RElbow#左手首
121
122
123    root=[0,0,0,0,0,0]#座標、回転
124    #腰から上半身、右足、左足の三つに分かれる
125    #左足================================================================
126    LeftShoulder=LShoulder-HeartAbsol#左腰
127    RightShoulder=RShoulder-HeartAbsol#左腰
128    LeftElbow=LElbow-LShoulder#右ひじ
129    RightElbow=RElbow-RShoulder#左ひじ
130    LeftWrist=LWrist-LElbow#右手首
131    RightWrist=RWrist-RElbow#左手首
132
133    #角度
134    #左
135    LeftShoulder_rot=rotxyz(VectorUp,LeftShoulder)
136    LeftElbow_rot=rotxyz(LeftShoulder,LeftElbow)
137    LeftWrist_rot=rotxyz(LeftElbow,LeftWrist)
138
139    #右
140    RightShoulder_rot=rotxyz(VectorUp,RightShoulder)
141    RightElbow_rot=rotxyz(RightShoulder,RightElbow)
142    RightWrist_rot=rotxyz(RightElbow,RightWrist)
143
144
145
146    #モーション作成
147    Motions.append(np2strForList([
148                    np.array([0,0,0]),np.array([0,0,0]),
149                    LeftShoulder_rot,LeftElbow_rot,LeftWrist_rot,
150                    RightShoulder_rot,RightElbow_rot,RightWrist_rot
151                    ]))
152
153
154bvh_content_complex_japanese = f"""
155HIERARCHY
156ROOT Base
157{{
158    OFFSET 0.00 0.00 0.00 	
159    CHANNELS 6 Xposition Yposition Zposition Zrotation Yrotation Xrotation
160    JOINT LeftShoulder
161    {{
162        OFFSET 0.00 0.00 0.00
163        CHANNELS 3 Zrotation Yrotation Xrotation
164        JOINT LeftElbow
165        {{
166            OFFSET {np2str(LeftShoulder_origin)}
167            CHANNELS 3 Zrotation Yrotation Xrotation
168            JOINT RightWrist
169            {{
170                OFFSET {np2str(LeftElbow_origin)}
171                CHANNELS 3 Zrotation Yrotation Xrotation
172                    End Site
173                    {{
174                        OFFSET {np2str(LeftWrist_origin)}
175                    }}
176            }}
177        }}
178    }}
179    JOINT RightShoulder
180    {{
181        OFFSET 0.00 0.00 0.00
182        CHANNELS 3 Zrotation Yrotation Xrotation
183        JOINT RightElbow
184        {{
185            OFFSET {np2str(RightShoulder_origin)}
186            CHANNELS 3 Zrotation Yrotation Xrotation
187            JOINT RightWrist
188            {{
189                OFFSET {np2str(RightElbow_origin)}
190                CHANNELS 3 Zrotation Yrotation Xrotation
191                        End Site
192                        {{
193                            OFFSET {np2str(RightWrist_origin)}
194                        }}
195            }}
196        }}
197    }}
198}}
199MOTION
200Frames: {len(frames)}
201Frame Time: 0.0333333
202"""
203
204
205
206
207#BVHファイル書き込み
208bvh_complex_japanese_file_path = './BVHfile/bvh_check.bvh'
209with open(bvh_complex_japanese_file_path, 'w') as file:
210    file.write(bvh_content_complex_japanese.strip())
211    file.write("\n")#改行
212
213    for i in Motions:
214        file.write(i)
215        file.write("\n")#改行
216
217        pass

以下が出力されるbvhファイルです

bvh
1HIERARCHY
2ROOT Base
3{
4    OFFSET 0.00 0.00 0.00 	
5    CHANNELS 6 Xposition Yposition Zposition Zrotation Yrotation Xrotation
6    JOINT LeftShoulder
7    {
8        OFFSET 0.00 0.00 0.00
9        CHANNELS 3 Zrotation Yrotation Xrotation
10        JOINT LeftElbow
11        {
12            OFFSET 0.148 0.011 0.002
13            CHANNELS 3 Zrotation Yrotation Xrotation
14            JOINT RightWrist
15            {
16                OFFSET 0.057 0.198 -0.058
17                CHANNELS 3 Zrotation Yrotation Xrotation
18                    End Site
19                    {
20                        OFFSET 0.126 -0.111 -0.172
21                    }
22            }
23        }
24    }
25    JOINT RightShoulder
26    {
27        OFFSET 0.00 0.00 0.00
28        CHANNELS 3 Zrotation Yrotation Xrotation
29        JOINT RightElbow
30        {
31            OFFSET -0.148 -0.01 -0.002
32            CHANNELS 3 Zrotation Yrotation Xrotation
33            JOINT RightWrist
34            {
35                OFFSET -0.128 0.184 -0.033
36                CHANNELS 3 Zrotation Yrotation Xrotation
37                        End Site
38                        {
39                            OFFSET -0.102 -0.098 -0.169
40                        }
41            }
42        }
43    }
44}
45MOTION
46Frames: 6
47Frame Time: 0.0333333
480 0 0 0 0 0 0.0 57.291 57.151 51.0 39.603 19.72 -16.282 56.699 -24.503 0.0 -57.291 -57.151 -53.507 55.67 29.297 -19.541 40.915 -8.992
490 0 0 0 0 0 0.0 57.288 57.14 48.617 35.606 19.717 -13.207 57.107 -25.697 0.0 -57.288 -57.14 -52.07 55.192 29.548 -20.722 42.343 -11.809
500 0 0 0 0 0 0.0 57.288 57.142 48.426 36.07 20.28 -12.94 56.952 -26.212 0.0 -57.288 -57.142 -51.933 55.316 30.933 -21.937 41.332 -13.064
510 0 0 0 0 0 0.0 57.295 57.145 51.944 36.577 20.497 -12.6 56.935 -25.959 0.0 -57.295 -57.145 -55.276 56.031 32.594 -27.259 40.624 -12.133
520 0 0 0 0 0 0.0 57.291 57.147 49.847 38.36 21.484 -13.486 56.356 -26.559 0.0 -57.291 -57.147 -52.964 55.717 33.78 -25.769 41.355 -11.885
530 0 0 0 0 0 0.0 57.288 57.135 48.852 38.894 22.142 -13.486 56.317 -25.397 0.0 -57.288 -57.135 -52.458 56.075 34.838 -27.178 39.234 -12.924
54

試したこと・調べたこと

teratailやGoogle等で検索した
ソースコードを自分なりに変更した
知人に聞いた
その他

上記の詳細・結果

出力したbvhファイルををblenderにインポートして動かしました。
右が上記のbvhをインポートしたもの、左がbvhファイルのFrame Time: 0.0333333以降を削除したものです。左のような動きを出力したいです

補足

仕様を確認したもの①

仕様を確認したもの②

ikedas

2024/03/03 12:12

> MOTIONの角度を設定した時に動かした時にうまくいきません。・入力されるデータの具体的な例・そのデータが与えられたときに得られるはずの出力の具体的な値・実際に出力されたものの具体的な値を記してください。なお、このコメント欄に書くのではありません。質問文を編集して書いてください。

ikedas

2024/03/03 13:40

100行の一部でもいいので・入力されるデータの具体的な例・そのデータが与えられたときに得られるはずの出力の具体的な値・実際に出力されたものの具体的な値を記してください。

ikedas

2024/03/04 09:33

・出力されたMOTIONの数値だと各関節が1フレームで数十度もの回転をするという意味になってしまいますから、ご提示のコードでの計算が間違っていることは明らかです。コードをよく見直してください。・まずは計算を手でも行ってみて、プログラムが出す結果を比べるこることで正しく計算できているか確認してはどうでしょうか。・最初はもっと単純なデータで動作確認してはどうでしょうか。つまり、、 1. まずはベースと左肩だけの1ボーンのみ実装し、最初の1フレーム分の変化を正しく計算できるようにする。 2. 次に右肩を加えて、同様にする。 3. …… …… 7. 全体が正しく動いたら、次のフレームのデータも加えて確認する。といった具合です。もしもご自分で解決できた場合は、どうやって解決したかを自分で回答として投稿し、質問を解決済みにしておいてください。

行動規範の内容に同意します

回答1件

回答ではないですが、ご提示のコードについて気づいた点を指摘します (なお、arccos関数の呼び出しを取り除いたら正しい結果にならないという点は自明なので置いておきます)。

#1

オイラー角の求めかたが正式な方法でないように思います。

コードで採用している計算方法は、z-x-z系やz-y-z系のような正規オイラー角ではないですし、x-y-z系やz-y-x系のようなカルダン角 (テイト-ブライアン角) とも違います ~~(BVHでどちらの方式のどういう系を前提としているのかは、私は知らないです)~~。

【2024-03-05追記】Jeff Thingvold (1999) “Biovision BVH” という資料がBVHフォーマットの説明としては最も信頼できそうです (著者は画像解析の専門家です)。これによると (訳)

(オイラー角のデータから実際の回転を実行するために必要な) 回転行列を作成するわかりやすい方法は、回転の軸毎にひとつずつ、3つの別々の回転行列を作成することだ。そして、それを左から右へY、X、Zの順に連結する。

と言っています。ということで、BVHのMOTIONでのオイラー角というのはy-x-z系のカルダン角であると考えられます。【追記終わり】

ただし、コードのような方法 (x、y、zそれぞれの軸についての射影ベクトル同士が成す角を求める) でも、微小な回転については十分な近似になるのかもしれません。もっともそういう意図で採用した方法ではないのなら、これが原因で期待した結果にならないということも考えられます。

#2

回転の向きを判別できていません。

ご提示のコードでは、ベクトルa = [ax, ay], b = [bx, by] に対し、それらの内積 (スカラー積) に

a・b = ax*bx + ay*by = |a| |b| cos(θ)
(ただし |a|、|b| はa、bのノルム、θはaとbが成す角)

という関係があることから、θを求めています。しかし内積は可換な演算なので、この方法だとどちら向きに回転しているかはわからないですね。

一方、a、bの外積 (ベクトル積) には

a × b = (ax*by - bx*ay) n = |a| |b| sin(θ) n
(ただしnはz軸方向の単位ベクトル)

という関係があります。これを使ってθを求めれば回転の方向もわかります (θの正負で方向が判別できます)。ただし今度は角度が90度を超えたかどうかを判別できませんが、1フレームの間にそんなに大きく回転しないのであれば問題ないでしょう。

【2024-03-11追記】さらに、θが微小な角度である場合は sin(θ) = θ とみなしてしまってもいいですよね。そうすると上の関係はさらに簡単になります。結局オイラー角 (の近似値) は、

python
1xrot = np.cross(vec1yz, vec2yz)[0] / (vec1yzLen * vec2yzLen)
2yrot = np.cross(vec1zx, vec2zx)[1] / (vec1zxLen * vec2zxLen)
3zrot = np.cross(vec1xy, vec2xy)[2] / (vec1xyLen * vec2xyLen)

のようにして計算できることになります (これはラジアン単位なので、度への換算は必要ですが)。【追記終わり】

#3

計算誤差が蓄積します。

作成したBVHデータを実際に実行すると、小さな角度の回転の計算を何度も繰り返すため、計算誤差が急速に蓄積すると考えられます。その結果、期待した通りの動きをしなくなるのではないかと考えます。いろいろ考えているうちに、この計算誤差の問題が一番大きいのではないかと思うようになりました。

#1ではオイラー角の求め方が正式なものではないことを指摘しましたが、そのことによる数値の不正確さは大したことではないと思われます。MediaPipeで取得できる座標データの精度は有効数字2、3桁程度のものでしょうから、どんなに正確な方式で角度を計算したところでかなり大きな誤差が避けられません。逆に言うと#1、#2で述べたような近似計算でも大勢に影響ないでしょう。

誤差を低減・解消する方法を考えてみました。

とりあえず、単純な一点だけの運動を考えます。方向ベクトルの系列vec0、vec1、vec2、vec3、……があるとします。

質問では次のようにして計算していると言えます。

vec0とvec1から、オイラー角rot0を計算する。
vec1とvec2から、オイラー角rot1を計算する。
vec2とvec3から、オイラー角rot2を計算する。
...

こうして得られたrot0、rot1、rot2、……を、MOTIONデータとして用いています。これだと、計算されるそれぞれの角度には座標の不正確さと計算による誤差が乗っているため、実際に動作させて小さな回転を次々に適用していくと、結果の方向ベクトルはどんどん元のデータとかけ離れていってしまいます。

そこで、次のようにしてはどうでしょうか。

vec0とvec1から、オイラー角rot0を計算する。
vec0をrot0で回転させたものをvec1pとする。
vec1pとvec2から、オイラー角rot1を計算する。
vec1をrot1で回転させたものをvec2pとする。
vec2pとvec3から、オイラー角rot2を計算する。
...

元の方向ベクトルそのものから角度を求めるのではなく、一つ前に計算した角度による回転を実際に適用してみた結果と次の方向ベクトルとの間の角度を求めます。このようにすれば直前の回転の計算による誤差が打ち消され、安定した動作をするようになるのではないかと考えます。

まとめ

オイラー角の計算方法として、質問で提示されたコードのような方法 (x、y、zそれぞれの軸についての射影ベクトル同士が成す角を求める) でも、微小な回転については十分な近似になるのかもしれません。
ただし、質問で提示されたコード (内積の性質を使う) では回転の向きが判別できていません。判別できる方法 (外積の性質を使う) に変える必要があります。
小さな回転を次々に適用していくと、誤差が蓄積して結果の方向ベクトルはどんどん元のデータとかけ離れていってしまいます。誤差を打ち消すような計算方法をとるべきです。

投稿2024/03/05 03:08

編集2024/03/12 23:19

ikedas

総合スコア4443

bajapan

2024/03/10 18:17 編集

回答ありがとうございます。数学にあまり詳しくないので申し訳ないのですが、「BVHのMOTIONでのオイラー角というのはy-x-z系のカルダン角である」というのはベクトルを(ax,ay,az)から(bx,by,bz)へ回転させたい場合、ベクトルの回転は R = Ry(θy)Rx(θx)Rz(θz) ただし、 Rx(θx) = [ 1 0 0 0 cos(θx) -sin(θx) 0 sin(θx) cos(θx) ] Ry(θy) = [ cos(θy) 0 sin(θy) 0 1 0 -sin(θy) 0 cos(θy) ] Rz(θz) = [ cos(θz) -sin(θz) 0 sin(θz) cos(θz) 0 0 0 1 ] θx = atan2(by, bz) θy = asin(ax / cos(θx)) θz = atan2(ay / ax, bx / ax) の回転で座標が求められるということでしょうか。また、θx,θy,θz を角度に変換したものがbvhファイルに書くべき値であるという認識であっていますでしょうか。

ikedas

2024/03/11 03:14

計算が合っているかどうかは私はわかりません。とにかく3つの軸のまわりの回転を表す行列を作ったら、それらをy-x-zの順に合成するということです (そうすると、実際に角度を算出するにはz-x-yの順に計算していかないといけないのかもしれないですね)。逆に、ご質問でご提示の計算方法 (x-y平面、y-z平面、z-x平面それぞれで2つのベクトルの射影が成す角度を求める) という方法をとっておられるのはどういう理由からでしょうか。何か参考にしたコードや解説があるのならその出所を提示してください (質問文に追記してください)。

行動規範の内容に同意します