From 5f7519025cc4ae82e8030812ca770b02bdd349ef Mon Sep 17 00:00:00 2001 From: russian_proger Date: Sun, 31 May 2026 16:41:39 +0300 Subject: [PATCH] feat: support metrics in meters --- main.py | 50 ++++++++++++++++++++++++++++++++++-------------- visualization.py | 42 ++++++++++++++++++++-------------------- 2 files changed, 57 insertions(+), 35 deletions(-) diff --git a/main.py b/main.py index df3286b..8b75727 100644 --- a/main.py +++ b/main.py @@ -99,6 +99,7 @@ def build(name: str, map_name: str, lat: float, lon: float): (p[0] - points[0][0]) * width, (points[0][1] - p[1]) * height ], points))) + points_coords_pixels = points_coords.copy() points_coords *= online_map.pixel_ratio # Начнём симуляцию полёта с первой точки @@ -168,7 +169,11 @@ def build(name: str, map_name: str, lat: float, lon: float): 'flight_pixel_ratio': online_map.pixel_ratio, 'map_extent': make_map_extent(width, height, points[0], map_pixel_ratio), 'route_length': calc_polyline_length(points_coords), + 'route_length_meters': calc_polyline_length(points_coords), + 'route_length_pixels': calc_polyline_length(points_coords_pixels), 'build_flight_length': calc_polyline_length([[pos.x, pos.y] for pos in positions]), + 'build_flight_length_meters': calc_polyline_length([[pos.x, pos.y] for pos in positions]), + 'build_flight_length_pixels': calc_polyline_length([[pos.x / online_map.pixel_ratio, pos.y / online_map.pixel_ratio] for pos in positions]), } print(points_coords) @@ -226,13 +231,15 @@ def run( print("R: ", r) forward_points = data['points'] - planned_route_length = float(data.get('route_length', calc_polyline_length(forward_points))) - build_flight_length = float(data.get('build_flight_length', calc_polyline_length([[pos.x, pos.y] for pos in data.get('chunk_positions', [])]))) + planned_route_length_meters = float(data.get('route_length_meters', data.get('route_length', calc_polyline_length(forward_points)))) + planned_route_length_pixels = float(data.get('route_length_pixels', planned_route_length_meters / data.get('map_pixel_ratio', online_map.pixel_ratio))) + build_flight_length_meters = float(data.get('build_flight_length_meters', data.get('build_flight_length', calc_polyline_length([[pos.x, pos.y] for pos in data.get('chunk_positions', [])])))) + build_flight_length_pixels = float(data.get('build_flight_length_pixels', build_flight_length_meters / online_map.pixel_ratio)) return_points = forward_points[::-1].copy() points = return_points / online_map.pixel_ratio print("READ RTH POINTS:", points) - print("Saved planned route length:", planned_route_length) - print("Saved build flight length:", build_flight_length) + print("Saved planned route length:", planned_route_length_meters, "m /", planned_route_length_pixels, "px") + print("Saved build flight length:", build_flight_length_meters, "m /", build_flight_length_pixels, "px") vis_manager = VisualizationManager() map_path = dir / data.get('map_image', 'map.jpg') @@ -288,7 +295,7 @@ def run( errors = [] start_point = forward_points[0] final_return_error = None - return_route_length = 0.0 + return_route_length_meters = 0.0 prev_return_point = np.array([turn_position.x, turn_position.y], dtype=float) odometry_warmup_frames = 2 @@ -320,7 +327,7 @@ def run( vis_manager.set_target_index(pilot.target_idx) current_return_point = np.array([simulator.pos.x * online_map.pixel_ratio, simulator.pos.y * online_map.pixel_ratio], dtype=float) - return_route_length += float(np.hypot(*(current_return_point - prev_return_point))) + return_route_length_meters += float(np.hypot(*(current_return_point - prev_return_point))) prev_return_point = current_return_point vis_manager.update_drone_trajectory(pilot.pos.x * online_map.pixel_ratio, pilot.pos.y * online_map.pixel_ratio) @@ -346,14 +353,17 @@ def run( final_point = np.array([simulator.pos.x * online_map.pixel_ratio, simulator.pos.y * online_map.pixel_ratio]) final_return_error = float(np.hypot(final_point[0] - start_point[0], final_point[1] - start_point[1])) - full_route_length = build_flight_length + return_route_length + return_route_length_pixels = return_route_length_meters / online_map.pixel_ratio + full_route_length_meters = build_flight_length_meters + return_route_length_meters + full_route_length_pixels = build_flight_length_pixels + return_route_length_pixels + final_return_error_pixels = final_return_error / online_map.pixel_ratio vis_manager.set_final_point(final_point[0], final_point[1], final_return_error) print("Errors:", errors) print("MSE:", (np.array(errors) ** 2).mean()) print("RMSE:", (np.array(errors) ** 2).mean() ** 0.5) - print("Return route length:", return_route_length) - print("Full route length:", full_route_length) - print("Final-to-start error:", final_return_error) + print("Return route length:", return_route_length_meters, "m /", return_route_length_pixels, "px") + print("Full route length:", full_route_length_meters, "m /", full_route_length_pixels, "px") + print("Final-to-start error:", final_return_error, "m /", final_return_error_pixels, "px") print("Average FPS:", 1 / proc_time.mean()) test_run_dir = make_test_run_dir(name) vis_manager.save_plots(test_run_dir) @@ -361,11 +371,23 @@ def run( 'position_errors': errors, 'mse': float((np.array(errors) ** 2).mean()) if errors else None, 'rmse': float((np.array(errors) ** 2).mean() ** 0.5) if errors else None, - 'planned_route_length': planned_route_length, - 'build_flight_length': build_flight_length, - 'return_route_length': return_route_length, - 'full_route_length': full_route_length, + 'planned_route_length': planned_route_length_meters, + 'build_flight_length': build_flight_length_meters, + 'return_route_length': return_route_length_meters, + 'full_route_length': full_route_length_meters, 'final_to_start_error': final_return_error, + 'planned_route_length_meters': planned_route_length_meters, + 'planned_route_length_pixels': planned_route_length_pixels, + 'build_flight_length_meters': build_flight_length_meters, + 'build_flight_length_pixels': build_flight_length_pixels, + 'return_route_length_meters': return_route_length_meters, + 'return_route_length_pixels': return_route_length_pixels, + 'full_route_length_meters': full_route_length_meters, + 'full_route_length_pixels': full_route_length_pixels, + 'final_to_start_error_meters': final_return_error, + 'final_to_start_error_pixels': final_return_error_pixels, + 'map_pixel_ratio': data.get('map_pixel_ratio'), + 'flight_pixel_ratio': online_map.pixel_ratio, 'start_point': start_point, 'turn_point': np.array([turn_position.x, turn_position.y]), 'final_point': final_point, diff --git a/visualization.py b/visualization.py index 9768d2f..b07172c 100644 --- a/visualization.py +++ b/visualization.py @@ -87,17 +87,17 @@ class VisualizationManager: gs = self.fig.add_gridspec(2, 3, hspace=0.3, wspace=0.3, width_ratios=[1, 0.7, 1]) # График погрешности позиции (левый верхний угол) - self.ax_error_plot = self.fig.add_subplot(gs[0, 0]) - self.ax_error_plot.set_title('Погрешность позиции от времени') - self.ax_error_plot.set_xlabel('Время (кадры)') - self.ax_error_plot.set_ylabel('Погрешность (пиксели)') + self.ax_error_plot = self.fig.add_subplot(gs[0, 0]) + self.ax_error_plot.set_title('Погрешность позиции от времени') + self.ax_error_plot.set_xlabel('Время (кадры)') + self.ax_error_plot.set_ylabel('Погрешность (м)') self.ax_error_plot.grid(True, alpha=0.3) # Глобальная карта (левый средний угол) - self.ax_global_map = self.fig.add_subplot(gs[1, 0]) - self.ax_global_map.set_title('Global Map - Траектория полета беспилотника') - self.ax_global_map.set_xlabel('X координата') - self.ax_global_map.set_ylabel('Y координата') + self.ax_global_map = self.fig.add_subplot(gs[1, 0]) + self.ax_global_map.set_title('Global Map - Траектория полета беспилотника') + self.ax_global_map.set_xlabel('X координата (м)') + self.ax_global_map.set_ylabel('Y координата (м)') self.ax_global_map.grid(True, alpha=0.3) self.ax_global_map.axhline(y=0, color='k', linestyle='-', alpha=0.3) self.ax_global_map.axvline(x=0, color='k', linestyle='-', alpha=0.3) @@ -110,8 +110,8 @@ class VisualizationManager: # Сопоставление точек (средний средний угол) self.ax_route_map = self.fig.add_subplot(gs[1, 1]) self.ax_route_map.set_title('RTH Map - маршрут на карте') - self.ax_route_map.set_xlabel('X координата') - self.ax_route_map.set_ylabel('Y координата') + self.ax_route_map.set_xlabel('X координата (м)') + self.ax_route_map.set_ylabel('Y координата (м)') self.ax_route_map.grid(True, alpha=0.3) self.ax_chunk_matches = self.fig.add_subplot(gs[1, 2]) @@ -169,8 +169,8 @@ class VisualizationManager: self.ax_route_map.clear() self.ax_route_map.set_title('RTH Map - маршрут на карте') - self.ax_route_map.set_xlabel('X координата') - self.ax_route_map.set_ylabel('Y координата') + self.ax_route_map.set_xlabel('X координата (м)') + self.ax_route_map.set_ylabel('Y координата (м)') self.ax_route_map.grid(True, alpha=0.3) if self.map_image is not None and self.map_extent is not None: @@ -215,7 +215,7 @@ class VisualizationManager: self.ax_route_map.text( 0.02, 0.98, - f"Ошибка возврата: {self.final_return_error:.2f}", + f"Ошибка возврата: {self.final_return_error:.2f} м", transform=self.ax_route_map.transAxes, va='top', fontsize=8, @@ -241,10 +241,10 @@ class VisualizationManager: self.trajectory_x.append(x) self.trajectory_y.append(y) - self.ax_global_map.clear() - self.ax_global_map.set_title('Global Map - Траектория полета беспилотника') - self.ax_global_map.set_xlabel('X координата') - self.ax_global_map.set_ylabel('Y координата') + self.ax_global_map.clear() + self.ax_global_map.set_title('Global Map - Траектория полета беспилотника') + self.ax_global_map.set_xlabel('X координата (м)') + self.ax_global_map.set_ylabel('Y координата (м)') self.ax_global_map.grid(True, alpha=0.3) self.ax_global_map.axhline(y=0, color='k', linestyle='-', alpha=0.3) self.ax_global_map.axvline(x=0, color='k', linestyle='-', alpha=0.3) @@ -314,10 +314,10 @@ class VisualizationManager: self.error_times.append(frame_count) self.position_errors.append(error) - self.ax_error_plot.clear() - self.ax_error_plot.set_title('Погрешность позиции от времени') - self.ax_error_plot.set_xlabel('Время (кадры)') - self.ax_error_plot.set_ylabel('Погрешность (метры)') + self.ax_error_plot.clear() + self.ax_error_plot.set_title('Погрешность позиции от времени') + self.ax_error_plot.set_xlabel('Время (кадры)') + self.ax_error_plot.set_ylabel('Погрешность (м)') self.ax_error_plot.grid(True, alpha=0.3) if len(self.error_times) > 1: