3D姿勢推定・モーションキャプチャの応用演習：生活・社会に根ざした全身動作の活用

pip install --no-user mediapipe opencv-python open3d numpy

"""
演習1: 転倒・姿勢崩れの見守りセンサ
- 単眼カメラ + MediaPipe Pose (VIDEOモード) で全身3D姿勢を取得
- 体幹の傾き角・腰の落下速度から転倒を検知し、イベントのみをCSVに記録(映像は保存しない)
- 3D骨格を匿名表示
依存: mediapipe, opencv-python, open3d, numpy
"""

import os
import csv
import time
import urllib.request
import cv2
import numpy as np
import open3d as o3d
import mediapipe as mp
from mediapipe.tasks import python as mp_python
from mediapipe.tasks.python import vision

# === 保存先はコード保存フォルダ。モデルが無ければDL ==========================
MODEL_URL = "https://storage.googleapis.com/mediapipe-models/pose_landmarker/pose_landmarker_lite/float16/1/pose_landmarker_lite.task"
BASE_DIR = os.path.dirname(os.path.abspath(__file__))
MODEL_PATH = os.path.join(BASE_DIR, "pose_landmarker_lite.task")
CSV_PATH = os.path.join(BASE_DIR, "fall_events.csv")
if not os.path.exists(MODEL_PATH):
    urllib.request.urlretrieve(MODEL_URL, MODEL_PATH)
# === ここまで ================================================================

POSE_CONNECTIONS = [
    (11, 12), (11, 13), (13, 15), (12, 14), (14, 16),
    (11, 23), (12, 24), (23, 24),
    (23, 25), (25, 27), (24, 26), (26, 28),
    (0, 11), (0, 12),
]
NUM_LANDMARKS = 33

# 転倒判定のしきい値(目安。利用環境に合わせて調整する)
TILT_THRESHOLD_DEG = 45.0   # 体幹が鉛直からこの角度以上傾いたら「傾き」
FALL_SPEED_MPS = 1.2        # 腰がこの速度(m/s)以上で下降したら「落下」
LOW_HIP_RATIO = 0.4         # 腰の高さが立位時のこの割合以下なら「低姿勢」

base = mp_python.BaseOptions(model_asset_path=MODEL_PATH)
options = vision.PoseLandmarkerOptions(
    base_options=base, running_mode=vision.RunningMode.VIDEO, num_poses=1)
landmarker = vision.PoseLandmarker.create_from_options(options)


def landmarks_to_points(world_landmarks):
    pts = np.zeros((NUM_LANDMARKS, 3))
    for i, lm in enumerate(world_landmarks):
        pts[i] = [lm.x, -lm.y, -lm.z]
    return pts


def trunk_tilt_deg(pts):
    """体幹ベクトル(腰中心->肩中心)と鉛直(+y)のなす角[度]。"""
    hip = 0.5 * (pts[23] + pts[24])
    sho = 0.5 * (pts[11] + pts[12])
    trunk = sho - hip
    n = np.linalg.norm(trunk)
    if n < 1e-6:
        return 0.0
    cos = np.clip(np.dot(trunk / n, np.array([0.0, 1.0, 0.0])), -1.0, 1.0)
    return float(np.degrees(np.arccos(cos)))


def draw_landmarks_2d(image, norm_landmarks):
    h, w = image.shape[:2]
    px = [(int(lm.x * w), int(lm.y * h)) for lm in norm_landmarks]
    for a, b in POSE_CONNECTIONS:
        cv2.line(image, px[a], px[b], (255, 200, 0), 2)
    for (x, y) in px:
        cv2.circle(image, (x, y), 4, (0, 0, 255), -1)


if not os.path.exists(CSV_PATH):
    with open(CSV_PATH, "w", newline="") as f:
        csv.writer(f).writerow(["unix_time", "tilt_deg", "hip_speed_mps", "reason"])

cap = cv2.VideoCapture(0)   # カメラ。動画で試す場合は VideoCapture("movie.mp4")
FPS_ASSUMED = 30.0
frame_index = 0

init_pts = np.zeros((NUM_LANDMARKS, 3))
line_set = o3d.geometry.LineSet(
    points=o3d.utility.Vector3dVector(init_pts),
    lines=o3d.utility.Vector2iVector(np.array(POSE_CONNECTIONS)))
line_set.paint_uniform_color([0.1, 0.4, 0.9])
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(init_pts)
pcd.paint_uniform_color([0.85, 0.2, 0.2])

vis = o3d.visualization.Visualizer()
vis.create_window(window_name="Watch (3D skeleton)", width=900, height=700)
vis.add_geometry(line_set)
vis.add_geometry(pcd)
vis.add_geometry(o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5))

prev_hip_y = None
prev_t = None
standing_hip_y = None     # 立位時の腰の高さの基準(観測中の最大値で更新)
alert_until = 0.0         # 警告表示を一定時間保持

while True:
    ret, frame = cap.read()
    if not ret:
        break

    rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
    mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=rgb)
    timestamp_ms = int(frame_index * 1000.0 / FPS_ASSUMED)
    frame_index += 1
    now = time.time()

    result = landmarker.detect_for_video(mp_image, timestamp_ms)

    if result.pose_world_landmarks:
        pts = landmarks_to_points(result.pose_world_landmarks[0])
        new_pts = o3d.utility.Vector3dVector(pts)
        line_set.points = new_pts
        pcd.points = new_pts
        vis.update_geometry(line_set)
        vis.update_geometry(pcd)

        tilt = trunk_tilt_deg(pts)
        hip_y = 0.5 * (pts[23][1] + pts[24][1])

        if standing_hip_y is None or hip_y > standing_hip_y:
            standing_hip_y = hip_y

        hip_speed = 0.0
        if prev_hip_y is not None and prev_t is not None:
            dt = max(now - prev_t, 1e-3)
            hip_speed = (prev_hip_y - hip_y) / dt   # 正なら下降
        prev_hip_y, prev_t = hip_y, now

        reasons = []
        if tilt >= TILT_THRESHOLD_DEG:
            reasons.append("tilt")
        if hip_speed >= FALL_SPEED_MPS:
            reasons.append("fast_drop")
        if standing_hip_y is not None and hip_y <= standing_hip_y * LOW_HIP_RATIO:
            reasons.append("low_posture")

        if reasons:
            alert_until = now + 2.0
            with open(CSV_PATH, "a", newline="") as f:
                csv.writer(f).writerow(
                    [f"{now:.3f}", f"{tilt:.1f}", f"{hip_speed:.2f}", "|".join(reasons)])

    if result.pose_landmarks:
        draw_landmarks_2d(frame, result.pose_landmarks[0])

    if now < alert_until:
        cv2.putText(frame, "FALL ALERT", (30, 60),
                    cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 0, 255), 3)

    if not vis.poll_events():
        break
    vis.update_renderer()
    cv2.imshow("camera", frame)

    if cv2.waitKey(1) & 0xFF == 27:
        break

cap.release()
cv2.destroyAllWindows()
vis.destroy_window()
landmarker.close()

"""
演習2: リハビリ・運動指導の左右対称性チェッカー
- 単眼カメラ + MediaPipe Pose (VIDEOモード)
- 左右の肩・肘・股・膝の関節角度を3Dで計測し、左右差を数値化・可視化
依存: mediapipe, opencv-python, open3d, numpy
"""

import os
import urllib.request
import cv2
import numpy as np
import open3d as o3d
import mediapipe as mp
from mediapipe.tasks import python as mp_python
from mediapipe.tasks.python import vision

# === 保存先はコード保存フォルダ。モデルが無ければDL ==========================
MODEL_URL = "https://storage.googleapis.com/mediapipe-models/pose_landmarker/pose_landmarker_lite/float16/1/pose_landmarker_lite.task"
BASE_DIR = os.path.dirname(os.path.abspath(__file__))
MODEL_PATH = os.path.join(BASE_DIR, "pose_landmarker_lite.task")
if not os.path.exists(MODEL_PATH):
    urllib.request.urlretrieve(MODEL_URL, MODEL_PATH)
# === ここまで ================================================================

POSE_CONNECTIONS = [
    (11, 12), (11, 13), (13, 15), (12, 14), (14, 16),
    (11, 23), (12, 24), (23, 24),
    (23, 25), (25, 27), (24, 26), (26, 28),
    (0, 11), (0, 12),
]
NUM_LANDMARKS = 33

# 計測する関節角度: (名前, 親, 中心, 子) を左右ペアで定義
ANGLES = {
    "elbow": ((11, 13, 15), (12, 14, 16)),   # 肩-肘-手首
    "shoulder": ((23, 11, 13), (24, 12, 14)),  # 腰-肩-肘
    "hip": ((11, 23, 25), (12, 24, 26)),     # 肩-腰-膝
    "knee": ((23, 25, 27), (24, 26, 28)),    # 腰-膝-足首
}
DIFF_WARN_DEG = 15.0   # 左右差がこれ以上で強調

base = mp_python.BaseOptions(model_asset_path=MODEL_PATH)
options = vision.PoseLandmarkerOptions(
    base_options=base, running_mode=vision.RunningMode.VIDEO, num_poses=1)
landmarker = vision.PoseLandmarker.create_from_options(options)


def landmarks_to_points(world_landmarks):
    pts = np.zeros((NUM_LANDMARKS, 3))
    for i, lm in enumerate(world_landmarks):
        pts[i] = [lm.x, -lm.y, -lm.z]
    return pts


def joint_angle(pts, a, b, c):
    """3点 a-b-c のうち b を頂点とする角度[度]。"""
    v1 = pts[a] - pts[b]
    v2 = pts[c] - pts[b]
    n1, n2 = np.linalg.norm(v1), np.linalg.norm(v2)
    if n1 < 1e-6 or n2 < 1e-6:
        return 0.0
    cos = np.clip(np.dot(v1 / n1, v2 / n2), -1.0, 1.0)
    return float(np.degrees(np.arccos(cos)))


def draw_landmarks_2d(image, norm_landmarks):
    h, w = image.shape[:2]
    px = [(int(lm.x * w), int(lm.y * h)) for lm in norm_landmarks]
    for a, b in POSE_CONNECTIONS:
        cv2.line(image, px[a], px[b], (255, 200, 0), 2)
    for (x, y) in px:
        cv2.circle(image, (x, y), 4, (0, 0, 255), -1)


cap = cv2.VideoCapture(0)   # カメラ。動画で試す場合は VideoCapture("movie.mp4")
FPS_ASSUMED = 30.0
frame_index = 0

init_pts = np.zeros((NUM_LANDMARKS, 3))
line_set = o3d.geometry.LineSet(
    points=o3d.utility.Vector3dVector(init_pts),
    lines=o3d.utility.Vector2iVector(np.array(POSE_CONNECTIONS)))
line_set.paint_uniform_color([0.1, 0.4, 0.9])
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(init_pts)
pcd.paint_uniform_color([0.85, 0.2, 0.2])

vis = o3d.visualization.Visualizer()
vis.create_window(window_name="Symmetry checker (3D)", width=900, height=700)
vis.add_geometry(line_set)
vis.add_geometry(pcd)
vis.add_geometry(o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5))

while True:
    ret, frame = cap.read()
    if not ret:
        break

    rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
    mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=rgb)
    timestamp_ms = int(frame_index * 1000.0 / FPS_ASSUMED)
    frame_index += 1

    result = landmarker.detect_for_video(mp_image, timestamp_ms)

    if result.pose_world_landmarks:
        pts = landmarks_to_points(result.pose_world_landmarks[0])
        new_pts = o3d.utility.Vector3dVector(pts)
        line_set.points = new_pts
        pcd.points = new_pts
        vis.update_geometry(line_set)
        vis.update_geometry(pcd)

        # カメラ映像の左上に左右角度と差を表示
        y = 30
        for name, (left, right) in ANGLES.items():
            la = joint_angle(pts, *left)
            ra = joint_angle(pts, *right)
            diff = abs(la - ra)
            color = (0, 0, 255) if diff >= DIFF_WARN_DEG else (0, 200, 0)
            cv2.putText(frame, f"{name:9s} L={la:5.1f} R={ra:5.1f} d={diff:4.1f}",
                        (10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2)
            y += 28

    if result.pose_landmarks:
        draw_landmarks_2d(frame, result.pose_landmarks[0])

    if not vis.poll_events():
        break
    vis.update_renderer()
    cv2.imshow("camera", frame)

    if cv2.waitKey(1) & 0xFF == 27:
        break

cap.release()
cv2.destroyAllWindows()
vis.destroy_window()
landmarker.close()

"""
演習3: 作業現場の人間工学リスク評価
- 単眼カメラ + MediaPipe Pose (VIDEOモード)
- 体幹前傾角・上肢挙上角・膝屈曲角を計測し、簡易的な負担スコアに区分してCSV記録
依存: mediapipe, opencv-python, open3d, numpy
"""

import os
import csv
import time
import urllib.request
import cv2
import numpy as np
import open3d as o3d
import mediapipe as mp
from mediapipe.tasks import python as mp_python
from mediapipe.tasks.python import vision

# === 保存先はコード保存フォルダ。モデルが無ければDL ==========================
MODEL_URL = "https://storage.googleapis.com/mediapipe-models/pose_landmarker/pose_landmarker_lite/float16/1/pose_landmarker_lite.task"
BASE_DIR = os.path.dirname(os.path.abspath(__file__))
MODEL_PATH = os.path.join(BASE_DIR, "pose_landmarker_lite.task")
CSV_PATH = os.path.join(BASE_DIR, "ergo_log.csv")
if not os.path.exists(MODEL_PATH):
    urllib.request.urlretrieve(MODEL_URL, MODEL_PATH)
# === ここまで ================================================================

POSE_CONNECTIONS = [
    (11, 12), (11, 13), (13, 15), (12, 14), (14, 16),
    (11, 23), (12, 24), (23, 24),
    (23, 25), (25, 27), (24, 26), (26, 28),
    (0, 11), (0, 12),
]
NUM_LANDMARKS = 33

base = mp_python.BaseOptions(model_asset_path=MODEL_PATH)
options = vision.PoseLandmarkerOptions(
    base_options=base, running_mode=vision.RunningMode.VIDEO, num_poses=1)
landmarker = vision.PoseLandmarker.create_from_options(options)


def landmarks_to_points(world_landmarks):
    pts = np.zeros((NUM_LANDMARKS, 3))
    for i, lm in enumerate(world_landmarks):
        pts[i] = [lm.x, -lm.y, -lm.z]
    return pts


def angle_from_vertical(vec):
    """ベクトルと鉛直(+y)のなす角[度]。"""
    n = np.linalg.norm(vec)
    if n < 1e-6:
        return 0.0
    cos = np.clip(np.dot(vec / n, np.array([0.0, 1.0, 0.0])), -1.0, 1.0)
    return float(np.degrees(np.arccos(cos)))


def joint_angle(pts, a, b, c):
    v1 = pts[a] - pts[b]
    v2 = pts[c] - pts[b]
    n1, n2 = np.linalg.norm(v1), np.linalg.norm(v2)
    if n1 < 1e-6 or n2 < 1e-6:
        return 0.0
    cos = np.clip(np.dot(v1 / n1, v2 / n2), -1.0, 1.0)
    return float(np.degrees(np.arccos(cos)))


def score_by_bands(value, bands):
    """value が bands=[(上限, 点数), ...] のどの区分かを返す。"""
    for upper, pts_score in bands:
        if value < upper:
            return pts_score
    return bands[-1][1]


def draw_landmarks_2d(image, norm_landmarks):
    h, w = image.shape[:2]
    px = [(int(lm.x * w), int(lm.y * h)) for lm in norm_landmarks]
    for a, b in POSE_CONNECTIONS:
        cv2.line(image, px[a], px[b], (255, 200, 0), 2)
    for (x, y) in px:
        cv2.circle(image, (x, y), 4, (0, 0, 255), -1)


if not os.path.exists(CSV_PATH):
    with open(CSV_PATH, "w", newline="") as f:
        csv.writer(f).writerow(
            ["unix_time", "trunk_deg", "arm_deg", "knee_flex_deg", "score"])

cap = cv2.VideoCapture(0)   # カメラ。動画で試す場合は VideoCapture("movie.mp4")
FPS_ASSUMED = 30.0
frame_index = 0

init_pts = np.zeros((NUM_LANDMARKS, 3))
line_set = o3d.geometry.LineSet(
    points=o3d.utility.Vector3dVector(init_pts),
    lines=o3d.utility.Vector2iVector(np.array(POSE_CONNECTIONS)))
line_set.paint_uniform_color([0.1, 0.4, 0.9])
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(init_pts)
pcd.paint_uniform_color([0.85, 0.2, 0.2])

vis = o3d.visualization.Visualizer()
vis.create_window(window_name="Ergonomics (3D)", width=900, height=700)
vis.add_geometry(line_set)
vis.add_geometry(pcd)
vis.add_geometry(o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5))

