feat: add coordinates tracking

autopilot.py

@@ -1,9 +1,8 @@
 import random
-
 import cv2
-
 import numpy as np
 from PIL import Image
+import math
 
 random.seed(1)
 
@@ -15,12 +14,19 @@ class Pilot:
 class AutoPilot(Pilot):
     prev_image: np.ndarray | None
     angle: float
+    x: float  # Координата X БПЛА
+    y: float  # Координата Y БПЛА
     orb_detector: cv2.ORB
     bf_matcher: cv2.BFMatcher
+    frame_count: int
 
-    def __init__(self):
+    def __init__(self, initial_x: float = 0.0, initial_y: float = 0.0):
         self.prev_image = None
-        self.angle = 0
+        self.angle = 0.0
+        self.x = initial_x
+        self.y = initial_y
+        self.frame_count = 0
+
         # Инициализация ORB детектора
         self.orb_detector = cv2.ORB_create(
             nfeatures=1000,
@@ -32,6 +38,7 @@ class AutoPilot(Pilot):
             patchSize=31,
             fastThreshold=20
         )
+
         # Инициализация матчера для сопоставления ключевых точек
         self.bf_matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
 
@@ -48,7 +55,7 @@ class AutoPilot(Pilot):
         kp2, des2 = self.orb_detector.detectAndCompute(img2, None)
         
         if des1 is None or des2 is None:
-            return None, None, None
+            return None, None, None, None, None
         
         # Сопоставление ключевых точек
         matches = self.bf_matcher.match(des1, des2)
@@ -63,7 +70,7 @@ class AutoPilot(Pilot):
                 good_matches.append(match)
         
         if len(good_matches) < 4:
-            return None, None, None
+            return None, None, None, None, None
         
         # Извлечение координат сопоставленных точек
         src_pts = np.float32([kp1[m.queryIdx].pt for m in good_matches]).reshape(-1, 1, 2)
@@ -109,6 +116,48 @@ class AutoPilot(Pilot):
             'mask': mask
         }
 
+    def update_drone_position(self, transformation_info: dict):
+        """
+        Обновляет позицию и угол БПЛА на основе трансформации изображения
+        """
+        tx, ty = transformation_info['translation']
+        rotation = transformation_info['rotation']
+        scale = transformation_info['scale']
+        
+        # Конвертируем смещение в пикселях в метры
+        dx_meters = tx
+        dy_meters = ty
+        
+        # Применяем поворот к смещению (учитываем текущий угол БПЛА)
+        cos_angle = math.cos(self.angle)
+        sin_angle = math.sin(self.angle)
+        
+        # Поворачиваем смещение в глобальные координаты
+        dx_global = dx_meters * cos_angle - dy_meters * sin_angle
+        dy_global = dx_meters * sin_angle + dy_meters * cos_angle
+        
+        # Обновляем координаты БПЛА
+        self.x += dx_global
+        self.y += dy_global
+        
+        # Обновляем угол БПЛА
+        self.angle += rotation
+        
+        # Нормализуем угол в диапазоне [-π, π]
+        self.angle = math.atan2(math.sin(self.angle), math.cos(self.angle))
+
+    def get_drone_state(self) -> dict:
+        """
+        Возвращает текущее состояние БПЛА
+        """
+        return {
+            'x': self.x,
+            'y': self.y,
+            'angle': self.angle,
+            'angle_degrees': math.degrees(self.angle),
+            'frame_count': self.frame_count
+        }
+
     def visualize_matches(self, img1: np.ndarray, img2: np.ndarray, 
                          kp1, kp2, matches, transformation_info):
         """
@@ -132,9 +181,19 @@ class AutoPilot(Pilot):
             cv2.putText(img_matches, info_text2, (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
             cv2.putText(img_matches, info_text3, (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
         
+        # Добавляем информацию о позиции БПЛА
+        drone_state = self.get_drone_state()
+        pos_text = f"Drone Pos: ({drone_state['x']:.2f}, {drone_state['y']:.2f})"
+        angle_text = f"Drone Angle: {drone_state['angle_degrees']:.1f}°"
+        
+        cv2.putText(img_matches, pos_text, (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
+        cv2.putText(img_matches, angle_text, (10, 150), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
+        
         return img_matches
 
     def handle(self, image: Image):
+        self.frame_count += 1
+        
         if self.prev_image is None:
             self.prev_image = self.image_to_numpy(image)
             return
@@ -144,23 +203,29 @@ class AutoPilot(Pilot):
         
         # Обнаруживаем и сопоставляем ключевые точки
         src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(self.prev_image, current_image)
-        
+
         if src_pts is not None and dst_pts is not None:
             # Оцениваем матрицу трансформации
             transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts)
             
             if transformation_info:
-                print(f"Translation: {transformation_info['translation']}")
-                print(f"Rotation: {transformation_info['rotation']:.4f} rad")
-                print(f"Scale: {transformation_info['scale']:.4f}")
+                print(f"Frame {self.frame_count}:")
+                print(f"  Translation: {transformation_info['translation']}")
+                print(f"  Rotation: {transformation_info['rotation']:.4f} rad")
+                print(f"  Scale: {transformation_info['scale']:.4f}")
                 
-                # Обновляем угол дрона
-                self.angle += transformation_info['rotation']
+                # Обновляем позицию и угол БПЛА
+                self.update_drone_position(transformation_info)
+                
+                # Выводим текущее состояние БПЛА
+                drone_state = self.get_drone_state()
+                print(f"  Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
+                print(f"  Drone Angle: {drone_state['angle_degrees']:.1f}°")
                 
                 # Визуализация (опционально)
                 img_matches = self.visualize_matches(self.prev_image, current_image, 
                                                    kp1, kp2, matches, transformation_info)
-                cv2.imshow('Matches', img_matches)
+                cv2.imshow('Drone Tracking', img_matches)
                 cv2.waitKey(1)
         
         # Обновляем предыдущее изображение
@@ -170,6 +235,13 @@ class AutoPilot(Pilot):
         """Возвращает угол поворота для управления дроном"""
         return self.angle
 
+    def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0):
+        """Сбрасывает позицию и угол БПЛА"""
+        self.x = x
+        self.y = y
+        self.angle = angle
+        self.frame_count = 0
+
 
 class RandomPilot(Pilot):
     def __init__(self, velocity: float = 1):