本記事の概要
2つのベクトルが成す角度、といえば内積などを利用して簡単に求められそうです。
ときにその鋭角を求めるだけでは済まない場合があり、条件分岐で頭もコードも大混乱になりました。
本記事では、そんな私が行きついた条件分岐をほぼゼロにした解決策をお伝えします。
長くなりますので、function編、OpenPose編に分けて投稿します。
本記事はfunction編です。
本記事のターゲット
- 角度の算出に困っている方
- 特に動作分析における関節角度の算出に困っている方
- 既に算出できていて、この記事の方法より優れた手法をご存知の方
目次
- 経緯
- 本functionの有用性
- functionと使用例
- function内部の解説
- 次回予告
- 余談
経緯
ここで扱うのは、2つのベクトルの成す角です。
(実際には右を向くヒトの足関節背屈角度を求めようとしていました。)
一般的な角度算出方法
一般に、原点O、点A、点Bからなる∠AOBは以下の式で求められます。
θ = arccos (\frac{x_A x_B + y_A y_B}{|A||B|})
この式はベクトルA、ベクトルBのx, y成分とも考えられますね。
今回のサンプルデータ
ここではそれぞれのベクトルを基本軸(base)、移動軸(move)とします。
(関節可動域は基本軸と移動軸の位置関係ですので、線分でなくベクトルで考える必要があります。)
ここで問題なのは、上の式で求められる角度は180°以下であることです。
足関節ならあまり問題はありませんが、肩関節屈曲-伸展などの広い可動域を持つ関節では屈曲180°以上や伸展域を適切に求めるための条件分岐が煩雑です。
肩関節矢状面角度算出時の条件分岐の例
求めた角度を以下の3つのケースに場合分けして調整する必要があります。
- 屈曲 0°~180°
- 屈曲 180°~
- 伸展域
基本軸と移動軸の位置関係で条件分岐できますがコードが煩雑になります。
さらに肩関節でも内-外転、また他関節の角度を求めるとなると、各関節角度算出時に条件分岐が必要で面倒です。私は最初、条件分岐で11関節角度を求めるコードを書いたところコードは600行まで膨らみました...
本funuctionの有用性
行きついたのは、常に基本軸から移動軸へ時計回りの角度を求めることでした。
「時計回り」と固定するのがミソです。
追加で関節角度を求めようとする際、この手法を使えば関節角度の追加算出ひとつにつきコード1行の追加で済みます。いわば、煩雑だった条件分岐を無くす手法です。
角度を求めるfunction
今回はMATLAB言語で書くこととします。
入力する座標を準備
構造体で整理しつつ座標を定義します。(上図と対応させます)
base = struct ('x1', 2, 'y1', 2, ...
'x2', 5, 'y2', 17);
move = struct ('x1', 1, 'y1', -1, ...
'x2', 16, 'y2', -2);
MATLAB初学者にはこの記法のほうが分かりやすいか
base.x1 = 2; base.y1 = 2; base.x2 = 5; base.y2 = 17; move.x1 = 1; move.y1 = -1; move.x2 = 16; base.y2 = -2;
functionと使用例
function result = coo2angle (base, move)
q.tmp = [11; 1; 0; 10]; % 象限No. [1, 2, 3, 4]
q.k.base = [0,1; -1,0; 0,-1; 1,0]; % 基本軸部の角度算出時の係数
q.k.move = [1,0; 0,1; -1,0; 0,-1]; % 移動軸部の角度算出時の係数
base.tmp = ((base.x2 - base.x1) > 0)*10 + ((base.y2 - base.y1) > 0);
move.tmp = ((move.x2 - move.x1) > 0)*10 + ((move.y2 - move.y1) > 0);
% 角度算出は、関節座標に欠損値が含まれる場合にはスキップ(そのフレームでは関節座標を算出しない)
if ~any (isnan ([base.tmp, move.tmp]))
base.quadID = find (q.tmp==base.tmp, 1);
move.quadID = find (q.tmp==move.tmp, 1);
quadBetw = mod (move.tmp - base.tmp, 4); % 基本軸~移動軸でいくつの象限をまたいでいるか
% 基本軸から、反時計回りにその象限の境界までの角度
k = q.k.base(base.quadID, :);
angle.base = acosd (((base.x2 - base.x1) * k(1) + (base.y2- base.y1) * k(2)) ...
/ sqrt((base.x2 - base.x1)^2 + (base.y2- base.y1)^2));
clearvars k;
% 移動軸から、時計回りにその象限の境界までの角度
k = q.k.move(move.quadID, :);
angle.move = acosd ((k(1) * (move.x2 - move.x1) + k(2) * (move.y2- move.y1)) ...
/ sqrt((move.x2 - move.x1)^2 + (move.y2- move.y1)^2));
clearvars k;
angle.betw = (quadBetw - 1) * 90; % 基本軸~移動軸の間にある象限の数 *90
% ここまで3つに分けて算出した角度を合算
result = mod (angle.base + angle.betw + angle.move, 360); % 負にならないようにmod
else
result = nan;
end
end
使用例
先に定義した変数base
, move
を、自作関数coo2angle
の引数にして実行します。
(base
, move
を構造体にしたのは、計8つの座標をfunctionの引数に指定すると長ったらしいからです。)
result = coo2angle (base, move);
disp (result);
277.4959
上の図の橙θ部分を277.4959
と求められました。
この角度は時計回りですので負になりません。(0 ≦ result
< 360)
function内部の解説
それぞれ象限の取得
base.tmp = ((base.x2 - base.x1) > 0)*10 + ((base.y2 - base.y1) > 0);
move.tmp = ((move.x2 - move.x1) > 0)*10 + ((move.y2 - move.y1) > 0);
基本軸ベクトル・移動軸ベクトルが、始点を原点に平行移動させた際に終点がどの象限にあるかを調べています。
式の通り、十の位がx、一の位がyに対応しています。
例えば、base.tmp == 1
の場合は第4象限、base.tmp == 11
の場合は第1象限に対応しています。
もし式に含まれるそれぞれ4つの座標のうち1つでもnanがあれば、それぞれの値はnanになります。
今回の例
disp (base.tmp); 11 disp (move.tmp); 1
基本軸と移動軸の位置関係を取得
q.tmp = [11; 1; 0; 10]; % 象限No. [1, 2, 3, 4]
base.quadID = find (q.tmp == base.tmp, 1);
move.quadID = find (q.tmp == move.tmp, 1);
quadBetw = mod (move.tmp - base.tmp, 4);
q.tmp
は、角度を時計回りに求めるための、象限の順序を規定する変数です。
base.quadID
は、基本軸の象限がq.tmp
の何番目にあるかを示します。
move.quadID
は、移動軸の象限がq.tmp
の何番目にあるかを示します。
quadBetw
は、移動軸の象限が基本軸の象限のいくつ先にあるかを示します。先なので、負にならないようにmod
で正にしつつ時計回りを保っています。
今回の例
disp (base.quadID); 1 disp (move.quadID); 4 disp (quadBetw); 3
それぞれ象限内での角度を算出
q.k.base = [0,1; -1,0; 0,-1; 1,0]; % 基本軸部の角度算出時の係数
q.k.move = [1,0; 0,1; -1,0; 0,-1]; % 移動軸部の角度算出時の係数
% 基本軸から、反時計回りにその象限の境界までの角度
k = q.k.base(base.quadID, :);
angle.base = acosd (((base.x2 - base.x1) * k(1) + (base.y2- base.y1) * k(2)) ...
/ sqrt((base.x2 - base.x1)^2 + (base.y2- base.y1)^2));
clearvars k;
% 移動軸から、時計回りにその象限の境界までの角度
k = q.k.move(move.quadID, :);
angle.move = acosd ((k(1) * (move.x2 - move.x1) + k(2) * (move.y2- move.y1)) ...
/ sqrt((move.x2 - move.x1)^2 + (move.y2- move.y1)^2));
clearvars k;
angle.base
は、象限内の基本軸からの時計回りの角度を示します。
angle.move
は、象限内の移動軸までの時計回りの角度を示します。
q.k.base
q.k.move
およびk
の解説は、少し煩雑ですので以下トグル内を参照ください。
係数の解説
座標から鋭角を求める式は以下のものでした。
θ = arccos (\frac{x_A x_B + y_A y_B}{|A||B|})
基本軸部の角度(angle.base)
ベクトルBを以下になるように定義します。
- 始点:基本軸の始点と同じ
- 終点:基本軸から時計回りでの象限の境界
- 長さ:1
このとき、ベクトルBの成分は基本軸の存在する象限により4通りあります。
q.k.base = [0,1; -1,0; 0,-1; 1,0]; % 基本軸部の角度算出時の係数 k = q.k.base(base.quadID, :);
q.k.base
はこの際のxB, yBに対応しています。
base.quadID
は、基本軸がどの象限にあるかを示すものでした。
これらをもとにxB, yBであるk
を求めます。
またベクトルBの長さは1になるので、分母も簡潔になります。移動軸部の角度(angle.move)
基本軸部とほぼ同じですが、移動軸部ではベクトルAを定義します。
clearvars k;
はそれぞれの角度算出時にk
を上書きするので省略できますが、同じ変数名を使っているため可読性向上とエラー回避のために実行しています。
今回の例
disp (angle.base); 11.3099 disp (angle.move); 86.1859
基本軸象限と移動軸象限の間の角度を算出
angle.betw = (quadBetw - 1) * 90; % 基本軸~移動軸の間にある象限の数 *90
quadBetw
は、移動軸象限と基本軸象限の差を示し、[0, 1, 2, 3]
のいずれかです。
quadBetw
とangle.betw
は以下のように対応します。
quadBetw |
angle.betw |
---|---|
0 | -90 |
1 | 0 |
2 | 90 |
3 | 180 |
ここで、quadBetw == 0
つまり2つの軸が同じ象限にある場合を説明します。
このとき2つのケースがあります。
- ケース1:0 ≦
angle.betw
< 90 - ケース2:270 ≦
angle.betw
< 360
しかしこれらの条件分岐は、次のステップでのmod
により解消できます。
今回の例
disp (angle.betw); 180
3つの角度を合計
result = mod (angle.base + angle.betw + angle.move, 360);
3つに分けて算出した角度を合計します。
この際、前のステップで問題になっていた箇所は、mod
で負にならないようにすることで解消できます。
次回予告
本記事で紹介した自作関数coo2angle
で、関節角度を求めてみます。
一部先出しすると、右向きのヒトの股関節屈曲角度はcoo2angle(base, move) - 180;
となります。
関数coo2angle
の恩恵により、求める角度によりこのように式を調整するだけで済み、細かい条件分岐が不要です。
余談
自作関数coo2angle
内にはひっそりと条件分岐が3つあります。
うち2つはmod
により、1つはq_tmp
を軸としたfind
により、if
やswitch
の登場無く実行できます。
そのぶん処理実行時間はわずかに短縮できています。
やや可読性は劣りますが、シンプルにする方法として私はよく使います。
また本記事と次回で紹介するコードは、11関節の座標を求めるものとして、初稿の600行から改良して100行強にまで簡潔にさせたものです。
以上、初投稿でしたが次回の記事も近々挙げますので、ご意見ございましたらお寄せください!