commit 14fb06a90331e8ad15217e893cbd37cdebd3c2bc Author: moscovskayaliza Date: Fri Mar 20 17:01:27 2026 +0300 Initial commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..53fe3f3 --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +__pycache__/ +*.pyc +*.pyo +*.pyd +.env diff --git a/README.md b/README.md new file mode 100644 index 0000000..ca8b5f4 --- /dev/null +++ b/README.md @@ -0,0 +1 @@ +Тут будут зависимости, инструкции и краткие описания diff --git a/config.py b/config.py new file mode 100644 index 0000000..5805fe3 --- /dev/null +++ b/config.py @@ -0,0 +1,35 @@ +class Config: + # ===== Камера ===== + CAMERA_ID = 0 + MIRROR_CAMERA = True + + # ===== Управление руками (скорости) ===== + ARM_CONTROL = { + 'linear_arm': 'right', + 'angular_arm': 'left', + 'max_speed_linear': 1.0, + 'max_speed_angular': 1.0, + 'dead_zone': 0.2, + 'debug': False + # минимальное относительное смещение для отклика + } + + # ===== Специальные жесты ===== + SPECIAL_GESTURE_MODE = 'ml' #ml #geometric + ML_GESTURE_MODEL = '/home/ubuntu/sirius/gesture_rec/special_model.pkl' + ML_GESTURE_CLASSES = ['dome', 'cross', 'none'] + + # ===== Робот ===== + ROBOT_MODE = 'simulator' + ROBOT_IMAGE_PATH = 'robot.png' # или None + + # ===== Симулятор карты ===== + MAP_WIDTH = 800 + MAP_HEIGHT = 600 + MAP_OBSTACLES = [ + (200, 150, 100, 200), + (500, 300, 150, 50), + ] + START_POS = (100, 100) + FINISH_POS = (700, 500) + ROBOT_RADIUS = 20 diff --git a/gesture_control/arm_control.py b/gesture_control/arm_control.py new file mode 100644 index 0000000..09c4725 --- /dev/null +++ b/gesture_control/arm_control.py @@ -0,0 +1,98 @@ +import numpy as np + +class ArmController: + def __init__(self, config, mirror=False): + self.config = config + self.mirror = mirror + self.shoulder_idx = {'left': 11, 'right': 12} + self.wrist_idx = {'left': 15, 'right': 16} + self.hip_idx = {'left': 23, 'right': 24} + self.dead_zone = config.get('dead_zone', 0.2) + self.debug = config.get('debug', False) + + def _get_shoulder_width(self, landmarks): + """Ширина плеч для нормировки горизонтальных смещений.""" + left = landmarks[11][:2] + right = landmarks[12][:2] + width = np.linalg.norm(right - left) + if width < 50 or width > 300: + return None + return width + + def _get_torso_height(self, landmarks): + """Высота торса для нормировки вертикальных смещений.""" + left_shoulder = landmarks[11][:2] + right_shoulder = landmarks[12][:2] + left_hip = landmarks[23][:2] + right_hip = landmarks[24][:2] + shoulder_center = (left_shoulder + right_shoulder) / 2 + hip_center = (left_hip + right_hip) / 2 + height = np.linalg.norm(shoulder_center - hip_center) + if height < 50: + return None + return height + + def _horizontal_displacement_rel(self, landmarks, side): + """ + Нормированное горизонтальное смещение запястья относительно плеча. + При mirror=True инвертируем знак, чтобы скомпенсировать отражение. + """ + if landmarks[self.shoulder_idx[side]][3] < 0.5 or landmarks[self.wrist_idx[side]][3] < 0.5: + return 0.0 + shoulder = landmarks[self.shoulder_idx[side]][:2] + wrist = landmarks[self.wrist_idx[side]][:2] + shoulder_width = self._get_shoulder_width(landmarks) + if shoulder_width is None: + return 0.0 + disp = wrist[0] - shoulder[0] + if self.mirror: + disp = -disp + return disp / shoulder_width + + def _vertical_displacement_rel(self, landmarks, side): + """ + Нормированное вертикальное смещение запястья относительно плеча. + Положительное – запястье выше плеча (рука вверх), отрицательное – ниже. + Зеркало не влияет. + """ + if landmarks[self.shoulder_idx[side]][3] < 0.5 or landmarks[self.wrist_idx[side]][3] < 0.5: + return 0.0 + shoulder = landmarks[self.shoulder_idx[side]][:2] + wrist = landmarks[self.wrist_idx[side]][:2] + torso_height = self._get_torso_height(landmarks) + if torso_height is None: + return 0.0 + # Так как y растёт вниз, то (shoulder[1] - wrist[1]) > 0, если запястье выше + disp = shoulder[1] - wrist[1] + return disp / torso_height + + def compute_speeds(self, landmarks): + # Проверка видимости ключевых точек + if (landmarks[11][3] < 0.5 or landmarks[12][3] < 0.5 or + landmarks[15][3] < 0.5 or landmarks[16][3] < 0.5): + #if self.debug: + #print("Руки не видны") + return 0.0, 0.0 + + # Правая рука (индекс 16) — линейная скорость + lin_rel = self._horizontal_displacement_rel(landmarks, 'right') + # Левая рука (индекс 15) — угловая скорость + ang_rel = self._vertical_displacement_rel(landmarks, 'left') + + if self.debug: + print(f"lin_rel={lin_rel:.3f}, ang_rel={ang_rel:.3f}") + + # Линейная скорость (только вперёд) + if lin_rel < self.dead_zone: + linear = 0.0 + else: + linear = min(lin_rel, 1.0) * self.config['max_speed_linear'] + + # Угловая скорость (положительная — вправо, отрицательная — влево) + if abs(ang_rel) < self.dead_zone: + angular = 0.0 + else: + ang_rel_clipped = np.clip(ang_rel, -1.0, 1.0) + angular = ang_rel_clipped * self.config['max_speed_angular'] + + return linear, angular diff --git a/gesture_control/feature_extractor.py b/gesture_control/feature_extractor.py new file mode 100644 index 0000000..06a18ab --- /dev/null +++ b/gesture_control/feature_extractor.py @@ -0,0 +1,26 @@ +import numpy as np + +def normalize_landmarks(landmarks): + """ + Нормализует полный скелет MediaPipe (33 точки) с использованием x,y,z. + Центрирует относительно центра бёдер и масштабирует по росту. + Возвращает плоский вектор (99,) из x,y,z всех точек. + """ + lm = landmarks[:, :3].copy() # (33,3) + + # Центр бёдер (индексы 23 и 24) + hip_center = (lm[23] + lm[24]) / 2 + + # Центр плеч (11 и 12) + shoulder_center = (lm[11] + lm[12]) / 2 + + # Рост – расстояние от бёдер до плеч + height = np.linalg.norm(shoulder_center - hip_center) + if height < 1e-6: + height = 1.0 + + # Центрируем и масштабируем + lm_centered = lm - hip_center + lm_normalized = lm_centered / height + + return lm_normalized.flatten() # (99,) diff --git a/gesture_control/special_gestures.py b/gesture_control/special_gestures.py new file mode 100644 index 0000000..03cf5c9 --- /dev/null +++ b/gesture_control/special_gestures.py @@ -0,0 +1,102 @@ +import numpy as np +from ml_gestures.predict import MLGesturePredictor + +class SpecialGestureDetector: + def __init__(self, mode='geometric', model_path=None, class_names=None): + self.mode = mode + if mode == 'ml': + if model_path is None or class_names is None: + raise ValueError("Для ML нужны model_path и class_names") + self.ml_predictor = MLGesturePredictor(model_path, class_names) + print("Использую статический ML классификатор") + else: + self.ml_predictor = None + print("Использую геометрические отношения для детекции специальных жестовq") + self.debug = True # Включите для отладки + + def predict(self, landmarks): + if self.mode == 'geometric': + return self._geometric_predict(landmarks) + else: + return self.ml_predictor.predict(landmarks) + + def _geometric_predict(self, landmarks): + # Индексы MediaPipe + idx = { + 'nose': 0, + 'left_shoulder': 11, + 'right_shoulder': 12, + 'left_elbow': 13, + 'right_elbow': 14, + 'left_wrist': 15, + 'right_wrist': 16, + 'left_hip': 23, + 'right_hip': 24, + } + + # Повышенный порог уверенности для специальных жестов + min_conf = 0.7 + required = ['left_shoulder', 'right_shoulder', 'left_elbow', 'right_elbow', + 'left_wrist', 'right_wrist', 'left_hip', 'right_hip'] + for p in required: + if landmarks[idx[p]][3] < min_conf: + if self.debug: + print(f"{p} low confidence") + return 'none' + + # Координаты (x, y) + l_sh = landmarks[idx['left_shoulder']][:2] + r_sh = landmarks[idx['right_shoulder']][:2] + l_el = landmarks[idx['left_elbow']][:2] + r_el = landmarks[idx['right_elbow']][:2] + l_wr = landmarks[idx['left_wrist']][:2] + r_wr = landmarks[idx['right_wrist']][:2] + l_hip = landmarks[idx['left_hip']][:2] + r_hip = landmarks[idx['right_hip']][:2] + + hip_center = (l_hip + r_hip) / 2 + shoulder_width = np.linalg.norm(r_sh - l_sh) + if shoulder_width < 50 or shoulder_width > 300: + if self.debug: + print("shoulder_width out of range") + return 'none' + + # ----- КРЕСТ (предплечья скрещены на груди) ----- + # 1. Запястья перекрещены (левое правее правого) И на уровне груди (ниже плеч, выше бедер) + wrists_crossed = l_wr[0] > r_wr[0] + wrists_chest_level = (max(l_wr[1], r_wr[1]) > max(l_sh[1], r_sh[1]) and + min(l_wr[1], r_wr[1]) < hip_center[1]) + + # 2. Локти тоже на уровне груди (примерно) + elbows_chest_level = (max(l_el[1], r_el[1]) > max(l_sh[1], r_sh[1]) * 0.9 and + min(l_el[1], r_el[1]) < hip_center[1] * 1.1) + + # 3. Руки согнуты (предплечья короче верхней части руки) + left_bent = np.linalg.norm(l_wr - l_el) < np.linalg.norm(l_el - l_sh) * 0.9 + right_bent = np.linalg.norm(r_wr - r_el) < np.linalg.norm(r_el - r_sh) * 0.9 + arms_bent = left_bent and right_bent + + # 4. Запястья близко к центру тела (не сильно отведены, типично для креста на груди) + body_center_x = (l_sh[0] + r_sh[0]) / 2 + wrists_near_center = (abs(l_wr[0] - body_center_x) < shoulder_width * 0.6 and + abs(r_wr[0] - body_center_x) < shoulder_width * 0.6) + + cross = (wrists_crossed and wrists_chest_level and elbows_chest_level and + arms_bent and wrists_near_center) + + if self.debug and cross: + print(f"CROSS: crossed={wrists_crossed}, chest_w={wrists_chest_level}, " + f"chest_e={elbows_chest_level}, bent={arms_bent}, near_center={wrists_near_center}") + if cross: + return 'cross' + + # ----- ДОМИК (руки над головой) ----- + head_y = landmarks[idx['nose']][1] - 50 + arms_up = (l_wr[1] < head_y and r_wr[1] < head_y) + if arms_up: + dist = np.linalg.norm(l_wr - r_wr) + rel_dist = dist / shoulder_width + if rel_dist < 1.5: + return 'dome' + + return 'none' diff --git a/gesture_control/state.py b/gesture_control/state.py new file mode 100644 index 0000000..a2041be --- /dev/null +++ b/gesture_control/state.py @@ -0,0 +1,9 @@ +class ControlState: + def __init__(self, initial_enabled=True): + self.enabled = initial_enabled + + def update(self, special_gesture): + if special_gesture == 'dome': + self.enabled = True + elif special_gesture == 'cross': + self.enabled = False diff --git a/main.py b/main.py new file mode 100644 index 0000000..b8bbab2 --- /dev/null +++ b/main.py @@ -0,0 +1,101 @@ +import cv2 +import sys +from config import Config +from skeleton.mediapipe_detector import MediaPipeDetector +from gesture_control.arm_control import ArmController +from gesture_control.special_gestures import SpecialGestureDetector +from gesture_control.state import ControlState +from robot.map_simulator import MapSimulator +from robot.dummy import DummyRobot + +def main(): + cfg = Config() + + # Детектор + detector = MediaPipeDetector() + + # Состояние управления + state = ControlState() + + # Специальные жесты + special_detector = SpecialGestureDetector( + mode=cfg.SPECIAL_GESTURE_MODE, + model_path=cfg.ML_GESTURE_MODEL, + class_names=cfg.ML_GESTURE_CLASSES + ) + + # Управление скоростями + arm_control = ArmController(cfg.ARM_CONTROL, mirror=cfg.MIRROR_CAMERA) + + # Робот + if cfg.ROBOT_MODE == 'simulator': + robot = MapSimulator(cfg) + else: + robot = DummyRobot() + + # Камера + cap = cv2.VideoCapture(cfg.CAMERA_ID) + if not cap.isOpened(): + print("Ошибка: не удалось открыть камеру") + sys.exit(1) + + print("Управление: крест руками = СТОП, домик = ПУСК") + print("Линейная скорость: правая рука в сторону, угловая: левая рука") + print("В симуляторе: R – перезапуск после Game Over/победы") + print("q – выход") + + while True: + ret, frame = cap.read() + if not ret: + break + + if cfg.MIRROR_CAMERA: + frame = cv2.flip(frame, 1) + + result = detector.detect(frame) + + if result['success']: + landmarks = result['landmarks'] + vis_frame = detector.draw_landmarks(frame, result['pose_landmarks']) + + special = special_detector.predict(landmarks) + state.update(special) + + if state.enabled: + linear, angular = arm_control.compute_speeds(landmarks) + else: + linear, angular = 0.0, 0.0 + + robot.set_speeds(linear, angular) + + # Отрисовка на видео + cv2.putText(vis_frame, f"State: {'ON' if state.enabled else 'OFF'}", + (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, + (0,255,0) if state.enabled else (0,0,255), 2) + cv2.putText(vis_frame, f"L:{linear:.2f} A:{angular:.2f}", + (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0), 2) + if special != 'none': + cv2.putText(vis_frame, f"Special: {special}", (10, 90), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0), 2) + else: + vis_frame = frame + cv2.putText(vis_frame, "No pose detected", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 2) + + cv2.imshow('Camera', vis_frame) + + if cfg.ROBOT_MODE == 'simulator': + if not robot.step(): + break + else: + robot.step() + + if cv2.waitKey(1) & 0xFF == ord('q'): + break + + cap.release() + cv2.destroyAllWindows() + robot.quit() + +if __name__ == '__main__': + main() diff --git a/ml_gestures/evaluate.py b/ml_gestures/evaluate.py new file mode 100644 index 0000000..312c1d4 --- /dev/null +++ b/ml_gestures/evaluate.py @@ -0,0 +1,47 @@ +import pandas as pd +import numpy as np +import joblib +import argparse +from sklearn.metrics import classification_report, confusion_matrix, accuracy_score + +def evaluate(csv_path, model_path, test_size=0.2, random_state=42): + # Загрузка модели + data = joblib.load(model_path) + model = data['model'] + class_names = data['class_names'] + print(f"Loaded model with classes: {class_names}") + + # Загрузка данных + df = pd.read_csv(csv_path) + X = df.iloc[:, 1:].values.astype(np.float32) + y_labels = df.iloc[:, 0].values + + label_to_id = {label: i for i, label in enumerate(class_names)} + y = np.array([label_to_id[label] for label in y_labels]) + + # Разделение (должно совпадать с параметрами обучения) + from sklearn.model_selection import train_test_split + _, X_test, _, y_test, _, y_labels_test = train_test_split( + X, y, y_labels, test_size=test_size, random_state=random_state, stratify=y + ) + print(f"Test size: {len(X_test)}") + + y_pred = model.predict(X_test) + acc = accuracy_score(y_test, y_pred) + report = classification_report(y_test, y_pred, target_names=class_names) + cm = confusion_matrix(y_test, y_pred) + + print(f"\nAccuracy: {acc:.4f}") + print("\nClassification Report:") + print(report) + print("Confusion Matrix:") + print(cm) + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('--csv', required=True, help='CSV с данными') + parser.add_argument('--model', required=True, help='Путь к модели') + parser.add_argument('--test_size', type=float, default=0.2) + parser.add_argument('--random_state', type=int, default=42) + args = parser.parse_args() + evaluate(args.csv, args.model, args.test_size, args.random_state) diff --git a/ml_gestures/feature_extractor.py b/ml_gestures/feature_extractor.py new file mode 100644 index 0000000..06a18ab --- /dev/null +++ b/ml_gestures/feature_extractor.py @@ -0,0 +1,26 @@ +import numpy as np + +def normalize_landmarks(landmarks): + """ + Нормализует полный скелет MediaPipe (33 точки) с использованием x,y,z. + Центрирует относительно центра бёдер и масштабирует по росту. + Возвращает плоский вектор (99,) из x,y,z всех точек. + """ + lm = landmarks[:, :3].copy() # (33,3) + + # Центр бёдер (индексы 23 и 24) + hip_center = (lm[23] + lm[24]) / 2 + + # Центр плеч (11 и 12) + shoulder_center = (lm[11] + lm[12]) / 2 + + # Рост – расстояние от бёдер до плеч + height = np.linalg.norm(shoulder_center - hip_center) + if height < 1e-6: + height = 1.0 + + # Центрируем и масштабируем + lm_centered = lm - hip_center + lm_normalized = lm_centered / height + + return lm_normalized.flatten() # (99,) diff --git a/ml_gestures/predict.py b/ml_gestures/predict.py new file mode 100644 index 0000000..59b3277 --- /dev/null +++ b/ml_gestures/predict.py @@ -0,0 +1,18 @@ +import joblib +import numpy as np +from .feature_extractor import normalize_landmarks + +class MLGesturePredictor: + def __init__(self, model_path, class_names): + data = joblib.load(model_path) + self.model = data['model'] + self.class_names = data['class_names'] + if 'none' not in self.class_names: + self.class_names.append('none') # запасной вариант + + def predict(self, landmarks): + features = normalize_landmarks(landmarks).reshape(1, -1) + pred_id = self.model.predict(features)[0] + if pred_id < len(self.class_names): + return self.class_names[pred_id] + return 'none' diff --git a/ml_gestures/train.py b/ml_gestures/train.py new file mode 100644 index 0000000..fce4f87 --- /dev/null +++ b/ml_gestures/train.py @@ -0,0 +1,135 @@ +import pandas as pd +import numpy as np +import joblib +import json +import argparse +from sklearn.linear_model import LogisticRegression +from sklearn.neural_network import MLPClassifier +from sklearn.ensemble import RandomForestClassifier +from sklearn.model_selection import train_test_split +from sklearn.metrics import classification_report, confusion_matrix, accuracy_score + +def balance_data(df, target_classes, random_state=42): + """ + Балансирует только указанные классы до минимального размера среди них. + Класс 'none' (и любые другие) остаются без изменений. + """ + # Разделяем на целевые и остальные + target_df = df[df['class'].isin(target_classes)] + other_df = df[~df['class'].isin(target_classes)] + + # Определяем минимальный размер среди целевых классов + class_counts = target_df['class'].value_counts() + min_count = class_counts.min() + + # Балансируем каждый целевой класс + balanced_parts = [] + for cls in target_classes: + cls_df = target_df[target_df['class'] == cls] + if len(cls_df) > min_count: + cls_df = cls_df.sample(n=min_count, random_state=random_state) + balanced_parts.append(cls_df) + + balanced_target = pd.concat(balanced_parts, ignore_index=True) + + # Объединяем с остальными данными (none и др.) + balanced_df = pd.concat([balanced_target, other_df], ignore_index=True) + return balanced_df + +def train(csv_path, model_path, model_type='mlp', test_size=0.2, random_state=42, balance=False, target_classes=None): + # Загрузка данных + df = pd.read_csv(csv_path) + print(f"Total samples: {len(df)}") + + # Балансировка + if balance: + if target_classes is None: + # По умолчанию балансируем все классы кроме 'none' (если есть) + all_classes = df['class'].unique() + target_classes = [c for c in all_classes if c != 'none'] + if not target_classes: + raise ValueError("No target classes found (none is the only class).") + print(f"Balancing target classes: {target_classes}") + df = balance_data(df, target_classes, random_state) + print(f"After balancing: {len(df)} samples") + print(df['class'].value_counts()) + + # Разделение на признаки и метки + X = df.iloc[:, 1:].values.astype(np.float32) + y_labels = df.iloc[:, 0].values + class_names = sorted(df['class'].unique()) + label_to_id = {label: i for i, label in enumerate(class_names)} + y = np.array([label_to_id[label] for label in y_labels]) + + print(f"Classes: {class_names}") + print(f"Feature count: {X.shape[1]}") + + # Стратифицированное разбиение + X_train, X_test, y_train, y_test = train_test_split( + X, y, test_size=test_size, random_state=random_state, stratify=y + ) + print(f"Train size: {len(X_train)}, Test size: {len(X_test)}") + + # Выбор модели + if model_type == 'linear': #попробовать добавить логистическую регрессию с полиномиальными признаками, можно чисто для сравнения + model = LogisticRegression(max_iter=1000, random_state=random_state) + elif model_type == 'mlp': + model = MLPClassifier(hidden_layer_sizes=(64, 32), activation='relu', + solver='adam', max_iter=500, random_state=random_state, + early_stopping=True, validation_fraction=0.2) + elif model_type == 'rf': + model = RandomForestClassifier(n_estimators=50, max_depth=10, random_state=random_state) + else: + raise ValueError("model_type должен быть linear, mlp или rf") + + # Обучение + model.fit(X_train, y_train) + + # Предсказание на тесте + y_pred = model.predict(X_test) + + # Метрики + accuracy = accuracy_score(y_test, y_pred) + report = classification_report(y_test, y_pred, target_names=class_names, output_dict=True) + conf_matrix = confusion_matrix(y_test, y_pred).tolist() + + print(f"\nAccuracy: {accuracy:.4f}") + print("\nClassification Report:") + for cls in class_names: + print(f"{cls}: precision={report[cls]['precision']:.3f}, recall={report[cls]['recall']:.3f}, f1={report[cls]['f1-score']:.3f}") + print("\nConfusion Matrix:") + print(conf_matrix) + + # Сохранение модели и отчёта + joblib.dump({'model': model, 'class_names': class_names}, model_path) + report_data = { + 'model_type': model_type, + 'accuracy': accuracy, + 'classification_report': report, + 'confusion_matrix': conf_matrix, + 'train_samples': len(X_train), + 'test_samples': len(X_test), + 'classes': class_names, + 'balance': balance, + 'target_classes': target_classes if balance else None, + } + with open(model_path.replace('.pkl', '_report.json'), 'w') as f: + json.dump(report_data, f, indent=2) + + print(f"\nModel saved to {model_path}") + print(f"Report saved to {model_path.replace('.pkl', '_report.json')}") + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description='Обучение модели для распознавания жестов') + parser.add_argument('--csv', required=True, help='Путь к CSV-файлу с данными') + parser.add_argument('--model', required=True, help='Путь для сохранения модели (.pkl)') + parser.add_argument('--type', default='mlp', choices=['linear', 'mlp', 'rf'], help='Тип модели') + parser.add_argument('--test_size', type=float, default=0.2, help='Доля тестовой выборки') + parser.add_argument('--random_state', type=int, default=42, help='Seed для воспроизводимости') + parser.add_argument('--balance', action='store_true', help='Балансировать классы (кроме none)') + parser.add_argument('--target_classes', type=str, default=None, + help='Список целевых классов для балансировки через запятую (по умолчанию все, кроме none)') + args = parser.parse_args() + + target_classes = args.target_classes.split(',') if args.target_classes else None + train(args.csv, args.model, args.type, args.test_size, args.random_state, args.balance, target_classes) diff --git a/robot.png b/robot.png new file mode 100644 index 0000000..2246f68 Binary files /dev/null and b/robot.png differ diff --git a/robot/annotate.py b/robot/annotate.py new file mode 100644 index 0000000..eb88308 --- /dev/null +++ b/robot/annotate.py @@ -0,0 +1,95 @@ +import cv2 +import os +import csv +import sys +from pathlib import Path +import argparse + +sys.path.append(str(Path(__file__).parent.parent)) +from skeleton.mediapipe_detector import MediaPipeDetector +from ml_gestures.feature_extractor import normalize_landmarks + +def main(): + parser = argparse.ArgumentParser(description='Разметка изображений для обучения') + parser.add_argument('--folder', required=True, help='Папка с изображениями') + parser.add_argument('--classes', default='dome,cross,none', + help='Список классов через запятую') + parser.add_argument('--output', default='gesture_data.csv', + help='Имя выходного CSV-файла') + args = parser.parse_args() + + classes = [c.strip() for c in args.classes.split(',')] + key_to_class = {str(i+1): cls for i, cls in enumerate(classes)} + print("Классы:", classes) + + detector = MediaPipeDetector() + + image_extensions = ('.jpg', '.jpeg', '.png', '.bmp') + image_files = [f for f in os.listdir(args.folder) if f.lower().endswith(image_extensions)] + image_files.sort() + print(f"Найдено {len(image_files)} изображений.") + + csv_file = args.output + file_exists = os.path.isfile(csv_file) + if not file_exists: + with open(csv_file, 'w', newline='', encoding='utf-8') as f: + writer = csv.writer(f) + # 99 признаков (33 точки * 3 координаты) + writer.writerow(['class'] + [f'f{i}' for i in range(99)]) + + for idx, filename in enumerate(image_files): + filepath = os.path.join(args.folder, filename) + print(f"\n[{idx+1}/{len(image_files)}] {filename}") + + image = cv2.imread(filepath) + if image is None: + continue + + result = detector.detect(image) + if not result['success']: + cv2.imshow('No skeleton', image) + cv2.waitKey(0) + cv2.destroyAllWindows() + continue + + landmarks = result['landmarks'] + features = normalize_landmarks(landmarks) # вектор из 99 чисел + + display = detector.draw_landmarks(image, result['pose_landmarks']) + h, w = display.shape[:2] + + # Панель с инструкцией + overlay = display.copy() + cv2.rectangle(overlay, (0, h-80), (w, h), (50,50,50), -1) + cv2.addWeighted(overlay, 0.6, display, 0.4, 0, display) + + y = h - 60 + for i, cls in enumerate(classes): + cv2.putText(display, f"{i+1}:{cls}", (10 + i*120, y), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,255), 2) + cv2.putText(display, "n:skip q:quit", (10, y+30), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0), 2) + + cv2.imshow('Annotation', display) + key = cv2.waitKey(0) & 0xFF + cv2.destroyAllWindows() + + if key == ord('q'): + break + elif key == ord('n'): + continue + else: + key_char = chr(key) if key < 256 else None + if key_char in key_to_class: + selected = key_to_class[key_char] + with open(csv_file, 'a', newline='', encoding='utf-8') as f: + writer = csv.writer(f) + writer.writerow([selected] + features.tolist()) + print(f"Сохранено: {selected}") + else: + print("Неверная клавиша") + + print("Готово.") + +if __name__ == '__main__': + main() diff --git a/robot/dummy.py b/robot/dummy.py new file mode 100644 index 0000000..3c12ea9 --- /dev/null +++ b/robot/dummy.py @@ -0,0 +1,9 @@ +class DummyRobot: + def set_speeds(self, linear, angular): + print(f"Robot: linear={linear:.2f}, angular={angular:.2f}") + + def step(self): + pass + + def quit(self): + pass diff --git a/robot/map_simulator.py b/robot/map_simulator.py new file mode 100644 index 0000000..695df79 --- /dev/null +++ b/robot/map_simulator.py @@ -0,0 +1,137 @@ +import pygame +import sys +import math + +class MapSimulator: + def __init__(self, config): + pygame.init() + self.width = config.MAP_WIDTH + self.height = config.MAP_HEIGHT + self.screen = pygame.display.set_mode((self.width, self.height)) + pygame.display.set_caption("Robot Map Simulator") + self.clock = pygame.time.Clock() + self.font = pygame.font.SysFont(None, 24) + + self.obstacles = config.MAP_OBSTACLES + self.start = config.START_POS + self.finish = config.FINISH_POS + self.robot_radius = config.ROBOT_RADIUS + + if config.ROBOT_IMAGE_PATH: + try: + self.robot_image = pygame.image.load(config.ROBOT_IMAGE_PATH) + self.robot_image = pygame.transform.scale(self.robot_image, + (self.robot_radius*2, self.robot_radius*2)) + except: + self.robot_image = None + else: + self.robot_image = None + + self.reset() + + def reset(self): + self.x, self.y = self.start + self.angle = 0.0 + self.linear_speed = 0.0 + self.angular_speed = 0.0 + self.game_over = False + self.won = False + + def set_speeds(self, linear, angular): + self.linear_speed = linear + self.angular_speed = angular + + def update(self): + if self.game_over or self.won: + return + + dt = 1/30.0 + self.angle += self.angular_speed * dt * 2 + dx = self.linear_speed * math.cos(self.angle) * dt * 100 + dy = self.linear_speed * math.sin(self.angle) * dt * 100 + new_x = self.x + dx + new_y = self.y + dy + + # Ограничение границами карты + new_x = max(self.robot_radius, min(self.width - self.robot_radius, new_x)) + new_y = max(self.robot_radius, min(self.height - self.robot_radius, new_y)) + + # Проверка столкновений с препятствиями + if not self.check_collision(new_x, new_y): + self.x, self.y = new_x, new_y + else: + self.game_over = True + + # Проверка финиша + dist = math.hypot(self.x - self.finish[0], self.y - self.finish[1]) + if dist < self.robot_radius: + self.won = True + + def check_collision(self, x, y): + for ox, oy, ow, oh in self.obstacles: + closest_x = max(ox, min(x, ox + ow)) + closest_y = max(oy, min(y, oy + oh)) + dist = math.hypot(x - closest_x, y - closest_y) + if dist < self.robot_radius: + return True + return False + + def draw(self): + self.screen.fill((255,255,255)) + + for obs in self.obstacles: + pygame.draw.rect(self.screen, (100,100,100), obs) + + pygame.draw.circle(self.screen, (0,255,0), + (int(self.finish[0]), int(self.finish[1])), self.robot_radius, 2) + + if self.robot_image: + rotated = pygame.transform.rotate(self.robot_image, -math.degrees(self.angle)) + rect = rotated.get_rect(center=(int(self.x), int(self.y))) + self.screen.blit(rotated, rect) + else: + nose = (self.x + self.robot_radius * math.cos(self.angle), + self.y + self.robot_radius * math.sin(self.angle)) + left = (self.x + self.robot_radius * math.cos(self.angle + 2.5), + self.y + self.robot_radius * math.sin(self.angle + 2.5)) + right = (self.x + self.robot_radius * math.cos(self.angle - 2.5), + self.y + self.robot_radius * math.sin(self.angle - 2.5)) + pygame.draw.polygon(self.screen, (0,0,255), [nose, left, right]) + + texts = [ + f"Linear: {self.linear_speed:.2f}", + f"Angular: {self.angular_speed:.2f}", + ] + y = 10 + for t in texts: + surf = self.font.render(t, True, (0,0,0)) + self.screen.blit(surf, (10, y)) + y += 25 + + if self.game_over: + msg = self.font.render("GAME OVER – Press R to restart", True, (255,0,0)) + self.screen.blit(msg, (self.width//2 - 150, self.height//2)) + elif self.won: + msg = self.font.render("YOU WIN! – Press R to restart", True, (0,100,0)) + self.screen.blit(msg, (self.width//2 - 120, self.height//2)) + + pygame.display.flip() + + def handle_events(self): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return False + if event.type == pygame.KEYDOWN and event.key == pygame.K_r: + self.reset() + return True + + def step(self): + if not self.handle_events(): + return False + self.update() + self.draw() + self.clock.tick(30) + return True + + def quit(self): + pygame.quit() diff --git a/skeleton/mediapipe_detector.py b/skeleton/mediapipe_detector.py new file mode 100644 index 0000000..214782f --- /dev/null +++ b/skeleton/mediapipe_detector.py @@ -0,0 +1,41 @@ +import cv2 +import mediapipe as mp +import numpy as np + +class MediaPipeDetector: + def __init__(self, model_complexity=1, min_detection_confidence=0.5): + self.mp_pose = mp.solutions.pose + self.pose = self.mp_pose.Pose( + static_image_mode=False, + model_complexity=model_complexity, + enable_segmentation=False, + min_detection_confidence=min_detection_confidence + ) + self.mp_drawing = mp.solutions.drawing_utils + + def detect(self, image): + """Возвращает словарь с ключами: success, landmarks (33,4), pose_landmarks (для отрисовки)""" + rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) + results = self.pose.process(rgb) + + if results.pose_landmarks: + h, w, _ = image.shape + landmarks = [] + for lm in results.pose_landmarks.landmark: + x = lm.x * w + y = lm.y * h + z = lm.z + v = lm.visibility + landmarks.append([x, y, z, v]) + landmarks = np.array(landmarks, dtype=np.float32) + return {'success': True, 'landmarks': landmarks, 'pose_landmarks': results.pose_landmarks} + else: + return {'success': False, 'landmarks': None, 'pose_landmarks': None} + + def draw_landmarks(self, image, pose_landmarks): + """Рисует скелет на копии изображения и возвращает её""" + if pose_landmarks is None: + return image.copy() + vis = image.copy() + self.mp_drawing.draw_landmarks(vis, pose_landmarks, self.mp_pose.POSE_CONNECTIONS) + return vis diff --git a/utils/annotate.py b/utils/annotate.py new file mode 100644 index 0000000..cda39f4 --- /dev/null +++ b/utils/annotate.py @@ -0,0 +1,107 @@ +import cv2 +import os +import csv +import sys +from pathlib import Path +import argparse + +sys.path.append(str(Path(__file__).parent.parent)) +from skeleton.mediapipe_detector import MediaPipeDetector +from ml_gestures.feature_extractor import normalize_landmarks + +def main(): + parser = argparse.ArgumentParser(description='Разметка изображений для обучения') + parser.add_argument('--folder', required=True, help='Папка с изображениями') + parser.add_argument('--classes', default='dome,cross,none', + help='Список классов через запятую') + parser.add_argument('--output', default='gesture_data.csv', + help='Имя выходного CSV-файла') + parser.add_argument('--max_display_size', default='800,600', + help='Максимальный размер для отображения (ширина,высота)') + args = parser.parse_args() + + classes = [c.strip() for c in args.classes.split(',')] + key_to_class = {str(i+1): cls for i, cls in enumerate(classes)} + print("Классы:", classes) + + detector = MediaPipeDetector() + + max_width, max_height = map(int, args.max_display_size.split(',')) + + image_extensions = ('.jpg', '.jpeg', '.png', '.bmp') + image_files = [f for f in os.listdir(args.folder) if f.lower().endswith(image_extensions)] + image_files.sort() + print(f"Найдено {len(image_files)} изображений.") + + csv_file = args.output + file_exists = os.path.isfile(csv_file) + if not file_exists: + with open(csv_file, 'w', newline='', encoding='utf-8') as f: + writer = csv.writer(f) + writer.writerow(['class'] + [f'f{i}' for i in range(99)]) + + for idx, filename in enumerate(image_files): + filepath = os.path.join(args.folder, filename) + print(f"\n[{idx+1}/{len(image_files)}] {filename}") + + image = cv2.imread(filepath) + if image is None: + print("Не удалось загрузить") + continue + + result = detector.detect(image) + + if result['success']: + landmarks = result['landmarks'] + features = normalize_landmarks(landmarks) + vis_image = detector.draw_landmarks(image.copy(), result['pose_landmarks']) + else: + vis_image = image.copy() + landmarks = None + + # Масштабирование для отображения + h, w = vis_image.shape[:2] + scale = min(max_width / w, max_height / h, 1.0) + if scale < 1.0: + new_w = int(w * scale) + new_h = int(h * scale) + display = cv2.resize(vis_image, (new_w, new_h)) + else: + display = vis_image.copy() + + # Панель с инструкцией + dh, dw = display.shape[:2] + overlay = display.copy() + cv2.rectangle(overlay, (0, dh-80), (dw, dh), (50,50,50), -1) + cv2.addWeighted(overlay, 0.6, display, 0.4, 0, display) + + y = dh - 60 + for i, cls in enumerate(classes): + cv2.putText(display, f"{i+1}:{cls}", (10 + i*120, y), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,255), 2) + cv2.putText(display, "n:skip q:quit", (10, y+30), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0), 2) + + cv2.imshow('Annotation', display) + key = cv2.waitKey(0) & 0xFF + cv2.destroyAllWindows() + + if key == ord('q'): + break + elif key == ord('n'): + continue + else: + key_char = chr(key) if key < 256 else None + if key_char in key_to_class: + selected = key_to_class[key_char] + with open(csv_file, 'a', newline='', encoding='utf-8') as f: + writer = csv.writer(f) + writer.writerow([selected] + features.tolist()) + print(f"Сохранено: {selected}") + else: + print("Неверная клавиша") + + print("Готово.") + +if __name__ == '__main__': + main() diff --git a/utils/capture_photo.py b/utils/capture_photo.py new file mode 100644 index 0000000..640656e --- /dev/null +++ b/utils/capture_photo.py @@ -0,0 +1,79 @@ +import cv2 +import os +import sys +import time +from pathlib import Path + +def capture_photo(output_dir='captured', camera_id=0, mirror=True): + """ + Простой инструмент для захвата фото с камеры. + При нажатии 's' запускается отсчёт 3 секунды, затем делается снимок. + При нажатии 'q' выход. + """ + # Создаём папку, если её нет + Path(output_dir).mkdir(parents=True, exist_ok=True) + + cap = cv2.VideoCapture(camera_id) + if not cap.isOpened(): + print("Не удалось открыть камеру") + sys.exit(1) + + print("Нажмите 's' для захвата фото (отсчёт 3 секунды)") + print("Нажмите 'q' для выхода") + + capture_count = 0 + countdown = 0 + countdown_start = 0 + + while True: + ret, frame = cap.read() + if not ret: + break + + if mirror: + frame = cv2.flip(frame, 1) + + display = frame.copy() + + # Отображение отсчёта, если активен + if countdown > 0: + elapsed = time.time() - countdown_start + remaining = max(0, 3 - elapsed) + if remaining > 0: + cv2.putText(display, f"Capturing in {int(remaining)}", (display.shape[1]//2-100, display.shape[0]//2), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 2) + else: + # Сохраняем фото + timestamp = time.strftime("%Y%m%d_%H%M%S") + filename = f"photo_{timestamp}.jpg" + filepath = os.path.join(output_dir, filename) + cv2.imwrite(filepath, frame) + print(f"Сохранено: {filepath}") + capture_count += 1 + countdown = 0 + else: + # Подсказка + cv2.putText(display, "Press 's' to capture, 'q' to quit", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0), 2) + + cv2.imshow('Capture', display) + + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + break + elif key == ord('s') and countdown == 0: + countdown = 1 + countdown_start = time.time() + + cap.release() + cv2.destroyAllWindows() + print(f"Завершено. Сохранено {capture_count} фото.") + +if __name__ == '__main__': + import argparse + parser = argparse.ArgumentParser() + parser.add_argument('--dir', default='captured', help='Папка для сохранения') + parser.add_argument('--camera', type=int, default=0, help='ID камеры') + parser.add_argument('--no-mirror', action='store_true', help='Отключить зеркалирование') + args = parser.parse_args() + capture_photo(output_dir=args.dir, camera_id=args.camera, mirror=not args.no_mirror)