From 48885d567a750c7534321c63fa8e5d8d7a136a3b Mon Sep 17 00:00:00 2001 From: russian_proger Date: Fri, 19 Dec 2025 23:26:25 +0300 Subject: [PATCH] feat: add zoom --- autopilot.py | 29 +++++++++++++++++++---------- main.py | 9 +++++++++ simulator.py | 18 ++++++++++++++---- 3 files changed, 42 insertions(+), 14 deletions(-) diff --git a/autopilot.py b/autopilot.py index 2494e8b..cc9f260 100644 --- a/autopilot.py +++ b/autopilot.py @@ -42,6 +42,7 @@ class AutoPilot(Pilot): angle: float x: float # Координата X БПЛА y: float # Координата Y БПЛА + zoom: float # Коэффициент увеличения # Ориентиры points: list[tuple[float,float]] # Координаты @@ -65,6 +66,7 @@ class AutoPilot(Pilot): self.angle = 0.0 self.x = 0.0 self.y = 0.0 + self.zoom = 1.0 self.frame_count = 0 self.image_center = (0, 0) # Будет обновлено при получении первого изображения self.vis_manager = viz_manager # Менеджер визуализации @@ -238,7 +240,8 @@ class AutoPilot(Pilot): mat: np.ndarray, x_source: float, y_source: float, - angle_source: float + angle_source: float, + zoom_source: float ) -> tuple[float, float, float] | None: """ Возвращает новые координаты и поворот на основе матрицы преобразования """ @@ -247,6 +250,8 @@ class AutoPilot(Pilot): # Вычисляем угол поворота rotation = -np.arctan2(mat[1, 0], mat[0, 0]) + scale = np.hypot(mat[0, 0], mat[0, 1]) + print("Scale: ", scale) # print("[HOMOGRAPHY]", mat) # print("[ROTATION]", rotation) @@ -266,13 +271,16 @@ class AutoPilot(Pilot): dy_global = dx_meters * sin_angle + dy_meters * cos_angle # Обновляем координаты БПЛА - x = x_source + dx_global - y = y_source + dy_global + print("[Matrix Transformation]: ") + print(mat) + zoom = zoom_source / scale + x = x_source + dx_global * zoom + y = y_source + dy_global * zoom # Нормализуем угол в диапазоне [-π, π] angle = math.atan2(math.sin(angle_global), math.cos(angle_global)) - return x, y, angle + return x, y, angle, zoom def update_drone_position(self, transformation_info: dict): """ @@ -281,9 +289,9 @@ class AutoPilot(Pilot): """ homography = transformation_info['homography'] - self.x, self.y, self.angle = self.calc_position( + self.x, self.y, self.angle, self.zoom = self.calc_position( homography, - self.x, self.y, self.angle + self.x, self.y, self.angle, self.zoom ) if self.reserved_pos is not None: @@ -305,7 +313,7 @@ class AutoPilot(Pilot): - def get_position_by_chunk(self) -> tuple[float, float, float] | None: + def get_position_by_chunk(self) -> tuple[float, float, float, float] | None: # Пытаемся найти ориентир на картинке: current_image = self.prev_image landmark_image = cv2.imread(Path('chunks') / f'chunk_{self.target_idx}.png', cv2.IMREAD_COLOR_RGB) @@ -338,10 +346,11 @@ class AutoPilot(Pilot): # Считаем абсолютную позу относительно координат ориентира landmark_world_x, landmark_world_y = self.points[self.target_idx] landmark_angle = 0 + landmark_zoom = 1 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})") - return self.calc_position(homography, landmark_world_x, landmark_world_y, landmark_angle) + print(f" [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})") + return self.calc_position(homography, landmark_world_x, landmark_world_y, landmark_angle, landmark_zoom) return None @@ -437,7 +446,7 @@ class AutoPilot(Pilot): ) if self.reserved_pos is not None: - self.x, self.y, self.angle = self.reserved_pos + self.x, self.y, self.angle, self.zoom = self.reserved_pos self.reserved_pos = None # Вычисляем угол к цели diff --git a/main.py b/main.py index a2f8829..9662616 100644 --- a/main.py +++ b/main.py @@ -4,6 +4,7 @@ from trajectory_drawer import TrajectoryDrawer from yandex_map import YandexMap from time import sleep from pathlib import Path +import random import cv2 import matplotlib.pyplot as plt @@ -106,8 +107,16 @@ def main(): proc_time = np.array([]) + zoom_next_event = random.randint(5, 10) + for i in range(10000000000): print(f"Image #{i}") + if i == zoom_next_event: + r = random.randint(0, 1) + direction = ['up', 'down'][r] + simulator.change_zoom(direction) + zoom_next_event = i + random.randint(20, 40) + photo = simulator.get_photo() command = pilot.handle(photo) diff --git a/simulator.py b/simulator.py index 2c5653e..b802267 100644 --- a/simulator.py +++ b/simulator.py @@ -31,6 +31,7 @@ class Simulator: self.angle = 0.0 self.current_x = 0.0 self.current_y = 0.0 + self.zoom = 1 # Создаем папку для изображений, если её нет os.makedirs('./images', exist_ok=True) @@ -68,8 +69,8 @@ class Simulator: Обновляет траекторию полета беспилотника """ # Обновляем текущие координаты - self.current_x += dx - self.current_y += dy + self.current_x += dx * self.zoom + self.current_y += dy * self.zoom def update_map(self): """ @@ -88,14 +89,23 @@ class Simulator: self.angle += dangle # print(f" [Simulator] angle: {self.angle / math.pi * 180:.1f}°") velocity = max(velocity, 10) - dx = math.cos(math.pi / 2 + self.angle) * velocity - dy = math.sin(math.pi / 2 + self.angle) * velocity + dx = math.cos(math.pi / 2 + self.angle) * velocity / self.zoom + dy = math.sin(math.pi / 2 + self.angle) * velocity / self.zoom # print(" [Simulator] dx, dy:", [dx, dy]) self.update_trajectory(dx, dy) action.move_by_offset(-dx, dy) action.release() action.perform() # print(f" [Simulator] Position: {self.current_x}, {self.current_y}") + + def change_zoom(self, direction: str = 'down'): + """ Изменение масштаба """ + self.yandexMap.scroll(0.5, 0.5, 2, direction == 'down') + sleep(0.4) + if direction == 'down': + self.zoom /= 2 + else: + self.zoom *= 2 def get_photo(self) -> Image.Image: png = self.yandexMap.driver.get_screenshot_as_png()