ref: geolocation -> position
This commit is contained in:
213
geolocation.py
213
geolocation.py
@@ -1,213 +0,0 @@
|
||||
|
||||
import math
|
||||
import numpy as np
|
||||
from typing import Optional
|
||||
|
||||
|
||||
class Geolocation:
|
||||
"""Класс геопозиции с полной ориентацией БПЛА в 3D пространстве"""
|
||||
|
||||
x: float # Координата X (метры)
|
||||
y: float # Координата Y (метры)
|
||||
z: float # Масштаб (высота)
|
||||
yaw: float # Рыскание (rotation around Z-axis)
|
||||
pitch: float # Тангаж (rotation around Y-axis)
|
||||
roll: float # Крен (rotation around X-axis)
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
x: float = 0,
|
||||
y: float = 0,
|
||||
z: float = 1,
|
||||
yaw: float = 0,
|
||||
pitch: float = 0,
|
||||
roll: float = 0
|
||||
):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.z = z
|
||||
self.yaw = yaw
|
||||
self.pitch = pitch
|
||||
self.roll = roll
|
||||
|
||||
def __str__(self) -> str:
|
||||
return (
|
||||
f"Geolocation(x={self.x:.2f}, y={self.y:.2f}, z={self.z:.2f}, "
|
||||
f"yaw={math.degrees(self.yaw):.1f}°, "
|
||||
f"pitch={math.degrees(self.pitch):.1f}°, "
|
||||
f"roll={math.degrees(self.roll):.1f}°)"
|
||||
)
|
||||
|
||||
def __matmul__(self, mat: np.ndarray) -> 'Geolocation':
|
||||
return self.copy().apply(mat)
|
||||
|
||||
def __imatmul__(self, mat: np.ndarray) -> 'Geolocation':
|
||||
return self.apply(mat)
|
||||
|
||||
def apply(self, mat: np.ndarray) -> 'Geolocation':
|
||||
"""
|
||||
Применяет матрицу трансформации для вычисления новой позиции и ориентации.
|
||||
|
||||
Для 2D трансформаций (3x3 матрица) обновляет только x, y, z и yaw.
|
||||
Для 3D трансформаций (4x4 матрица) обновляет все параметры включая pitch и roll.
|
||||
"""
|
||||
if mat.shape == (3, 3):
|
||||
return self._apply_2d(mat)
|
||||
elif mat.shape == (4, 4):
|
||||
return self._apply_3d(mat)
|
||||
else:
|
||||
raise ValueError(f"Unsupported matrix shape: {mat.shape}. Expected (3,3) or (4,4)")
|
||||
|
||||
def _apply_2d(self, mat: np.ndarray) -> 'Geolocation':
|
||||
"""Применяет 2D трансформацию"""
|
||||
tx, ty = -mat[0, 2], -mat[1, 2]
|
||||
rotation = -np.arctan2(mat[1, 0], mat[0, 0])
|
||||
scale = np.hypot(mat[0, 0], mat[0, 1])
|
||||
|
||||
dx_meters = tx
|
||||
dy_meters = ty
|
||||
angle_global = self.yaw + rotation
|
||||
|
||||
cos_angle = math.cos(angle_global)
|
||||
sin_angle = math.sin(angle_global)
|
||||
|
||||
dx_global = dx_meters * cos_angle - dy_meters * sin_angle
|
||||
dy_global = dx_meters * sin_angle + dy_meters * cos_angle
|
||||
|
||||
self.z /= scale if scale != 0 else 1
|
||||
self.x += dx_global * self.z
|
||||
self.y += dy_global * self.z
|
||||
self.yaw = self._normalize_angle(angle_global)
|
||||
|
||||
return self
|
||||
|
||||
def _apply_3d(self, mat: np.ndarray) -> 'Geolocation':
|
||||
"""Применяет 3D трансформацию с извлечением углов Эйлера"""
|
||||
position = np.array([self.x, self.y, 0, 1])
|
||||
new_position = mat @ position
|
||||
|
||||
self.x = new_position[0]
|
||||
self.y = new_position[1]
|
||||
|
||||
rotation_mat = mat[:3, :3]
|
||||
scale = np.linalg.norm(rotation_mat[:, 0])
|
||||
self.z *= scale
|
||||
|
||||
normalized_mat = rotation_mat / scale
|
||||
yaw, pitch, roll = self._extract_euler_angles(normalized_mat)
|
||||
|
||||
self.yaw = self._normalize_angle(self.yaw + yaw)
|
||||
self.pitch = self._normalize_angle(self.pitch + pitch)
|
||||
self.roll = self._normalize_angle(self.roll + roll)
|
||||
|
||||
return self
|
||||
|
||||
@staticmethod
|
||||
def _extract_euler_angles(R: np.ndarray) -> tuple[float, float, float]:
|
||||
"""
|
||||
Извлекает углы Эйлера (yaw, pitch, roll) из матрицы вращения.
|
||||
Использует ZYX convention (yaw-pitch-roll).
|
||||
"""
|
||||
sy = math.sqrt(R[0, 0]**2 + R[1, 0]**2)
|
||||
|
||||
singular = sy < 1e-6
|
||||
|
||||
if not singular:
|
||||
roll = math.atan2(R[2, 1], R[2, 2])
|
||||
pitch = math.atan2(-R[2, 0], sy)
|
||||
yaw = math.atan2(R[1, 0], R[0, 0])
|
||||
else:
|
||||
roll = math.atan2(-R[1, 2], R[1, 1])
|
||||
pitch = math.atan2(-R[2, 0], sy)
|
||||
yaw = 0
|
||||
|
||||
return yaw, pitch, roll
|
||||
|
||||
@staticmethod
|
||||
def _normalize_angle(angle: float) -> float:
|
||||
"""Нормализует угол в диапазон [-π, π]"""
|
||||
return math.atan2(math.sin(angle), math.cos(angle))
|
||||
|
||||
def copy(self) -> 'Geolocation':
|
||||
"""Создает полную копию объекта"""
|
||||
return Geolocation(self.x, self.y, self.z, self.yaw, self.pitch, self.roll)
|
||||
|
||||
def get_rotation_matrix(self) -> np.ndarray:
|
||||
"""
|
||||
Матрица вращения с порядком применения: yaw → pitch → roll
|
||||
Умножение: R = Rx(roll) * Ry(pitch) * Rz(yaw)
|
||||
"""
|
||||
cy, sy = math.cos(self.yaw), math.sin(self.yaw)
|
||||
cp, sp = math.cos(self.pitch), math.sin(self.pitch)
|
||||
cr, sr = math.cos(self.roll), math.sin(self.roll)
|
||||
|
||||
# Порядок умножения: Rx * Ry * Rz (справа налево: yaw→pitch→roll)
|
||||
return np.array([
|
||||
[cy*cp, -sy*cr + cy*sp*sr, sy*sr + cy*sp*cr],
|
||||
[sy*cp, cy*cr + sy*sp*sr, -cy*sr + sy*sp*cr],
|
||||
[-sp, cp*sr, cp*cr]
|
||||
])
|
||||
|
||||
def to_dict(self) -> dict:
|
||||
"""Экспорт в словарь"""
|
||||
return {
|
||||
'x': self.x,
|
||||
'y': self.y,
|
||||
'z': self.z,
|
||||
'yaw': self.yaw,
|
||||
'pitch': self.pitch,
|
||||
'roll': self.roll
|
||||
}
|
||||
|
||||
@classmethod
|
||||
def from_dict(cls, data: dict) -> 'Geolocation':
|
||||
"""Импорт из словаря"""
|
||||
return cls(
|
||||
x=data.get('x', 0),
|
||||
y=data.get('y', 0),
|
||||
z=data.get('z', 1),
|
||||
yaw=data.get('yaw', 0),
|
||||
pitch=data.get('pitch', 0),
|
||||
roll=data.get('roll', 0)
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def transform(g: 'Geolocation', mat: np.ndarray) -> Optional['Geolocation']:
|
||||
"""Статический метод трансформации (создает копию и применяет матрицу)"""
|
||||
if g is None:
|
||||
return None
|
||||
return g.copy().apply(mat)
|
||||
|
||||
|
||||
# Пример использования
|
||||
if __name__ == "__main__":
|
||||
# Создание геопозиции
|
||||
geo = Geolocation(x=100, y=200, z=50, yaw=math.radians(45), pitch=math.radians(10), roll=math.radians(5))
|
||||
print("Исходная позиция:", geo)
|
||||
|
||||
# 2D трансформация (обратная совместимость)
|
||||
mat_2d = np.array([
|
||||
[0.9, -0.1, 5],
|
||||
[0.1, 0.9, 10],
|
||||
[0, 0, 1]
|
||||
])
|
||||
geo_2d = geo @ mat_2d
|
||||
print("\nПосле 2D трансформации:", geo_2d)
|
||||
|
||||
# 3D трансформация
|
||||
angle = math.radians(30)
|
||||
mat_3d = np.array([
|
||||
[math.cos(angle), -math.sin(angle), 0, 15],
|
||||
[math.sin(angle), math.cos(angle), 0, 20],
|
||||
[0, 0, 1, 0],
|
||||
[0, 0, 0, 1]
|
||||
])
|
||||
geo_3d = geo.copy()
|
||||
geo_3d @= mat_3d
|
||||
print("\nПосле 3D трансформации:", geo_3d)
|
||||
|
||||
# Получение матрицы вращения
|
||||
rot_mat = geo.get_rotation_matrix()
|
||||
print("\nМатрица вращения:\n", rot_mat)
|
||||
|
||||
print("\n✓ Код успешно выполнен")
|
||||
132
position.py
Normal file
132
position.py
Normal file
@@ -0,0 +1,132 @@
|
||||
|
||||
import cv2
|
||||
import math
|
||||
import numpy as np
|
||||
from numpy.linalg import inv
|
||||
from typing import Optional
|
||||
import constants
|
||||
from scipy.spatial.transform import Rotation
|
||||
import utility
|
||||
|
||||
class Position:
|
||||
"""Класс позиции с полной ориентацией БПЛА в 3D пространстве"""
|
||||
|
||||
x: float # Координата X
|
||||
y: float # Координата Y
|
||||
z: float # Масштаб
|
||||
yaw: float # Рыскание (rotation around Z-axis)
|
||||
pitch: float # Тангаж (rotation around Y-axis)
|
||||
roll: float # Крен (rotation around X-axis)
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
x: float = 0,
|
||||
y: float = 0,
|
||||
z: float = 1,
|
||||
yaw: float = 0,
|
||||
pitch: float = 0,
|
||||
roll: float = 0
|
||||
):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.z = z
|
||||
self.yaw = yaw
|
||||
self.pitch = pitch
|
||||
self.roll = roll
|
||||
|
||||
def __str__(self) -> str:
|
||||
return (
|
||||
f"Position(x={self.x:.2f}, y={self.y:.2f}, z={self.z:.2f}, "
|
||||
f"yaw={math.degrees(self.yaw):.1f}°, "
|
||||
f"pitch={math.degrees(self.pitch):.1f}°, "
|
||||
f"roll={math.degrees(self.roll):.1f}°)"
|
||||
)
|
||||
|
||||
def get_homography_matrix(self, K: np.ndarray = constants.K, sliding: bool = True) -> np.ndarray:
|
||||
""" Возвращает матрицу гомографии """
|
||||
R = self.get_rotation_matrix()
|
||||
T = self.get_translation_matrix()
|
||||
if not sliding:
|
||||
T[0, 2] = T[1, 2] = 0
|
||||
return K @ R @ T @ np.linalg.inv(K)
|
||||
|
||||
def copy(self) -> 'Position':
|
||||
"""Создает полную копию объекта"""
|
||||
return Position(self.x, self.y, self.z, self.yaw, self.pitch, self.roll)
|
||||
|
||||
def get_translation_matrix(self) -> np.ndarray:
|
||||
return np.array([
|
||||
[1, 0, self.x / constants._K_FOCUS_DISTANCE],
|
||||
[0, 1, self.y / constants._K_FOCUS_DISTANCE],
|
||||
[0, 0, self.z]
|
||||
])
|
||||
|
||||
def get_rotation_matrix(self) -> np.ndarray:
|
||||
"""
|
||||
Матрица вращения с порядком применения: yaw → pitch → roll
|
||||
Умножение: R = Rx(roll) * Ry(pitch) * Rz(yaw)
|
||||
"""
|
||||
cy, sy = math.cos(self.yaw), math.sin(self.yaw)
|
||||
cp, sp = math.cos(self.pitch), math.sin(self.pitch)
|
||||
cr, sr = math.cos(self.roll), math.sin(self.roll)
|
||||
|
||||
Rz = np.array([
|
||||
[cy, -sy, 0],
|
||||
[sy, cy, 0],
|
||||
[0, 0, 1],
|
||||
])
|
||||
|
||||
Ry = np.array([
|
||||
[cp, 0, sp],
|
||||
[0, 1, 0],
|
||||
[-sp, 0, cp],
|
||||
])
|
||||
|
||||
Rx = np.array([
|
||||
[1, 0, 0],
|
||||
[0, cr, -sr],
|
||||
[0, sr, cr],
|
||||
])
|
||||
|
||||
return Rx @ Ry @ Rz
|
||||
|
||||
def iapply(self, homography_matrix: np.ndarray, K = constants.K) -> 'Position':
|
||||
"""Применяет матрицу трансформации для вычисления новой позиции и ориентации."""
|
||||
|
||||
np.set_printoptions(suppress=True)
|
||||
H = homography_matrix @ self.get_homography_matrix(sliding=False)
|
||||
|
||||
# Decompose homography
|
||||
_, R, t, _ = cv2.decomposeHomographyMat(H, K)
|
||||
R = np.array(R)
|
||||
t = np.array(t)
|
||||
|
||||
T = inv(R) @ inv(K) @ H @ K
|
||||
ind = np.array([A[2][0] ** 2 + A[2][1] ** 2 for A in T])
|
||||
top_k = max(1, len(T) // 2)
|
||||
if (len(T) == 3): raise "len(T) == 3"
|
||||
ind = np.argpartition(ind, top_k - 1)[:top_k]
|
||||
T = T[ind[0]]
|
||||
T = T @ np.array([0, 0, 1]) / np.mean((T[0][0], T[1][1]))
|
||||
T[2] -= 1
|
||||
R = R[ind]
|
||||
t = t[ind]
|
||||
|
||||
best_id = ((t - T) ** 2).sum((1, 2)).argmin()
|
||||
|
||||
R = R[best_id]
|
||||
rot = Rotation.from_matrix(R).as_euler('XYZ').flatten()
|
||||
self.roll = rot[0]
|
||||
self.pitch = rot[1]
|
||||
self.yaw = rot[2]
|
||||
|
||||
t = t[best_id].flatten()
|
||||
self.x += -T[0] * constants._K_FOCUS_DISTANCE * self.z
|
||||
self.y += T[1] * constants._K_FOCUS_DISTANCE * self.z
|
||||
self.z = 1 + T[2]
|
||||
|
||||
def apply(self, homography_matrix: np.ndarray, K = constants.K) -> 'Position':
|
||||
"""Применяет матрицу трансформации для вычисления новой позиции и ориентации."""
|
||||
pos = self.copy()
|
||||
pos.iapply(homography_matrix, K)
|
||||
return pos
|
||||
Reference in New Issue
Block a user