|
|
|
|
|
|
|
from typing import Union |
|
|
|
import numpy as np |
|
import torch |
|
|
|
from .. import logger |
|
from .geo_opensfm import TopocentricConverter |
|
|
|
|
|
class BoundaryBox: |
|
def __init__(self, min_: np.ndarray, max_: np.ndarray): |
|
self.min_ = np.asarray(min_) |
|
self.max_ = np.asarray(max_) |
|
assert np.all(self.min_ <= self.max_) |
|
|
|
@classmethod |
|
def from_string(cls, string: str): |
|
return cls(*np.split(np.array(string.split(","), float), 2)) |
|
|
|
@property |
|
def left_top(self): |
|
return np.stack([self.min_[..., 0], self.max_[..., 1]], -1) |
|
|
|
@property |
|
def right_bottom(self) -> (np.ndarray, np.ndarray): |
|
return np.stack([self.max_[..., 0], self.min_[..., 1]], -1) |
|
|
|
@property |
|
def center(self) -> np.ndarray: |
|
return (self.min_ + self.max_) / 2 |
|
|
|
@property |
|
def size(self) -> np.ndarray: |
|
return self.max_ - self.min_ |
|
|
|
def translate(self, t: float): |
|
return self.__class__(self.min_ + t, self.max_ + t) |
|
|
|
def contains(self, xy: Union[np.ndarray, "BoundaryBox"]): |
|
if isinstance(xy, self.__class__): |
|
return self.contains(xy.min_) and self.contains(xy.max_) |
|
return np.all((xy >= self.min_) & (xy <= self.max_), -1) |
|
|
|
def normalize(self, xy): |
|
min_, max_ = self.min_, self.max_ |
|
if isinstance(xy, torch.Tensor): |
|
min_ = torch.from_numpy(min_).to(xy) |
|
max_ = torch.from_numpy(max_).to(xy) |
|
return (xy - min_) / (max_ - min_) |
|
|
|
def unnormalize(self, xy): |
|
min_, max_ = self.min_, self.max_ |
|
if isinstance(xy, torch.Tensor): |
|
min_ = torch.from_numpy(min_).to(xy) |
|
max_ = torch.from_numpy(max_).to(xy) |
|
return xy * (max_ - min_) + min_ |
|
|
|
def format(self) -> str: |
|
return ",".join(np.r_[self.min_, self.max_].astype(str)) |
|
|
|
def __add__(self, x): |
|
if isinstance(x, (int, float)): |
|
return self.__class__(self.min_ - x, self.max_ + x) |
|
else: |
|
raise TypeError(f"Cannot add {self.__class__.__name__} to {type(x)}.") |
|
|
|
def __and__(self, other): |
|
return self.__class__( |
|
np.maximum(self.min_, other.min_), np.minimum(self.max_, other.max_) |
|
) |
|
|
|
def __repr__(self): |
|
return self.format() |
|
|
|
|
|
class Projection: |
|
def __init__(self, lat, lon, alt=0, max_extent=25e3): |
|
|
|
|
|
self.latlonalt = (lat, lon, alt) |
|
self.converter = TopocentricConverter(lat, lon, alt) |
|
min_ = self.converter.to_lla(*(-max_extent,) * 2, 0)[:2] |
|
max_ = self.converter.to_lla(*(max_extent,) * 2, 0)[:2] |
|
self.bounds = BoundaryBox(min_, max_) |
|
|
|
@classmethod |
|
def from_points(cls, all_latlon): |
|
assert all_latlon.shape[-1] == 2 |
|
all_latlon = all_latlon.reshape(-1, 2) |
|
latlon_mid = (all_latlon.min(0) + all_latlon.max(0)) / 2 |
|
return cls(*latlon_mid) |
|
|
|
def check_bbox(self, bbox: BoundaryBox): |
|
if self.bounds is not None and not self.bounds.contains(bbox): |
|
raise ValueError( |
|
f"Bbox {bbox.format()} is not contained in " |
|
f"projection with bounds {self.bounds.format()}." |
|
) |
|
|
|
def project(self, geo, return_z=False): |
|
if isinstance(geo, BoundaryBox): |
|
return BoundaryBox(*self.project(np.stack([geo.min_, geo.max_]))) |
|
geo = np.asarray(geo) |
|
assert geo.shape[-1] in (2, 3) |
|
if self.bounds is not None: |
|
if not np.all(self.bounds.contains(geo[..., :2])): |
|
raise ValueError( |
|
f"Points {geo} are out of the valid bounds " |
|
f"{self.bounds.format()}." |
|
) |
|
lat, lon = geo[..., 0], geo[..., 1] |
|
if geo.shape[-1] == 3: |
|
alt = geo[..., -1] |
|
else: |
|
alt = np.zeros_like(lat) |
|
x, y, z = self.converter.to_topocentric(lat, lon, alt) |
|
return np.stack([x, y] + ([z] if return_z else []), -1) |
|
|
|
def unproject(self, xy, return_z=False): |
|
if isinstance(xy, BoundaryBox): |
|
return BoundaryBox(*self.unproject(np.stack([xy.min_, xy.max_]))) |
|
xy = np.asarray(xy) |
|
x, y = xy[..., 0], xy[..., 1] |
|
if xy.shape[-1] == 3: |
|
z = xy[..., -1] |
|
else: |
|
z = np.zeros_like(x) |
|
lat, lon, alt = self.converter.to_lla(x, y, z) |
|
return np.stack([lat, lon] + ([alt] if return_z else []), -1) |
