feat: chunks from google
This commit is contained in:
4
.gitignore
vendored
4
.gitignore
vendored
@@ -1,4 +1,6 @@
|
|||||||
.venv
|
.venv
|
||||||
__pycache__
|
__pycache__
|
||||||
*.png
|
*.png
|
||||||
images
|
images
|
||||||
|
trajectories
|
||||||
|
z
|
||||||
|
|||||||
19
autopilot.py
19
autopilot.py
@@ -197,12 +197,6 @@ class AutoPilot(Pilot):
|
|||||||
self.timer.stop()
|
self.timer.stop()
|
||||||
return PilotCommand(processing_time=self.timer.get_elapsed())
|
return PilotCommand(processing_time=self.timer.get_elapsed())
|
||||||
|
|
||||||
# Расстояние до цели
|
|
||||||
distance_to_target = math.sqrt(
|
|
||||||
(self.points[self.target_idx][0] - self.pos.x) ** 2 +
|
|
||||||
(self.points[self.target_idx][1] - self.pos.y) ** 2
|
|
||||||
)
|
|
||||||
|
|
||||||
# Вычисляем оптический поток для покадрового сравнения
|
# Вычисляем оптический поток для покадрового сравнения
|
||||||
matching_timer = Timer()
|
matching_timer = Timer()
|
||||||
matching_timer.start()
|
matching_timer.start()
|
||||||
@@ -246,6 +240,16 @@ class AutoPilot(Pilot):
|
|||||||
|
|
||||||
chunk_timer.stop()
|
chunk_timer.stop()
|
||||||
print(f"Chunk timer: {chunk_timer.get_elapsed() * 1000:.2f} ms")
|
print(f"Chunk timer: {chunk_timer.get_elapsed() * 1000:.2f} ms")
|
||||||
|
command = self.make_command()
|
||||||
|
self.timer.reset()
|
||||||
|
return command
|
||||||
|
|
||||||
|
def make_command(self) -> PilotCommand:
|
||||||
|
# Расстояние до цели
|
||||||
|
distance_to_target = math.sqrt(
|
||||||
|
(self.points[self.target_idx][0] - self.pos.x) ** 2 +
|
||||||
|
(self.points[self.target_idx][1] - self.pos.y) ** 2
|
||||||
|
)
|
||||||
|
|
||||||
if distance_to_target < 35:
|
if distance_to_target < 35:
|
||||||
self.target_idx += 1
|
self.target_idx += 1
|
||||||
@@ -267,8 +271,6 @@ class AutoPilot(Pilot):
|
|||||||
|
|
||||||
angle_trajectory = self.pos.yaw + math.pi / 2
|
angle_trajectory = self.pos.yaw + math.pi / 2
|
||||||
|
|
||||||
# print("[ANGLE]", angle_trajectory, "->", target_angle)
|
|
||||||
|
|
||||||
# Вычисляем разность углов (направление поворота)
|
# Вычисляем разность углов (направление поворота)
|
||||||
angle_diff = target_angle - angle_trajectory
|
angle_diff = target_angle - angle_trajectory
|
||||||
|
|
||||||
@@ -284,7 +286,6 @@ class AutoPilot(Pilot):
|
|||||||
max(min(d_a_limit, angle_diff), -d_a_limit),
|
max(min(d_a_limit, angle_diff), -d_a_limit),
|
||||||
d_r, False, self.timer.get_elapsed()
|
d_r, False, self.timer.get_elapsed()
|
||||||
)
|
)
|
||||||
self.timer.reset()
|
|
||||||
return command
|
return command
|
||||||
|
|
||||||
def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0):
|
def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0):
|
||||||
|
|||||||
@@ -1,5 +1,12 @@
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
|
# Ширина 1 пикселя в метрах
|
||||||
|
YANDEX_PIXEL_RATIO_18 = 0.332697807435653
|
||||||
|
GOOGLE_PIXEL_RATIO_15 = 2766 / 1031
|
||||||
|
GOOGLE_PIXEL_RATIO_18 = 346 / 1031
|
||||||
|
|
||||||
|
WINDOW_HEIGHT = 1031
|
||||||
|
|
||||||
# Ширина и высота снимка в пикселях
|
# Ширина и высота снимка в пикселях
|
||||||
CHUNK_WIDTH = 700
|
CHUNK_WIDTH = 700
|
||||||
|
|
||||||
|
|||||||
83
google_map.py
Normal file
83
google_map.py
Normal file
@@ -0,0 +1,83 @@
|
|||||||
|
|
||||||
|
import cv2
|
||||||
|
import math
|
||||||
|
import numpy as np
|
||||||
|
import os
|
||||||
|
import utility
|
||||||
|
|
||||||
|
from io import BytesIO
|
||||||
|
from PIL import Image
|
||||||
|
from selenium.webdriver.common.actions.wheel_input import ScrollOrigin
|
||||||
|
from selenium import webdriver
|
||||||
|
from selenium.webdriver.common.by import By
|
||||||
|
from selenium.webdriver.common.action_chains import ActionChains
|
||||||
|
from time import sleep
|
||||||
|
|
||||||
|
def generateURL(lat: float, lon: float, zoom: int):
|
||||||
|
return f"https://www.google.com/maps/@{lon},{lat},{zoom}z"
|
||||||
|
|
||||||
|
class GoogleMap:
|
||||||
|
initial_zoom: int
|
||||||
|
initial_lat: float
|
||||||
|
initial_lon: float
|
||||||
|
|
||||||
|
def __init__(self, initial_lat=49.103814, initial_lon=55.794258, initial_zoom=18):
|
||||||
|
self.initial_lat = initial_lat
|
||||||
|
self.initial_lon = initial_lon
|
||||||
|
self.initial_zoom = initial_zoom
|
||||||
|
|
||||||
|
options = webdriver.ChromeOptions()
|
||||||
|
# options.add_experimental_option("detach", True)
|
||||||
|
self.driver = webdriver.Chrome(options)
|
||||||
|
self.driver.get(generateURL(initial_lat, initial_lon, initial_zoom))
|
||||||
|
self.driver.maximize_window()
|
||||||
|
|
||||||
|
action = ActionChains(self.driver)
|
||||||
|
sleep(5)
|
||||||
|
self.driver.execute_script('document.querySelector(\'.yHc72.qk5Wte\').click()')
|
||||||
|
sleep(5)
|
||||||
|
|
||||||
|
def open(self, lat, lon, zoom):
|
||||||
|
self.driver.get(generateURL(lat, lon, zoom))
|
||||||
|
|
||||||
|
def save_photo(self, filename: str):
|
||||||
|
im = self.make_screenshot()
|
||||||
|
im.save(filename)
|
||||||
|
|
||||||
|
def destroy(self):
|
||||||
|
self.driver.close()
|
||||||
|
|
||||||
|
def get_size(self) -> tuple[int, int]:
|
||||||
|
html = self.driver.find_element(By.TAG_NAME, 'html')
|
||||||
|
return (html.size['width'], html.size['height'])
|
||||||
|
|
||||||
|
def scroll(self, x: float, y: float, count: int = 1, inner_zoom: bool = True):
|
||||||
|
html = self.driver.find_element(By.TAG_NAME, 'html')
|
||||||
|
|
||||||
|
x_offset = (x - 0.5) * (html.size['width'] - 72) + 72
|
||||||
|
y_offset = (y - 0.5) * html.size['height']
|
||||||
|
action = ActionChains(self.driver)
|
||||||
|
|
||||||
|
for i in range(count-1):
|
||||||
|
action.scroll_from_origin(ScrollOrigin(html, int(x_offset), int(y_offset)), 0, -100 if inner_zoom else 100)
|
||||||
|
action.perform()
|
||||||
|
if i != count - 1:
|
||||||
|
sleep(0.25)
|
||||||
|
|
||||||
|
def move(self, dx: float, dy: float):
|
||||||
|
self.driver.execute_script(utility.google_map_js_move_script(dx, dy))
|
||||||
|
|
||||||
|
def make_as_center(self, x: float, y: float):
|
||||||
|
dx = (x - 0.5) * self.get_size()[0]
|
||||||
|
dy = (0.5 - y) * self.get_size()[1]
|
||||||
|
self.move(dx, dy)
|
||||||
|
sleep(1)
|
||||||
|
|
||||||
|
def make_screenshot(self) -> Image.Image:
|
||||||
|
png = self.driver.get_screenshot_as_png()
|
||||||
|
im = Image.open(BytesIO(png))
|
||||||
|
return utility.cv2_to_pil(np.array(im)[:, 72:])
|
||||||
|
|
||||||
|
def get_geolocation(self):
|
||||||
|
current_url = self.driver.current_url
|
||||||
|
return utility.parse_google_maps_url(current_url)
|
||||||
252
main.py
252
main.py
@@ -1,23 +1,28 @@
|
|||||||
|
from google_map import GoogleMap
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
|
from position import Position
|
||||||
from simulator import Simulator
|
from simulator import Simulator
|
||||||
from time import sleep
|
from time import sleep
|
||||||
from trajectory_drawer import TrajectoryDrawer
|
from trajectory_drawer import TrajectoryDrawer
|
||||||
|
from utility import cv2_to_pil
|
||||||
|
from vision_chunk import VisionChunk
|
||||||
from visualization import VisualizationManager
|
from visualization import VisualizationManager
|
||||||
from yandex_map import YandexMap
|
from yandex_map import YandexMap
|
||||||
from vision_chunk import VisionChunk
|
|
||||||
from utility import cv2_to_pil
|
|
||||||
import random
|
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import autopilot
|
||||||
|
import constants
|
||||||
import cv2
|
import cv2
|
||||||
import matplotlib.pyplot as plt
|
import matplotlib.pyplot as plt
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
import pickle
|
||||||
|
import random
|
||||||
|
import utility
|
||||||
|
|
||||||
import autopilot
|
def make_global_photo(filename, initial_zoom=13):
|
||||||
|
google_map = GoogleMap(initial_zoom=initial_zoom)
|
||||||
def make_global_photo(filename):
|
google_map.save_photo(filename)
|
||||||
yandexMap = YandexMap()
|
google_map.destroy()
|
||||||
yandexMap.save_photo(filename)
|
|
||||||
yandexMap.destroy()
|
|
||||||
|
|
||||||
def get_trajectory_points(bg_img: str) -> list[(float, float)]:
|
def get_trajectory_points(bg_img: str) -> list[(float, float)]:
|
||||||
trajectoryDrawer = TrajectoryDrawer(bg_img)
|
trajectoryDrawer = TrajectoryDrawer(bg_img)
|
||||||
@@ -26,97 +31,111 @@ def get_trajectory_points(bg_img: str) -> list[(float, float)]:
|
|||||||
points = list(map(lambda p: [p[0] / trajectoryDrawer.img.shape[1], p[1] / trajectoryDrawer.img.shape[0]], trajectoryDrawer.points))
|
points = list(map(lambda p: [p[0] / trajectoryDrawer.img.shape[1], p[1] / trajectoryDrawer.img.shape[0]], trajectoryDrawer.points))
|
||||||
return points
|
return points
|
||||||
|
|
||||||
def main():
|
def build(name: str):
|
||||||
# Скриншот местности
|
|
||||||
# make_global_photo('map.jpg')
|
|
||||||
|
|
||||||
# Получаем траекторию от пользователя
|
# Создание папки с информацией о маршруте
|
||||||
# points = get_trajectory_points('map.jpg')
|
dir = Path('trajectories')
|
||||||
|
if not dir.exists(): dir.mkdir()
|
||||||
|
dir /= name
|
||||||
|
assert not dir.exists()
|
||||||
|
dir.mkdir()
|
||||||
|
dir_chunks = dir / 'chunks'
|
||||||
|
dir_chunks.mkdir()
|
||||||
|
|
||||||
# Trajectory #1
|
# make_global_photo('map.jpg', 15)
|
||||||
# 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)]]
|
points = get_trajectory_points('map.jpg')
|
||||||
|
google_map = GoogleMap(initial_zoom=15)
|
||||||
# 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)
|
google_map.make_as_center(*points[0])
|
||||||
sleep(0.2)
|
sleep(3)
|
||||||
yandexMap.make_as_center(*points[0])
|
google_map.scroll(0.5, 0.5, 10, True)
|
||||||
sleep(1)
|
sleep(2)
|
||||||
|
geo = google_map.get_geolocation()
|
||||||
|
google_map.open(geo['lat'], geo['lon'], 18)
|
||||||
|
sleep(20)
|
||||||
|
|
||||||
vis_manager = VisualizationManager()
|
width, height = google_map.get_size()
|
||||||
width, height = yandexMap.get_size()
|
|
||||||
# print(width, height)
|
|
||||||
points_coords = np.array(list(map(lambda p: [
|
points_coords = np.array(list(map(lambda p: [
|
||||||
(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 *= 2 ** 4
|
|
||||||
pilot = autopilot.AutoPilot(points_coords, chunks, vis_manager)
|
points_coords *= constants.GOOGLE_PIXEL_RATIO_15
|
||||||
simulator = Simulator(yandexMap)
|
points_coords_pixel = points_coords / constants.GOOGLE_PIXEL_RATIO_18
|
||||||
|
|
||||||
|
pilot = autopilot.AutoPilot(points_coords_pixel)
|
||||||
|
simulator = Simulator(google_map)
|
||||||
|
pilot.target_idx = 0
|
||||||
|
|
||||||
|
pilot.pos = simulator.pos.copy()
|
||||||
|
command = pilot.make_command()
|
||||||
|
|
||||||
|
positions: list[Position] = []
|
||||||
|
|
||||||
|
print("points_coords_pixel:", points_coords_pixel)
|
||||||
|
for i in range(100000):
|
||||||
|
|
||||||
|
if command.stop:
|
||||||
|
break
|
||||||
|
|
||||||
|
# simulator.handle(command.dangle, command.velocity)
|
||||||
|
chunk = simulator.get_chunk()
|
||||||
|
pilot.pos = simulator.pos.copy()
|
||||||
|
command = pilot.make_command()
|
||||||
|
command.velocity /= constants.GOOGLE_PIXEL_RATIO_18
|
||||||
|
|
||||||
|
print("Position:", simulator.pos)
|
||||||
|
|
||||||
|
# Save Image
|
||||||
|
chunk.save_image(dir_chunks / f"chunk_{len(positions)}.png")
|
||||||
|
positions.append(simulator.pos)
|
||||||
|
|
||||||
|
simulator.handle(command.dangle, command.velocity)
|
||||||
|
sleep(1.5)
|
||||||
|
|
||||||
|
data = {
|
||||||
|
'points': points_coords,
|
||||||
|
'chunk_positions': positions,
|
||||||
|
'initial_geolocation': geo
|
||||||
|
}
|
||||||
|
|
||||||
|
file_positions = dir / 'positions.pkl'
|
||||||
|
with file_positions.open('wb') as file:
|
||||||
|
pickle.dump(data, file)
|
||||||
|
|
||||||
|
with file_positions.open('rb') as file:
|
||||||
|
r = pickle.load(file)
|
||||||
|
print(r)
|
||||||
|
|
||||||
|
sleep(15)
|
||||||
|
google_map.destroy()
|
||||||
|
|
||||||
|
def run(name: str):
|
||||||
|
dir = Path('trajectories')
|
||||||
|
assert dir.exists()
|
||||||
|
dir /= name
|
||||||
|
assert dir.exists()
|
||||||
|
dir_chunks = dir / 'chunks'
|
||||||
|
file_positions = dir / 'positions.pkl'
|
||||||
|
|
||||||
|
with file_positions.open('rb') as file:
|
||||||
|
data = pickle.load(file)
|
||||||
|
|
||||||
|
initial_geolocation = data['initial_geolocation']
|
||||||
|
chunks: list[VisionChunk] = []
|
||||||
|
for i in range(len(data['chunk_positions'])):
|
||||||
|
chunk = VisionChunk.load_image(dir_chunks / f"chunk_{i}.png")
|
||||||
|
chunk.pos = data['chunk_positions'][i]
|
||||||
|
|
||||||
|
points = data['points'] / constants.YANDEX_PIXEL_RATIO_18
|
||||||
|
|
||||||
|
yandex_map = YandexMap(initial_geolocation['lat'], initial_geolocation['lon'], 18)
|
||||||
|
sleep(10)
|
||||||
|
|
||||||
|
vis_manager = VisualizationManager()
|
||||||
|
width, height = yandex_map.get_size()
|
||||||
|
pilot = autopilot.AutoPilot(points, chunks, vis_manager)
|
||||||
|
simulator = Simulator(yandex_map)
|
||||||
pilot.target_idx = 0
|
pilot.target_idx = 0
|
||||||
|
|
||||||
chunk = simulator.get_chunk()
|
chunk = simulator.get_chunk()
|
||||||
@@ -125,7 +144,7 @@ def main():
|
|||||||
vis_manager.update_display()
|
vis_manager.update_display()
|
||||||
vis_manager.pause(1)
|
vis_manager.pause(1)
|
||||||
|
|
||||||
vis_manager.set_target_points(points_coords)
|
vis_manager.set_target_points(points)
|
||||||
|
|
||||||
proc_time = np.array([])
|
proc_time = np.array([])
|
||||||
|
|
||||||
@@ -157,6 +176,7 @@ def main():
|
|||||||
# simulator.handle(command.dangle, command.velocity)
|
# simulator.handle(command.dangle, command.velocity)
|
||||||
chunk = simulator.get_chunk()
|
chunk = simulator.get_chunk()
|
||||||
command = pilot.handle(chunk)
|
command = pilot.handle(chunk)
|
||||||
|
command.velocity /= constants.YANDEX_PIXEL_RATIO_18
|
||||||
|
|
||||||
proc_time = np.append(proc_time, command.proccessing_time)
|
proc_time = np.append(proc_time, command.proccessing_time)
|
||||||
|
|
||||||
@@ -194,7 +214,47 @@ def main():
|
|||||||
print("Average FPS:", 1 / proc_time.mean())
|
print("Average FPS:", 1 / proc_time.mean())
|
||||||
vis_manager.show_final()
|
vis_manager.show_final()
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
|
|
||||||
#TODO
|
def main(mode: str, name: str):
|
||||||
|
if name is None:
|
||||||
|
name = utility.generate_folder_name()
|
||||||
|
|
||||||
|
if mode == 'build' or mode == 'standalone':
|
||||||
|
build(name)
|
||||||
|
|
||||||
|
if mode == 'run' or mode == 'standalone':
|
||||||
|
run(name)
|
||||||
|
|
||||||
|
def parse_args():
|
||||||
|
"""Парсер аргументов командной строки"""
|
||||||
|
parser = argparse.ArgumentParser(description='Обработка траекторий')
|
||||||
|
|
||||||
|
# Добавляем обязательный аргумент --mode
|
||||||
|
parser.add_argument(
|
||||||
|
'--mode',
|
||||||
|
type=str,
|
||||||
|
required=True,
|
||||||
|
choices=['standalone', 'build', 'run'],
|
||||||
|
help='Режим работы: standalone, build или run'
|
||||||
|
)
|
||||||
|
|
||||||
|
# Добавляем опциональный аргумент --name
|
||||||
|
parser.add_argument(
|
||||||
|
'--name',
|
||||||
|
type=str,
|
||||||
|
help='Название траектории (обязательно для режимов build и run)'
|
||||||
|
)
|
||||||
|
|
||||||
|
# Парсим аргументы
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
# Проверяем, что для build и run указан --name
|
||||||
|
if args.mode in ['build', 'run'] and not args.name:
|
||||||
|
parser.error(f"--name обязателен для режима {args.mode}")
|
||||||
|
|
||||||
|
return args
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
args = parse_args()
|
||||||
|
main(args.mode, args.name)
|
||||||
|
|
||||||
|
|||||||
BIN
map.jpg
BIN
map.jpg
Binary file not shown.
|
Before Width: | Height: | Size: 3.4 MiB After Width: | Height: | Size: 428 KiB |
@@ -42,13 +42,14 @@ class Position:
|
|||||||
f"roll={math.degrees(self.roll):.1f}°)"
|
f"roll={math.degrees(self.roll):.1f}°)"
|
||||||
)
|
)
|
||||||
|
|
||||||
def get_homography_matrix(self, K: np.ndarray = constants.K, sliding: bool = True) -> np.ndarray:
|
def get_homography_matrix(self, K_in: np.ndarray = constants.K, K_out: np.ndarray | None = None, sliding: bool = True) -> np.ndarray:
|
||||||
""" Возвращает матрицу гомографии """
|
""" Возвращает матрицу гомографии """
|
||||||
R = self.get_rotation_matrix()
|
R = self.get_rotation_matrix()
|
||||||
T = self.get_translation_matrix()
|
T = self.get_translation_matrix()
|
||||||
if not sliding:
|
if not sliding:
|
||||||
T[0, 2] = T[1, 2] = 0
|
T[0, 2] = T[1, 2] = 0
|
||||||
return K @ R @ T @ np.linalg.inv(K)
|
if K_out is None: K_out = K_in
|
||||||
|
return K_out @ R @ T @ np.linalg.inv(K_in)
|
||||||
|
|
||||||
def copy(self) -> 'Position':
|
def copy(self) -> 'Position':
|
||||||
"""Создает полную копию объекта"""
|
"""Создает полную копию объекта"""
|
||||||
|
|||||||
32
simulator.py
32
simulator.py
@@ -8,12 +8,13 @@ import numpy as np
|
|||||||
from position import Position
|
from position import Position
|
||||||
from vision_chunk import VisionChunk
|
from vision_chunk import VisionChunk
|
||||||
from yandex_map import YandexMap
|
from yandex_map import YandexMap
|
||||||
|
from google_map import GoogleMap
|
||||||
import constants
|
import constants
|
||||||
import utility
|
import utility
|
||||||
|
|
||||||
class Simulator:
|
class Simulator:
|
||||||
def __init__(self, yandex_map: YandexMap = None):
|
def __init__(self, online_map: YandexMap | GoogleMap = None):
|
||||||
self.yandex_map = yandex_map
|
self.online_map = online_map
|
||||||
# Используем новый конструктор с yaw, pitch, roll
|
# Используем новый конструктор с yaw, pitch, roll
|
||||||
self.pos = Position(x=0, y=0, z=1, yaw=0, pitch=0, roll=0)
|
self.pos = Position(x=0, y=0, z=1, yaw=0, pitch=0, roll=0)
|
||||||
|
|
||||||
@@ -35,17 +36,19 @@ class Simulator:
|
|||||||
Возвращает квадратное изображение 700x700.
|
Возвращает квадратное изображение 700x700.
|
||||||
"""
|
"""
|
||||||
img_array = np.array(image)
|
img_array = np.array(image)
|
||||||
print(img_array.shape)
|
|
||||||
h, w, _ = img_array.shape
|
h, w, _ = img_array.shape
|
||||||
|
|
||||||
# Применяем трансформацию
|
# Применяем трансформацию
|
||||||
pos = self.pos.copy()
|
pos = self.pos.copy()
|
||||||
pos.x = 0
|
pos.x = 0
|
||||||
pos.y = 0
|
pos.y = 0
|
||||||
K = utility.calc_camera_matrix(w, h)
|
|
||||||
K = constants.K
|
K_in = utility.calc_camera_matrix(w, h)
|
||||||
img_array = img_array[:constants.CHUNK_WIDTH, :constants.CHUNK_WIDTH]
|
K_out = constants.K
|
||||||
transformed = cv2.warpPerspective(img_array, pos.get_homography_matrix(K), (constants.CHUNK_WIDTH, constants.CHUNK_WIDTH))
|
H = pos.get_homography_matrix(K_in, K_out)
|
||||||
|
|
||||||
|
shape = (constants.CHUNK_WIDTH, constants.CHUNK_WIDTH)
|
||||||
|
transformed = cv2.warpPerspective(img_array, H, shape)
|
||||||
|
|
||||||
return Image.fromarray(transformed)
|
return Image.fromarray(transformed)
|
||||||
|
|
||||||
@@ -60,13 +63,6 @@ class Simulator:
|
|||||||
dangle - изменение угла курса (радианы)
|
dangle - изменение угла курса (радианы)
|
||||||
velocity - скорость движения
|
velocity - скорость движения
|
||||||
"""
|
"""
|
||||||
from selenium.webdriver.common.by import By
|
|
||||||
from selenium.webdriver.common.action_chains import ActionChains
|
|
||||||
|
|
||||||
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()
|
|
||||||
|
|
||||||
# Обновляем yaw в объекте Position
|
# Обновляем yaw в объекте Position
|
||||||
self.pos.yaw += dangle
|
self.pos.yaw += dangle
|
||||||
@@ -77,10 +73,7 @@ class Simulator:
|
|||||||
dy = math.sin(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)
|
self.update_trajectory(dx, dy)
|
||||||
|
self.online_map.move(dx, dy)
|
||||||
action.move_by_offset(-dx, dy)
|
|
||||||
action.release()
|
|
||||||
action.perform()
|
|
||||||
|
|
||||||
def set_zoom(self, zoom_level: float):
|
def set_zoom(self, zoom_level: float):
|
||||||
"""Программное изменение масштаба"""
|
"""Программное изменение масштаба"""
|
||||||
@@ -88,8 +81,7 @@ class Simulator:
|
|||||||
|
|
||||||
def get_chunk(self) -> VisionChunk:
|
def get_chunk(self) -> VisionChunk:
|
||||||
"""Получить текущий снимок с камеры дрона"""
|
"""Получить текущий снимок с камеры дрона"""
|
||||||
png = self.yandex_map.driver.get_screenshot_as_png()
|
im = self.online_map.make_screenshot()
|
||||||
im = Image.open(BytesIO(png))
|
|
||||||
|
|
||||||
# Применяем перспективную трансформацию
|
# Применяем перспективную трансформацию
|
||||||
transformed_im = self._apply_perspective_transform(im)
|
transformed_im = self._apply_perspective_transform(im)
|
||||||
|
|||||||
115
utility.py
115
utility.py
@@ -1,7 +1,9 @@
|
|||||||
from PIL import Image
|
from PIL import Image
|
||||||
|
from datetime import datetime
|
||||||
|
import constants
|
||||||
import cv2
|
import cv2
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import constants
|
import re
|
||||||
|
|
||||||
def cv2_to_pil(cv_image: np.ndarray) -> Image.Image:
|
def cv2_to_pil(cv_image: np.ndarray) -> Image.Image:
|
||||||
"""
|
"""
|
||||||
@@ -19,7 +21,7 @@ def get_normals(H: np.ndarray, R: np.ndarray, T: np.ndarray) -> np.ndarray:
|
|||||||
n = cv2.decomposeHomographyMat(H, constants.K, R, T)
|
n = cv2.decomposeHomographyMat(H, constants.K, R, T)
|
||||||
return n
|
return n
|
||||||
|
|
||||||
def estimate_transformation_matrix(src_pts: np.ndarray, dst_pts: np.ndarray) -> tuple[np.ndarray, float | None]:
|
def estimate_transformation_matrix(src_pts: np.ndarray, dst_pts: np.ndarray) -> np.ndarray | None:
|
||||||
"""Оценивает матрицу трансформации на основе сопоставленных точек"""
|
"""Оценивает матрицу трансформации на основе сопоставленных точек"""
|
||||||
# Используем RANSAC для оценки матрицы гомографии
|
# Используем RANSAC для оценки матрицы гомографии
|
||||||
H, _ = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 3.0, maxIters=1000)
|
H, _ = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 3.0, maxIters=1000)
|
||||||
@@ -32,3 +34,112 @@ def calc_camera_matrix(w: float, h: float):
|
|||||||
[0, f, h / 2],
|
[0, f, h / 2],
|
||||||
[0, 0, 1]
|
[0, 0, 1]
|
||||||
])
|
])
|
||||||
|
|
||||||
|
|
||||||
|
def generate_folder_name():
|
||||||
|
"""
|
||||||
|
Генерирует название для папки с текущей датой и временем до секунд.
|
||||||
|
Формат: YYYY-MM-DD_HH-MM-SS
|
||||||
|
"""
|
||||||
|
now = datetime.now()
|
||||||
|
folder_name = now.strftime("%Y-%m-%d_%H-%M-%S")
|
||||||
|
return folder_name
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
def parse_google_maps_url(url):
|
||||||
|
"""
|
||||||
|
Парсит URL Google Maps и извлекает lat, lon и zoom
|
||||||
|
Формат: /@lat,lon,zoom[m|z]
|
||||||
|
"""
|
||||||
|
pattern = r'/@([-\d.]+),([-\d.]+),(\d+)([mz])'
|
||||||
|
match = re.search(pattern, url)
|
||||||
|
|
||||||
|
if match:
|
||||||
|
lon = float(match.group(1))
|
||||||
|
lat = float(match.group(2))
|
||||||
|
zoom_value = int(match.group(3))
|
||||||
|
zoom_unit = match.group(4)
|
||||||
|
|
||||||
|
return {
|
||||||
|
'lat': lat,
|
||||||
|
'lon': lon,
|
||||||
|
'zoom': zoom_value,
|
||||||
|
'zoom_unit': zoom_unit
|
||||||
|
}
|
||||||
|
return None
|
||||||
|
|
||||||
|
def google_map_js_move_script(dx, dy) -> str:
|
||||||
|
return """
|
||||||
|
async function sleep(ms) {
|
||||||
|
return new Promise((resolve, reject) => {
|
||||||
|
setTimeout(() => resolve(), ms);
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
async function simulateDrag(element, offsetX, offsetY) {
|
||||||
|
const rect = element.getBoundingClientRect();
|
||||||
|
const startX = rect.left + rect.width / 2;
|
||||||
|
const startY = rect.top + rect.height / 2;
|
||||||
|
const step = 7
|
||||||
|
const endX = startX + offsetX + step;
|
||||||
|
const endY = startY + offsetY + step;
|
||||||
|
|
||||||
|
element.dispatchEvent(new MouseEvent('mousedown', {
|
||||||
|
view: window,
|
||||||
|
bubbles: true,
|
||||||
|
cancelable: true,
|
||||||
|
clientX: startX,
|
||||||
|
clientY: startY,
|
||||||
|
button: 0
|
||||||
|
}));
|
||||||
|
|
||||||
|
let currentX = startX;
|
||||||
|
let currentY = startY;
|
||||||
|
|
||||||
|
function move(stepX, stepY) {
|
||||||
|
currentX += stepX;
|
||||||
|
currentY += stepY;
|
||||||
|
|
||||||
|
document.dispatchEvent(new MouseEvent('mousemove', {
|
||||||
|
view: window,
|
||||||
|
bubbles: true,
|
||||||
|
cancelable: false,
|
||||||
|
clientX: currentX,
|
||||||
|
clientY: currentY
|
||||||
|
}));
|
||||||
|
}
|
||||||
|
|
||||||
|
await sleep(50);
|
||||||
|
move(step, step)
|
||||||
|
|
||||||
|
while (currentX != endX || currentY != endY) {
|
||||||
|
await sleep(50);
|
||||||
|
const stepX = Math.min(step, Math.max(-step, endX - currentX));
|
||||||
|
const stepY = Math.min(step, Math.max(-step, endY - currentY));
|
||||||
|
move(stepX, stepY);
|
||||||
|
}
|
||||||
|
|
||||||
|
await sleep(50)
|
||||||
|
document.dispatchEvent(new MouseEvent('mouseup', {
|
||||||
|
view: window,
|
||||||
|
bubbles: true,
|
||||||
|
cancelable: false,
|
||||||
|
clientX: endX,
|
||||||
|
clientY: endY,
|
||||||
|
button: 0
|
||||||
|
}));
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
const canvas = document.querySelector('canvas.H1VXrf.JRr1M.DnOnV');
|
||||||
|
""" + f"simulateDrag(canvas, {-dx}, {dy});" + """
|
||||||
|
}
|
||||||
|
"""
|
||||||
@@ -1,10 +1,11 @@
|
|||||||
import cv2
|
import cv2
|
||||||
import json
|
import json
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
from PIL import Image
|
||||||
from dataclasses import dataclass, field
|
from dataclasses import dataclass, field
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
|
from position import Position
|
||||||
from typing import Literal, Optional, Tuple
|
from typing import Literal, Optional, Tuple
|
||||||
from PIL import Image
|
|
||||||
|
|
||||||
FeatureMethod = Literal["orb", "sift", "akaze", "brisk"]
|
FeatureMethod = Literal["orb", "sift", "akaze", "brisk"]
|
||||||
DEFAULT_METHOD = "orb"
|
DEFAULT_METHOD = "orb"
|
||||||
@@ -14,6 +15,7 @@ class VisionChunk:
|
|||||||
image: Image.Image
|
image: Image.Image
|
||||||
feature_method: FeatureMethod = DEFAULT_METHOD
|
feature_method: FeatureMethod = DEFAULT_METHOD
|
||||||
|
|
||||||
|
pos: Optional[Position] = field(default=None, init=False)
|
||||||
keypoints: Optional[list] = field(default=None, init=False)
|
keypoints: Optional[list] = field(default=None, init=False)
|
||||||
descriptors: Optional[np.ndarray] = field(default=None, init=False)
|
descriptors: Optional[np.ndarray] = field(default=None, init=False)
|
||||||
_detector: Optional[cv2.Feature2D] = field(default=None, init=False, repr=False)
|
_detector: Optional[cv2.Feature2D] = field(default=None, init=False, repr=False)
|
||||||
@@ -25,7 +27,7 @@ class VisionChunk:
|
|||||||
|
|
||||||
if self.feature_method == "orb":
|
if self.feature_method == "orb":
|
||||||
self._detector = cv2.ORB_create(
|
self._detector = cv2.ORB_create(
|
||||||
nfeatures=1000,
|
nfeatures=10000,
|
||||||
scaleFactor=1.2,
|
scaleFactor=1.2,
|
||||||
nlevels=32,
|
nlevels=32,
|
||||||
edgeThreshold=31,
|
edgeThreshold=31,
|
||||||
|
|||||||
@@ -1,16 +1,16 @@
|
|||||||
import math
|
|
||||||
from io import BytesIO
|
|
||||||
from time import sleep
|
|
||||||
import os
|
|
||||||
|
|
||||||
import cv2
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
from PIL import Image
|
from PIL import Image
|
||||||
|
from io import BytesIO
|
||||||
from selenium.webdriver.common.actions.wheel_input import ScrollOrigin
|
from selenium.webdriver.common.actions.wheel_input import ScrollOrigin
|
||||||
from selenium import webdriver
|
from selenium import webdriver
|
||||||
from selenium.webdriver.common.by import By
|
from selenium.webdriver.common.by import By
|
||||||
from selenium.webdriver.common.action_chains import ActionChains
|
from selenium.webdriver.common.action_chains import ActionChains
|
||||||
|
from time import sleep
|
||||||
|
|
||||||
|
import cv2
|
||||||
|
import math
|
||||||
|
import numpy as np
|
||||||
|
import os
|
||||||
|
import utility
|
||||||
|
|
||||||
def generateURL(lat: float, lon: float, zoom: int):
|
def generateURL(lat: float, lon: float, zoom: int):
|
||||||
return f"https://yandex.ru/maps/43/kazan/?l=sat&ll={lat}%2C{lon}&z={zoom}"
|
return f"https://yandex.ru/maps/43/kazan/?l=sat&ll={lat}%2C{lon}&z={zoom}"
|
||||||
@@ -68,51 +68,22 @@ class YandexMap:
|
|||||||
if i != count - 1:
|
if i != count - 1:
|
||||||
sleep(0.25)
|
sleep(0.25)
|
||||||
|
|
||||||
def make_as_center(self, x: float, y: float):
|
def move(self, dx: float, dy: float):
|
||||||
html = self.driver.find_element(By.TAG_NAME, 'html')
|
html = self.driver.find_element(By.TAG_NAME, 'html')
|
||||||
|
|
||||||
action = ActionChains(self.driver)
|
action = ActionChains(self.driver)
|
||||||
action.move_to_element_with_offset(html, 0, 0)
|
action.move_to_element_with_offset(html, 0, 0)
|
||||||
action.click_and_hold()
|
action.click_and_hold()
|
||||||
|
|
||||||
dx = (x - 0.5) * self.get_size()[0]
|
|
||||||
dy = (0.5 - y) * self.get_size()[1]
|
|
||||||
print(dx, dy)
|
|
||||||
action.move_by_offset(-dx, dy)
|
action.move_by_offset(-dx, dy)
|
||||||
action.release()
|
action.release()
|
||||||
action.perform()
|
action.perform()
|
||||||
|
|
||||||
def make_screenshot(self, x: float, y: float, width: float, height: float) -> cv2.typing.MatLike:
|
|
||||||
# Сохраняем скриншот
|
def make_as_center(self, x: float, y: float):
|
||||||
self.save_photo("temp.png")
|
dx = (x - 0.5) * self.get_size()[0]
|
||||||
|
dy = (0.5 - y) * self.get_size()[1]
|
||||||
# Загружаем изображение
|
self.move(dx, dy)
|
||||||
image = cv2.imread("temp.png")
|
|
||||||
|
def make_screenshot(self) -> Image.Image:
|
||||||
if image is None:
|
png = self.driver.get_screenshot_as_png()
|
||||||
raise ValueError("Не удалось загрузить изображение temp.png")
|
im = Image.open(BytesIO(png))
|
||||||
|
return utility.cv2_to_pil(np.array(im)[:, :])
|
||||||
# Получаем размеры исходного изображения
|
|
||||||
img_height, img_width = image.shape[:2]
|
|
||||||
|
|
||||||
# Преобразуем относительные координаты в абсолютные пиксели
|
|
||||||
center_x = int(x * img_width)
|
|
||||||
center_y = int(y * img_height)
|
|
||||||
crop_width = int(width * img_width)
|
|
||||||
crop_height = int(height * img_height)
|
|
||||||
|
|
||||||
# Вычисляем координаты прямоугольника для кадрирования
|
|
||||||
x1 = max(0, center_x - crop_width // 2)
|
|
||||||
y1 = max(0, center_y - crop_height // 2)
|
|
||||||
x2 = min(img_width, center_x + crop_width // 2)
|
|
||||||
y2 = min(img_height, center_y + crop_height // 2)
|
|
||||||
|
|
||||||
# Проверяем, что прямоугольник имеет положительные размеры
|
|
||||||
if x2 <= x1 or y2 <= y1:
|
|
||||||
raise ValueError("Некорректные размеры для кадрирования")
|
|
||||||
|
|
||||||
# Кадрируем изображение
|
|
||||||
cropped_image = image[y1:y2, x1:x2]
|
|
||||||
|
|
||||||
# Если нужно вернуть изображение как результат функции:
|
|
||||||
return cropped_image
|
|
||||||
Reference in New Issue
Block a user