while True:
    ret, frame = cap.read()
    if not ret:
        break

    rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
    mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=rgb)
    timestamp_ms = int(frame_index * 1000.0 / FPS_ASSUMED)
    frame_index += 1

    result = landmarker.detect_for_video(mp_image, timestamp_ms)

    if result.pose_world_landmarks:
        pts = landmarks_to_points(result.pose_world_landmarks[0])
        new_pts = o3d.utility.Vector3dVector(pts)
        line_set.points = new_pts
        pcd.points = new_pts
        vis.update_geometry(line_set)
        vis.update_geometry(pcd)

        hip = 0.5 * (pts[23] + pts[24])
        sho = 0.5 * (pts[11] + pts[12])
        trunk_deg = angle_from_vertical(sho - hip)            # 体幹前傾
        # 上肢挙上は左右の大きい方(肩->肘 と鉛直下向きのなす角を挙上角に換算)
        arm_l = 180.0 - angle_from_vertical(pts[13] - pts[11])
        arm_r = 180.0 - angle_from_vertical(pts[14] - pts[12])
        arm_deg = max(arm_l, arm_r)
        # 膝屈曲は 180度(伸展)からの差。左右の大きい方
        knee_l = 180.0 - joint_angle(pts, 23, 25, 27)
        knee_r = 180.0 - joint_angle(pts, 24, 26, 28)
        knee_flex = max(knee_l, knee_r)

        # 簡易スコア(数字が大きいほど高負担)
        score = (score_by_bands(trunk_deg, [(20, 1), (45, 2), (60, 3), (999, 4)])
                 + score_by_bands(arm_deg, [(20, 1), (45, 2), (90, 3), (999, 4)])
                 + score_by_bands(knee_flex, [(30, 1), (60, 2), (999, 3)]))

        with open(CSV_PATH, "a", newline="") as f:
            csv.writer(f).writerow(
                [f"{time.time():.3f}", f"{trunk_deg:.1f}", f"{arm_deg:.1f}",
                 f"{knee_flex:.1f}", score])

        for i, txt in enumerate([
                f"trunk {trunk_deg:5.1f} deg",
                f"arm   {arm_deg:5.1f} deg",
                f"knee  {knee_flex:5.1f} deg",
                f"SCORE {score}"]):
            color = (0, 0, 255) if (txt.startswith("SCORE") and score >= 7) else (0, 200, 0)
            cv2.putText(frame, txt, (10, 30 + i * 28),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2)

    if result.pose_landmarks:
        draw_landmarks_2d(frame, result.pose_landmarks[0])

    if not vis.poll_events():
        break
    vis.update_renderer()
    cv2.imshow("camera", frame)

    if cv2.waitKey(1) & 0xFF == 27:
        break

cap.release()
cv2.destroyAllWindows()
vis.destroy_window()
landmarker.close()

"""
演習4: 匿名3D骨格による行動記録・動作教材アーカイブ
- MODE="record": カメラから pose_world_landmarks を時系列で蓄積し npz 保存(映像は保存しない)
- MODE="play"  : 保存済み座標を骨格アニメーションとして再生
依存: mediapipe(recordのみ), opencv-python(recordのみ), open3d, numpy
"""

import os
import time
import urllib.request
import numpy as np
import open3d as o3d

# === 保存先はコード保存フォルダ ==============================================
BASE_DIR = os.path.dirname(os.path.abspath(__file__))
MODEL_URL = "https://storage.googleapis.com/mediapipe-models/pose_landmarker/pose_landmarker_lite/float16/1/pose_landmarker_lite.task"
MODEL_PATH = os.path.join(BASE_DIR, "pose_landmarker_lite.task")
REC_PATH = os.path.join(BASE_DIR, "motion_record.npz")
# === ここまで ================================================================

MODE = "record"   # "record"=記録, "play"=再生
FPS_ASSUMED = 30.0

POSE_CONNECTIONS = [
    (11, 12), (11, 13), (13, 15), (12, 14), (14, 16),
    (11, 23), (12, 24), (23, 24),
    (23, 25), (25, 27), (24, 26), (26, 28),
    (0, 11), (0, 12),
]
NUM_LANDMARKS = 33


def landmarks_to_points(world_landmarks):
    pts = np.zeros((NUM_LANDMARKS, 3))
    for i, lm in enumerate(world_landmarks):
        pts[i] = [lm.x, -lm.y, -lm.z]
    return pts


def make_skeleton():
    init = np.zeros((NUM_LANDMARKS, 3))
    ls = o3d.geometry.LineSet(
        points=o3d.utility.Vector3dVector(init),
        lines=o3d.utility.Vector2iVector(np.array(POSE_CONNECTIONS)))
    ls.paint_uniform_color([0.1, 0.4, 0.9])
    pc = o3d.geometry.PointCloud()
    pc.points = o3d.utility.Vector3dVector(init)
    pc.paint_uniform_color([0.85, 0.2, 0.2])
    return ls, pc


if MODE == "record":
    import cv2
    import mediapipe as mp
    from mediapipe.tasks import python as mp_python
    from mediapipe.tasks.python import vision

    if not os.path.exists(MODEL_PATH):
        urllib.request.urlretrieve(MODEL_URL, MODEL_PATH)

    base = mp_python.BaseOptions(model_asset_path=MODEL_PATH)
    options = vision.PoseLandmarkerOptions(
        base_options=base, running_mode=vision.RunningMode.VIDEO, num_poses=1)
    landmarker = vision.PoseLandmarker.create_from_options(options)

    cap = cv2.VideoCapture(0)   # カメラ。動画で試す場合は VideoCapture("movie.mp4")
    frame_index = 0
    recorded = []

    line_set, pcd = make_skeleton()
    vis = o3d.visualization.Visualizer()
    vis.create_window(window_name="Recording (3D skeleton)", width=900, height=700)
    vis.add_geometry(line_set)
    vis.add_geometry(pcd)
    vis.add_geometry(o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5))

    while True:
        ret, frame = cap.read()
        if not ret:
            break
        rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
        mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=rgb)
        timestamp_ms = int(frame_index * 1000.0 / FPS_ASSUMED)
        frame_index += 1
        result = landmarker.detect_for_video(mp_image, timestamp_ms)
        if result.pose_world_landmarks:
            pts = landmarks_to_points(result.pose_world_landmarks[0])
            recorded.append(pts)
            new_pts = o3d.utility.Vector3dVector(pts)
            line_set.points = new_pts
            pcd.points = new_pts
            vis.update_geometry(line_set)
            vis.update_geometry(pcd)
        if not vis.poll_events():
            break
        vis.update_renderer()
        cv2.imshow("camera (NOT saved)", frame)
        if cv2.waitKey(1) & 0xFF == 27:
            break

    cap.release()
    cv2.destroyAllWindows()
    vis.destroy_window()
    landmarker.close()

    if recorded:
        np.savez_compressed(REC_PATH, frames=np.array(recorded), fps=FPS_ASSUMED)
        print(f"保存しました: {REC_PATH}  ({len(recorded)} フレーム)")
    else:
        print("記録なし(姿勢が検出されませんでした)")

else:  # MODE == "play"
    if not os.path.exists(REC_PATH):
        raise SystemExit("再生する記録がありません。先に MODE='record' で記録してください。")
    data = np.load(REC_PATH)
    all_pos = data["frames"]
    fps = float(data["fps"])
    n_frames = len(all_pos)
    print(f"再生: {n_frames} フレーム / {fps:.1f} FPS")

    line_set, pcd = make_skeleton()
    line_set.points = o3d.utility.Vector3dVector(all_pos[0])
    pcd.points = o3d.utility.Vector3dVector(all_pos[0])

    vis = o3d.visualization.Visualizer()
    vis.create_window(window_name="Playback (3D skeleton)", width=900, height=700)
    vis.add_geometry(line_set)
    vis.add_geometry(pcd)
    vis.add_geometry(o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5))

    frame = 0
    while True:
        pts = o3d.utility.Vector3dVector(all_pos[frame])
        line_set.points = pts
        pcd.points = pts
        vis.update_geometry(line_set)
        vis.update_geometry(pcd)
        if not vis.poll_events():
            break
        vis.update_renderer()
        time.sleep(1.0 / fps)
        frame = (frame + 1) % n_frames

    vis.destroy_window()

"""
演習5: 歩行解析による健康スクリーニング
- 単眼カメラ + MediaPipe Pose (VIDEOモード)
- 足首の前後位置の入れ替わりから歩数を検出し、歩幅の目安・左右リズムを推定してCSV記録
依存: mediapipe, opencv-python, open3d, numpy
"""

import os
import csv
import time
import urllib.request
import cv2
import numpy as np
import open3d as o3d
import mediapipe as mp
from mediapipe.tasks import python as mp_python
from mediapipe.tasks.python import vision

# === 保存先はコード保存フォルダ。モデルが無ければDL ==========================
MODEL_URL = "https://storage.googleapis.com/mediapipe-models/pose_landmarker/pose_landmarker_lite/float16/1/pose_landmarker_lite.task"
BASE_DIR = os.path.dirname(os.path.abspath(__file__))
MODEL_PATH = os.path.join(BASE_DIR, "pose_landmarker_lite.task")
CSV_PATH = os.path.join(BASE_DIR, "gait_log.csv")
if not os.path.exists(MODEL_PATH):
    urllib.request.urlretrieve(MODEL_URL, MODEL_PATH)
# === ここまで ================================================================

POSE_CONNECTIONS = [
    (11, 12), (11, 13), (13, 15), (12, 14), (14, 16),
    (11, 23), (12, 24), (23, 24),
    (23, 25), (25, 27), (24, 26), (26, 28),
    (0, 11), (0, 12),
]
NUM_LANDMARKS = 33

base = mp_python.BaseOptions(model_asset_path=MODEL_PATH)
options = vision.PoseLandmarkerOptions(
    base_options=base, running_mode=vision.RunningMode.VIDEO, num_poses=1)
landmarker = vision.PoseLandmarker.create_from_options(options)


def landmarks_to_points(world_landmarks):
    pts = np.zeros((NUM_LANDMARKS, 3))
    for i, lm in enumerate(world_landmarks):
        pts[i] = [lm.x, -lm.y, -lm.z]
    return pts


