MediaPipe BlazePose による人間の無意識の姿勢からの感情予測（ソースコードと実行結果）

REM Python をシステム領域にインストール
winget install --scope machine --id Python.Python.3.12 -e --silent
REM Python のパス設定
set "PYTHON_PATH=C:\Program Files\Python312"
set "PYTHON_SCRIPTS_PATH=C:\Program Files\Python312\Scripts"
echo "%PATH%" | find /i "%PYTHON_PATH%" >nul
if errorlevel 1 setx PATH "%PATH%;%PYTHON_PATH%" /M >nul
echo "%PATH%" | find /i "%PYTHON_SCRIPTS_PATH%" >nul
if errorlevel 1 setx PATH "%PATH%;%PYTHON_SCRIPTS_PATH%" /M >nul

winget install --scope machine Codeium.Windsurf -e --silent

# MediaPipe BlazePose による人間の無意識の姿勢からの感情予測プログラム
# 特徴技術名: MediaPipe BlazePose
# 出典: Bazarevsky, V., Grishchenko, I., Raveendran, K., Zhu, T., Grundmann, M., & Kartynnik, Y. (2020). BlazePose: On-device real-time body pose tracking with MediaPipe. Presented at CV4ARVR workshop at CVPR 2020.
# 特徴機能: 単一のRGBビデオフレームから33個の3次元ランドマークをリアルタイムで推定する機能。従来の17ランドマークを上回る検出点数により，フィットネスアプリケーションにおいて詳細な姿勢解析を可能とする。
# 学習済みモデル: MediaPipe BlazePose GHUM 3D - Googleが提供するオープンソースの3D人体姿勢推定モデル。lite，full，heavyの3つのバリエーションを提供し，精度と処理速度のトレードオフに対応。モバイルデバイスでのリアルタイム推論が可能。URL: https://github.com/google-ai-edge/mediapipe
# Z座標系: カメラからの相対的な深度を表し、小さい値はカメラに近く、大きい値はカメラから遠い
# 方式設計:
#   関連利用技術: OpenCV - 画像・動画処理およびカメラ入力処理, NumPy - 数値計算およびランドマーク座標処理
#   入力と出力: 入力: 動画（ユーザは「0:動画ファイル，1:カメラ，2:サンプル動画」のメニューで選択．0:動画ファイルの場合はtkinterでファイル選択．1の場合はOpenCVでカメラが開く．2の場合はhttps://raw.githubusercontent.com/opencv/opencv/master/samples/data/vtest.aviを使用） 出力: リアルタイム3D姿勢推定結果をOpenCV画面で表示，各フレームごとにprint()出力，プログラム終了時にresult.txtに保存
#   処理手順: 1.MediaPipe BlazePoseによる33個3Dランドマーク抽出 2.リアルタイム結果表示
#   前処理，後処理: 前処理: 入力画像の正規化 後処理: 結果のファイル出力
#   追加処理: なし
#   調整を必要とする設定値: detection_confidence: MediaPipe姿勢検出の信頼度閾値（デフォルト0.5）, tracking_confidence: MediaPipe追跡の信頼度閾値（デフォルト0.5）
# 将来方策: なし
# その他の重要事項: なし
# 前準備: pip install mediapipe opencv-python numpy pillow matplotlib japanize-matplotlib

import cv2
import mediapipe as mp
import numpy as np
import tkinter as tk
from tkinter import filedialog
import urllib.request
import time
import math
from PIL import Image, ImageDraw, ImageFont
from datetime import datetime
from collections import deque
import matplotlib.pyplot as plt
import japanize_matplotlib
from mpl_toolkits.mplot3d import Axes3D

# 定数定義
MIN_DETECTION_CONFIDENCE = 0.7  # 検出信頼度閾値
MIN_TRACKING_CONFIDENCE = 0.7   # トラッキング信頼度閾値

# 平滑化設定
SMOOTHING_WINDOW_SIZE = 15          # 時系列平滑化ウィンドウサイズ
SMOOTHING_FACTOR = 0.1              # 平滑化係数
BONE_LENGTH_CHANGE_THRESHOLD = 0.05  # 骨長変化許容値（メートル）
OUTLIER_DISTANCE_THRESHOLD = 0.1     # 外れ値距離閾値（メートル）

# 3Dプロット設定
PLOT_RANGE = 0.5      # 3Dプロットの表示範囲（メートル）
PLOT_ELEV = 10        # 3Dプロットの仰角
PLOT_AZIM = -90       # 3Dプロットの方位角

# MediaPipe設定
mp_pose = mp.solutions.pose
mp_drawing = mp.solutions.drawing_utils
mp_drawing_styles = mp.solutions.drawing_styles

# グローバル変数
pose = None
frame_count = 0
results_log = []
base_y = None  # 基準Y座標
base_z = None  # 基準Z座標
show_3d_plot = False
font = None
current_pose_3d = None
physics_constraints = None
temporal_smoother = None

# 33個の特徴点の定義
POSE_LANDMARKS = {
    0: "鼻",
    1: "左目（内側）",
    2: "左目",
    3: "左目（外側）",
    4: "右目（内側）",
    5: "右目",
    6: "右目（外側）",
    7: "左耳",
    8: "右耳",
    9: "口（左端）",
    10: "口（右端）",
    11: "左肩",
    12: "右肩",
    13: "左肘",
    14: "右肘",
    15: "左手首",
    16: "右手首",
    17: "左小指",
    18: "右小指",
    19: "左人差し指",
    20: "右人差し指",
    21: "左親指",
    22: "右親指",
    23: "左腰",
    24: "右腰",
    25: "左膝",
    26: "右膝",
    27: "左足首",
    28: "右足首",
    29: "左かかと",
    30: "右かかと",
    31: "左足指先",
    32: "右足指先"
}

# 特徴点の色設定（BGR形式）
LANDMARK_COLORS = {
    # 顔関連（赤系）
    0: (0, 0, 255), 1: (0, 50, 255), 2: (0, 100, 255), 3: (0, 150, 255),
    4: (0, 50, 255), 5: (0, 100, 255), 6: (0, 150, 255),
    7: (0, 200, 255), 8: (0, 200, 255), 9: (50, 0, 255), 10: (50, 0, 255),
    # 上半身（緑系）
    11: (0, 255, 0), 12: (0, 255, 0), 13: (50, 255, 0), 14: (50, 255, 0),
    15: (100, 255, 0), 16: (100, 255, 0), 17: (150, 255, 0), 18: (150, 255, 0),
    19: (200, 255, 0), 20: (200, 255, 0), 21: (255, 255, 0), 22: (255, 255, 0),
    # 下半身（青系）
    23: (255, 0, 0), 24: (255, 0, 0), 25: (255, 50, 0), 26: (255, 50, 0),
    27: (255, 100, 0), 28: (255, 100, 0), 29: (255, 150, 0), 30: (255, 150, 0),
    31: (255, 200, 0), 32: (255, 200, 0)
}

# 骨格ペア定義（修正版：MediaPipe公式定義に基づく）
BONE_PAIRS = {
    'left_upper_arm': (11, 13),   # 左肩→左肘
    'right_upper_arm': (12, 14),  # 右肩→右肘
    'left_forearm': (13, 15),     # 左肘→左手首
    'right_forearm': (14, 16),    # 右肘→右手首
    'left_thigh': (23, 25),       # 左腰→左膝
    'right_thigh': (24, 26),      # 右腰→右膝
    'left_shin': (25, 27),        # 左膝→左足首
    'right_shin': (26, 28)        # 右膝→右足首
}

class PhysicsConstraints:
    def __init__(self):
        self.bone_length_ratios = {
            'head_neck': 0.15,
            'neck_torso': 0.30,
            'upper_arm': 0.18,
            'forearm': 0.16,
            'thigh': 0.25,
            'shin': 0.25,
            'foot': 0.08
        }
        self.joint_limits = {
            'shoulder': (-180, 180),
            'elbow': (0, 150),
            'hip': (-120, 120),
            'knee': (0, 150),
            'ankle': (-45, 45)
        }
        self._cache = {}
        self._cache_frame = -1

    def validate_bone_lengths(self, pose_3d, frame_num=None):
        if frame_num is not None and frame_num == self._cache_frame:
            if 'bone_lengths_score' in self._cache:
                return self._cache['bone_lengths_score']

        if pose_3d is None:
            return 0.0
        bone_lengths = self._calculate_bone_lengths(pose_3d)
        if len(bone_lengths) < 4:
            return 0.5
        symmetry_score = 0.0
        symmetry_count = 0
        # 修正版：正しい左右ペア対応
        pairs = [
            ('left_upper_arm', 'right_upper_arm'),
            ('left_forearm', 'right_forearm'),
            ('left_thigh', 'right_thigh'),
            ('left_shin', 'right_shin')
        ]
        epsilon = 0.001
        for left, right in pairs:
            if left in bone_lengths and right in bone_lengths:
                max_length = max(bone_lengths[left], bone_lengths[right])
                if max_length > epsilon:
                    ratio = min(bone_lengths[left], bone_lengths[right]) / max_length
                    symmetry_score += ratio
                    symmetry_count += 1
        if symmetry_count > 0:
            symmetry_score /= symmetry_count
        else:
            symmetry_score = 0.5

        if frame_num is not None:
            self._cache_frame = frame_num
            self._cache['bone_lengths_score'] = symmetry_score

        return symmetry_score

    def _calculate_bone_lengths(self, pose_3d):
        bone_lengths = {}
        for name, (a, b) in BONE_PAIRS.items():
            if is_joint_visible(pose_3d, a) and is_joint_visible(pose_3d, b):
                bone_lengths[name] = calculate_3d_distance(pose_3d, a, b)
        return bone_lengths

class TemporalSmoother:
    def __init__(self, window_size=None, smoothing_factor=None):
        self.window_size = window_size or SMOOTHING_WINDOW_SIZE
        self.smoothing_factor = smoothing_factor or SMOOTHING_FACTOR
        self.pose_history = deque(maxlen=self.window_size)
        self.velocity_history = deque(maxlen=5)

    def smooth_pose(self, pose_3d):
        if pose_3d is None:
            return None
        self.pose_history.append(pose_3d.copy())
        n = len(self.pose_history)
        if n < 3:
            return pose_3d
        poses = np.array(list(self.pose_history))
        smoothed = poses[-1].copy()
        if n >= 5:
            window_size = min(5, n)
            window = poses[-window_size:]
            if window_size == 5:
                weights = np.array([0.1, 0.15, 0.2, 0.25, 0.3])
            else:
                weights = np.ones(window_size) / window_size
            ma_result = np.average(window, axis=0, weights=weights)
            smoothed = 0.7 * smoothed + 0.3 * ma_result
        if n >= 3:
            velocity = poses[-1] - poses[-2]
            predicted = poses[-1] + velocity * 0.5
            smoothed = 0.8 * smoothed + 0.2 * predicted
        smoothed = self._outlier_correction(smoothed, poses)
        if n >= 2:
            prev_pose = self.pose_history[-2]
            smoothed = self._maintain_bone_lengths(smoothed, prev_pose)
        return smoothed

    def _outlier_correction(self, pose, history):
        if len(history) < 3:
            return pose
        corrected = pose.copy()
        recent = history[-3:]
        for joint_idx in range(pose.shape[0]):
            if not is_joint_visible(pose, joint_idx, 0.3):
                continue
            distances = []
            for frame in recent[:-1]:
                if is_joint_visible(frame, joint_idx, 0.3):
                    dist = np.linalg.norm(pose[joint_idx, :3] - frame[joint_idx, :3])
                    distances.append(dist)
            if distances and np.mean(distances) > OUTLIER_DISTANCE_THRESHOLD:
                valid_frames = [f for f in recent if is_joint_visible(f, joint_idx, 0.3)]
                if len(valid_frames) >= 2:
                    corrected[joint_idx, :3] = np.mean([f[joint_idx, :3] for f in valid_frames], axis=0)
        return corrected

    def _maintain_bone_lengths(self, current, previous):
        corrected = current.copy()
        for start, end in BONE_PAIRS.values():
            if is_joint_visible(current, start) and is_joint_visible(current, end) and \
               is_joint_visible(previous, start) and is_joint_visible(previous, end):
                prev_length = calculate_3d_distance(previous, start, end)
                curr_length = calculate_3d_distance(current, start, end)
                if abs(curr_length - prev_length) > BONE_LENGTH_CHANGE_THRESHOLD:
                    direction = current[end, :3] - current[start, :3]
                    if np.linalg.norm(direction) > 0:
                        direction = direction / np.linalg.norm(direction)
                        corrected[end, :3] = current[start, :3] + direction * prev_length
        return corrected

def calculate_3d_distance(pose_3d, joint1, joint2):
    if isinstance(pose_3d, np.ndarray):
        return np.linalg.norm(pose_3d[joint1][:3] - pose_3d[joint2][:3])
    else:
        x1, y1, z1 = pose_3d[joint1].x, pose_3d[joint1].y, pose_3d[joint1].z
        x2, y2, z2 = pose_3d[joint2].x, pose_3d[joint2].y, pose_3d[joint2].z
        return math.sqrt((x2 - x1)**2 + (y2 - y1)**2 + (z2 - z1)**2)

def is_joint_visible(pose_3d, joint_id, threshold=0.5):
    if isinstance(pose_3d, np.ndarray):
        return pose_3d[joint_id][3] > threshold
    else:
        return pose_3d[joint_id].visibility > threshold

def calculate_angle_3d(p1, p2, p3):
    """3D座標から角度を計算（p2が頂点）"""
    if isinstance(p1, np.ndarray):
        v1 = np.array([p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2]])
        v2 = np.array([p3[0] - p2[0], p3[1] - p2[1], p3[2] - p2[2]])
    else:
        v1 = np.array([p1.x - p2.x, p1.y - p2.y, p1.z - p2.z])
        v2 = np.array([p3.x - p2.x, p3.y - p2.y, p3.z - p2.z])
    norm_v1 = np.linalg.norm(v1)
    norm_v2 = np.linalg.norm(v2)
    if norm_v1 == 0 or norm_v2 == 0:
        return 0.0
    cos_angle = np.dot(v1, v2) / (norm_v1 * norm_v2)
    cos_angle = np.clip(cos_angle, -1.0, 1.0)
    angle = np.arccos(cos_angle)
    return np.degrees(angle)

def calculate_features(landmarks):
    """11個の特徴量を計算"""
    global base_y, base_z

    # 必要な座標を取得
    x0, y0, z0 = landmarks[0].x, landmarks[0].y, landmarks[0].z  # 鼻
    x11, y11, z11 = landmarks[11].x, landmarks[11].y, landmarks[11].z  # 左肩
    x12, y12, z12 = landmarks[12].x, landmarks[12].y, landmarks[12].z  # 右肩
    x15, y15, z15 = landmarks[15].x, landmarks[15].y, landmarks[15].z  # 左手首
    x16, y16, z16 = landmarks[16].x, landmarks[16].y, landmarks[16].z  # 右手首

    # 基準座標の初期化（最初のフレームの値を使用）
    if base_y is None:
        base_y = (y11 + y12) / 2
    if base_z is None:
        base_z = (z11 + z12) / 2

    # 特徴量1: 肩の位置・距離
    shoulder_elevation = (y11 + y12) / 2 - base_y  # 肩の上昇度（正の値で上昇）
    shoulder_retraction = (z11 + z12) / 2 - base_z  # 肩の後退度（正の値で後退、自信を表す）
    shoulder_distance = calculate_3d_distance(landmarks, 11, 12)  # 肩間距離

    # 特徴量2: 肩-手首間距離
    body_center_x = (x11 + x12) / 2
    left_wrist_approach = abs(x15 - body_center_x)
    right_wrist_approach = abs(x16 - body_center_x)
    epsilon = 0.001  # 0除算防止
    defensiveness = 1 / (left_wrist_approach + right_wrist_approach + epsilon)  # 防御度

    left_shoulder_wrist_distance = calculate_3d_distance(landmarks, 11, 15)  # 左肩-左手首距離
    right_shoulder_wrist_distance = calculate_3d_distance(landmarks, 12, 16)  # 右肩-右手首距離

    # 特徴量3: 頭部-肩間距離
    head_left_wrist_distance = calculate_3d_distance(landmarks, 0, 15)  # 頭部-左手首距離
    head_right_wrist_distance = calculate_3d_distance(landmarks, 0, 16)  # 頭部-右手首距離

    # 頭部傾斜角（MediaPipe座標系に基づく - 修正版）
    shoulder_center_y = (y11 + y12) / 2
    shoulder_center_z = (z11 + z12) / 2
    if abs(y0 - shoulder_center_y) > epsilon:
        head_tilt_angle = math.atan((z0 - shoulder_center_z) / (y0 - shoulder_center_y))
        head_tilt_angle = math.degrees(head_tilt_angle)  # ラジアンから度に変換
        # 正の角度: 頭が後ろに傾く（誇らしげ）、負の角度: 頭が前に傾く（うなだれる）
    else:
        head_tilt_angle = 90.0 if (z0 - shoulder_center_z) > 0 else -90.0

    head_left_shoulder_distance = calculate_3d_distance(landmarks, 0, 11)  # 頭部-左肩距離
    head_right_shoulder_distance = calculate_3d_distance(landmarks, 0, 12)  # 頭部-右肩距離

    return {
        "肩の上昇度": shoulder_elevation,
        "肩の後退度": shoulder_retraction,
        "肩間距離": shoulder_distance,
        "防御度": defensiveness,
        "左肩-左手首距離": left_shoulder_wrist_distance,
        "右肩-右手首距離": right_shoulder_wrist_distance,
        "頭部-左手首距離": head_left_wrist_distance,
        "頭部-右手首距離": head_right_wrist_distance,
        "頭部傾斜角": head_tilt_angle,
        "頭部-左肩距離": head_left_shoulder_distance,
        "頭部-右肩距離": head_right_shoulder_distance
    }

def extract_landmarks(results):
    """MediaPoseの結果から特徴量を抽出"""
    if results.pose_landmarks:
        landmarks = []
        for landmark in results.pose_landmarks.landmark:
            landmarks.extend([landmark.x, landmark.y, landmark.z, landmark.visibility])
        return np.array(landmarks)
    else:
        return np.zeros(132)  # 33 landmarks * 4 features

def draw_landmarks_with_colors(image, landmarks):
    """特徴点を色付きで描画"""
    height, width = image.shape[:2]

    for idx, landmark in enumerate(landmarks.landmark):
        x = int(landmark.x * width)
        y = int(landmark.y * height)

        # 画像範囲内かチェック
        if 0 <= x < width and 0 <= y < height:
            color = LANDMARK_COLORS.get(idx, (255, 255, 255))
            cv2.circle(image, (x, y), 5, color, -1)
            cv2.circle(image, (x, y), 7, color, 2)

            # ID番号を表示
            cv2.putText(image, str(idx), (x + 10, y - 5),
                       cv2.FONT_HERSHEY_SIMPLEX, 0.3, color, 1)

def add_japanese_text_to_frame(frame, text, position, font_size=20):
    """PILを使用してOpenCV画像に日本語テキストを追加"""
    if font is None:
        return frame
    try:
        # OpenCV画像をPIL画像に変換
        img_pil = Image.fromarray(cv2.cvtColor(frame, cv2.COLOR_BGR2RGB))
        draw = ImageDraw.Draw(img_pil)

        # テキスト描画
        draw.text(position, text, font=font, fill=(0, 255, 0))

        # PIL画像をOpenCV画像に変換して返す
        return cv2.cvtColor(np.array(img_pil), cv2.COLOR_RGB2BGR)
    except Exception:
        # フォント読み込みに失敗した場合は元の画像を返す
        return frame

def video_frame_processing(frame):
    """メインの動画処理関数"""
    global frame_count, results_log, show_3d_plot, current_pose_3d
    current_time = time.time()
    frame_count += 1

    rgb_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)

    # MediaPipe姿勢推定
    results = pose.process(rgb_frame)

    if results.pose_landmarks:
        # カスタム描画（色付き特徴点）
        draw_landmarks_with_colors(frame, results.pose_landmarks)

        # 接続線の描画
        mp_drawing.draw_landmarks(
            frame, results.pose_landmarks, mp_pose.POSE_CONNECTIONS,
            landmark_drawing_spec=None,
            connection_drawing_spec=mp_drawing.DrawingSpec(color=(200, 200, 200), thickness=1))

        # セグメンテーション表示（有効な場合）
        if getattr(results, "segmentation_mask", None) is not None:
            mask = results.segmentation_mask
            if mask is not None and hasattr(mask, 'astype'):
                mask_colored = cv2.applyColorMap((mask * 255).astype(np.uint8), cv2.COLORMAP_JET)
                frame = cv2.addWeighted(frame, 0.8, mask_colored, 0.2, 0)

        # 3D座標取得と平滑化
        world = results.pose_world_landmarks.landmark if results.pose_world_landmarks else None
        if world is not None:
            pose_3d = np.zeros((33, 4))
            for i, landmark in enumerate(world):
                pose_3d[i] = [landmark.x, landmark.y, landmark.z, landmark.visibility]

            # 時系列平滑化
            pose_3d = temporal_smoother.smooth_pose(pose_3d)
            current_pose_3d = pose_3d

            # 物理スコア計算
            physics_score = physics_constraints.validate_bone_lengths(pose_3d, frame_count)

            # 信頼度計算
            visibility_scores = pose_3d[:, 3]
            positives = visibility_scores[visibility_scores > 0]
            if positives.size > 0:
                avg_visibility = float(np.mean(positives))
            else:
                avg_visibility = float(np.mean(visibility_scores)) if visibility_scores.size > 0 else 0.0
            key_joints = [11, 12, 23, 24, 15, 16, 25, 26]
            key_visibility = float(np.mean(pose_3d[key_joints, 3]))
            confidence = 0.6 * avg_visibility + 0.4 * key_visibility
            confidence = float(np.nan_to_num(confidence, nan=0.0, posinf=1.0, neginf=0.0))

            # 信頼度に基づく枠線の色
            if confidence > 0.8:
                color = (0, 255, 0)
            elif confidence > 0.6:
                color = (0, 255, 255)
            else:
                color = (0, 0, 255)
            cv2.rectangle(frame, (0, 0), (frame.shape[1] - 1, frame.shape[0] - 1), color, 3)

            # 情報表示
            frame = add_japanese_text_to_frame(frame, f"信頼度: {confidence:.3f}", (10, 30), 24)
            frame = add_japanese_text_to_frame(frame, f"物理スコア: {physics_score:.3f}", (10, 60), 24)
        else:
            physics_score = 0.0
            confidence = 0.0

        # 日本語テキスト表示（姿勢検出状態）
        frame = add_japanese_text_to_frame(frame, "姿勢検出中", (10, 10), 24)

        # 特徴量抽出（将来使用予定）
        features = extract_landmarks(results)

        # 11個の特徴量を計算
        feature_values = calculate_features(results.pose_landmarks.landmark)

        # 結果文字列の作成
        result = f"Frame {frame_count}: "
        for name, value in feature_values.items():
            result += f"{name}={value:.3f}, "
        result += f"信頼度={confidence:.3f}, 物理スコア={physics_score:.3f}"

        # ログに保存
        log_entry = f'Frame {frame_count}\n'
        log_entry += 'ID, x, y, z:\n'
        for idx, landmark in enumerate(results.pose_landmarks.landmark):
            log_entry += f'{idx:2d}, {landmark.x:.4f}, {landmark.y:.4f}, {landmark.z:.4f}\n'
        log_entry += '\n特徴量:\n'
        for name, value in feature_values.items():
            log_entry += f'{name}: {value:.4f}\n'
        log_entry += f'信頼度: {confidence:.4f}\n'
        log_entry += f'物理スコア: {physics_score:.4f}\n'
        results_log.append(log_entry)

        return frame, result, current_time
    else:
        # 姿勢が検出されない場合
        frame = add_japanese_text_to_frame(frame, "姿勢未検出", (10, 10), 24)
        result = f"Frame {frame_count}: 姿勢未検出"
        return frame, result, current_time

# プログラム開始時の説明とガイダンス
print('人間の姿勢からの感情予測システム')
print('MediaPipe BlazePoseによる3D姿勢推定を使用')
print('概要: 33個の身体特徴点から感情に関連する11個の特徴量を計算')
print('操作方法:')
print('  q キー: プログラム終了')
print('  s キー: 3Dプロット表示')
print('注意事項: 姿勢が明瞭に映るよう適切な距離と照明を確保してください')
print('重要: MediaPipeのz座標はカメラからの相対的な深度を表します')

# 日本語フォント初期化
try:
    font = ImageFont.truetype('C:/Windows/Fonts/meiryo.ttc', 20)
    print('日本語フォントを読み込みました: C:/Windows/Fonts/meiryo.ttc')
except Exception as e:
    print(f'日本語フォントの読み込みに失敗しました: {e}')
    font = None

# プログラムの機能と特徴点情報を表示（インライン化）
print('=' * 80)
print('プログラムの機能:')
print('- MediaPipe BlazePoseを使用した3D人体姿勢推定')
print('- 33個の身体特徴点をリアルタイムで検出・追跡')
print('- 各特徴点の3次元座標（x, y, z）を各フレームごとに表示・記録')
print('- 検出結果をresult.txtファイルに保存')
print('- qキーでプログラム終了')
print('=' * 80)
print('\n33個の特徴点の対応表:')
print('-' * 40)
for id, name in POSE_LANDMARKS.items():
    print(f'ID {id:2d}: {name}')
print('-' * 40)
print()

# 3つの主要特徴量と計算式の説明を表示（インライン化）
print("""MediaPipeポーズ推定における3つの主要特徴量と計算式
【MediaPipe座標系】
- X座標: 左から右（0-1の正規化座標）
- Y座標: 上から下（0-1の正規化座標）
- Z座標: カメラからの相対的な深度（小さい値はカメラに近く、大きい値はカメラから遠い）

特徴量1: 肩の位置・距離（MediaPipe 11番・12番の距離）: 緊張・ストレス・自信との関連
MediaPipeでは11番が左肩、12番が右肩として定義されています。肩関節は可動性の高い関節であり、肩を上げることは緊張やストレスを示し、肩を後ろに引くことは自信を表現します。
計算式:
肩を上げる動作（緊張・ストレス）の検出
肩の上昇度 = (y₁₁ + y₁₂) / 2 - 基準Y座標

肩を後ろに引く動作（自信）の検出
肩の後退度 = (z₁₁ + z₁₂) / 2 - 基準Z座標  ※正の値で後退を表す

肩間距離
肩間距離 = √[(x₁₂ - x₁₁)² + (y₁₂ - y₁₁)² + (z₁₂ - z₁₁)²]

where:
* x₁₁, y₁₁, z₁₁ = 左肩(11番)の3D座標
* x₁₂, y₁₂, z₁₂ = 右肩(12番)の3D座標
* 基準座標 = リラックス状態での肩位置

特徴量2: 肩-手首間距離（MediaPipe 11番・15番, 12番・16番の距離）: 開放性・防御性との関連
MediaPipeでは15番が左手首、16番が右手首として定義されています。腕を胸の前で組むことは防御的な身体言語として解釈され、不安感、苛立ち、または閉鎖性を示します。身体の開放性や手の位置は感情の知覚に影響を与えます。
計算式:
防御性の検出（腕を胸の前で組む）
体中心X = (x₁₁ + x₁₂) / 2
左手首の体中心接近度 = |x₁₅ - 体中心X|
右手首の体中心接近度 = |x₁₆ - 体中心X|
防御度 = 1 / (左手首の体中心接近度 + 右手首の体中心接近度 + ε)
※ ε は0除算を防ぐための小さな定数

開放性の検出
左肩-左手首距離 = √[(x₁₅ - x₁₁)² + (y₁₅ - y₁₁)² + (z₁₅ - z₁₁)²]
右肩-右手首距離 = √[(x₁₆ - x₁₂)² + (y₁₆ - y₁₂)² + (z₁₆ - z₁₂)²]

特徴量3: 頭部-肩間距離（MediaPipe 0番・11番, 0番・12番の距離）: 注意・関心・疲労との関連
MediaPipeでは0番が鼻として定義されており、頭部を代表する特徴点として使用されます。片手で頭を支えることは関心を示し、両手で頭を支えることは退屈や疲労を示唆します。頭を上に傾けることは優越感情（自信、誇り、軽蔑）を示し、下に傾けることは劣等感情（恥、恥ずかしさ、敬意）を示します。
計算式:
頭を支える動作の検出
頭部-左手首距離 = √[(x₁₅ - x₀)² + (y₁₅ - y₀)² + (z₁₅ - z₀)²]
頭部-右手首距離 = √[(x₁₆ - x₀)² + (y₁₆ - y₀)² + (z₁₆ - z₀)²]

頭の前後傾きの検出（MediaPipe座標系に基づく）
頭部傾斜角 = arctan((z₀ - (z₁₁ + z₁₂)/2) / (y₀ - (y₁₁ + y₁₂)/2))
※正の角度: 頭が後ろに傾く（誇らしげ）、負の角度: 頭が前に傾く（うなだれる）

参考：頭部-肩間距離
頭部-左肩距離 = √[(x₁₁ - x₀)² + (y₁₁ - y₀)² + (z₁₁ - z₀)²]
頭部-右肩距離 = √[(x₁₂ - x₀)² + (y₁₂ - y₀)² + (z₁₂ - z₀)²]""")

print("0: 動画ファイル")
print("1: カメラ")
print("2: サンプル動画")

choice = input("選択: ").strip()
try:
    choice_int = int(choice)
except Exception:
    choice_int = 2  # 非数値は2とみなす
if choice_int >= 2:
    mode = '2'
elif choice_int == 1:
    mode = '1'
else:
    mode = '0'

if mode == '0':
    root = tk.Tk()
    root.withdraw()
    path = filedialog.askopenfilename()
    if not path:
        exit()
    cap = cv2.VideoCapture(path)
elif mode == '1':
    cap = cv2.VideoCapture(0, cv2.CAP_DSHOW)
    if not cap.isOpened():
        cap = cv2.VideoCapture(0)
    cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
else:
    # サンプル動画ダウンロード・処理
    SAMPLE_URL = 'https://raw.githubusercontent.com/opencv/opencv/master/samples/data/vtest.avi'
    SAMPLE_FILE = 'vtest.avi'
    try:
        urllib.request.urlretrieve(SAMPLE_URL, SAMPLE_FILE)
    except Exception as e:
        print(f'サンプル動画のダウンロードに失敗しました: {e}')
        exit()
    cap = cv2.VideoCapture(SAMPLE_FILE)

if not cap.isOpened():
    print('動画ファイル・カメラを開けませんでした')
    exit()

# MODEL_COMPLEXITYの自動選択
frame_width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
frame_height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
fps = int(cap.get(cv2.CAP_PROP_FPS))

if frame_width * frame_height > 1920 * 1080 or fps > 60:
    MODEL_COMPLEXITY = 2  # heavy
elif frame_width * frame_height > 1280 * 720 or fps > 30:
    MODEL_COMPLEXITY = 1  # full
else:
    MODEL_COMPLEXITY = 0  # lite

print(f'MODEL_COMPLEXITY: {MODEL_COMPLEXITY} (解像度: {frame_width}x{frame_height}, FPS: {fps})')

# クラスインスタンス初期化
physics_constraints = PhysicsConstraints()
temporal_smoother = TemporalSmoother()

# MediaPipe Poseの初期化
pose = mp_pose.Pose(
    static_image_mode=False,
    model_complexity=MODEL_COMPLEXITY,
    smooth_landmarks=True,
    enable_segmentation=True,
    min_detection_confidence=MIN_DETECTION_CONFIDENCE,
    min_tracking_confidence=MIN_TRACKING_CONFIDENCE
)

# 3Dプロット表示関数
def show_3d_plot_func(pose_3d):
    """3D座標をプロット表示"""
    if pose_3d is None:
        return

    fig = plt.figure(figsize=(12, 10))
    ax = fig.add_subplot(111, projection='3d')

    x_coords = []
    y_coords = []
    z_coords = []
    colors = []
    sizes = []

    for i in range(33):
        # MediaPipe座標系（Y軸は上向きが正、Z軸は手前が正）
        x = pose_3d[i][0]
        y = -pose_3d[i][1]  # 表示のためY軸反転
        z = -pose_3d[i][2]  # 表示のためZ軸反転

        x_coords.append(x)
        y_coords.append(y)
        z_coords.append(z)

        visibility = pose_3d[i][3]
        if visibility > 0.7:
            colors.append('red')
            sizes.append(60)
        elif visibility > 0.3:
            colors.append('orange')
            sizes.append(40)
        else:
            colors.append('gray')
            sizes.append(20)

    ax.scatter(x_coords, y_coords, z_coords, c=colors, s=sizes, alpha=0.8)

    # 接続線の描画
    connections = mp_pose.POSE_CONNECTIONS
    for connection in connections:
        start_idx, end_idx = connection
        if start_idx < 33 and end_idx < 33:
            start_vis = pose_3d[start_idx][3]
            end_vis = pose_3d[end_idx][3]
            if start_vis > 0.3 and end_vis > 0.3:
                ax.plot([x_coords[start_idx], x_coords[end_idx]],
                        [y_coords[start_idx], y_coords[end_idx]],
                        [z_coords[start_idx], z_coords[end_idx]],
                        'b-', linewidth=1, alpha=0.7)

    # 座標軸ラベル（表示上の反転を明記）
    ax.set_xlabel('X軸（左右）[m]')
    ax.set_ylabel('Y軸（上下・表示反転）[m]')
    ax.set_zlabel('Z軸（前後・表示反転）[m]')
    ax.set_title('3次元姿勢推定結果（世界座標系、表示上Y/Z反転）')
    ax.set_xlim([-PLOT_RANGE, PLOT_RANGE])
    ax.set_ylim([-PLOT_RANGE, PLOT_RANGE])
    ax.set_zlim([-PLOT_RANGE, PLOT_RANGE])
    ax.view_init(elev=PLOT_ELEV, azim=PLOT_AZIM)
    plt.show()

# メイン処理
print('\n=== 動画処理開始 ===')
print('操作方法:')
print('  q キー: プログラム終了')
print('  s キー: 3Dプロット表示')
try:
    while True:
        ret, frame = cap.read()
        if not ret:
            break

        MAIN_FUNC_DESC = "姿勢推定による感情予測"
        processed_frame, result, current_time = video_frame_processing(frame)
        cv2.imshow(MAIN_FUNC_DESC, processed_frame)

        # キー入力チェック
        key = cv2.waitKey(1) & 0xFF
        if key == ord('q'):
            break
        elif key == ord('s'):
            show_3d_plot = True

        # 3Dプロット表示
        if show_3d_plot and current_pose_3d is not None:
            show_3d_plot = False
            show_3d_plot_func(current_pose_3d)

        if mode == '1':  # カメラの場合
            print(datetime.fromtimestamp(current_time).strftime("%Y-%m-%d %H:%M:%S.%f")[:-3], result)
        else:  # 動画ファイルの場合
            print(frame_count, result)

finally:
    print('\n=== プログラム終了 ===')
    pose.close()  # MediaPipeリソースの解放を追加
    cap.release()
    cv2.destroyAllWindows()
    if results_log:
        header = []
        header.append('=== 結果 ===')
        header.append(f'処理フレーム数: {frame_count}')
        header.append('MediaPipe座標系の説明:')
        header.append('- X座標: 左から右（0-1の正規化座標）')
        header.append('- Y座標: 上から下（0-1の正規化座標）')
        header.append('- Z座標: カメラからの相対的な深度')
        header.append('')
        with open('result.txt', 'w', encoding='utf-8') as f:
            f.write('\n'.join(header + results_log))
        print(f'\n処理結果をresult.txtに保存しました')