webカメラの認識した動きをbvhに変換するプログラムの作成に行き詰まっている

import cv2
import mediapipe as mp
import numpy as np
from datetime import datetime
import time
import tkinter as tk
from tkinter import ttk
import threading
from PIL import Image, ImageTk
from scipy.signal import savgol_filter
import ssl
import os
import logging

# 警告メッセージの抑制 
os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2'
logging.getLogger('mediapipe').setLevel(logging.ERROR)

# SSL証明書の検証をスキップ
ssl._create_default_https_context = ssl._create_unverified_context


class MotionToBVH:
    def __init__(self):
        self.mp_pose = mp.solutions.pose
        self.pose = mp.solutions.pose.Pose(
            min_detection_confidence=0.5,
            min_tracking_confidence=0.5,
            model_complexity=2,
            enable_segmentation=True,
            smooth_landmarks=True
        )
        self.landmarks_history = []
        self.smoothed_landmarks = []
        self.is_recording = False
        self.calibrated = False
        self.base_rotation = None
        self.gui = None
        self.video_thread = None
        self.camera_active = True
        self.frame_count = 0
        
    def setup_gui(self):
        self.gui = tk.Tk()
        self.gui.title("Motion Capture to BVH")
        
        main_frame = ttk.Frame(self.gui, padding="10")
        main_frame.grid(row=0, column=0, sticky=(tk.W, tk.E, tk.N, tk.S))

        self.camera_label = ttk.Label(main_frame)
        self.camera_label.grid(row=0, column=0, columnspan=3, padx=5, pady=5)

        control_frame = ttk.Frame(main_frame)
        control_frame.grid(row=1, column=0, columnspan=3, pady=5)

        self.start_button = tk.Button(
            control_frame, 
            text="Start Recording",
            command=self.toggle_recording,
            width=15,
            height=2
        )
        self.start_button.grid(row=0, column=0, padx=5)

        self.calibrate_button = tk.Button(
            control_frame,
            text="Calibrate",
            command=self.calibrate,
            width=15,
            height=2
        )
        self.calibrate_button.grid(row=0, column=1, padx=5)

        self.save_button = tk.Button(
            control_frame,
            text="Save BVH",
            command=self.save_bvh,
            width=15,
            height=2
        )
        self.save_button.grid(row=0, column=2, padx=5)

        self.frame_label = ttk.Label(main_frame, text="Frames: 0", font=('Arial', 12, 'bold'))
        self.frame_label.grid(row=2, column=0, columnspan=3, pady=5)

        self.status_label = ttk.Label(main_frame, text="Ready")
        self.status_label.grid(row=3, column=0, columnspan=3, pady=5)

        self.progress = ttk.Progressbar(
            main_frame,
            length=300,
            mode='determinate'
        )
        self.progress.grid(row=4, column=0, columnspan=3, pady=5)

        self.gui.update_idletasks()
        self.gui.after(100, self.update_gui)

        self.video_thread = threading.Thread(target=self.update_camera)
        self.video_thread.daemon = True
        self.video_thread.start()

    def toggle_recording(self):
        self.is_recording = not self.is_recording
        if self.is_recording:
            self.start_button.config(text="Stop Recording", bg='red')
            self.landmarks_history = []
            self.frame_count = 0
            self.status_label.config(text="Recording...")
        else:
            self.start_button.config(text="Start Recording", bg='SystemButtonFace')
            self.status_label.config(text=f"Recorded {self.frame_count} frames")
            self.update_gui()

    def update_gui(self):
        self.gui.update_idletasks()
        self.gui.after(100, self.update_gui)

    def update_camera(self):
        try:
            cap = cv2.VideoCapture(0, cv2.CAP_AVFOUNDATION)
            
            if not cap.isOpened():
                self.status_label.config(text="カメラを開けませんでした")
                return

            cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
            cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
            cap.set(cv2.CAP_PROP_FPS, 30)

            time.sleep(1)

            while self.camera_active and cap.isOpened():
                try:
                    ret, frame = cap.read()
                    if not ret:
                        continue

                    image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
                    results = self.pose.process(image)

                    if results.pose_landmarks:
                        self.draw_landmarks(image, results.pose_landmarks)
                        if self.is_recording:
                            self.landmarks_history.append(results.pose_landmarks.landmark)
                            self.frame_count += 1
                            if len(self.landmarks_history) >= 5:
                                self.smoothed_landmarks = self.smooth_landmarks(
                                    self.landmarks_history[-5:]
                                )
                            self.gui.after(0, lambda: self.frame_label.config(
                                text=f"Frames: {self.frame_count}",
                                foreground='red' if self.is_recording else 'black'
                            ))

                    self.update_camera_frame(image)

                except Exception as e:
                    print(f"フレーム処理エラー: {e}")
                    continue

        except Exception as e:
            print(f"カメラエラー: {e}")
            self.status_label.config(text=f"カメラエラー: {e}")

        finally:
            if 'cap' in locals():
                cap.release()

    def calibrate(self):
        self.calibrate_button.config(state='disabled', bg='yellow')
        self.status_label.config(text="Calibrating... Stand in T-pose")
        
        def calibration_thread():
            self.calibrated = False
            time.sleep(3)

            if hasattr(self, 'pose') and self.pose is not None:
                cap = cv2.VideoCapture(0)
                ret, frame = cap.read()
                cap.release()
                
                if ret:
                    results = self.pose.process(cv2.cvtColor(frame, cv2.COLOR_BGR2RGB))
                    if results and results.pose_landmarks:
                        self.base_rotation = self.calculate_base_rotation(
                            results.pose_landmarks.landmark
                        )
                        self.calibrated = True
                        self.gui.after(0, lambda: self.status_label.config(
                            text="Calibration Complete"
                        ))
                    else:
                        self.gui.after(0, lambda: self.status_label.config(
                            text="Calibration Failed"
                        ))
            
            self.gui.after(0, lambda: self.calibrate_button.config(
                state='normal', bg='SystemButtonFace'
            ))

        threading.Thread(target=calibration_thread, daemon=True).start()

    def update_camera_frame(self, frame):
        try:
            image = Image.fromarray(frame)
            image = image.resize((640, 480), Image.Resampling.LANCZOS)
            photo = ImageTk.PhotoImage(image=image)
            self.camera_label.configure(image=photo)
            self.camera_label.image = photo
        except Exception as e:
            print(f"フレーム更新エラー: {e}")

    def draw_landmarks(self, image, landmarks):
        mp_drawing = mp.solutions.drawing_utils
        mp_drawing.draw_landmarks(
            image,
            landmarks,
            self.mp_pose.POSE_CONNECTIONS,
            mp_drawing.DrawingSpec(color=(245,117,66), thickness=2, circle_radius=2),
            mp_drawing.DrawingSpec(color=(245,66,230), thickness=2, circle_radius=2)
        )

    def calculate_base_rotation(self, landmarks):
        left_hip = np.array([landmarks[23].x, landmarks[23].y, landmarks[23].z])
        right_hip = np.array([landmarks[24].x, landmarks[24].y, landmarks[24].z])
        hip_vector = right_hip - left_hip
        angle = np.arctan2(hip_vector[1], hip_vector[0])
        return angle

    def normalize_vector(self, v):
        norm = np.linalg.norm(v)
        return v / norm if norm > 0 else v

    def smooth_landmarks(self, landmarks_history, window=5):
        if len(landmarks_history) < window:
            return landmarks_history

        smoothed = []
        num_landmarks = len(landmarks_history[0])
        
        for i in range(num_landmarks):
            x_data = [frame[i].x for frame in landmarks_history]
            y_data = [frame[i].y for frame in landmarks_history]
            z_data = [frame[i].z for frame in landmarks_history]
            
            if len(x_data) >= window:
                x_smooth = savgol_filter(x_data, window, 3)
                y_smooth = savgol_filter(y_data, window, 3)
                z_smooth = savgol_filter(z_data, window, 3)
                
                current_landmark = type('', (), {})()
                current_landmark.x = float(x_smooth[-1])
                current_landmark.y = float(y_smooth[-1])
                current_landmark.z = float(z_smooth[-1])
                
                smoothed.append(current_landmark)
            else:
                smoothed.append(landmarks_history[-1][i])
        
        return smoothed

    def create_hierarchy(self):
        hierarchy = [
            "HIERARCHY",
            "ROOT Hips",
            "{",
            "\tOFFSET 0.00 0.00 0.00",
            "\tCHANNELS 3 Zrotation Yrotation Xrotation",
            "\tJOINT LeftUpLeg",
            "\t{",
            "\t\tOFFSET 3.91 0.00 0.00",
            "\t\tCHANNELS 3 Zrotation Yrotation Xrotation",
            "\t\tJOINT LeftLeg",
            "\t\t{",
            "\t\t\tOFFSET 0.00 -18.34 0.00",
            "\t\t\tCHANNELS 3 Zrotation Yrotation Xrotation",
            "\t\t\tEnd Site",
            "\t\t\t{",
            "\t\t\t\tOFFSET 0.00 -17.37 0.00",
            "\t\t\t}",
            "\t\t}",
            "\t}",
            "\tJOINT RightUpLeg",
            "\t{",
            "\t\tOFFSET -3.91 0.00 0.00",
            "\t\tCHANNELS 3 Zrotation Yrotation Xrotation",
            "\t\tJOINT RightLeg",
            "\t\t{",
            "\t\t\tOFFSET 0.00 -18.34 0.00",
            "\t\t\tCHANNELS 3 Zrotation Yrotation Xrotation",
            "\t\t\tEnd Site",
            "\t\t\t{",
            "\t\t\t\tOFFSET 0.00 -17.37 0.00",
            "\t\t\t}",
            "\t\t}",
            "\t}",
            "}"
        ]
        return "\n".join(hierarchy)

    def calculate_joint_angles(self, landmarks):
        angles = []
        
        # Hips rotation
        angles.extend([0.0, 0.0, 0.0])
        
        # LeftUpLeg
        hip_left = np.array([landmarks[23].x, landmarks[23].y, landmarks[23].z])
        knee_left = np.array([landmarks[25].x, landmarks[25].y, landmarks[25].z])
        hip_knee_left = self.normalize_vector(knee_left - hip_left)
        angles.extend([45.0, 0.0, 0.0])
        
        # LeftLeg
        ankle_left = np.array([landmarks[27].x, landmarks[27].y, landmarks[27].z])
        knee_ankle_left = self.normalize_vector(ankle_left - knee_left)
        knee_angle_left = np.clip(np.degrees(np.arccos(
            np.dot(hip_knee_left, knee_ankle_left))), 0, 160)
        angles.extend([knee_angle_left, 0.0, 0.0])
        
        # RightUpLeg
        hip_right = np.array([landmarks[24].x, landmarks[24].y, landmarks[24].z])
        knee_right = np.array([landmarks[26].x, landmarks[26].y, landmarks[26].z])
        hip_knee_right = self.normalize_vector(knee_right - hip_right)
        angles.extend([-45.0, 0.0, 0.0])
        
        # RightLeg
        ankle_right = np.array([landmarks[28].x, landmarks[28].y, landmarks[28].z])
        knee_ankle_right = self.normalize_vector(ankle_right - knee_right)
        knee_angle_right = np.clip(np.degrees(np.arccos(
            np.dot(hip_knee_right, knee_ankle_right))), 0, 160)
        angles.extend([-knee_angle_right, 0.0, 0.0])
        
        return angles

    def create_bvh(self, output_file):
        if not self.landmarks_history:
            return False

        try:
            with open(output_file, 'w') as f:
                f.write(self.create_hierarchy())
                f.write("\nMOTION\n")
                f.write(f"Frames: {len(self.landmarks_history)}\n")
                f.write("Frame Time: 0.033333\n")
                
                total_frames = len(self.landmarks_history)
                for i, frame_landmarks in enumerate(self.landmarks_history):
                    angles = self.calculate_joint_angles(frame_landmarks)
                    f.write(" ".join([f"{angle:.6f}" for angle in angles]) + "\n")
                    
                    self.progress['value'] = (i + 1) / total_frames * 100
                    self.gui.update_idletasks()
                    
            return True
        except Exception as e:
            print(f"Error creating BVH file: {e}")
            return False

    def save_bvh(self):
        self.save_button.config(state='disabled')
        if not self.landmarks_history:
            self.status_label.config(text="No motion data to save")
            self.save_button.config(state='normal')
            return

        def save_thread():
            filename = f"motion_capture_{datetime.now().strftime('%Y%m%d_%H%M%S')}.bvh"
            if self.create_bvh(filename):
                self.gui.after(0, lambda: self.status_label.config(text=f"Saved as {filename}"))
            else:
                self.gui.after(0, lambda: self.status_label.config(text="Failed to save BVH file"))
            self.gui.after(0, lambda: self.save_button.config(state='normal'))

        threading.Thread(target=save_thread, daemon=True).start()

    def run(self):
        self.setup_gui()
        self.gui.protocol("WM_DELETE_WINDOW", self.on_closing)
        self.gui.mainloop()

    def on_closing(self):
        self.camera_active = False
        if self.video_thread:
            self.video_thread.join(timeout=1.0)
        self.gui.destroy()


def main():
    app = MotionToBVH()
    app.run()


if __name__ == "__main__":
    main()