zenloger/autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: support metrics in meters

Browse Source
This commit is contained in:
russian_proger
2026-05-31 16:41:39 +03:00
parent a091b89466
commit 5f7519025c
2 changed files with 57 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
main.py
View File

@@ -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 (p[0] - points[0][0]) * width, (points[0][1] - p[1]) * height
], points))) ], points)))
points_coords_pixels = points_coords.copy()
points_coords *= online_map.pixel_ratio 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, 'flight_pixel_ratio': online_map.pixel_ratio,
'map_extent': make_map_extent(width, height, points[0], map_pixel_ratio), 'map_extent': make_map_extent(width, height, points[0], map_pixel_ratio),
'route_length': calc_polyline_length(points_coords), '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': 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) print(points_coords)
@@ -226,13 +231,15 @@ def run(
print("R: ", r) print("R: ", r)
forward_points = data['points'] forward_points = data['points']
planned_route_length = float(data.get('route_length', calc_polyline_length(forward_points))) planned_route_length_meters = float(data.get('route_length_meters', 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_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() return_points = forward_points[::-1].copy()
points = return_points / online_map.pixel_ratio points = return_points / online_map.pixel_ratio
print("READ RTH POINTS:", points) print("READ RTH POINTS:", points)
print("Saved planned route length:", planned_route_length) print("Saved planned route length:", planned_route_length_meters, "m /", planned_route_length_pixels, "px")
print("Saved build flight length:", build_flight_length) print("Saved build flight length:", build_flight_length_meters, "m /", build_flight_length_pixels, "px")
vis_manager = VisualizationManager() vis_manager = VisualizationManager()
map_path = dir / data.get('map_image', 'map.jpg') map_path = dir / data.get('map_image', 'map.jpg')
@@ -288,7 +295,7 @@ def run(
errors = [] errors = []
start_point = forward_points[0] start_point = forward_points[0]
final_return_error = None 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) prev_return_point = np.array([turn_position.x, turn_position.y], dtype=float)
odometry_warmup_frames = 2 odometry_warmup_frames = 2
@@ -320,7 +327,7 @@ def run(
vis_manager.set_target_index(pilot.target_idx) 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) 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 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) 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_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])) 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) vis_manager.set_final_point(final_point[0], final_point[1], final_return_error)
print("Errors:", errors) print("Errors:", errors)
print("MSE:", (np.array(errors) ** 2).mean()) print("MSE:", (np.array(errors) ** 2).mean())
print("RMSE:", (np.array(errors) ** 2).mean() ** 0.5) print("RMSE:", (np.array(errors) ** 2).mean() ** 0.5)
print("Return route length:", return_route_length) print("Return route length:", return_route_length_meters, "m /", return_route_length_pixels, "px")
print("Full route length:", full_route_length) print("Full route length:", full_route_length_meters, "m /", full_route_length_pixels, "px")
print("Final-to-start error:", final_return_error) print("Final-to-start error:", final_return_error, "m /", final_return_error_pixels, "px")
print("Average FPS:", 1 / proc_time.mean()) print("Average FPS:", 1 / proc_time.mean())
test_run_dir = make_test_run_dir(name) test_run_dir = make_test_run_dir(name)
vis_manager.save_plots(test_run_dir) vis_manager.save_plots(test_run_dir)
@@ -361,11 +371,23 @@ def run(
'position_errors': errors, 'position_errors': errors,
'mse': float((np.array(errors) ** 2).mean()) if errors else None, 'mse': float((np.array(errors) ** 2).mean()) if errors else None,
'rmse': float((np.array(errors) ** 2).mean() ** 0.5) if errors else None, 'rmse': float((np.array(errors) ** 2).mean() ** 0.5) if errors else None,
'planned_route_length': planned_route_length, 'planned_route_length': planned_route_length_meters,
'build_flight_length': build_flight_length, 'build_flight_length': build_flight_length_meters,
'return_route_length': return_route_length, 'return_route_length': return_route_length_meters,
'full_route_length': full_route_length, 'full_route_length': full_route_length_meters,
'final_to_start_error': final_return_error, '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, 'start_point': start_point,
'turn_point': np.array([turn_position.x, turn_position.y]), 'turn_point': np.array([turn_position.x, turn_position.y]),
'final_point': final_point, 'final_point': final_point,

42
visualization.py
View File

@@ -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]) 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 = self.fig.add_subplot(gs[0, 0])
self.ax_error_plot.set_title('Погрешность позиции от времени') self.ax_error_plot.set_title('Погрешность позиции от времени')
self.ax_error_plot.set_xlabel('Время (кадры)') self.ax_error_plot.set_xlabel('Время (кадры)')
self.ax_error_plot.set_ylabel('Погрешность (пиксели)') self.ax_error_plot.set_ylabel('Погрешность (м)')
self.ax_error_plot.grid(True, alpha=0.3) self.ax_error_plot.grid(True, alpha=0.3)
# Глобальная карта (левый средний угол) # Глобальная карта (левый средний угол)
self.ax_global_map = self.fig.add_subplot(gs[1, 0]) self.ax_global_map = self.fig.add_subplot(gs[1, 0])
self.ax_global_map.set_title('Global Map - Траектория полета беспилотника') self.ax_global_map.set_title('Global Map - Траектория полета беспилотника')
self.ax_global_map.set_xlabel('X координата') self.ax_global_map.set_xlabel('X координата (м)')
self.ax_global_map.set_ylabel('Y координата') self.ax_global_map.set_ylabel('Y координата (м)')
self.ax_global_map.grid(True, alpha=0.3) 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.axhline(y=0, color='k', linestyle='-', alpha=0.3)
self.ax_global_map.axvline(x=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 = self.fig.add_subplot(gs[1, 1])
self.ax_route_map.set_title('RTH Map - маршрут на карте') self.ax_route_map.set_title('RTH Map - маршрут на карте')
self.ax_route_map.set_xlabel('X координата') self.ax_route_map.set_xlabel('X координата (м)')
self.ax_route_map.set_ylabel('Y координата') self.ax_route_map.set_ylabel('Y координата (м)')
self.ax_route_map.grid(True, alpha=0.3) self.ax_route_map.grid(True, alpha=0.3)
self.ax_chunk_matches = self.fig.add_subplot(gs[1, 2]) 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.clear()
self.ax_route_map.set_title('RTH Map - маршрут на карте') self.ax_route_map.set_title('RTH Map - маршрут на карте')
self.ax_route_map.set_xlabel('X координата') self.ax_route_map.set_xlabel('X координата (м)')
self.ax_route_map.set_ylabel('Y координата') self.ax_route_map.set_ylabel('Y координата (м)')
self.ax_route_map.grid(True, alpha=0.3) self.ax_route_map.grid(True, alpha=0.3)
if self.map_image is not None and self.map_extent is not None: 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( self.ax_route_map.text(
0.02, 0.02,
0.98, 0.98,
f"Ошибка возврата: {self.final_return_error:.2f}", f"Ошибка возврата: {self.final_return_error:.2f} м",
transform=self.ax_route_map.transAxes, transform=self.ax_route_map.transAxes,
va='top', va='top',
fontsize=8, fontsize=8,
@@ -241,10 +241,10 @@ class VisualizationManager:
self.trajectory_x.append(x) self.trajectory_x.append(x)
self.trajectory_y.append(y) self.trajectory_y.append(y)
self.ax_global_map.clear() self.ax_global_map.clear()
self.ax_global_map.set_title('Global Map - Траектория полета беспилотника') self.ax_global_map.set_title('Global Map - Траектория полета беспилотника')
self.ax_global_map.set_xlabel('X координата') self.ax_global_map.set_xlabel('X координата (м)')
self.ax_global_map.set_ylabel('Y координата') self.ax_global_map.set_ylabel('Y координата (м)')
self.ax_global_map.grid(True, alpha=0.3) 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.axhline(y=0, color='k', linestyle='-', alpha=0.3)
self.ax_global_map.axvline(x=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.error_times.append(frame_count)
self.position_errors.append(error) self.position_errors.append(error)
self.ax_error_plot.clear() self.ax_error_plot.clear()
self.ax_error_plot.set_title('Погрешность позиции от времени') self.ax_error_plot.set_title('Погрешность позиции от времени')
self.ax_error_plot.set_xlabel('Время (кадры)') self.ax_error_plot.set_xlabel('Время (кадры)')
self.ax_error_plot.set_ylabel('Погрешность (метры)') self.ax_error_plot.set_ylabel('Погрешность (м)')
self.ax_error_plot.grid(True, alpha=0.3) self.ax_error_plot.grid(True, alpha=0.3)
if len(self.error_times) > 1: if len(self.error_times) > 1: