feat: add perspective transform

autopilot.py

@@ -11,7 +11,9 @@ from timer import Timer
 
 from vision_chunk import VisionChunk
 from visualization import VisualizationManager
-from geolocation import Geolocation
+from position import Position
+from constants import CAMERA_HEIGHT
+from utility import estimate_transformation_matrix
 
 random.seed(1)
 
@@ -41,7 +43,7 @@ class PilotCommand:
 class AutoPilot(Pilot):
 
     # Состояние одометрии
-    geo: Geolocation
+    pos: Position
 
     chunks: list[VisionChunk] # Ориентиры
     target_idx: int           # Текущий ориентир
@@ -51,17 +53,17 @@ class AutoPilot(Pilot):
     prev_chunk: VisionChunk | None
     frame_count: int
     vis_manager: VisualizationManager  # Менеджер визуализации (опционально)
-  
+
     # Положение на основе ориентира
-    reserved_geo: Geolocation | None
+    reserved_pos: Position | None
 
     def __init__(self, points = [], chunks = [], viz_manager=None):
         self.prev_chunk = None
-        self.geo = Geolocation(0, 0, 1, 0)
+        self.pos = Position(0, 0, 1, 0, 0, 0)
         self.chunks = chunks
         self.frame_count = 0
         self.vis_manager = viz_manager  # Менеджер визуализации
-        self.reserved_geo = None
+        self.reserved_pos = None
 
         # Пороговые значения качества сопоставления/гомографии
         self.min_inliers: int = 12
@@ -76,7 +78,7 @@ class AutoPilot(Pilot):
         self.timer = Timer()
 
     def get_position(self) -> tuple[float, float]:
-        return self.geo.x, self.geo.y
+        return self.pos.x, self.pos.y
 
     def calculate_optical_flow(self, prev_chunk: VisionChunk, current_chunk: VisionChunk):
         """
@@ -87,7 +89,6 @@ class AutoPilot(Pilot):
         current_gray = current_chunk.to_cv2_gray()
         
         h, w = prev_gray.shape[:2]
-        cx, cy = w // 2, h // 2
         
         # Создаем сетку точек для отслеживания (аналогично вашему step=20)
         step = 35
@@ -120,99 +121,34 @@ class AutoPilot(Pilot):
             return None, None
 
         # Преобразуем в отцентрированные координаты (Y вверх)
-        src_pts = np.float32([[x - cx, cy - y] for x, y in good_old]).reshape(-1, 1, 2)
+        src_pts = np.float32([[x, y] for x, y in good_old]).reshape(-1, 1, 2)
-        dst_pts = np.float32([[x - cx, cy - y] for x, y in good_new]).reshape(-1, 1, 2)
+        dst_pts = np.float32([[x, y] for x, y in good_new]).reshape(-1, 1, 2)
 
         return src_pts, dst_pts
 
-    def estimate_transformation_matrix(self, src_pts: np.ndarray, dst_pts: np.ndarray):
+    def update_drone_position(self, homography_matrix: np.ndarray):
-        """
-        Оценивает матрицу трансформации на основе сопоставленных точек
-        Точки уже отцентрированы относительно центра изображения
-        """
-        # Используем RANSAC для оценки матрицы гомографии
-        print("Count of points: ", len(src_pts), len(dst_pts))
-        H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 3.0, maxIters=1000)
-        # RH, rmask = cv2.findHomography(dst_pts, src_pts, cv2.RANSAC, 1.0)
-        
-        if H is None:
-            return None
-        
-        # Маска инлайеров
-        inliers = int(mask.sum()) if mask is not None else 0
-        total = int(mask.size) if mask is not None else 0
-        inlier_ratio = (inliers / total) if total > 0 else 0.0
-        
-        # Ошибка репроекции по инлайерам (RMSE)
-        rmse = None
-        try:
-            # Проецируем src через H и сравниваем с dst
-            proj = cv2.perspectiveTransform(src_pts, H)
-            errs = np.linalg.norm((proj - dst_pts).reshape(-1, 2), axis=1)
-            if mask is not None and mask.shape[0] == errs.shape[0]:
-                errs = errs[mask.ravel().astype(bool)]
-            if errs.size > 0:
-                rmse = float(np.sqrt(np.mean(errs ** 2)))
-        except Exception:
-            rmse = None
-
-        # Извлекаем параметры трансформации из матрицы гомографии
-        # H = [a11 a12 tx]
-        #     [a21 a22 ty]
-        #     [0   0   1 ]
-        
-        # Масштаб и поворот
-        a11, a12 = H[0, 0], H[0, 1]
-        a21, a22 = H[1, 0], H[1, 1]
-        
-        # Смещение (уже отцентрировано)
-        tx, ty = -H[0, 2], -H[1, 2]
-        
-        # print("  [Pilot] translate:", tx, ty)
-        
-        # Вычисляем угол поворота
-        angle = -np.arctan2(a21, a11)
-        
-        # Вычисляем масштаб
-        scale_x = np.sqrt(a11**2 + a21**2)
-        scale_y = np.sqrt(a12**2 + a22**2)
-        scale = (scale_x + scale_y) / 2
-        
-        return {
-            'translation': (tx, ty),  # Уже отцентрировано
-            'rotation': angle,
-            'scale': scale,
-            'homography': H,
-            'mask': mask,
-            'inliers': inliers,
-            'inliers_ratio': inlier_ratio,
-            'rmse': rmse
-        }
-
-    def update_drone_position(self, transformation_info: dict):
         """
         Обновляет позицию и угол БПЛА на основе трансформации изображения
         Координаты уже отцентрированы относительно центра изображения
         """
-        homography = transformation_info['homography']
+        self.pos.iapply(homography_matrix)
-        self.geo @= homography
 
-        if self.reserved_geo is not None:
+        if self.reserved_pos is not None:
-            self.reserved_geo @= homography
+            self.reserved_pos.iapply(homography_matrix)
 
     def get_drone_state(self) -> dict:
         """
         Возвращает текущее состояние БПЛА
         """
         return {
-            'x': self.geo.x,
+            'x': self.pos.x,
-            'y': self.geo.y,
+            'y': self.pos.y,
-            'angle': self.geo.yaw,
+            'angle': self.pos.yaw,
-            'angle_degrees': math.degrees(self.geo.yaw),
+            'angle_degrees': math.degrees(self.pos.yaw),
             'frame_count': self.frame_count
         }
 
-    def get_position_by_chunk(self) -> Geolocation | None:
+    def get_position_by_chunk(self) -> Position | None:
         # Пытаемся найти ориентир на картинке:
         current_chunk = self.prev_chunk
         landmark_chunk = self.chunks[self.target_idx]
@@ -244,7 +180,7 @@ class AutoPilot(Pilot):
                 # if False:
                     # Считаем абсолютную позу относительно координат ориентира
                     landmark_world_x, landmark_world_y = self.points[self.target_idx]
-                    landmark = Geolocation(landmark_world_x, landmark_world_y, 1, 0)
+                    landmark = Position(landmark_world_x, landmark_world_y, 1, 0)
                     homography = landmark_transform['homography']
                     # homography = np.linalg.inv(homography)
                     print(f"  [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})")
@@ -260,55 +196,53 @@ class AutoPilot(Pilot):
             self.prev_chunk = current_chunk
             self.timer.stop()
             return PilotCommand(processing_time=self.timer.get_elapsed())
-        
+
         # Расстояние до цели
         distance_to_target = math.sqrt(
-            (self.points[self.target_idx][0] - self.geo.x) ** 2 +
+            (self.points[self.target_idx][0] - self.pos.x) ** 2 +
-            (self.points[self.target_idx][1] - self.geo.y) ** 2
+            (self.points[self.target_idx][1] - self.pos.y) ** 2
         )
-        
+
         # Вычисляем оптический поток для покадрового сравнения
-        optical_flow_timer = Timer()
+        matching_timer = Timer()
-        optical_flow_timer.start()
+        matching_timer.start()
-        src_pts, dst_pts = self.calculate_optical_flow(self.prev_chunk, current_chunk)
+        # src_pts, dst_pts = self.calculate_optical_flow(self.prev_chunk, current_chunk)
-        optical_flow_timer.stop()
+        src_pts, dst_pts, _, _, _ = self.prev_chunk.detect_and_match_keypoints(current_chunk)
-        print(f"Optical flow calculating: {optical_flow_timer.get_elapsed() * 1000:.2f} ms")
+        matching_timer.stop()
+        print(f"Matching calculating: {matching_timer.get_elapsed() * 1000:.2f} ms")
 
         # Оцениваем смещение БПЛА
         if src_pts is not None and dst_pts is not None:
             # Оцениваем матрицу трансформации
-            
+
             matrix_estimation_timer = Timer()
             matrix_estimation_timer.start()
-            transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts)
+            homography_matrix = estimate_transformation_matrix(src_pts, dst_pts)
             matrix_estimation_timer.stop()
-            print(f"Transformation matrix estimating: {matrix_estimation_timer.get_elapsed() * 1000:.2f} ms")
+            print(f"Transformation matrix updating: {matrix_estimation_timer.get_elapsed() * 1000:.2f} ms")
-            
+
-            if transformation_info:
+            if homography_matrix is not None:
-                # Обновляем позицию и угол БПЛА (одометрия по кадрам)
+                self.update_drone_position(homography_matrix)
-                self.update_drone_position(transformation_info)
-                
                 self.timer.stop()
-                
+
                 # Обновляем визуализацию
                 if self.vis_manager:
                     # Используем визуализацию движения точек по сетке
-                    homography_matrix = transformation_info['homography']
                     self.vis_manager.update_homography_grid(
-                        current_chunk.to_numpy(), 
+                        current_chunk.to_numpy(),
-                        homography_matrix, 
+                        homography_matrix,
-                        grid_step=85)
+                        grid_step=35)
 
                 self.timer.start()
 
-        
         chunk_timer = Timer()
         chunk_timer.start()
+
         # Пытаемся найти ориентир на картинке:
         self.prev_chunk = current_chunk
         # npos = self.get_position_by_chunk()
         # if npos is not None:
-        #     self.reserved_geo = npos
+        #     self.reserved_pos = npos
 
         chunk_timer.stop()
         print(f"Chunk timer: {chunk_timer.get_elapsed() * 1000:.2f} ms")
@@ -320,18 +254,18 @@ class AutoPilot(Pilot):
                 return PilotCommand(stop=True)
 
             distance_to_target = math.sqrt(
-                (self.points[self.target_idx][0] - self.geo.x) ** 2 +
+                (self.points[self.target_idx][0] - self.pos.x) ** 2 +
-                (self.points[self.target_idx][1] - self.geo.y) ** 2
+                (self.points[self.target_idx][1] - self.pos.y) ** 2
             )
 
-            if self.reserved_geo is not None:
+            if self.reserved_pos is not None:
-                self.geo = self.reserved_geo
+                self.pos = self.reserved_pos
-                self.reserved_geo = None
+                self.reserved_pos = None
 
         # Вычисляем угол к цели
-        target_angle = math.atan2(self.points[self.target_idx][1] - self.geo.y, self.points[self.target_idx][0] - self.geo.x)
+        target_angle = math.atan2(self.points[self.target_idx][1] - self.pos.y, self.points[self.target_idx][0] - self.pos.x)
         
-        angle_trajectory = self.geo.yaw + math.pi / 2
+        angle_trajectory = self.pos.yaw + math.pi / 2
 
         # print("[ANGLE]", angle_trajectory, "->", target_angle)
 
@@ -355,4 +289,4 @@ class AutoPilot(Pilot):
 
     def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0):
         """Сбрасывает позицию и угол БПЛА"""
-        self.geo = Geolocation(x, y, 1, angle)
+        self.pos = Position(x, y, 1, angle)

main.py

@@ -142,9 +142,19 @@ def main():
         if i == zoom_next_event:
             r = random.randint(0, 1)
             direction = ['up', 'down'][r]
-            simulator.change_zoom(direction)
+            # simulator.change_zoom(direction)
             zoom_next_event = i + random.randint(20, 40)
 
+
+        # if i > 0:
+        #     simulator.set_pitch(np.sin(i / 10) * 5)
+        #     simulator.set_roll(np.sin(i / 15) * 5)
+        #     simulator.set_zoom(1.0 + np.sin(i / 10) * 0.3)
+
+        if command.stop:
+            break
+
+        # simulator.handle(command.dangle, command.velocity)
         chunk = simulator.get_chunk()
         command = pilot.handle(chunk)
 
@@ -157,26 +167,24 @@ def main():
         vis_manager.pause(0.2)
 
         vis_manager.set_target_index(pilot.target_idx)
-        vis_manager.update_drone_trajectory(pilot.geo.x, pilot.geo.y)
+        vis_manager.update_drone_trajectory(pilot.pos.x, pilot.pos.y)
-        vis_manager.update_global_map(simulator.geo.x, simulator.geo.y)
+        vis_manager.update_global_map(simulator.pos.x, simulator.pos.y)
-        vis_manager.update_error_plot(i, pilot.geo.x, pilot.geo.y, simulator.geo.x, simulator.geo.y)
+        vis_manager.update_error_plot(i, pilot.pos.x, pilot.pos.y, simulator.pos.x, simulator.pos.y)
 
-        errors.append(np.hypot(pilot.geo.x - simulator.geo.x, pilot.geo.y - simulator.geo.y))
+        errors.append(np.hypot(pilot.pos.x - simulator.pos.x, pilot.pos.y - simulator.pos.y))
         if last_chunk_index != pilot.target_idx:
             last_chunk_index = pilot.target_idx
             chunk_errors.append(errors[-1])
             chunk_improves.append(errors[-1] - errors[max(len(errors) - 2, 0)])
 
-        if command.stop:
-            break
-
-        simulator.handle(command.dangle, command.velocity)
-
         vis_manager.update_display()
         vis_manager.pause(0.2)
 
         last_proc_times = proc_time[-10:]
         print("Average FPS:", 1 / last_proc_times.mean())
+        print("Pilot coords:", pilot.pos)
+        print("Simulator coords:", simulator.pos)
+        simulator.handle(command.dangle, command.velocity)
 
     print("Errors:", errors)
     print("MSE:", (np.array(errors) ** 2).mean())

simulator.py

@@ -5,128 +5,55 @@ from time import sleep
 from PIL import Image
 import cv2
 import numpy as np
-from geolocation import Geolocation
+from position import Position
 from vision_chunk import VisionChunk
 from yandex_map import YandexMap
-
+import constants
+import utility
 
 class Simulator:
     def __init__(self, yandex_map: YandexMap = None):
         self.yandex_map = yandex_map
-        self.geo = Geolocation(0, 0, 1, 0)
+        # Используем новый конструктор с yaw, pitch, roll
-        
+        self.pos = Position(x=0, y=0, z=1, yaw=0, pitch=0, roll=0)
-        # Параметры камеры (в градусах)
+
-        self.pitch = 0.0  # тангаж (-10 до 10)
-        self.roll = 0.0   # крен (-10 до 10)
-        
         os.makedirs('./images', exist_ok=True)
-    
+
-    def set_pitch(self, pitch: float):
+    def set_pitch(self, pitch_deg: float):
         """Установить тангаж камеры (градусы, -10 до 10)"""
-        self.pitch = max(-10, min(10, pitch))
+        clamped_pitch = max(-10, min(10, pitch_deg))
-    
+        self.pos.pitch = math.radians(clamped_pitch)
-    def set_roll(self, roll: float):
+
+    def set_roll(self, roll_deg: float):
         """Установить крен камеры (градусы, -10 до 10)"""
-        self.roll = max(-10, min(10, roll))
+        clamped_roll = max(-10, min(10, roll_deg))
-    
+        self.pos.roll = math.radians(clamped_roll)
-    def _calculate_camera_angles(self, velocity: float, dangle: float):
+
-        """Автоматический расчёт тангажа и крена на основе скорости и поворота"""
-        # Тангаж: чем больше скорость, тем больше тангаж (до 10°)
-        self.pitch = min(10, velocity / 10)
-        
-        # Крен: чем больше угол поворота, тем больше крен
-        self.roll = max(-10, min(10, math.degrees(dangle) * 2))
-    
-    def _get_perspective_points(self, image_width: int, image_height: int, 
-                                yaw_deg: float) -> tuple:
-        """
-        Вычисляет 4 точки для перспективной трансформации.
-        Учитывает тангаж, крен, поворот и масштаб.
-        """
-        center_x = image_width / 2
-        center_y = image_height / 2
-        
-        # Базовый размер области захвата (квадрат)
-        base_size = min(image_width, image_height) * 0.7
-        half_size = base_size / 2
-        
-        # Исходные углы квадрата (до применения перспективы)
-        corners = np.float32([
-            [center_x - half_size, center_y - half_size],  # top-left
-            [center_x + half_size, center_y - half_size],  # top-right
-            [center_x + half_size, center_y + half_size],  # bottom-right
-            [center_x - half_size, center_y + half_size],  # bottom-left
-        ])
-        
-        # Применяем смещения для имитации тангажа (pitch)
-        # Положительный тангаж - дрон наклонён вперёд, камера смотрит дальше
-        pitch_offset = self.pitch * 3  # коэффициент для видимого эффекта
-        corners[0][1] -= pitch_offset  # top-left y
-        corners[1][1] -= pitch_offset  # top-right y
-        corners[2][1] += pitch_offset  # bottom-right y
-        corners[3][1] += pitch_offset  # bottom-left y
-        
-        # Применяем смещения для имитации крена (roll)
-        # Положительный крен - дрон наклонён вправо
-        roll_offset = self.roll * 3
-        corners[0][0] += roll_offset  # top-left x
-        corners[1][0] -= roll_offset  # top-right x
-        corners[2][0] -= roll_offset  # bottom-right x
-        corners[3][0] += roll_offset  # bottom-left x
-        
-        # Поворот вокруг центра (yaw)
-        angle_rad = math.radians(yaw_deg)
-        cos_a = math.cos(angle_rad)
-        sin_a = math.sin(angle_rad)
-        
-        rotated_corners = []
-        for corner in corners:
-            # Смещаем к началу координат
-            x = corner[0] - center_x
-            y = corner[1] - center_y
-            
-            # Поворачиваем
-            new_x = x * cos_a - y * sin_a
-            new_y = x * sin_a + y * cos_a
-            
-            # Возвращаем обратно
-            rotated_corners.append([new_x + center_x, new_y + center_y])
-        
-        return np.float32(rotated_corners)
-    
     def _apply_perspective_transform(self, image: Image.Image) -> Image.Image:
         """
         Применяет перспективную трансформацию к изображению.
         Возвращает квадратное изображение 700x700.
         """
         img_array = np.array(image)
-        h, w = img_array.shape[:2]
+        print(img_array.shape)
-        
+        h, w, _ = img_array.shape
-        # Получаем исходные точки с учётом перспективы
+
-        yaw_deg = -math.degrees(self.geo.yaw)
-        src_points = self._get_perspective_points(w, h, yaw_deg)
-        
-        # Целевые точки - квадрат 700x700
-        dst_points = np.float32([
-            [0, 0],
-            [700, 0],
-            [700, 700],
-            [0, 700]
-        ])
-        
-        # Вычисляем матрицу трансформации
-        matrix = cv2.getPerspectiveTransform(src_points, dst_points)
-        
         # Применяем трансформацию
-        transformed = cv2.warpPerspective(img_array, matrix, (700, 700))
+        pos = self.pos.copy()
-        
+        pos.x = 0
+        pos.y = 0
+        K = utility.calc_camera_matrix(w, h)
+        K = constants.K
+        img_array = img_array[:constants.CHUNK_WIDTH, :constants.CHUNK_WIDTH]
+        transformed = cv2.warpPerspective(img_array, pos.get_homography_matrix(K), (constants.CHUNK_WIDTH, constants.CHUNK_WIDTH))
+
         return Image.fromarray(transformed)
-    
+
     def update_trajectory(self, dx: float, dy: float):
         """Обновляет координаты дрона"""
-        self.geo.x += dx * self.geo.z
+        self.pos.x += dx * self.pos.z
-        self.geo.y += dy * self.geo.z
+        self.pos.y += dy * self.pos.z
-    
+
     def handle(self, dangle: float, velocity: float = 50) -> None:
         """
         Управление движением дрона.
@@ -135,37 +62,46 @@ class Simulator:
         """
         from selenium.webdriver.common.by import By
         from selenium.webdriver.common.action_chains import ActionChains
-        
+
-        # Автоматический расчёт углов камеры
-        self._calculate_camera_angles(velocity, dangle)
-        
         html = self.yandex_map.driver.find_element(By.TAG_NAME, 'html')
         action = ActionChains(self.yandex_map.driver)
         action.move_to_element_with_offset(html, 200, 200)
         action.click_and_hold()
-        
+
-        self.geo.yaw += dangle
+        # Обновляем yaw в объекте Position
+        self.pos.yaw += dangle
         velocity = max(velocity, 10)
-        
+
-        dx = math.cos(math.pi / 2 + self.geo.yaw) * velocity / self.geo.z
+        # Вычисляем смещение на основе текущего yaw
-        dy = math.sin(math.pi / 2 + self.geo.yaw) * velocity / self.geo.z
+        dx = math.cos(math.pi / 2 + self.pos.yaw) * velocity / self.pos.z
-        
+        dy = math.sin(math.pi / 2 + self.pos.yaw) * velocity / self.pos.z
+
         self.update_trajectory(dx, dy)
-        
+
         action.move_by_offset(-dx, dy)
         action.release()
         action.perform()
-    
+
     def set_zoom(self, zoom_level: float):
         """Программное изменение масштаба"""
-        self.geo.z = zoom_level
+        self.pos.z = zoom_level
-    
+
     def get_chunk(self) -> VisionChunk:
         """Получить текущий снимок с камеры дрона"""
         png = self.yandex_map.driver.get_screenshot_as_png()
         im = Image.open(BytesIO(png))
-        
+
         # Применяем перспективную трансформацию
         transformed_im = self._apply_perspective_transform(im)
-        
+
         return VisionChunk(transformed_im)
+
+    def get_orientation_info(self) -> dict:
+        """Получить полную информацию об ориентации дрона"""
+        return {
+            'position': (self.pos.x, self.pos.y, self.pos.z),
+            'yaw_deg': math.degrees(self.pos.yaw),
+            'pitch_deg': math.degrees(self.pos.pitch),
+            'roll_deg': math.degrees(self.pos.roll),
+            'rotation_matrix': self.pos.get_rotation_matrix()
+        }

utility.py

@@ -1,6 +1,7 @@
 from PIL import Image
 import cv2
 import numpy as np
+import constants
 
 def cv2_to_pil(cv_image: np.ndarray) -> Image.Image:
     """
@@ -12,4 +13,22 @@ def cv2_to_pil(cv_image: np.ndarray) -> Image.Image:
 
 def image_to_numpy(self, image: Image.Image) -> np.ndarray:
     """Конвертирует PIL Image в numpy array для OpenCV"""
-    return np.array(image)
+    return np.array(image)
+
+def get_normals(H: np.ndarray, R: np.ndarray, T: np.ndarray) -> np.ndarray:
+    n = cv2.decomposeHomographyMat(H, constants.K, R, T)
+    return n
+
+def estimate_transformation_matrix(src_pts: np.ndarray, dst_pts: np.ndarray) -> tuple[np.ndarray, float | None]:
+    """Оценивает матрицу трансформации на основе сопоставленных точек"""
+    # Используем RANSAC для оценки матрицы гомографии
+    H, _ = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 3.0, maxIters=1000)
+    return H
+
+def calc_camera_matrix(w: float, h: float):
+    f = constants._K_FOCUS_DISTANCE
+    return np.array([
+        [f, 0, w / 2],
+        [0, f, h / 2],
+        [0, 0, 1]
+    ])

vision_chunk.py

@@ -161,10 +161,10 @@ class VisionChunk:
         
         for match in good_matches:
             pt1 = kp1[match.queryIdx].pt
-            src_pts.append([pt1[0] - cx1, cy1 - pt1[1]])  # Y вверх!
+            src_pts.append([pt1[0], pt1[1]])
             
             pt2 = kp2[match.trainIdx].pt
-            dst_pts.append([pt2[0] - cx2, cy2 - pt2[1]])
+            dst_pts.append([pt2[0], pt2[1]])
 
         src_pts = np.float32(src_pts).reshape(-1, 1, 2)
         dst_pts = np.float32(dst_pts).reshape(-1, 1, 2)

visualization.py

@@ -407,7 +407,6 @@ class VisualizationManager:
         
         # Получаем размеры изображения и центр
         height, width = current_frame.shape[:2]
-        center_x, center_y = width // 2, height // 2
         
         # Создаем сетку точек с заданным шагом
         grid_points = []
@@ -424,8 +423,8 @@ class VisualizationManager:
         grid_points_centered = []
         for pt in grid_points:
             # Отцентрируем координаты точно так же, как в detect_and_match_keypoints
-            centered_x = pt[0] - center_x
+            centered_x = pt[0]
-            centered_y = center_y - pt[1]  # Инвертируем Y (изображение Y направлен вниз)
+            centered_y = pt[1]
             grid_points_centered.append([centered_x, centered_y])
         
         grid_points_centered = np.array(grid_points_centered, dtype=np.float32)
@@ -442,8 +441,8 @@ class VisualizationManager:
         transformed_points = []
         for pt in transformed_points_centered:
             # Обратное преобразование от центрированных координат к координатам изображения
-            img_x = pt[0] + center_x
+            img_x = pt[0]
-            img_y = center_y - pt[1]  # Инвертируем Y обратно
+            img_y = pt[1]  # Инвертируем Y обратно
             transformed_points.append([img_x, img_y])
         
         transformed_points = np.array(transformed_points, dtype=np.float32)