face detection

jericho/main.py

@@ -1,135 +1,336 @@
-import time
-import threading
+"""
+face_analysis.py  –  compatible MediaPipe 0.10+ (nouvelle API Tasks)
+Audio preservé via FFmpeg en fin de traitement.
+======================================================================
+Usage :
+  python face_analysis.py --video chemin/video.mp4
+  python face_analysis.py --video chemin/video.mp4 --output_dir resultats/
+"""
+
+import argparse
+import csv
+import json
+import math
+import os
+import subprocess
+import sys
+import urllib.request
+
+import cv2
+import mediapipe as mp
 import numpy as np
-
-from reachy_mini import ReachyMini, ReachyMiniApp
-from reachy_mini.utils import create_head_pose
-
-
-class TestDance1(ReachyMiniApp):
-
-    def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
-
-        reachy_mini.goto_target(create_head_pose(), antennas=[0.0, 0.0], duration=1.0)
-
-        start_time = time.time()
-
-        while not stop_event.is_set():
-            t = time.time()
-            elapsed_time = t - start_time
-
-            compteur = int(elapsed_time / 2)
-
-            antennas_offset = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * t))
-
-            sinus_rot = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * t))
-            sinus_trans = 0.03 * np.sin(2 * np.pi * 0.5 * t)
-
-            current_roll = 0.0
-            current_pitch = 0.0
-            current_yaw = 0.0
-            current_x = 0.0
-            current_y = 0.0
-            current_z = 0.0
-
-            if compteur < 3:
-                current_roll = sinus_rot
-            elif compteur < 6:
-                current_pitch = sinus_rot
-            elif compteur < 9:
-                current_yaw = sinus_rot
-            elif compteur < 12:
-                current_x = sinus_trans
-            elif compteur < 15:
-                current_y = sinus_trans
-            elif compteur < 18:
-                current_z = sinus_trans
-            else:
-                start_time = time.time()
-                continue
-
-            head_pose = create_head_pose(
-                x=current_x,
-                y=current_y,
-                z=current_z,
-                roll=current_roll,
-                pitch=current_pitch,
-                yaw=current_yaw,
-                degrees=False,
-                mm=False,
-            )
-
-            reachy_mini.set_target(head=head_pose, antennas=[antennas_offset, antennas_offset])
-
-            time.sleep(0.01)
-
-
-class TestDance2(ReachyMiniApp):
-
-    def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
-
-        reachy_mini.goto_target(create_head_pose(), antennas=[0.0, 0.0], duration=1.0)
-
-        start_time = time.time()
-
-        while not stop_event.is_set():
-            t = time.time()
-            elapsed_time = t - start_time
-
-            compteur = int(elapsed_time / 2)
-
-            antennas_offset = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * t))
-
-            sinus_rot = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * 4 * t))
-            sinus_trans = 0.03 * np.sin(2 * np.pi * 0.5 * t)
-            cosinus_trans = 0.03 * np.cos(2 * np.pi * 0.5 * t)
-
-            current_roll = 0.0
-            current_pitch = 0.0
-            current_yaw = 0.0
-            current_x = 0.0
-            current_y = 0.0
-            current_z = 0.0
-
-            if compteur < 3:
-                current_pitch = sinus_rot
-                current_y = sinus_trans
-            elif compteur < 6:
-                current_x = sinus_trans
-                current_y = cosinus_trans
-            else:
-                start_time = time.time()
-                continue
-
-            head_pose = create_head_pose(
-                x=current_x,
-                y=current_y,
-                z=current_z,
-                roll=current_roll,
-                pitch=current_pitch,
-                yaw=current_yaw,
-                degrees=False,
-                mm=False,
-            )
-
-            reachy_mini.set_target(head=head_pose, antennas=[antennas_offset, antennas_offset])
-
-            time.sleep(0.01)
+import matplotlib
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+
+from mediapipe.tasks import python as mp_python
+from mediapipe.tasks.python import vision as mp_vision
+from mediapipe.tasks.python.vision import FaceLandmarkerOptions, FaceLandmarker
+
+# ──────────────────────────────────────────────
+# Modèle MediaPipe
+# ──────────────────────────────────────────────
+MODEL_URL  = "https://storage.googleapis.com/mediapipe-models/face_landmarker/face_landmarker/float16/1/face_landmarker.task"
+MODEL_PATH = "face_landmarker.task"
+
+def ensure_model():
+    if not os.path.exists(MODEL_PATH):
+        print(f"Téléchargement du modèle MediaPipe -> {MODEL_PATH} ...")
+        urllib.request.urlretrieve(MODEL_URL, MODEL_PATH)
+        print("  Modele telecharge.")
+
+# ───────────────────���──────────────────────────
+# Indices landmarks
+# ──────────────────────────────────────────────
+LEFT_EYE    = [362, 385, 387, 263, 373, 380]
+RIGHT_EYE   = [33,  160, 158, 133, 153, 144]
+MOUTH_OUTER = [61,  291,   0,  17,  37, 267]
+MOUTH_L, MOUTH_R = 61, 291
+LEFT_PUP,  RIGHT_PUP  = 468, 473
+
+EAR_THRESHOLD = 0.20  # en dessous de ce seuil = œil considéré fermé
+
+HEAD_POSE_IDX = [1, 152, 226, 446, 57, 287]
+MODEL_3D = np.array([
+    [ 0.0,    0.0,    0.0 ],
+    [ 0.0,  -63.6,  -12.5],
+    [-43.3,  32.7,  -26.0],
+    [ 43.3,  32.7,  -26.0],
+    [-28.9, -28.9,  -24.1],
+    [ 28.9, -28.9,  -24.1],
+], dtype=np.float64)
+
+# ──────────────────────────────────────────────
+# Calculs
+# ──────────────────────────────────────────────
+
+def _pts(lms, indices, w, h):
+    return np.array([[lms[i].x * w, lms[i].y * h] for i in indices])
+
+def ear(lms, indices, w, h):
+    p = _pts(lms, indices, w, h)
+    A = np.linalg.norm(p[1]-p[5]); B = np.linalg.norm(p[2]-p[4])
+    return (A+B) / (2*np.linalg.norm(p[0]-p[3])+1e-6)
+
+def mar(lms, indices, w, h):
+    p = _pts(lms, indices, w, h)
+    A = np.linalg.norm(p[1]-p[5]); B = np.linalg.norm(p[2]-p[4])
+    return (A+B) / (2*np.linalg.norm(p[0]-p[3])+1e-6)
+
+def smile_index(lms, w, h):
+    ml = np.array([lms[MOUTH_L].x*w, lms[MOUTH_L].y*h])
+    mr = np.array([lms[MOUTH_R].x*w, lms[MOUTH_R].y*h])
+    pl = np.array([lms[LEFT_PUP].x*w,  lms[LEFT_PUP].y*h])
+    pr = np.array([lms[RIGHT_PUP].x*w, lms[RIGHT_PUP].y*h])
+    return np.linalg.norm(mr-ml) / (np.linalg.norm(pr-pl)+1e-6)
+
+def head_pose(lms, w, h):
+    img_pts = np.array([[lms[i].x*w, lms[i].y*h] for i in HEAD_POSE_IDX], dtype=np.float64)
+    cam  = np.array([[w,0,w/2],[0,w,h/2],[0,0,1]], dtype=np.float64)
+    dist = np.zeros((4,1))
+    ok, rvec, _ = cv2.solvePnP(MODEL_3D, img_pts, cam, dist, flags=cv2.SOLVEPNP_ITERATIVE)
+    if not ok:
+        return 0.0, 0.0, 0.0
+    R, _ = cv2.Rodrigues(rvec)
+    sy = math.sqrt(R[0,0]**2 + R[1,0]**2)
+    if sy > 1e-6:
+        roll  = math.degrees(math.atan2( R[2,1], R[2,2]))
+        pitch = math.degrees(math.atan2(-R[2,0], sy))
+        yaw   = math.degrees(math.atan2( R[1,0], R[0,0]))
+    else:
+        roll  = math.degrees(math.atan2(-R[1,2], R[1,1]))
+        pitch = math.degrees(math.atan2(-R[2,0], sy))
+        yaw   = 0.0
+    return yaw, pitch, roll
+
+def face_bbox(lms, w, h):
+    xs = [lm.x for lm in lms]; ys = [lm.y for lm in lms]
+    xmn,xmx,ymn,ymx = min(xs),max(xs),min(ys),max(ys)
+    return (xmn+xmx)/2,(ymn+ymx)/2,xmx-xmn,ymx-ymn,xmn,ymn,xmx,ymx
+
+# ──────────────────────────────────────────────
+# Annotation
+# ──────────────────────────────────────────────
+
+def annotate_frame(frame, lms, sig, w, h):
+    x1,y1 = int(sig["bbox_xmin"]*w), int(sig["bbox_ymin"]*h)
+    x2,y2 = int(sig["bbox_xmax"]*w), int(sig["bbox_ymax"]*h)
+    cv2.rectangle(frame,(x1,y1),(x2,y2),(0,255,0),2)
+    for idx in LEFT_EYE+RIGHT_EYE+MOUTH_OUTER:
+        cv2.circle(frame,(int(lms[idx].x*w),int(lms[idx].y*h)),2,(255,0,255),-1)
+    for idx in [LEFT_PUP, RIGHT_PUP]:
+        cv2.circle(frame,(int(lms[idx].x*w),int(lms[idx].y*h)),3,(0,200,255),-1)
+    lines = [
+        f"Yaw:{sig['yaw']:+.1f} Pitch:{sig['pitch']:+.1f} Roll:{sig['roll']:+.1f}",
+        f"EAR L:{sig['ear_left']:.3f}  R:{sig['ear_right']:.3f}",
+        f"MAR:{sig['mar']:.3f}  Smile:{sig['smile']:.3f}",
+        f"T:{sig['timestamp_s']:.2f}s",
+    ]
+    for i, line in enumerate(lines):
+        cv2.putText(frame, line, (x1, max(y1-10-i*18, 14)),
+                    cv2.FONT_HERSHEY_SIMPLEX, 0.48, (0,255,0), 1, cv2.LINE_AA)
+    nose = (int(lms[1].x*w), int(lms[1].y*h))
+    tip  = (int(nose[0]+60*math.sin(math.radians(sig["yaw"]))),
+            int(nose[1]-60*math.sin(math.radians(sig["pitch"]))))
+    cv2.arrowedLine(frame, nose, tip, (0,100,255), 2, tipLength=0.3)
+    return frame
+
+# ──────────────────────────────────────────────
+# Tracés temporels
+# ──────────────────────────────────────────────
+
+def make_plots(records, path):
+    ts = [r["timestamp_s"] for r in records]
+    fig, axes = plt.subplots(4,1, figsize=(14,12), sharex=True)
+    fig.suptitle("Signaux faciaux extraits", fontsize=14, fontweight="bold")
+    axes[0].plot(ts,[r["yaw"]   for r in records],label="Yaw")
+    axes[0].plot(ts,[r["pitch"] for r in records],label="Pitch")
+    axes[0].plot(ts,[r["roll"]  for r in records],label="Roll")
+    axes[0].set_ylabel("Orientation (deg)"); axes[0].legend(); axes[0].grid(alpha=.3)
+    axes[1].plot(ts,[r["ear_left"]  for r in records],label="EAR gauche")
+    axes[1].plot(ts,[r["ear_right"] for r in records],label="EAR droite")
+    axes[1].axhline(0.20,ls="--",color="gray",label="seuil ~0.20")
+    axes[1].set_ylabel("EAR"); axes[1].legend(); axes[1].grid(alpha=.3)
+    axes[2].plot(ts,[r["mar"]   for r in records],label="MAR bouche")
+    axes[2].plot(ts,[r["smile"] for r in records],label="Sourire")
+    axes[2].set_ylabel("MAR / Sourire"); axes[2].legend(); axes[2].grid(alpha=.3)
+    axes[3].plot(ts,[r["face_cx"] for r in records],label="Centre X")
+    axes[3].plot(ts,[r["face_cy"] for r in records],label="Centre Y")
+    axes[3].set_ylabel("Position (norm.)"); axes[3].set_xlabel("Temps (s)")
+    axes[3].legend(); axes[3].grid(alpha=.3)
+    plt.tight_layout(); plt.savefig(path, dpi=130); plt.close(fig)
+    print(f"  -> Traces : {path}")
+
+# ──────────────────────────────────────────────
+# Fusion audio via FFmpeg
+# ──────────────────────────────────────────────
+
+def merge_audio(original_video, silent_video, output_path):
+    """
+    Copie la piste audio de original_video dans silent_video.
+    Nécessite que ffmpeg soit dans le PATH.
+    Produit output_path.
+    """
+    # Vérifie si ffmpeg est disponible
+    try:
+        subprocess.run(
+            ["ffmpeg", "-version"],
+            stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, check=True
+        )
+    except (subprocess.CalledProcessError, FileNotFoundError):
+        print("\n  [AVERTISSEMENT] ffmpeg introuvable dans le PATH.")
+        print("  -> L'audio n'a pas ete recollé.")
+        print("     Installez ffmpeg puis lancez manuellement :")
+        print(f'     ffmpeg -i "{silent_video}" -i "{original_video}" '
+              f'-c copy -map 0:v:0 -map 1:a:0 -shortest "{output_path}"')
+        return False
+
+    cmd = [
+        "ffmpeg", "-y",
+        "-i", silent_video,      # vidéo annotée (sans audio)
+        "-i", original_video,    # vidéo originale (source audio)
+        "-c", "copy",            # pas de ré-encodage
+        "-map", "0:v:0",         # flux vidéo de la vidéo annotée
+        "-map", "1:a:0",         # flux audio de l'originale
+        "-shortest",             # aligne sur la durée la plus courte
+        output_path
+    ]
+    result = subprocess.run(cmd, capture_output=True, text=True)
+    if result.returncode == 0:
+        print(f"  -> Video finale avec audio : {output_path}")
+        return True
+    else:
+        print(f"  [ERREUR FFmpeg] {result.stderr[-300:]}")
+        return False
+
+# ──────────────────────────────────────────────
+# Pipeline principal
+# ──────────────────────────────────────────────
+
+def process_video(video_path, output_dir):
+    os.makedirs(output_dir, exist_ok=True)
+    ensure_model()
+
+    cap = cv2.VideoCapture(video_path)
+    if not cap.isOpened():
+        sys.exit(f"[ERREUR] Impossible d'ouvrir : {video_path}")
+    fps   = cap.get(cv2.CAP_PROP_FPS) or 30.0
+    W     = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
+    H     = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
+    total = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
+    print(f"Video : {W}x{H}  {fps:.1f} fps  {total} frames")
+
+    base_opts = mp_python.BaseOptions(model_asset_path=MODEL_PATH)
+    options   = FaceLandmarkerOptions(
+        base_options=base_opts,
+        running_mode=mp_vision.RunningMode.VIDEO,
+        num_faces=1,
+        min_face_detection_confidence=0.5,
+        min_face_presence_confidence=0.5,
+        min_tracking_confidence=0.5,
+        output_face_blendshapes=False,
+        output_facial_transformation_matrixes=False,
+    )
+    landmarker = FaceLandmarker.create_from_options(options)
+
+    # Fichier intermédiaire sans audio
+    silent_path = os.path.join(output_dir, "_annotated_silent.mp4")
+    final_path  = os.path.join(output_dir, "output_annotated.mp4")
+
+    writer = cv2.VideoWriter(
+        silent_path, cv2.VideoWriter_fourcc(*"mp4v"), fps, (W, H)
+    )
+
+    records   = []
+    frame_idx = 0
+
+    while True:
+        ret, frame = cap.read()
+        if not ret:
+            break
+
+        ts_ms = int(frame_idx * 1000 / fps)
+        ts_s  = frame_idx / fps
+
+        rgb      = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+        mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=rgb)
+        result   = landmarker.detect_for_video(mp_image, ts_ms)
+
+        if result.face_landmarks:
+            lms = result.face_landmarks[0]
+            cx,cy,bw,bh,xmn,ymn,xmx,ymx = face_bbox(lms, W, H)
+            yaw,pitch,roll = head_pose(lms, W, H)
+            el = ear(lms, LEFT_EYE,    W, H)
+            er = ear(lms, RIGHT_EYE,   W, H)
+            m  = mar(lms, MOUTH_OUTER, W, H)
+            sm = smile_index(lms, W, H)
+
+            rec = {
+                "frame":frame_idx, "timestamp_s":round(ts_s,4), "face_detected":True,
+                "face_cx":round(cx,4), "face_cy":round(cy,4),
+                "face_w":round(bw,4),  "face_h":round(bh,4),
+                "bbox_xmin":round(xmn,4), "bbox_ymin":round(ymn,4),
+                "bbox_xmax":round(xmx,4), "bbox_ymax":round(ymx,4),
+                "yaw":round(yaw,2), "pitch":round(pitch,2), "roll":round(roll,2),
+                "ear_left":round(el,4), "ear_right":round(er,4),
+                "eye_left_open":  bool(el >= EAR_THRESHOLD),
+                "eye_right_open": bool(er >= EAR_THRESHOLD),
+                "mar":round(m,4), "smile":round(sm,4),
+            }
+            records.append(rec)
+            frame = annotate_frame(frame, lms, rec, W, H)
+        else:
+            records.append({"frame":frame_idx,"timestamp_s":round(ts_s,4),"face_detected":False})
+            cv2.putText(frame,"Visage non detecte",(20,40),
+                        cv2.FONT_HERSHEY_SIMPLEX,0.8,(0,0,255),2)
+
+        writer.write(frame)
+        frame_idx += 1
+        if frame_idx % 100 == 0:
+            pct = 100*frame_idx//total if total else 0
+            print(f"  Frame {frame_idx}/{total} ({pct}%)")
+
+    cap.release(); writer.release(); landmarker.close()
+    print(f"  -> Video sans audio : {silent_path}")
+
+    # ── Recoller l'audio avec FFmpeg ──────────────
+    audio_ok = merge_audio(video_path, silent_path, final_path)
+    if audio_ok:
+        os.remove(silent_path)   # supprime le fichier intermédiaire
+    else:
+        # FFmpeg absent : renommer le silencieux en sortie finale
+        os.replace(silent_path, final_path)
+
+    # ── CSV ──────────────────────────────────────
+    csv_path = os.path.join(output_dir, "signals.csv")
+    fields = ["frame","timestamp_s","face_detected","face_cx","face_cy","face_w","face_h",
+              "bbox_xmin","bbox_ymin","bbox_xmax","bbox_ymax",
+              "yaw","pitch","roll",
+              "ear_left","ear_right","eye_left_open","eye_right_open",
+              "mar","smile"]
+    with open(csv_path,"w",newline="",encoding="utf-8") as f:
+        wc = csv.DictWriter(f, fieldnames=fields, extrasaction="ignore")
+        wc.writeheader(); wc.writerows(records)
+    print(f"  -> CSV : {csv_path}")
+
+    # ── JSON ─────────────────────────────────────
+    json_path = os.path.join(output_dir, "signals.json")
+    with open(json_path,"w",encoding="utf-8") as f:
+        json.dump(records, f, indent=2, ensure_ascii=False)
+    print(f"  -> JSON : {json_path}")
+
+    # ── Tracés ───────────────────────────────────
+    detected = [r for r in records if r.get("face_detected")]
+    if detected:
+        make_plots(detected, os.path.join(output_dir, "plots.png"))
+
+    print("\n  Traitement termine.")
+    print(f"  Fichiers dans : {os.path.abspath(output_dir)}/")
 
 
 if __name__ == "__main__":
-    print("Lancement manuel depuis le terminal...")
-
-    with ReachyMini(media_backend="no_media") as robot:
-
-        # 👉 Choisis ici ta danse
-        # mon_app = TestDance1()
-        mon_app = TestDance2()
-
-        stop_event = threading.Event()
-
-        try:
-            mon_app.run(robot, stop_event)
-        except KeyboardInterrupt:
-            print("\nArrêt du programme")
-            stop_event.set()
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--video",      required=True)
+    parser.add_argument("--output_dir", default="output")
+    args = parser.parse_args()
+    process_video(args.video, args.output_dir)