def draw_landmarks_2d(image, norm_landmarks):
    h, w = image.shape[:2]
    px = [(int(lm.x * w), int(lm.y * h)) for lm in norm_landmarks]
    for a, b in POSE_CONNECTIONS:
        cv2.line(image, px[a], px[b], (255, 200, 0), 2)
    for (x, y) in px:
        cv2.circle(image, (x, y), 4, (0, 0, 255), -1)


if not os.path.exists(CSV_PATH):
    with open(CSV_PATH, "w", newline="") as f:
        csv.writer(f).writerow(
            ["unix_time", "which_foot", "step_interval_s", "step_length_m"])

cap = cv2.VideoCapture(0)   # カメラ。動画で試す場合は VideoCapture("movie.mp4")
FPS_ASSUMED = 30.0
frame_index = 0

init_pts = np.zeros((NUM_LANDMARKS, 3))
line_set = o3d.geometry.LineSet(
    points=o3d.utility.Vector3dVector(init_pts),
    lines=o3d.utility.Vector2iVector(np.array(POSE_CONNECTIONS)))
line_set.paint_uniform_color([0.1, 0.4, 0.9])
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(init_pts)
pcd.paint_uniform_color([0.85, 0.2, 0.2])

vis = o3d.visualization.Visualizer()
vis.create_window(window_name="Gait (3D)", width=900, height=700)
vis.add_geometry(line_set)
vis.add_geometry(pcd)
vis.add_geometry(o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5))

step_count = 0
last_leading = None        # 直前に前に出ていた足 "L"/"R"
last_step_time = None
last_ankle_gap = 0.0

while True:
    ret, frame = cap.read()
    if not ret:
        break

    rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
    mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=rgb)
    timestamp_ms = int(frame_index * 1000.0 / FPS_ASSUMED)
    frame_index += 1

    result = landmarker.detect_for_video(mp_image, timestamp_ms)

    if result.pose_world_landmarks:
        pts = landmarks_to_points(result.pose_world_landmarks[0])
        new_pts = o3d.utility.Vector3dVector(pts)
        line_set.points = new_pts
        pcd.points = new_pts
        vis.update_geometry(line_set)
        vis.update_geometry(pcd)

        # 左足首=27, 右足首=28。z(前後)で「前に出ている足」を判定
        # 変換後 z は手前が負側のため、より小さい z の足が前
        lz, rz = pts[27][2], pts[28][2]
        leading = "L" if lz < rz else "R"
        ankle_gap = abs(lz - rz)   # 前後の開き(歩幅の目安)

        # 前に出る足が入れ替わった瞬間を1歩として数える
        if last_leading is not None and leading != last_leading:
            now = time.time()
            interval = (now - last_step_time) if last_step_time else 0.0
            step_length = max(ankle_gap, last_ankle_gap)
            step_count += 1
            with open(CSV_PATH, "a", newline="") as f:
                csv.writer(f).writerow(
                    [f"{now:.3f}", leading, f"{interval:.2f}", f"{step_length:.3f}"])
            last_step_time = now
        last_leading = leading
        last_ankle_gap = ankle_gap

        cv2.putText(frame, f"steps: {step_count}  lead: {leading}", (10, 40),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 200, 0), 2)

    if result.pose_landmarks:
        draw_landmarks_2d(frame, result.pose_landmarks[0])

    if not vis.poll_events():
        break
    vis.update_renderer()
    cv2.imshow("camera", frame)

    if cv2.waitKey(1) & 0xFF == 27:
        break

cap.release()
cv2.destroyAllWindows()
vis.destroy_window()
landmarker.close()

"""
演習6: 「型」の採点と差分可視化（BVH手本ゴースト + DTW時間合わせ）
- 手本: CMU BVH(cgspeed版ミラー)をFKで関節座標化し、MediaPipe 33点の主要関節へ対応づけ
- 学習者: カメラから一定秒数の姿勢を記録
- DTW(動的時間伸縮)で手本と学習者のフレーム対応をとり、対応づけた区間を並べて再生
- 関節ごとのずれを色で強調し、平均ずれスコアを表示
依存: mediapipe, opencv-python, open3d, numpy
"""

import os
import time
import urllib.request
import cv2
import numpy as np
import open3d as o3d
import mediapipe as mp
from mediapipe.tasks import python as mp_python
from mediapipe.tasks.python import vision

# === 保存先はコード保存フォルダ。モデル・BVHが無ければDL ====================
MODEL_URL = "https://storage.googleapis.com/mediapipe-models/pose_landmarker/pose_landmarker_lite/float16/1/pose_landmarker_lite.task"
BVH_URL = "https://raw.githubusercontent.com/una-dinosauria/cmu-mocap/master/data/001/01_01.bvh"
BASE_DIR = os.path.dirname(os.path.abspath(__file__))
MODEL_PATH = os.path.join(BASE_DIR, "pose_landmarker_lite.task")
BVH_PATH = os.path.join(BASE_DIR, "mocap_data", "001", "01_01.bvh")
if not os.path.exists(MODEL_PATH):
    urllib.request.urlretrieve(MODEL_URL, MODEL_PATH)
if not os.path.exists(BVH_PATH):
    os.makedirs(os.path.dirname(BVH_PATH), exist_ok=True)
    urllib.request.urlretrieve(BVH_URL, BVH_PATH)
# === ここまで ================================================================

NUM_LANDMARKS = 33
COMPARE_IDS = [11, 12, 13, 14, 15, 16, 23, 24, 25, 26, 27, 28]
POSE_CONNECTIONS = [
    (11, 12), (11, 13), (13, 15), (12, 14), (14, 16),
    (11, 23), (12, 24), (23, 24),
    (23, 25), (25, 27), (24, 26), (26, 28),
]
CAPTURE_SECONDS = 6.0      # 学習者の動作を記録する秒数
DIFF_WARN_M = 0.15         # この距離(m)以上ずれた関節を赤で強調
FPS_ASSUMED = 30.0


# --- BVH パーサ + FK (基礎演習2・3と同一方針) -------------------------------
class Joint:
    def __init__(self, name):
        self.name = name
        self.offset = np.zeros(3)
        self.channels = []
        self.parent = None
        self.children = []


def parse_bvh(path):
    with open(path, "r") as f:
        tokens = f.read().split()
    i = 0
    joints, stack = [], []

    def new_joint(name):
        j = Joint(name)
        if stack:
            j.parent = stack[-1]
            stack[-1].children.append(j)
        joints.append(j)
        return j

    while i < len(tokens):
        t = tokens[i]
        if t in ("ROOT", "JOINT"):
            stack.append(new_joint(tokens[i + 1])); i += 2
        elif t == "End":
            stack.append(None); i += 2
        elif t == "OFFSET":
            off = np.array([float(tokens[i + 1]), float(tokens[i + 2]), float(tokens[i + 3])])
            if stack[-1] is not None:
                stack[-1].offset = off
            i += 4
        elif t == "CHANNELS":
            n = int(tokens[i + 1])
            stack[-1].channels = tokens[i + 2:i + 2 + n]; i += 2 + n
        elif t == "{":
            i += 1
        elif t == "}":
            stack.pop(); i += 1
        elif t == "MOTION":
            i += 1; break
        else:
            i += 1

    num_frames = int(tokens[i + 1])
    frame_time = float(tokens[i + 4])
    i += 5
    total = sum(len(j.channels) for j in joints)
    values = np.array(tokens[i:i + num_frames * total], dtype=float)
    return joints, values.reshape(num_frames, total), frame_time


def rotation_matrix(axis, deg):
    r = np.radians(deg); c, s = np.cos(r), np.sin(r)
    if axis == "X":
        return np.array([[1, 0, 0], [0, c, -s], [0, s, c]])
    if axis == "Y":
        return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])
    return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])


def compute_joint_positions(joints, frame):
    positions = {}
    ch_index = 0
    ch_map = {}
    for j in joints:
        ch_map[j] = frame[ch_index:ch_index + len(j.channels)]
        ch_index += len(j.channels)

    def recurse(joint, parent_pos, parent_rot):
        vals = ch_map[joint]
        trans = np.zeros(3); rot = np.eye(3); vi = 0
        for ch in joint.channels:
            v = vals[vi]; vi += 1
            if ch.endswith("position"):
                trans["XYZ".index(ch[0])] = v
            elif ch.endswith("rotation"):
                rot = rot @ rotation_matrix(ch[0], v)
        world_pos = parent_pos + parent_rot @ (joint.offset + trans)
        world_rot = parent_rot @ rot
        positions[joint] = world_pos
        for c in joint.children:
            recurse(c, world_pos, world_rot)

    recurse(joints[0], np.zeros(3), np.eye(3))
    return positions


# BVH関節名(MotionBuilder命名) -> MediaPipe番号。鼻(0)はHead位置で代用
BVH_TO_MP = {
    "LeftArm": 11, "RightArm": 12,
    "LeftForeArm": 13, "RightForeArm": 14,
    "LeftHand": 15, "RightHand": 16,
    "LeftUpLeg": 23, "RightUpLeg": 24,
    "LeftLeg": 25, "RightLeg": 26,
    "LeftFoot": 27, "RightFoot": 28,
    "Head": 0,
}


def bvh_to_landmark_series(path):
    """BVHを読み、各フレームを MediaPipe 33点配列(腰中心・メートル)に変換した時系列を返す。"""
    joints, frames_data, _ = parse_bvh(path)
    name_to_joint = {j.name: j for j in joints}
    pairs = [(nm, mid) for nm, mid in BVH_TO_MP.items() if nm in name_to_joint]

    n = len(frames_data)
    series = np.zeros((n, NUM_LANDMARKS, 3))
    for fr in range(n):
        pos = compute_joint_positions(joints, frames_data[fr])
        for nm, mid in pairs:
            series[fr, mid] = pos[name_to_joint[nm]]
    series *= 0.01   # cm -> m (CMU BVHはcm規模)
    for fr in range(n):
        hip = 0.5 * (series[fr, 23] + series[fr, 24])
        series[fr] -= hip   # 腰中心を原点へ
    return series[1:]   # フレーム0(Tポーズ)は除外


# --- DTW (動的時間伸縮): numpyのみ ------------------------------------------
def pose_feature(pts):
    """比較用の特徴: 主要関節の座標を平らに並べたベクトル。"""
    return pts[COMPARE_IDS].reshape(-1)


def dtw_path(A_series, B_series):
    """手本Aと学習者Bのフレーム対応(ワーピングパス) [(ia, ib), ...] を返す。"""
    A = np.stack([pose_feature(p) for p in A_series])
    B = np.stack([pose_feature(p) for p in B_series])
    na, nb = len(A), len(B)
    D = np.linalg.norm(A[:, None, :] - B[None, :, :], axis=2)   # コスト行列
    acc = np.full((na + 1, nb + 1), np.inf)
    acc[0, 0] = 0.0
    for i in range(1, na + 1):
        for j in range(1, nb + 1):
            acc[i, j] = D[i - 1, j - 1] + min(
                acc[i - 1, j], acc[i, j - 1], acc[i - 1, j - 1])
    i, j = na, nb
    path = []
    while i > 0 and j > 0:
        path.append((i - 1, j - 1))
        step = np.argmin([acc[i - 1, j - 1], acc[i - 1, j], acc[i, j - 1]])
        if step == 0:
            i, j = i - 1, j - 1
        elif step == 1:
            i = i - 1
        else:
            j = j - 1
    path.reverse()
    return path


# --- MediaPipe / 描画ユーティリティ ----------------------------------------
def landmarks_to_points(world_landmarks):
    pts = np.zeros((NUM_LANDMARKS, 3))
    for i, lm in enumerate(world_landmarks):
        pts[i] = [lm.x, -lm.y, -lm.z]
    return pts


def align_to_hip(pts):
    hip = 0.5 * (pts[23] + pts[24])
    return pts - hip


def draw_landmarks_2d(image, norm_landmarks):
    h, w = image.shape[:2]
    px = [(int(lm.x * w), int(lm.y * h)) for lm in norm_landmarks]
    for a, b in POSE_CONNECTIONS:
        cv2.line(image, px[a], px[b], (255, 200, 0), 2)
    for (x, y) in px:
        cv2.circle(image, (x, y), 4, (0, 0, 255), -1)


def make_lineset(color):
    ls = o3d.geometry.LineSet(
        points=o3d.utility.Vector3dVector(np.zeros((NUM_LANDMARKS, 3))),
        lines=o3d.utility.Vector2iVector(np.array(POSE_CONNECTIONS)))
    ls.paint_uniform_color(color)
    return ls


# === 1) 手本(BVH)をゴースト用の関節時系列に変換 =============================
teacher = bvh_to_landmark_series(BVH_PATH)
print(f"手本(BVH)フレーム数: {len(teacher)}")

# === 2) 学習者の動作を一定秒数だけ記録 ======================================
base = mp_python.BaseOptions(model_asset_path=MODEL_PATH)
options = vision.PoseLandmarkerOptions(
    base_options=base, running_mode=vision.RunningMode.VIDEO, num_poses=1)
landmarker = vision.PoseLandmarker.create_from_options(options)

cap = cv2.VideoCapture(0)   # カメラ。動画で試す場合は VideoCapture("movie.mp4")
frame_index = 0
learner = []

print(f"{CAPTURE_SECONDS:.0f}秒間、手本の動きをまねてください。記録中…")
start = time.time()
while time.time() - start < CAPTURE_SECONDS:
    ret, frame = cap.read()
    if not ret:
        break
    rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
    mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=rgb)
    timestamp_ms = int(frame_index * 1000.0 / FPS_ASSUMED)
    frame_index += 1
    result = landmarker.detect_for_video(mp_image, timestamp_ms)
    if result.pose_world_landmarks:
        learner.append(align_to_hip(landmarks_to_points(result.pose_world_landmarks[0])))
    if result.pose_landmarks:
        draw_landmarks_2d(frame, result.pose_landmarks[0])
    remain = CAPTURE_SECONDS - (time.time() - start)
    cv2.putText(frame, f"REC {remain:4.1f}s", (10, 40),
                cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 0, 255), 2)
    cv2.imshow("camera", frame)
    if cv2.waitKey(1) & 0xFF == 27:
        break

cap.release()
cv2.destroyAllWindows()
landmarker.close()

learner = np.array(learner)
if len(learner) < 2:
    raise SystemExit("学習者の姿勢が十分に記録できませんでした。明るい場所で再試行してください。")
print(f"学習者フレーム数: {len(learner)}")

# === 3) DTWで時間対応をとる =================================================
path = dtw_path(teacher, learner)
print(f"DTW対応ペア数: {len(path)}")

# === 4) 対応ペアを並べて再生し、ずれを可視化 ================================
teacher_ls = make_lineset([0.2, 0.8, 0.3])   # 手本ゴースト(緑)
learner_ls = make_lineset([0.1, 0.4, 0.9])   # 学習者(青)
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(np.zeros((NUM_LANDMARKS, 3)))

vis = o3d.visualization.Visualizer()
vis.create_window(window_name="Kata scoring (BVH ghost + DTW)", width=900, height=700)
vis.add_geometry(teacher_ls)
vis.add_geometry(learner_ls)
vis.add_geometry(pcd)
vis.add_geometry(o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5))

k = 0
while True:
    ia, ib = path[k]
    t_pts = teacher[ia]
    l_pts = learner[ib]
    teacher_ls.points = o3d.utility.Vector3dVector(t_pts)
    learner_ls.points = o3d.utility.Vector3dVector(l_pts)
    pcd.points = o3d.utility.Vector3dVector(l_pts)

    # 関節ごとのずれ -> 平均スコアと色
    diffs = {i: float(np.linalg.norm(l_pts[i] - t_pts[i])) for i in COMPARE_IDS}
    mean_dev = sum(diffs.values()) / len(diffs)
    colors = np.tile(np.array([0.1, 0.4, 0.9]), (NUM_LANDMARKS, 1))
    for i, d in diffs.items():
        if d >= DIFF_WARN_M:
            colors[i] = [0.95, 0.1, 0.1]
    pcd.colors = o3d.utility.Vector3dVector(colors)

    vis.update_geometry(teacher_ls)
    vis.update_geometry(learner_ls)
    vis.update_geometry(pcd)
    if not vis.poll_events():
        break
    vis.update_renderer()
    print(f"\rpair {k+1}/{len(path)}  mean deviation {mean_dev:.3f} m", end="")

    time.sleep(1.0 / FPS_ASSUMED)
    k = (k + 1) % len(path)   # ループ再生

vis.destroy_window()
print()