fix: reduce position error
This commit is contained in:
109
autopilot.py
109
autopilot.py
@@ -67,15 +67,13 @@ class AutoPilot(Pilot):
|
||||
|
||||
# Инициализация ORB детектора
|
||||
self.orb_detector = cv2.ORB_create(
|
||||
nfeatures=3000,
|
||||
scaleFactor=1.2,
|
||||
nlevels=8,
|
||||
edgeThreshold=15,
|
||||
firstLevel=0,
|
||||
WTA_K=2,
|
||||
nfeatures=300,
|
||||
scaleFactor=1.3,
|
||||
nlevels=4,
|
||||
edgeThreshold=19,
|
||||
patchSize=31,
|
||||
fastThreshold=8,
|
||||
scoreType=cv2.ORB_HARRIS_SCORE
|
||||
fastThreshold=15,
|
||||
scoreType=cv2.ORB_FAST_SCORE # FAST обычно быстрее
|
||||
)
|
||||
|
||||
# Инициализация матчера для сопоставления ключевых точек (kNN + Lowe ratio)
|
||||
@@ -113,7 +111,7 @@ class AutoPilot(Pilot):
|
||||
|
||||
if des1 is None or des2 is None or len(kp1) < 4 or len(kp2) < 4:
|
||||
return None, None, None, None, None
|
||||
|
||||
|
||||
# kNN сопоставление и тест Лоу
|
||||
matches_knn = self.bf_matcher.knnMatch(des1, des2, k=2)
|
||||
good_matches: list[cv2.DMatch] = []
|
||||
@@ -196,7 +194,7 @@ class AutoPilot(Pilot):
|
||||
a21, a22 = H[1, 0], H[1, 1]
|
||||
|
||||
# Смещение (уже отцентрировано)
|
||||
tx, ty = RH[0, 2] * MOVE_KOF, RH[1, 2] * MOVE_KOF
|
||||
tx, ty = -H[0, 2] * MOVE_KOF, -H[1, 2] * MOVE_KOF
|
||||
|
||||
print(" [Pilot] translate:", tx, ty)
|
||||
|
||||
@@ -317,54 +315,58 @@ class AutoPilot(Pilot):
|
||||
kp1, kp2, matches,
|
||||
transformation_info)
|
||||
|
||||
self.vis_manager.update_motion_vectors(
|
||||
current_image,
|
||||
kp1, kp2, matches)
|
||||
|
||||
mask = transformation_info['mask']
|
||||
self.vis_manager.update_motion_gomography(
|
||||
current_image,
|
||||
self.vis_manager.update_motion_vectors(
|
||||
current_image,
|
||||
kp1, kp2,
|
||||
np.array(matches)[mask.ravel().astype(bool)])
|
||||
|
||||
|
||||
# Используем новую визуализацию движения точек по сетке
|
||||
homography_matrix = transformation_info['homography']
|
||||
self.vis_manager.update_homography_grid(
|
||||
current_image,
|
||||
homography_matrix,
|
||||
grid_step=85)
|
||||
|
||||
# Пытаемся найти ориентир на картинке:
|
||||
landmark_image = cv2.imread(Path('chunks') / f'chunk_{self.target_idx}.png', cv2.IMREAD_COLOR_RGB)
|
||||
src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(current_image, landmark_image)
|
||||
# landmark_image = cv2.imread(Path('chunks') / f'chunk_{self.target_idx}.png', cv2.IMREAD_COLOR_RGB)
|
||||
# src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(current_image, landmark_image)
|
||||
|
||||
if src_pts is not None and dst_pts is not None and self.vis_manager:
|
||||
self.vis_manager.update_chunk_matches(current_image, landmark_image, kp1, kp2, matches)
|
||||
# if src_pts is not None and dst_pts is not None and self.vis_manager:
|
||||
# self.vis_manager.update_chunk_matches(current_image, landmark_image, kp1, kp2, matches)
|
||||
|
||||
if src_pts is not None and dst_pts is not None:
|
||||
# Оцениваем матрицу трансформации
|
||||
landmark_transform = self.estimate_transformation_matrix(src_pts, dst_pts)
|
||||
# Если ориентир достоверно найден — скорректируем глобальные координаты и угол
|
||||
if landmark_transform is not None:
|
||||
ok_scale = (self.min_scale <= landmark_transform['scale'] <= self.max_scale)
|
||||
ok_inliers = (landmark_transform.get('inliers', 0) >= self.min_inliers)
|
||||
ratio = landmark_transform.get('inliers_ratio', 0.0)
|
||||
ok_ratio = (ratio >= self.min_inlier_ratio)
|
||||
rmse = landmark_transform.get('rmse', None)
|
||||
ok_rmse = (rmse is not None and rmse <= self.max_reproj_rmse)
|
||||
if ok_scale and ok_inliers and ok_ratio and ok_rmse:
|
||||
print("[PILOT]", rmse, ratio, ok_rmse)
|
||||
# if False:
|
||||
# Считаем абсолютную позу относительно координат ориентира
|
||||
landmark_world_x, landmark_world_y = self.points[self.target_idx]
|
||||
tx, ty = landmark_transform['translation']
|
||||
# Смещение в системе БПЛА: (dx_meters, dy_meters)
|
||||
dx_meters = ty
|
||||
dy_meters = tx
|
||||
# Используем оценённый абсолютный угол из гомографии относительно карты (север-вверх)
|
||||
absolute_angle = landmark_transform['rotation']
|
||||
cos_a = math.cos(absolute_angle)
|
||||
sin_a = math.sin(absolute_angle)
|
||||
# Переход в глобальные координаты карты
|
||||
dx_global = dx_meters * cos_a - dy_meters * sin_a
|
||||
dy_global = dx_meters * sin_a + dy_meters * cos_a
|
||||
self.x = landmark_world_x + dx_global
|
||||
self.y = landmark_world_y + dy_global
|
||||
self.angle = math.atan2(math.sin(absolute_angle), math.cos(absolute_angle))
|
||||
print(f" [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})")
|
||||
# if src_pts is not None and dst_pts is not None:
|
||||
# # Оцениваем матрицу трансформации
|
||||
# landmark_transform = self.estimate_transformation_matrix(src_pts, dst_pts)
|
||||
# # Если ориентир достоверно найден — скорректируем глобальные координаты и угол
|
||||
# if landmark_transform is not None:
|
||||
# ok_scale = (self.min_scale <= landmark_transform['scale'] <= self.max_scale)
|
||||
# ok_inliers = (landmark_transform.get('inliers', 0) >= self.min_inliers)
|
||||
# ratio = landmark_transform.get('inliers_ratio', 0.0)
|
||||
# ok_ratio = (ratio >= self.min_inlier_ratio)
|
||||
# rmse = landmark_transform.get('rmse', None)
|
||||
# ok_rmse = (rmse is not None and rmse <= self.max_reproj_rmse)
|
||||
# if ok_scale and ok_inliers and ok_ratio and ok_rmse:
|
||||
# print("[PILOT]", rmse, ratio, ok_rmse)
|
||||
# # if False:
|
||||
# # Считаем абсолютную позу относительно координат ориентира
|
||||
# landmark_world_x, landmark_world_y = self.points[self.target_idx]
|
||||
# tx, ty = landmark_transform['translation']
|
||||
# # Смещение в системе БПЛА: (dx_meters, dy_meters)
|
||||
# dx_meters = ty
|
||||
# dy_meters = tx
|
||||
# # Используем оценённый абсолютный угол из гомографии относительно карты (север-вверх)
|
||||
# absolute_angle = landmark_transform['rotation']
|
||||
# cos_a = math.cos(absolute_angle)
|
||||
# sin_a = math.sin(absolute_angle)
|
||||
# # Переход в глобальные координаты карты
|
||||
# dx_global = dx_meters * cos_a - dy_meters * sin_a
|
||||
# dy_global = dx_meters * sin_a + dy_meters * cos_a
|
||||
# self.x = landmark_world_x + dx_global
|
||||
# self.y = landmark_world_y + dy_global
|
||||
# self.angle = math.atan2(math.sin(absolute_angle), math.cos(absolute_angle))
|
||||
# print(f" [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})")
|
||||
|
||||
if distance_to_target < 75:
|
||||
self.target_idx += 1
|
||||
@@ -372,6 +374,11 @@ class AutoPilot(Pilot):
|
||||
if self.target_idx == len(self.points):
|
||||
return PilotCommand(stop=True)
|
||||
|
||||
distance_to_target = math.sqrt(
|
||||
(self.points[self.target_idx][0] - self.x) ** 2 +
|
||||
(self.points[self.target_idx][1] - self.y) ** 2
|
||||
)
|
||||
|
||||
# Вычисляем угол к цели
|
||||
target_angle = math.atan2(self.points[self.target_idx][1] - self.y, self.points[self.target_idx][0] - self.x)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user