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