KIHARA BLOG：社内ラズコンでめざせ優勝！

Raspberry Pi（ラズパイ）で機械学習とか音声認識とか姿勢推定とかしながら組み込みシステムを構築して、社内ラズコンで優勝をめざすブログです。勉強中：Raspberry Pi、Linux、Python、Coral EdgeTPU、PoseNet、Julius

ラズパイ６日目②：カメラモジュールから動画撮影してポーズを判定する

Raspberry Pi EdgeTPU Coral PoseNet Python

Raspberry Pi + Edge TPU + PoseNetで、カメラモジュールで撮影した動画をリアルタイムに姿勢推定させてみます。
さらに、特定のポーズを判定してTerminalに出力するプログラムを作成しました。

作ったもの


右手を上げたら「POSE:RIGHT HAND UP」、左手を上げたら「POSE:LEFT HAND UP」、両手を上げたら「POSE:BANZAI」を出力するプログラムです。また、顔部分には円形オブジェクトを重ねています。

カメラモジュールで撮影した動画をリアルタイムに姿勢推定する

この部分は用意されているサンプルプログラム pose_camera.py をそのまま流用したので割愛します。

特定のポーズを判定する

苦労したのがこの部分。Keypointの座標からどうやってポーズを判定させればよいものか…

zigsow.jp

このあたりのサイトを参考しようとするも、リンク先「やってみよう！Kinectアプリ開発」のサイトは閉鎖されていて詳細がわからない。 （英語サイトはいくつか見つけたが読む気が起きず… 技術者にはやはり英語力が必要だ…）

それでも

関節の向きから方向ベクトルを取得する方法と、関節の向き同士の角度を調べるために内積を計算する方法

てな一文があったので、「ベクトル　内積　角度」で検索けんさく

nomoreretake.net

つまりこういう手順で関節の角度が求められる、と。

ベクトルの内積から関節角度を求める

numpyを利用して左右ひじ関節、左右ひざ関節の角度を求めることができました。 

import numpy as np

def calculate_jointAngles(pose):
    angles = []
    
    #左ひじの角度
    x1=pose.keypoints.get('left wrist').yx[1]
    y1=pose.keypoints.get('left wrist').yx[0]
    x0=pose.keypoints.get('left elbow').yx[1]
    y0=pose.keypoints.get('left elbow').yx[0]
    x2=pose.keypoints.get('left shoulder').yx[1]
    y2=pose.keypoints.get('left shoulder').yx[0]
    
    deg=np.round(inner_Calc(x0, x1, x2, y0, y1, y2))
    angles.append(deg)
    
    #右ひじの角度
    x1=pose.keypoints.get('right wrist').yx[1]
    y1=pose.keypoints.get('right wrist').yx[0]
    x0=pose.keypoints.get('right elbow').yx[1]
    y0=pose.keypoints.get('right elbow').yx[0]
    x2=pose.keypoints.get('right shoulder').yx[1]
    y2=pose.keypoints.get('right shoulder').yx[0]
    
    deg=np.round(inner_Calc(x0, x1, x2, y0, y1, y2))
    angles.append(deg)
    
    #左ひざの角度
    x1=pose.keypoints.get('left ankle').yx[1]
    y1=pose.keypoints.get('left ankle').yx[0]
    x0=pose.keypoints.get('left knee').yx[1]
    y0=pose.keypoints.get('left knee').yx[0]
    x2=pose.keypoints.get('left hip').yx[1]
    y2=pose.keypoints.get('left hip').yx[0]
    
    deg=np.round(inner_Calc(x0, x1, x2, y0, y1, y2))
    angles.append(deg)
    
    #右ひざの角度
    x1=pose.keypoints.get('right ankle').yx[1]
    y1=pose.keypoints.get('right ankle').yx[0]
    x0=pose.keypoints.get('right knee').yx[1]
    y0=pose.keypoints.get('right knee').yx[0]
    x2=pose.keypoints.get('right hip').yx[1]
    y2=pose.keypoints.get('right hip').yx[0]
    
    deg=np.round(inner_Calc(x0, x1, x2, y0, y1, y2))
    angles.append(deg)
        
    return angles
    
    
def inner_Calc(x0, x1, x2, y0, y1, y2):
    if all([x0, x1, x2, y0, y1, y2]):
        va=np.array([x1-x0,y1-y0])
        vb=np.array([x2-x0,y2-y0])
        
        innr=np.inner(va,vb)
        nrm=np.linalg.norm(va)*np.linalg.norm(vb)    
        deg = np.rad2deg(np.arccos(np.clip(innr/nrm, -1.0, 1.0)))
    
    else:
        deg = -1
        
    return deg

ポーズを判定する

関節の角度が取得できたので、次はポーズ判定させてみます。「右手をあげた状態」を
* 右ひじ関節の角度 >= 150° * 右手首が右目よりも上にある
として判定させます。

「左手をあげた状態」「両手を上げた状態」も同様にして判定させます。

def pose_check(pose,angles):
    result = ''
    rHandUp = 0
    lHandUp = 0
    
    if angles[1]>150 and pose.keypoints.get('right wrist').yx[0] < pose.keypoints.get('right eye').yx[0]:
        rHandUp=1
    
    if angles[0]>150 and pose.keypoints.get('left wrist').yx[0] < pose.keypoints.get('left eye').yx[0]:
        lHandUp=1
    
    if rHandUp==1 and lHandUp==1:
        result='BANZAI'
    elif rHandUp==1:
        result='RIGHT HAD UP'
    elif lHandUp==1:
        result='LEFT HAD UP'
    
    return result

単純なポーズなら判定できることがわかりました。
ただ、判定結果には若干ゆれがあるので、「一定フレーム数以上で同じポーズ判定をされたら正式に認定する」みたいな処理をしないと使えなさそうです。