import math
from dataclasses import dataclass, field
from functools import partial
from typing import Dict
import numpy as np
import yaml
from simulation.utils.basics.init_options import InitOptions
from simulation.utils.geometry import Vector
[docs]@dataclass
class CameraSpecs(InitOptions):
"""Camera specifications usually defined in kitcar-ros.
With this class, the real calibration is used to create the simulated camera model.
This way the camera specifications and calibration are very similar.
The calibrations are still not exactly the same
and therefore a separate calibration file is loaded!
There are, however, some nuances:
* Instead of considering the complete calibration matrix,
only the *pitch* angle is used to rotate the camera model
around the y axis in simulation.
* The real camera crops the image before publishing the image. This is reflected in
the different :py:attr:`capture_size` and :py:attr:`output_size`.
"""
focal_length_x: float
focal_length_y: float
optical_center_x: float
optical_center_y: float
r11: float
r12: float
r13: float
r21: float
r22: float
r23: float
r31: float
r32: float
r33: float
t1: float
t2: float
t3: float
capture_size: Dict[str, float] = field(
default_factory=partial(dict, {"width": 1280, "height": 1024})
)
capture_format: str = "R8G8B8"
clip: Dict[str, float] = field(default_factory=partial(dict, {"near": 0.1, "far": 4}))
output_size: Dict[str, float] = field(
default_factory=partial(dict, {"width": 1280, "height": 650})
)
update_rate: float = 60
image_topic: str = "/simulation/sensors/raw/camera"
info_topic: str = "/simulation/sensors/camera/info"
@property
def R(self) -> np.matrix:
"""np.matrix: Rotation matrix of the camera."""
return np.matrix(
[
[self.r11, self.r12, self.r13],
[self.r21, self.r22, self.r23],
[self.r31, self.r32, self.r33],
]
)
@property
def t(self) -> np.matrix:
"""np.ndarray: Translation vector from vehicle to camera.
In camera coordinates.
"""
return np.matrix([self.t1, self.t2, self.t3]).T
@property
def M(self) -> np.matrix:
"""np.matrix: Matrix M.
Transformation matrix from vehicle coordinates to camera coordinates.
"""
t = 0.001 * self.t
M = np.zeros((3, 4))
M[0:3, 0:3] = self.R
M[:, 3:4] = t
return M
@property
def A(self) -> np.matrix:
"""np.matrix: Matrix A.
Transformation matrix from camera coordinates to pixels.
"""
return np.matrix(
[
[self.focal_length_x, 0, self.optical_center_x],
[0, self.focal_length_y, self.optical_center_y],
[0, 0, 1],
]
)
@property
def P(self) -> np.matrix:
"""np.matrix: Matrix P.
Transformation matrix from vehicle coordinates to pixels.
"""
return self.A @ self.M
@property
def H(self) -> np.matrix:
"""np.matrix: Matrix H.
Transformation matrix from ground vehicle coordinates to pixels.
"""
# self.A * self.M (but without 3rd column)
return self.A @ np.concatenate([self.M[:, :2], self.M[:, 3:]], axis=1)
@property
def t_vehicle(self) -> np.ndarray:
"""np.ndarray: Translation vector to the camera in vehicle coordinates."""
return np.linalg.inv(self.R) @ np.array([self.t1, self.t2, self.t3])
@property
def xyz(self) -> Vector:
"""Vector: Translation vector to the camera in vehicle coordinates."""
t = (
-1 / 1000 * np.linalg.inv(self.R) @ np.array([self.t1, self.t2, self.t3])
).tolist()[0]
return Vector(*t)
@property
def rpy(self) -> Vector:
"""Vector: Vector of roll, pitch, yaw angles of the camera's pose."""
return Vector(0, math.asin(-self.r21), 0)
@property
def horizontal_fov(self) -> float:
return 2 * math.atan(self.capture_size["width"] / 2 / self.focal_length_x)
[docs] def simplify(self):
"""Create a simplified calibration from the real one."""
# Camera is only rotated around the y axis
s = math.sin(self.rpy.y)
c = math.cos(self.rpy.y)
self.r11, self.r12, self.r13 = 0, -1, 0
self.r21, self.r22, self.r23 = -s, 0, -c
self.r31, self.r32, self.r33 = c, 0, -s
self.optical_center_x = self.output_size["width"] - self.capture_size["width"] / 2
self.optical_center_y = self.output_size["height"] - self.capture_size["height"] / 2
# By creating a camera with horizontal_fov,
# the focal_length_x is implicitly used for y as well!
self.focal_length_y = self.focal_length_x
[docs] def save(self, file_path: str, simplify=True, remove_clip=True, remove_sizes=True):
"""Save to file.
Args:
file_path: Path to file.
simplify: Simplify the car specs before saving.
remove_clip: Don't save clip.
remove_sizes: Don't save capture and output_size.
"""
if simplify:
self.simplify()
data = dict(self.__dict__)
if remove_clip:
del data["clip"]
if remove_sizes:
del data["capture_size"]
del data["output_size"]
with open(file_path, "w+") as file:
yaml.dump(data, file, Dumper=yaml.SafeDumper, default_flow_style=False)