MediaPipeでラジオ体操解析 ~肩の開きを可視化~

はじめに

初めまして！
織物産業が有名だった（最近盛り上がってきた？）とある町の大学で学生をしています。masuです。

初投稿になります。ここでは、学んだことを備忘録として残し、共有していこうと思います。
偉大な先輩プログラマーの方々からみたら不十分である箇所があるかと思います。見つけた際にはご指摘いただけますと幸いです。

現在、動作解析に興味があり、ラジオ体操に関する動作解析を行っています。
「腕を振って足を上下に曲げ伸ばす運動」において、腕の振り具合（肩の開きの角度）を可視化しようとしたところ、ベクトルの内積をもちいて算出すると、定義域の関係から腕が肩より高くなった際にうまく算出できませんでした。そこで、外積を用いると望んだとおりに結果が得られました。この方法について解説していきます。

開発環境

anaconda with VScode
MediaPipe
OpenCV
Numpy

MediaPipeとは

MediaPipeとは、Googleが開発したオープンソースのAIおよび機械学習ツールで、リアルタイムの画像処理を効率的に行うためのライブラリを提供しています。顔検出、手指の検出、姿勢推定などを行う様々な機能が実装されています。
動作解析は、取得した映像のフレームごとにし姿勢推定を行い、一連の関節の動きの変化を分析することで行います。

MediaPipeによって、各関節には以下の画像の通り番号が振られています。
図中で11が右肩、12が左肩、13が右ひじです。

https://ai.google.dev/edge/mediapipe/solutions/vision/pose_landmarker?hl=ja&_gl=1*c6fi6d*_up*MQ..*_ga*MjA0NzE3Nzc5MC4xNzQ1NDY2NTIw*_ga_P1DBVKWT6V*MTc0NTQ2NjUyMC4xLjAuMTc0NTQ2NjUyMC4wLjAuNzMwMDQ4MTM. より引用。

右肩の開き具合を考えます。
基準(v1 = baseline_vec)を、右肩[11]→左肩[12]のベクトルとし、
腕のベクトル(v2 = arm_vec)を、右肩[11]→右ひじ[13]ベクトルと定義しました。

ソースコード

def calculate_angle_between_vector(v1, v2):
    v1 = np.array(v1)
    v2 = np.array(v2)

    # 単位ベクトル化
    v1_unit = v1 / (np.linalg.norm(v1) + 1e-6)
    v2_unit = v2 / (np.linalg.norm(v2) + 1e-6)
    
    # 内積と外積
    dot = np.dot(v1_unit, v2_unit)
    cross = v1_unit[0] * v2_unit[1] - v1_unit[1] * v2_unit[0]  # 2D外積（z成分のみ）

    # atan2を使えば符号付きで角度が出せる！（-π〜+π）
    angle_rad = np.arctan2(cross, dot)
    angle_deg = np.degrees(angle_rad)
    
    # 常に 0〜360度に変換
    if angle_deg < 0:
        angle_deg += 360

    return angle_deg

大枠はコメントアウトの通りですが、自分の理解の確認を含め解説していきます。

①ベクトルをNumPy配列に変換

内積と外積の計算をしやすくするため、ベクトルをNumPy配列に変換します。

②v1とv2を単位ベクトルに変換

のちの計算で内積と外積を求める際に、cosΘとsinΘが楽に求まるため、ベクトルの正規化を行います。
正規化は、np.linalg.norm()を用いてベクトルの長さを算出し、元のベクトルにその逆数を掛けることで行います。normに1e-6の微小な数を足すことで、正規化の際に0で割ることを防いでいます。

③内積と外積を求める。

内積はnp.dot()を用いました。ベクトルの内積を英語では dot productというようです。一方、外積は英語でcross productというようです。

ここで、先の正規化の恩恵を受けます。
内積 v1_unit・v2_unit = ｜v1_unit｜｜v2_unit｜cosΘ = cosΘ
外積 v1_unit × v2_unit = ｜v1_unit｜｜v2_unit｜sinΘ = sinΘ

となり、内積と外積を求める計算で、sinΘとcosΘが求まりました。(動作解析やゲームを作る人の中では常識なんでしょうか...？)

次に、求まったcosΘとsinΘをarctan2()(略してatan2())に代入し、肩の開き具合の角度を得ます。

④arctan2()に代入し、肩の開き具合を求める

arctan()は、戻り値が0~πであるのに対し、atan2()は戻り値が-π~πと広いため、atan2()を用います。その後、戻り値がラジアンであるため、np.degrees()を用いて度数法に直します。

⑤角度を0~360°に変換

atan2()の返す角度は-180°~180°であるため、-180°~0°の範囲に360°を加算し、使いやすくします。

まとめ

最後まで読んでいただきありがとうございました。
少しでも参考になる部分があれば幸いです。
ラジオ体操、分解してみるといろいろな動きがあり分析しがいのある運動だと思いました。
「こんな算出方法もあるよ」、「こうするともっと読みやすいブログになるよ」等ご意見お待ちしております。
いつか、機械学習なんかも用いて、健康な人と高齢者や疾患のある人との分類も行っていきたい。

次の記事では、この初めての投稿をChatGPTに鬼教官になってもらい、ダメ出しをしてもらった結果を共有します。(泣)
ただの自己満ブログで終わらないために...（作成中）

付録

import cv2
import numpy as np
import mediapipe as mp
from collections import deque

# Mediapipe Pose のセットアップ
mp_pose = mp.solutions.pose
pose = mp_pose.Pose()
mp_drawing = mp.solutions.drawing_utils

maxlen = 100
r_shoulder_traj = deque(maxlen = maxlen)
l_shoulder_traj = deque(maxlen = maxlen)
r_elbow_traj = deque(maxlen = maxlen)
l_elbow_traj = deque(maxlen = maxlen)

# 記録フラグ
recording = False

cap = cv2.VideoCapture(0)

frame_width = cap.get(cv2.CAP_PROP_FRAME_WIDTH)
frame_height = cap.get(cv2.CAP_PROP_FRAME_HEIGHT)
if frame_width == 640 or frame_height == 480:
    scale_facter = 2
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, frame_width * scale_facter)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, frame_height * scale_facter)

# 内積を使うと、腕が肩より上がったときに角度が180°以下になる
# def calculate_angle_between_vector(v1, v2):
#     v1 = np.array(v1)
#     v2 = np.array(v2)

#     # cosθをベクトルのノルムと内積で計算
#     dot_product = np.dot(v1, v2)
#     norm_product = np.linalg.norm(v1)* np.linalg.norm(v2)

#     # θの除算対策と角度計算
#     cos_angle = np.clip(dot_product / (norm_product + 1e-6), -1.0, 1.0)
#     angle_rad = np.arccos(cos_angle)
#     return np.degrees(angle_rad)

# なので、外積を使う
def calculate_angle_between_vector(v1, v2):
    v1 = np.array(v1)
    v2 = np.array(v2) 

    # 単位ベクトル化
    v1_unit = v1 / (np.linalg.norm(v1) + 1e-6)
    v2_unit = v2 / (np.linalg.norm(v2) + 1e-6)
    
    # 内積と外積
    dot = np.dot(v1_unit, v2_unit)
    cross = v1_unit[0] * v2_unit[1] - v1_unit[1] * v2_unit[0]  # 2D外積（z成分のみ）

    # atan2を使えば符号付きで角度が出せる！（-π〜+π）
    angle_rad = np.arctan2(cross, dot)
    angle_deg = np.degrees(angle_rad)
    
    # 常に 0〜360度に変換
    if angle_deg < 0:
        angle_deg += 360

    return angle_deg



while cap.isOpened(): 
    success, frame = cap.read()
    if not success:
        break

    # 鏡表示 0：垂直方向反転　1：水平方向反転　2：両方向に反転
    # 反転してから全ての処理に使う
    #fliped_frame = cv2.flip(frame, 1)

    # 反転しない場合、以下を使用
    fliped_frame = frame
    image = cv2.cvtColor(fliped_frame, cv2.COLOR_BGR2RGB)
    results = pose.process(image)

    if results.pose_landmarks:
        h, w, _ = fliped_frame.shape
        # mp_pose.POSE_CONNECTIONS：関節同士を線で結ぶ
        mp_drawing.draw_landmarks(fliped_frame, results.pose_landmarks, mp_pose.POSE_CONNECTIONS)
        landmarks = results.pose_landmarks.landmark

        # landmarksに全関節の情報が格納されているので、そこから取得
        l_shoulder = landmarks[11]
        r_shoulder = landmarks[12]
        l_elbow = landmarks[13]
        r_elbow = landmarks[14]

        # Numpy配列で各ランドマークを取得：ベクトル用
        # landmarksに全関節の情報が格納されている
        l_shoulder_np = np.array([landmarks[mp_pose.PoseLandmark.LEFT_SHOULDER.value].x,
                               landmarks[mp_pose.PoseLandmark.LEFT_SHOULDER.value].y])
        r_shoulder_np = np.array([landmarks[mp_pose.PoseLandmark.RIGHT_SHOULDER.value].x,
                               landmarks[mp_pose.PoseLandmark.RIGHT_SHOULDER.value].y])
        l_elbow_np = np.array([landmarks[mp_pose.PoseLandmark.LEFT_ELBOW.value].x,
                            landmarks[mp_pose.PoseLandmark.LEFT_ELBOW.value].y])
        r_elbow_np = np.array([landmarks[mp_pose.PoseLandmark.RIGHT_ELBOW.value].x,
                            landmarks[mp_pose.PoseLandmark.RIGHT_ELBOW.value].y])
        
        # intで各ランドマークを取得：dequeやcv2.circleに渡す用
        l_shoulder_px = (int(l_shoulder.x * w), int(l_shoulder.y * h))
        r_shoulder_px = (int(r_shoulder.x * w), int(r_shoulder.y * h))
        l_elbow_px = (int(l_elbow.x * w), int(l_elbow.y * h))
        r_elbow_px = (int(r_elbow.x * w), int(r_elbow.y * h))

        # ベクトル作成
        vec_baseline_l_sholder = r_shoulder_np - l_shoulder_np
        vec_baseline_r_sholder = -(vec_baseline_l_sholder)
        vec_l_arm = l_elbow_np - l_shoulder_np
        vec_r_arm = r_elbow_np - r_shoulder_np

        # 角度を計算
        angle_r_shoulder = calculate_angle_between_vector(vec_baseline_r_sholder, vec_r_arm)
        angle_l_shoulder = calculate_angle_between_vector(vec_baseline_l_sholder, vec_l_arm)

        # 左側は回転が逆なので
        angle_l_shoulder = 360 - angle_l_shoulder

        # 録画中なら軌跡保存
        if recording:
            r_shoulder_traj.append(r_shoulder_px)
            l_shoulder_traj.append(l_shoulder_px)
            r_elbow_traj.append(r_elbow_px)
            l_elbow_traj.append(l_elbow_px)

         # 軌跡描画：cv2.circle(img,円の中心,半径,色,-1で円を塗りつぶす)
        for pt in r_shoulder_traj:
            cv2.circle(frame, pt, 4, (0, 255, 0), -1)
        for pt in l_shoulder_traj:
            cv2.circle(frame, pt, 4, (255, 0, 0), -1)
        for pt in r_elbow_traj:
            cv2.circle(frame, pt, 4, (0, 0, 255), -1)
        for pt in l_elbow_traj:
            cv2.circle(frame, pt, 4, (0, 0, 0), -1)

        # 現在の関節位置を強調表示
        cv2.circle(frame, l_shoulder_px, 8, (255, 0, 0), 2)
        cv2.circle(frame, r_shoulder_px, 8, (0, 255, 0), 2)
        cv2.circle(frame, l_elbow_px, 8, (0, 0, 0), 2)
        cv2.circle(frame, r_elbow_px, 8, (0, 0, 255), 2)

        # 角度を表示：cv2.putText(img,text,文字の座標,フォントの種類,フォントサイズ,色,太さ)
        cv2.putText(frame, f"Angle L Shoulder: {angle_l_shoulder:.1f}", (10, 90),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 0, 0), 2)
        cv2.putText(frame, f"Angle R Shoulder: {angle_r_shoulder:.1f}", (10, 60),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
        
        # 録画状態表示
        cv2.putText(frame, f"Recording: {'ON' if recording else 'OFF'}", (10, 30),
                cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255) if recording else (128, 128, 128), 2)

        # cv2.flipを使うなら反転する
        print(f"angle_l_sholder = {angle_l_shoulder}  angle_r _sholder = {angle_r_shoulder}")
        
    cv2.imshow('MediaPipe Pose', fliped_frame)

    key = cv2.waitKey(1)
    if key == ord('r'): 
        recording = not recording
    elif key == 27:  # ESCで終了
        break

cap.release()
cv2.destroyAllWindows()