Skip to content

Utilities Module

camera_models ¤

Camera pydantic-model ¤

data: ndarray pydantic-field ¤

distortion_coefficient: ndarray pydantic-field ¤

fov: int pydantic-field ¤

image_size_x: int pydantic-field ¤

image_size_y: int pydantic-field ¤

intrinsics_matrix: ndarray pydantic-field ¤

transform: Transform pydantic-field ¤

__config__ ¤

Config ¤

__json_encoder__(obj) special staticmethod ¤

calculate_default_intrinsics_matrix(self) ¤

Calculate intrinsics matrix Will set the attribute intrinsic matrix so that re-calculation is not necessary. https://github.com/carla-simulator/carla/issues/56 [ ax, 0, cx, 0, ay, cy, 0 , 0, 1 ]

Returns:

Type Description
ndarray

Intrinsics_matrix

Source code in ROAR/utilities_module/camera_models.py
def calculate_default_intrinsics_matrix(self) -> np.ndarray:
    """
    Calculate intrinsics matrix
    Will set the attribute intrinsic matrix so that re-calculation is not
    necessary.
    https://github.com/carla-simulator/carla/issues/56
    [
            ax, 0, cx,
            0, ay, cy,
            0 , 0, 1
    ]
    Returns:
        Intrinsics_matrix
    """
    intrinsics_matrix = np.identity(3)
    intrinsics_matrix[0, 2] = self.image_size_x / 2.0
    intrinsics_matrix[1, 2] = self.image_size_y / 2.0
    intrinsics_matrix[0, 0] = self.image_size_x / (
        2.0 * np.tan(self.fov * np.pi / 360.0)
    )
    intrinsics_matrix[1, 1] = self.image_size_y / (
        2.0 * np.tan(self.fov * np.pi / 360.0)
    )
    self.intrinsics_matrix = intrinsics_matrix
    return intrinsics_matrix

visualize(self, title='CameraData', duration=1) ¤

Visualize camera data.

Parameters:

Name Type Description Default
title

title of cv2 image

'CameraData'
duration

in milisecond

1

Returns:

Type Description
None

None

Source code in ROAR/utilities_module/camera_models.py
def visualize(self, title="CameraData", duration=1) -> None:
    """
    Visualize camera data.
    Args:
        title: title of cv2 image
        duration: in milisecond

    Returns:
        None
    """
    if self.data is not None:
        cv2.imshow(title, self.data.data)
        cv2.waitKey(duration)

data_structures_models ¤

DepthData pydantic-model ¤

data: ndarray pydantic-field required ¤

Array of size (WIDTH, HEIGHT, 3)

__config__ ¤

Config ¤

__json_encoder__(obj) special staticmethod ¤

IMUData pydantic-model ¤

accelerometer: Vector3D pydantic-field ¤

Linear acceleration in m/s^2

gyroscope: Vector3D pydantic-field ¤

Angular velocity in rad/sec

__config__ ¤

__json_encoder__(obj) special staticmethod ¤

Location pydantic-model ¤

x: float pydantic-field required ¤

Distance in meters from origin to spot on X axis

y: float pydantic-field required ¤

Distance in meters from origin to spot on Y axis

z: float pydantic-field required ¤

Distance in meters from origin to spot on Z axis

__config__ ¤

__add__(self, other) special ¤

Source code in ROAR/utilities_module/data_structures_models.py
def __add__(self, other):
    """"""
    return Location(x=self.x + other.x, y=self.y + other.y, z=self.z + other.z)

__json_encoder__(obj) special staticmethod ¤

__str__(self) special ¤

Return str(self).

Source code in ROAR/utilities_module/data_structures_models.py
def __str__(self):
    return f"x: {self.x:.3}, y: {self.y:.3}, z: {self.z:.3}"

__truediv__(self, scalar) special ¤

Source code in ROAR/utilities_module/data_structures_models.py
def __truediv__(self, scalar):
    return Location(x=self.x / scalar, y=self.y / scalar, z=self.z / scalar)

distance(self, other_location) ¤

Euclidean distance between current location and other location

Source code in ROAR/utilities_module/data_structures_models.py
def distance(self, other_location):
    """Euclidean distance between current location and other location"""
    return distance.euclidean(
        (self.x, self.y, self.z),
        (other_location.x, other_location.y, other_location.z),
    )

from_array(array) staticmethod ¤

Source code in ROAR/utilities_module/data_structures_models.py
@staticmethod
def from_array(array):
    return Location(x=array[0], y=array[1], z=array[2])

to_array(self) ¤

Source code in ROAR/utilities_module/data_structures_models.py
def to_array(self) -> np.array:
    return np.array([self.x, self.y, self.z])

MapEntry pydantic-model ¤

point_a: float pydantic-field required ¤

point_b: float pydantic-field required ¤

__config__ ¤

__json_encoder__(obj) special staticmethod ¤

RGBData pydantic-model ¤

data: ndarray pydantic-field required ¤

Array of size (WIDTH, HEIGHT, 3)

__config__ ¤

Config ¤

__json_encoder__(obj) special staticmethod ¤

Rotation pydantic-model ¤

pitch: float pydantic-field required ¤

Degree around the Y-axis

roll: float pydantic-field required ¤

Degree around the X-axis

yaw: float pydantic-field required ¤

Degree around the Z-axis

__config__ ¤

__add__(self, other) special ¤

Source code in ROAR/utilities_module/data_structures_models.py
def __add__(self, other):
    """"""
    return Rotation(pitch=self.pitch + other.pitch, yaw=self.yaw + other.yaw, roll=self.roll + other.roll)

__json_encoder__(obj) special staticmethod ¤

__mul__(self, scalar) special ¤

Source code in ROAR/utilities_module/data_structures_models.py
def __rmul__(self, scalar):
    return Rotation(pitch=self.pitch * scalar, yaw=self.yaw * scalar, roll=self.roll * scalar)

__rmul__(self, scalar) special ¤

Source code in ROAR/utilities_module/data_structures_models.py
def __rmul__(self, scalar):
    return Rotation(pitch=self.pitch * scalar, yaw=self.yaw * scalar, roll=self.roll * scalar)

__str__(self) special ¤

Return str(self).

Source code in ROAR/utilities_module/data_structures_models.py
def __str__(self):
    return f"Roll: {round(self.roll, 2)}, Pitch: {round(self.pitch, 2)}, Yaw: {round(self.yaw, 2)}"

__truediv__(self, scalar) special ¤

Source code in ROAR/utilities_module/data_structures_models.py
def __truediv__(self, scalar):
    return Rotation(pitch=self.pitch / scalar, yaw=self.yaw / scalar, roll=self.roll / scalar)

from_array(array) staticmethod ¤

Source code in ROAR/utilities_module/data_structures_models.py
@staticmethod
def from_array(array):
    return Rotation(pitch=array[0], yaw=array[1], roll=array[2])

to_array(self) ¤

Source code in ROAR/utilities_module/data_structures_models.py
def to_array(self) -> np.array:
    return np.array([self.pitch, self.yaw, self.roll])

SensorsData pydantic-model ¤

front_depth: DepthData pydantic-field ¤

front_rgb: RGBData pydantic-field ¤

imu_data: IMUData pydantic-field ¤

location: Location pydantic-field ¤

rear_rgb: RGBData pydantic-field ¤

rotation: Rotation pydantic-field ¤

velocity: Vector3D pydantic-field ¤

__config__ ¤

__json_encoder__(obj) special staticmethod ¤

TrackingData pydantic-model ¤

transform: Transform pydantic-field ¤

velocity: Vector3D pydantic-field required ¤

__config__ ¤

__json_encoder__(obj) special staticmethod ¤

Transform pydantic-model ¤

location: Location pydantic-field ¤

rotation: Rotation pydantic-field ¤

__config__ ¤

__add__(self, other) special ¤

Source code in ROAR/utilities_module/data_structures_models.py
def __add__(self, other):
    return Transform.from_array(self.to_array() + other.to_array())

__json_encoder__(obj) special staticmethod ¤

__mul__(self, scalar) special ¤

Source code in ROAR/utilities_module/data_structures_models.py
def __rmul__(self, scalar):
    return Transform.from_array(self.to_array() * scalar)

__rmul__(self, scalar) special ¤

Source code in ROAR/utilities_module/data_structures_models.py
def __rmul__(self, scalar):
    return Transform.from_array(self.to_array() * scalar)

__str__(self) special ¤

Return str(self).

Source code in ROAR/utilities_module/data_structures_models.py
def __str__(self):
    return f"Location: {self.location.__str__()} | Rotation: {self.rotation.__str__()}"

__truediv__(self, scalar) special ¤

Source code in ROAR/utilities_module/data_structures_models.py
def __truediv__(self, scalar):
    return Transform.from_array(self.to_array() / scalar)

from_array(array) staticmethod ¤

Source code in ROAR/utilities_module/data_structures_models.py
@staticmethod
def from_array(array):
    return Transform(location=Location.from_array(array[:3]), rotation=Rotation.from_array(array[3:]))

get_matrix(self) ¤

Calculate extrinsics matrix with respect to parent object http://planning.cs.uiuc.edu/node104.html

Returns:

Type Description
ndarray

Extrinsics matrix

[R, T] [0 1]

Source code in ROAR/utilities_module/data_structures_models.py
def get_matrix(self) -> np.ndarray:
    """
    Calculate extrinsics matrix with respect to parent object
    http://planning.cs.uiuc.edu/node104.html

    Returns:
        Extrinsics matrix

    [R, T]
    [0 1]
    """
    location = self.location
    rotation = self.rotation

    roll, pitch, yaw = rotation.roll, rotation.pitch, rotation.yaw
    rotation_matrix = rotation_matrix_from_euler(roll=roll, pitch=pitch, yaw=yaw)

    matrix = np.identity(4)
    matrix[0, 3] = location.x
    matrix[1, 3] = location.y
    matrix[2, 3] = location.z
    matrix[0:3, 0:3] = rotation_matrix
    return matrix

record(self) ¤

Source code in ROAR/utilities_module/data_structures_models.py
def record(self):
    return f"{self.location.x},{self.location.y},{self.location.z},{self.rotation.roll},{self.rotation.pitch},{self.rotation.yaw}"

to_array(self) ¤

Source code in ROAR/utilities_module/data_structures_models.py
def to_array(self) -> np.ndarray:
    return np.array([self.location.x, self.location.y, self.location.z, self.rotation.roll, self.rotation.pitch,
                     self.rotation.yaw])

Vector3D pydantic-model ¤

x: float pydantic-field ¤

y: float pydantic-field ¤

z: float pydantic-field ¤

__config__ ¤

__json_encoder__(obj) special staticmethod ¤

to_array(self) ¤

Source code in ROAR/utilities_module/data_structures_models.py
def to_array(self):
    return np.array([self.x, self.y, self.z])

ViveTrackerData pydantic-model ¤

location: Location pydantic-field ¤

rotation: Rotation pydantic-field ¤

tracker_name: str pydantic-field ¤

velocity: Vector3D pydantic-field required ¤

__config__ ¤

__json_encoder__(obj) special staticmethod ¤

errors ¤

AgentException ¤

localization_map ¤

module ¤

Module ¤

__init__(self, threaded=False, update_interval=0.5, should_save=False, name='module', **kwargs) special ¤

Source code in ROAR/utilities_module/module.py
def __init__(self, threaded=False, update_interval: float = 0.5,
             should_save: bool = False, name: str = "module", **kwargs):
    self.threaded = threaded
    self.update_interval = update_interval
    self.should_continue_threaded = True
    self.should_save = should_save
    self.saving_dir_path: Path = Path(f"data/output/{name}")
    if should_save and self.saving_dir_path.exists() is False:
        self.saving_dir_path.mkdir(exist_ok=True, parents=True)

run_in_series(self, **kwargs) ¤

This is the none-threaded function. It run in series!

Parameters:

Name Type Description Default
**kwargs {}
Source code in ROAR/utilities_module/module.py
@abstractmethod
def run_in_series(self, **kwargs):
    """
    This is the none-threaded function. It run in series!
    Args:
        **kwargs:

    Returns:

    """
    pass

run_in_threaded(self, **kwargs) ¤

This is the threaded function.

Parameters:

Name Type Description Default
**kwargs {}
Source code in ROAR/utilities_module/module.py
def run_in_threaded(self, **kwargs):
    """
    This is the threaded function.
    Args:
        **kwargs:

    Returns:

    """
    while self.should_continue_threaded:
        self.run_in_series()
        if self.should_save:
            self.save()
        time.sleep(self.update_interval)

save(self, **kwargs) ¤

Source code in ROAR/utilities_module/module.py
@abstractmethod
def save(self, **kwargs):
    pass

shutdown(self) ¤

Source code in ROAR/utilities_module/module.py
def shutdown(self):
    self.should_continue_threaded = False

occupancy_map ¤

OccupancyGridMap ¤

A 2D Occupancy Grid Map representing the world Should be able to handle 1. Transformation of coordinate from world to grid cord 2. Transformation of cord from grid cord to world cord 3. Represent the vehicle size and position, vehicle is represented as 0 Note that this class does NOT remember the vehicle position, in order to visualize vehicle, vehicle position has to be passed in 4. Represent the obstacles' position once its world coordinate is given 5. Represent free space's position, once its world coordinate is given 6. visualize itself, including zoom to a certain area so that not the entire map is visualized 7. The range of values should be bewteen 0 - 1 - 0 = obstacles, 1 = free space 8 Failure tolerant, if I pass in a wrong world coordinate, it will prompt it, but do not fail. Similar with other functions in this class 9. Fixed map size for enhanced performance

__init__(self, absolute_maximum_map_size=10000, map_padding=40, vehicle_width=2, vehicle_height=2, world_coord_resolution=1, occu_prob=0.95, free_prob=0.05, max_points_to_convert=1000, **kwargs) special ¤

Parameters:

Name Type Description Default
absolute_maximum_map_size

Absolute maximum size of the map, will be used to compute a square occupancy map

10000
map_padding int

additional padding intended to add.

40

Note: This method pad to both sides, for example, it create padding to the left of min_x, and to the right of max_x Note: map_padding is for when when visualizing, we have to take a whole block and just in case the route is to the edge of the map, it will not error out

Source code in ROAR/utilities_module/occupancy_map.py
def __init__(self, absolute_maximum_map_size=10000, map_padding: int = 40, vehicle_width=2, vehicle_height=2,
             world_coord_resolution=1, occu_prob: float = 0.95, free_prob: float = 0.05,
             max_points_to_convert: int = 1000, **kwargs):
    """
    Args:
        absolute_maximum_map_size: Absolute maximum size of the map, will be used to compute a square occupancy map
        map_padding: additional padding intended to add.
    Note: This method pad to both sides, for example, it create padding
    to the left of min_x, and to the right of max_x
    Note: map_padding is for when when visualizing, we have to take a whole
     block and just in case the route is to the edge of the map,
     it will not error out
    """
    super().__init__(name="occupancy_map", **kwargs)
    self.logger = logging.getLogger(__name__)
    self._map: Optional[np.ndarray] = None
    self._world_coord_resolution = world_coord_resolution
    self._absolute_maximum_map_size = absolute_maximum_map_size

    self._min_x = -math.floor(self._absolute_maximum_map_size)
    self._min_y = -math.floor(self._absolute_maximum_map_size)

    self._max_x = math.ceil(self._absolute_maximum_map_size)
    self._max_y = math.ceil(self._absolute_maximum_map_size)

    self._map_additiona_padding = map_padding

    self._vehicle_width = vehicle_width
    self._vehicle_height = vehicle_height

    self._initialize_map()
    self._occu_prob = np.log(occu_prob / (1 - occu_prob))
    self._free_prob = 1 - self._occu_prob

    self._max_points_to_convert = max_points_to_convert
    self.curr_obstacle_world_coords = None
    self._curr_obstacle_occu_coords = None
    self._static_obstacles: Optional[np.ndarray] = None

cord_translation_from_world(self, world_cords_xy) ¤

Translate from world coordinate to occupancy coordinate If the given world coord is less than min or greater than maximum, then do not execute the translation, log error message

Parameters:

Name Type Description Default
world_cords_xy ndarray

Numpy array of shape (N, 2) representing [[x, y], [x, y], ...]

required

Returns:

Type Description
ndarray

occupancy grid map coordinate for this world coordinate of shape (N, 2) [ [x, y], [x, y] ]

Source code in ROAR/utilities_module/occupancy_map.py
def cord_translation_from_world(self,
                                world_cords_xy: np.ndarray) -> np.ndarray:
    """
    Translate from world coordinate to occupancy coordinate
    If the given world coord is less than min or greater than maximum,
    then do not execute the translation, log error message
    Args:
        world_cords_xy: Numpy array of shape (N, 2) representing
         [[x, y], [x, y], ...]
    Returns:
        occupancy grid map coordinate for this world coordinate of shape
        (N, 2)
        [
         [x, y],
         [x, y]
        ]
    """
    transformed = np.round(world_cords_xy - [self._min_x, self._min_y]).astype(np.int64)
    return transformed

get_map(self, transform=None, view_size=(10, 10)) ¤

Source code in ROAR/utilities_module/occupancy_map.py
def get_map(self, transform: Optional[Transform] = None,
              view_size: Tuple[int, int] = (10, 10)):
    if transform is None:
        return np.float32(self._map)
    else:
        occu_cord = self.location_to_occu_cord(
            location=transform.location)
        map_to_view = self._map.copy()
        x, y = occu_cord[0]
        map_to_view[
        y - math.floor(self._vehicle_height / 2): y + math.ceil(self._vehicle_height / 2),
        x - math.floor(self._vehicle_width / 2): x + math.ceil(self._vehicle_width / 2)] = 1
        map_to_view = map_to_view[y - view_size[1] // 2: y + 10,
                      x - view_size[0] // 2: x + view_size[0] // 2]
        return map_to_view

load_from_file(self, file_path) ¤

Load a map from file_path.

Expected to be the same size as the map

Parameters:

Name Type Description Default
file_path Path

a npy file that stores the static map

required
Source code in ROAR/utilities_module/occupancy_map.py
def load_from_file(self, file_path: Path):
    """
    Load a map from file_path.

    Expected to be the same size as the map

    Args:
        file_path: a npy file that stores the static map

    Returns:

    """
    m = np.load(file_path.as_posix())
    assert m.shape == self._map.shape, f"Loaded map is of shape [{m.shape}], " \
                                       f"does not match the expected shape [{self._map.shape}]"
    self._map = m
    self._static_obstacles = np.vstack([np.where(self._map == 1)]).T

location_to_occu_cord(self, location) ¤

Source code in ROAR/utilities_module/occupancy_map.py
def location_to_occu_cord(self, location: Location):
    return self.cord_translation_from_world(world_cords_xy=
                                            np.array([[location.x,
                                                       location.z]]) * self._world_coord_resolution)

record(self, map_xs, map_ys) ¤

Source code in ROAR/utilities_module/occupancy_map.py
def record(self, map_xs, map_ys):
    m: np.ndarray = np.zeros(shape=np.shape(self._map))
    m[map_ys, map_xs] = 1

run_in_series(self, **kwargs) ¤

This is the none-threaded function. It run in series!

Parameters:

Name Type Description Default
**kwargs {}
Source code in ROAR/utilities_module/occupancy_map.py
def run_in_series(self, **kwargs):
    if self.curr_obstacle_world_coords is not None:
        self.update(world_coords=self.curr_obstacle_world_coords)

save(self, **kwargs) ¤

Source code in ROAR/utilities_module/occupancy_map.py
def save(self, **kwargs):
    if self._curr_obstacle_occu_coords is not None:
        m = np.zeros(shape=self._map.shape)
        occu_cords_x, occu_cords_y = self._curr_obstacle_occu_coords[:, 0], self._curr_obstacle_occu_coords[:, 1]
        m[occu_cords_y, occu_cords_x] = 1
        sA = sparse.csr_matrix(m)
        # np.save(f"{self.saving_dir_path}/{datetime.now().strftime('%m_%d_%Y_%H_%M_%S')}", m)
        sparse.save_npz(f"{self.saving_dir_path}/{datetime.now().strftime('%m_%d_%Y_%H_%M_%S')}", sA)
        meta_data_fpath = Path(f"{self.saving_dir_path}/meta_data.npy")

        if meta_data_fpath.exists() is False:
            meta_data = np.array([self._min_x, self._min_y, self._max_x, self._max_y, self._map_additiona_padding])
            np.save(meta_data_fpath.as_posix(), meta_data)

update(self, world_coords) ¤

This is an easier to use update_grid_map method that can be directly called by an agent It will update grid map using the update_grid_map_from_world_cord method

Parameters:

Name Type Description Default
world_coords ndarray

N x 3 array of points

required

Returns:

Type Description

None

Source code in ROAR/utilities_module/occupancy_map.py
def update(self, world_coords: np.ndarray):
    """
    This is an easier to use update_grid_map method that can be directly called by an agent
    It will update grid map using the update_grid_map_from_world_cord method
    Args:
        world_coords: N x 3 array of points
    Returns:
        None
    """
    indices_to_select = np.random.choice(np.shape(world_coords)[0], size=min(self._max_points_to_convert,
                                                                             np.shape(world_coords)[0]))
    world_coords = world_coords[indices_to_select]
    world_coords_xy = world_coords[:, [0, 2]] * self._world_coord_resolution
    self._update_grid_map_from_world_cord(world_cords_xy=world_coords_xy)

update_async(self, world_coords) ¤

This is an easier to use update_grid_map method that can be directly called by an agent It will update grid map using the update_grid_map_from_world_cord method

Parameters:

Name Type Description Default
world_coords ndarray

N x 3 array of points

required

Returns:

Type Description

None

Source code in ROAR/utilities_module/occupancy_map.py
def update_async(self, world_coords: np.ndarray):
    """
    This is an easier to use update_grid_map method that can be directly called by an agent
    It will update grid map using the update_grid_map_from_world_cord method
    Args:
        world_coords: N x 3 array of points
    Returns:
        None
    """
    self.curr_obstacle_world_coords = world_coords

visualize(self, transform=None, view_size=(10, 10)) ¤

Else

Visualize an ego centric view, output size constraint to (500,500)

Parameters:

Name Type Description Default
transform Optional[ROAR.utilities_module.data_structures_models.Transform]

vehicle transform

None
view_size Tuple[int, int]

size of the view

(10, 10)
Source code in ROAR/utilities_module/occupancy_map.py
def visualize(self,
              transform: Optional[Transform] = None,
              view_size: Tuple[int, int] = (10, 10)):
    """
    if transform is None:
        Visualize the entire map, output size constraint to (500,500)
    else:
        Visualize an ego centric view, output size constraint to (500,500)
    Args:
        transform: vehicle transform
        view_size: size of the view

    Returns:

    """
    if transform is None:
        cv2.imshow("Occupancy Grid Map", cv2.resize(np.float32(self._map), dsize=(500, 500)))
    else:
        occu_cord = self.location_to_occu_cord(
            location=transform.location)
        map_copy = self._map.copy()
        x, y = occu_cord[0]
        map_copy[
        y - math.floor(self._vehicle_height / 2): y + math.ceil(self._vehicle_height / 2),
        x - math.floor(self._vehicle_width / 2):x + math.ceil(self._vehicle_width / 2)] = 1
        map_to_view = np.float32(map_copy[y - view_size[1] // 2: y + view_size[1] // 2,
                      x - view_size[0] // 2: x + view_size[0] // 2])

        # angle = np.deg2rad(transform.rotation.yaw)
        # rotation_matrix = np.array([[np.cos(angle), -np.sin(angle)],
        #                             [np.sin(angle), np.cos(angle)]])
        # obstacles = np.where(map_to_view == 1)
        # obstacles: np.ndarray = np.vstack([obstacles[0], obstacles[1]]).T
        # obstacles = (obstacles @ rotation_matrix).astype(np.int)  # TODO will rotate it out of screen
        # map_to_view = np.zeros(shape=(1000, 1000))
        # map_to_view[obstacles[:, 0], obstacles[:, 1]] = 1
        cv2.imshow("Occupancy Grid Map", cv2.resize(map_to_view, (500, 500)))

        # cv2.imshow("Occupancy Grid Map", cv2.resize(np.float32(map_to_view), (500, 500)))
    cv2.waitKey(1)

track_visualizer ¤

The purpose of this file is to take in a txt file in containing data x,y,z,roll,pitch,yaw or x,y,z ... and visualize the track

read_txt(file_path) ¤

Source code in ROAR/utilities_module/track_visualizer.py
def read_txt(file_path: Path) -> List[List[float]]:
    if file_path.exists() is False:
        raise FileNotFoundError(f"{file_path} is not found. Please double check")
    file = file_path.open('r')
    result: List[List[float]] = []
    for line in file.readlines():
        try:
            x, y, z = line.split(sep=',')
        except Exception as e:
            x, y, z, roll, pitch, yaw = line.split(sep=',')

        result.append([float(x), float(y), float(z)])
    return result

visualize_track_data(track_data) ¤

Source code in ROAR/utilities_module/track_visualizer.py
def visualize_track_data(track_data: List[List[float]]):
    print(f"Visualizing [{len(track_data)}] data points")
    track_data = np.asarray(track_data)
    fig = go.Figure(data=[go.Scatter3d(x=track_data[:, 0], y=[0] * len(track_data), z=track_data[:, 2],
                                       mode='markers')])
    fig.show()

utilities ¤

img_to_world(scaled_depth_image, intrinsics_matrix, veh_world_matrix, cam_veh_matrix) ¤

Compute image to world translation using the formula below

((R_world_veh)^(-1) @ (R_veh_cam)^(-1) @ ((intrinsics)^(-1) @ scaled_depth_image).pad_with_1)[:3, :] = [X Y Z]

Parameters:

Name Type Description Default
scaled_depth_image ndarray

3 x n numpy array

required
intrinsics_matrix ndarray

3 x 3 intrinsics

required
veh_world_matrix ndarray

4 x 4 vehicle to world transformation matrix

required
cam_veh_matrix ndarray

4 x 4 camera to vehicle transformation matrix

required

Returns:

Type Description
ndarray

n x 3 array of n points

Source code in ROAR/utilities_module/utilities.py
def img_to_world(scaled_depth_image: np.ndarray,
                 intrinsics_matrix: np.ndarray,
                 veh_world_matrix: np.ndarray,
                 cam_veh_matrix: np.ndarray) -> np.ndarray:
    """
    Compute image to world translation using the formula below

    ((R_world_veh)^(-1) @ (R_veh_cam)^(-1) @ ((intrinsics)^(-1) @ scaled_depth_image).pad_with_1)[:3, :] = [X Y Z]
    Args:
        scaled_depth_image: 3 x n numpy array
        intrinsics_matrix: 3 x 3 intrinsics
        veh_world_matrix: 4 x 4 vehicle to world transformation matrix
        cam_veh_matrix: 4 x 4 camera to vehicle transformation matrix

    Returns:
        n x 3 array of n points
    """
    assert scaled_depth_image.shape[0] == 3, f"scaled depth image has incorrect shape [{scaled_depth_image.shape}]"
    assert intrinsics_matrix.shape == (3, 3), f"Intrinsics matrix has incorrect shape [{intrinsics_matrix.shape}]"
    assert veh_world_matrix.shape == (4, 4), f"Intrinsics matrix has incorrect shape [{intrinsics_matrix.shape}]"
    assert cam_veh_matrix.shape == (4, 4), f"Intrinsics matrix has incorrect shape [{intrinsics_matrix.shape}]"
    # extrinsics @ inv(K) @ [u, v,1] = [X,Y,Z]
    k_inv = np.linalg.inv(intrinsics_matrix)
    raw_p3d = k_inv @ scaled_depth_image

    ones = np.ones(shape=np.shape(raw_p3d)[1])
    # raw_p3d_padded = np.vstack([raw_p3d, ones])

    raw_p3d_padded = np.vstack([
        raw_p3d[2, :],
        raw_p3d[0, :],
        -raw_p3d[1, :],
        ones
    ])
    points: np.ndarray = (veh_world_matrix @ cam_veh_matrix @ raw_p3d_padded)[:3, :].T

    return points

img_to_world2(depth_img, intrinsics_matrix, extrinsics_matrix, segmentation, criteria, depth_scaling_factor=1000) ¤

Source code in ROAR/utilities_module/utilities.py
def img_to_world2(depth_img,
                  intrinsics_matrix,
                  extrinsics_matrix,
                  segmentation: np.ndarray,
                  criteria,
                  depth_scaling_factor=1000) -> np.ndarray:
    # get a 2 x N array for their indices

    ground_loc = np.where(segmentation == criteria)[:2]
    # print(ground)
    # ground_loc = np.where(depth_img == criteria)
    depth_val = depth_img[ground_loc] * depth_scaling_factor
    ground_loc = ground_loc * depth_val

    # compute raw_points
    raw_points = np.vstack([ground_loc, depth_val])

    # convert to cords_y_minus_z_x
    cords_y_minus_z_x = np.linalg.inv(intrinsics_matrix) @ raw_points

    # convert to cords_xyz_1
    ones = np.ones((1, np.shape(cords_y_minus_z_x)[1]))

    cords_xyz_1 = np.vstack([
        cords_y_minus_z_x[2, :],
        cords_y_minus_z_x[0, :],
        -cords_y_minus_z_x[1, :],
        ones
    ])

    # multiply by cam_world_matrix
    points = extrinsics_matrix @ cords_xyz_1  # i have all points now
    return points

png_to_depth(im) ¤

Takes in an image read from cv2.imread(), whose output is simply a numpy array, turn it into a depth image according to carla's method of (R + G * 256 + B * 256 * 256) / (256 * 256 * 256 - 1).

Parameters:

Name Type Description Default
im <built-in function array>

input image, read from cv2.imread()

required

Returns:

Type Description
<built-in function array>

depth image

Source code in ROAR/utilities_module/utilities.py
def png_to_depth(im: np.array) -> np.array:
    """
    Takes in an image read from cv2.imread(), whose output is simply a numpy
    array,
    turn it into a depth image according to carla's method of
    (R + G * 256 + B * 256 * 256) / (256 * 256 * 256 - 1).
    Args:
        im: input image, read from cv2.imread()
    Returns:
        depth image
    """
    im = im.astype(np.float64)
    normalized_depth = np.dot(im[:, :, :3], [1, 256, 65536.0])
    normalized_depth /= 16777215.0
    return normalized_depth

rotation_matrix_from_euler(roll, pitch, yaw) ¤

Takes in roll pitch yaw and compute rotation matrix using the order of

R = R_yaw * R_pitch * R_roll

http://planning.cs.uiuc.edu/node104.html

Parameters:

Name Type Description Default
roll float

float of roll in degree

required
pitch float

float of pitch in degree

required
yaw float

float of yaw in degree

required

Returns:

Type Description
ndarray

3 x 3 array rotation matrix

Source code in ROAR/utilities_module/utilities.py
def rotation_matrix_from_euler(roll: float, pitch: float, yaw: float) -> np.ndarray:
    """
    Takes in roll pitch yaw and compute rotation matrix using the order of

    R = R_yaw * R_pitch * R_roll

    http://planning.cs.uiuc.edu/node104.html

    Args:
        roll: float of roll in degree
        pitch: float of pitch in degree
        yaw: float of yaw in degree

    Returns:
        3 x 3 array rotation matrix
    """
    ry, rx, rz = np.radians(yaw), np.radians(pitch), np.radians(roll)
    Rx = np.array([
        [1, 0, 0],
        [0, np.cos(rx), -np.sin(rx)],
        [0, np.sin(rx), np.cos(rx)]
    ])
    Ry = np.array([
        [np.cos(ry), 0, np.sin(ry)],
        [0, 1, 0],
        [-np.sin(ry), 0, np.cos(ry)]
    ])
    Rz = np.array([
        [np.cos(rz), -np.sin(rz), 0],
        [np.sin(rz), np.cos(rz), 0],
        [0, 0, 1]
    ])
    return Rx @ Ry @ Rz

vehicle_models ¤

Vehicle pydantic-model ¤

Encodes the Vehicle's state at the last tick

control: VehicleControl pydantic-field ¤

transform: Transform pydantic-field ¤

velocity: Vector3D pydantic-field ¤

wheel_base: float pydantic-field ¤

Default to tesla model 3's wheel base

__config__ ¤

__json_encoder__(obj) special staticmethod ¤

get_speed(vehicle) staticmethod ¤

Compute speed of a vehicle in Km/h.

:param vehicle: the vehicle for which speed is calculated
:return: speed as a float in Km/h
Source code in ROAR/utilities_module/vehicle_models.py
@staticmethod
def get_speed(vehicle):
    # TODO consider the jetson case
    """
    Compute speed of a vehicle in Km/h.

        :param vehicle: the vehicle for which speed is calculated
        :return: speed as a float in Km/h
    """
    vel = vehicle.velocity
    return 3.6 * math.sqrt(vel.x ** 2 + vel.y ** 2 + vel.z ** 2)

to_array(self) ¤

Source code in ROAR/utilities_module/vehicle_models.py
def to_array(self):
    return np.concatenate([self.velocity.to_array(), self.transform.to_array(), self.control.to_array()])

VehicleControl pydantic-model ¤

steering: float pydantic-field ¤

throttle: float pydantic-field ¤

__config__ ¤

__json_encoder__(obj) special staticmethod ¤

clamp(n, minn, maxn) staticmethod ¤

Source code in ROAR/utilities_module/vehicle_models.py
@staticmethod
def clamp(n, minn, maxn):
    return max(min(maxn, n), minn)

get_steering(self) ¤

Cap it between -1 and 1 :return:

Source code in ROAR/utilities_module/vehicle_models.py
def get_steering(self) -> float:
    """
    Cap it between -1  and 1
    :return:
    """
    return self.clamp(self.steering, -1, 1)

get_throttle(self) ¤

Cap it between -1 and 1 :return:

Source code in ROAR/utilities_module/vehicle_models.py
def get_throttle(self) -> float:
    """
    Cap it between -1  and 1
    :return:
    """
    return self.clamp(self.throttle, -1, 1)

to_array(self) ¤

Source code in ROAR/utilities_module/vehicle_models.py
def to_array(self) -> np.ndarray:
    return np.array([self.throttle, self.steering])