193 lines
7.6 KiB
Python
193 lines
7.6 KiB
Python
from pathlib import Path
|
||
from simulator import Simulator
|
||
from time import sleep
|
||
from trajectory_drawer import TrajectoryDrawer
|
||
from visualization import VisualizationManager
|
||
from yandex_map import YandexMap
|
||
from vision_chunk import VisionChunk
|
||
from utility import cv2_to_pil
|
||
import random
|
||
|
||
import cv2
|
||
import matplotlib.pyplot as plt
|
||
import numpy as np
|
||
|
||
import autopilot
|
||
|
||
def make_global_photo(filename):
|
||
yandexMap = YandexMap()
|
||
yandexMap.save_photo(filename)
|
||
yandexMap.destroy()
|
||
|
||
def get_trajectory_points(bg_img: str) -> list[(float, float)]:
|
||
trajectoryDrawer = TrajectoryDrawer(bg_img)
|
||
trajectoryDrawer.show()
|
||
trajectoryDrawer.wait()
|
||
points = list(map(lambda p: [p[0] / trajectoryDrawer.img.shape[1], p[1] / trajectoryDrawer.img.shape[0]], trajectoryDrawer.points))
|
||
return points
|
||
|
||
def main():
|
||
# Скриншот местности
|
||
# make_global_photo('map.jpg')
|
||
|
||
# Получаем траекторию от пользователя
|
||
# points = get_trajectory_points('map.jpg')
|
||
|
||
# Trajectory #1
|
||
# points = [[np.float64(0.5384504359393909), np.float64(0.4084520767967683)], [np.float64(0.4451750568707629), np.float64(0.38213330305374654)], [np.float64(0.49266070439660997), np.float64(0.2789637099811013)], [np.float64(0.36377108968359656), np.float64(0.3263375027185404)], [np.float64(0.3535955937852008), np.float64(0.4337180995900692)]]
|
||
|
||
# Trajectory #2
|
||
# points = [[np.float64(0.29197731306713737), np.float64(0.3452870198135161)], [np.float64(0.33494051797147517), np.float64(0.2010601397017569)], [np.float64(0.39768940934491587), np.float64(0.25369768718780034)], [np.float64(0.4027771572941138), np.float64(0.4158213334448144)], [np.float64(0.2914120077394487), np.float64(0.5547844588079692)]]
|
||
|
||
# Trajectory #3
|
||
# points = [[np.float64(0.2755834585641664), np.float64(0.45687862048392835)], [np.float64(0.295934450360958), np.float64(0.5021469113219258)], [np.float64(0.32872215936689997), np.float64(0.4810918923275084)], [np.float64(0.3649017003389739), np.float64(0.5295184360146684)], [np.float64(0.3999506306556705), np.float64(0.49477765467387963)]]
|
||
|
||
# Trajectory #4
|
||
# points = [[np.float64(0.42143223310783934), np.float64(0.6663760594783815)], [np.float64(0.4253893704016599), np.float64(0.5537317078582484)], [np.float64(0.5124463908657128), np.float64(0.5621537154560153)], [np.float64(0.5124463908657128), np.float64(0.6684815613778233)], [np.float64(0.42143223310783934), np.float64(0.6663760594783815)]]
|
||
|
||
# Trajectory #5
|
||
# points = [[np.float64(0.5983728006743884), np.float64(0.7348048712102382)], [np.float64(0.5966768846913225), np.float64(0.5453097002604814)], [np.float64(0.6345523416464622), np.float64(0.7190136069644251)], [np.float64(0.6402053949233488), np.float64(0.5495207040593649)], [np.float64(0.5983728006743884), np.float64(0.7348048712102382)]]
|
||
|
||
# Trajectory #6
|
||
# points = [[np.float64(0.4406526142492536), np.float64(0.28106921188054296)], [np.float64(0.38581799746345413), np.float64(0.2968604761263561)], [np.float64(0.3931669667234066), np.float64(0.353709027411283)], [np.float64(0.4248240650739713), np.float64(0.35265627646156217)], [np.float64(0.40616898926024564), np.float64(0.3179154951207735)]]
|
||
|
||
# Trajectory #7
|
||
# points = [[np.float64(0.5491912371654754), np.float64(0.7505961354560512)], [np.float64(0.5537136797869846), np.float64(0.6863783275230781)], [np.float64(0.5017055896396284), np.float64(0.6653233085286606)], [np.float64(0.5520177638039186), np.float64(0.6042637534448503)], [np.float64(0.5593667330638712), np.float64(0.516885424618018)]]
|
||
|
||
# Trajectory #8
|
||
# points =
|
||
|
||
# Trajectory #9
|
||
# points =
|
||
|
||
# Trajectory #10
|
||
# points =
|
||
|
||
print(points)
|
||
|
||
# Для каждой точки сделаем приближенный снимок
|
||
yandexMap = YandexMap()
|
||
chunks: list[VisionChunk] = []
|
||
|
||
plt.ion()
|
||
for i in range(len(points)):
|
||
point = points[i]
|
||
yandexMap.scroll(point[0], point[1], 5, True)
|
||
sleep(1)
|
||
cv2_img = yandexMap.make_screenshot(point[0], point[1], 0.2, 0.2)
|
||
img = cv2_to_pil(cv2_img)
|
||
chunk = VisionChunk(img)
|
||
Path('chunks').mkdir(exist_ok=True)
|
||
chunk.save_image(Path('.') / 'chunks' / f'chunk_{i}.png')
|
||
plt.subplot(1, len(points), i+1)
|
||
plt.imshow(img)
|
||
plt.pause(0.25)
|
||
yandexMap.scroll(point[0], point[1], 5, False)
|
||
|
||
plt.tight_layout()
|
||
|
||
# Выделим на каждой картинке ключевые точки
|
||
for i in range(len(points)):
|
||
chunk = VisionChunk.load_image(Path('chunks') / f'chunk_{i}.png')
|
||
chunks.append(chunk)
|
||
kp, des = chunk.compute_keypoints()
|
||
|
||
plt.subplot(1, len(points), i+1)
|
||
plt.imshow(chunk.image)
|
||
kp_coords = np.array([j.pt for j in kp])
|
||
if len(kp_coords) > 0:
|
||
plt.scatter(kp_coords[:, 0], kp_coords[:, 1], c='red', s=20, alpha=0.7, marker='o')
|
||
plt.pause(0.2)
|
||
plt.ioff()
|
||
|
||
plt.show(block=True)
|
||
|
||
# Начнём симуляцию полёта с первой точки
|
||
yandexMap.scroll(points[0][0], points[0][1], 5, True)
|
||
sleep(0.2)
|
||
yandexMap.make_as_center(*points[0])
|
||
sleep(1)
|
||
|
||
vis_manager = VisualizationManager()
|
||
width, height = yandexMap.get_size()
|
||
# print(width, height)
|
||
points_coords = np.array(list(map(lambda p: [
|
||
(p[0] - points[0][0]) * width, (points[0][1] - p[1]) * height
|
||
], points)))
|
||
points_coords *= 2 ** 4
|
||
pilot = autopilot.AutoPilot(points_coords, chunks, vis_manager)
|
||
simulator = Simulator(yandexMap)
|
||
pilot.target_idx = 0
|
||
|
||
chunk = simulator.get_chunk()
|
||
command = pilot.handle(chunk)
|
||
|
||
vis_manager.update_display()
|
||
vis_manager.pause(1)
|
||
|
||
vis_manager.set_target_points(points_coords)
|
||
|
||
proc_time = np.array([])
|
||
|
||
zoom_next_event = random.randint(5, 10)
|
||
|
||
errors = []
|
||
chunk_errors = []
|
||
chunk_improves = []
|
||
|
||
last_chunk_index = 0
|
||
|
||
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)
|
||
|
||
chunk = simulator.get_chunk()
|
||
command = pilot.handle(chunk)
|
||
|
||
proc_time = np.append(proc_time, command.proccessing_time)
|
||
|
||
# Save Image
|
||
chunk.save_image(Path('images') / f"photo_{i}.png")
|
||
|
||
vis_manager.update_display()
|
||
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_global_map(simulator.geo.x, simulator.geo.y)
|
||
vis_manager.update_error_plot(i, pilot.geo.x, pilot.geo.y, simulator.geo.x, simulator.geo.y)
|
||
|
||
errors.append(np.hypot(pilot.geo.x - simulator.geo.x, pilot.geo.y - simulator.geo.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("Errors:", errors)
|
||
print("MSE:", (np.array(errors) ** 2).mean())
|
||
print("RMSE:", (np.array(errors) ** 2).mean() ** 0.5)
|
||
print("Chunk errors:", chunk_errors)
|
||
print("Chunk error improves:", chunk_improves)
|
||
print("Average FPS:", 1 / proc_time.mean())
|
||
vis_manager.show_final()
|
||
|
||
if __name__ == "__main__":
|
||
main()
|
||
|
||
#TODO
|