def parse_ground_truth(self):
camera_mat = transform_to_matrix(self.sensor_camera.transform)
ego_mat = transform_to_matrix(self.ego_state.transform)
tf_mat = np.dot(np.linalg.inv(ego_mat), np.linalg.inv(camera_mat))
labels = []
for npc, npc_state in zip(self.npcs, self.npcs_state):
npc_tf = self.get_npc_tf_in_cam_space(npc_state.transform, tf_mat)
location = self.get_location(npc_tf)
rotation_y = self.get_rotation_y(npc_tf)
height, width, length = self.get_dimension(npc.bounding_box)
alpha = self.get_alpha(location, rotation_y)
corners_3D = self.get_corners_3D(location, rotation_y,
(height, width, length))
corners_2D = self.project_3D_to_2D(corners_3D)
p_min, p_max = corners_2D[:, 0], corners_2D[:, 0]
for i in range(corners_2D.shape[1]):
p_min = np.minimum(p_min, corners_2D[:, i])
p_max = np.maximum(p_max, corners_2D[:, i])
left, top = p_min
right, bottom = p_max
label = "Car -1 -1 {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f}".format(
alpha, left, top, right, bottom, height, width, length,
location[0], location[1], location[2], rotation_y)
labels.append(label)
return labels
def is_npc_obscured(self, npc_transform):
lidar_mat = np.dot(transform_to_matrix(self.sensor_lidar.transform),
transform_to_matrix(self.ego_state.transform))
start = lgsvl.Vector(
lidar_mat[3][0],
lidar_mat[3][1],
lidar_mat[3][2],
)
end = npc_transform.position
direction = lgsvl.Vector(
end.x - start.x,
end.y - start.y,
end.z - start.z,
)
distance = np.linalg.norm(
np.array([direction.x, direction.y, direction.z]))
layer_mask = 0
for bit in [0]:
layer_mask |= 1 << bit
hit = self.sim.raycast(start, direction, layer_mask, distance)
if hit:
return True
return False
def get_transform(self, parent_tf, child_tf):
tf = np.dot(transform_to_matrix(child_tf),
np.linalg.inv(transform_to_matrix(parent_tf)))
tf = np.array(tf)
tf[:, 3] = tf[3, :]
tf = tf[:3, :]
tf_flatten = tf.flatten()
return tf_flatten
def get_transform(self, parent_tf, child_tf):
"""Get transformation matrix between parent and child"""
tf = np.dot(transform_to_matrix(child_tf), np.linalg.inv(transform_to_matrix(parent_tf)))
tf = np.array(tf)
tf[:, 3] = tf[3, :]
tf = tf[:3, :]
tf_flatten = tf.flatten()
return tf_flatten
def parse_ground_truth(self):
camera_mat = transform_to_matrix(self.sensor_camera.transform)
ego_mat = transform_to_matrix(self.ego_state.transform)
tf_mat = np.dot(np.linalg.inv(ego_mat), np.linalg.inv(camera_mat))
labels = []
for npc, npc_state in zip(self.npcs, self.npcs_state):
# print("a", npc.transform)
# print("b", npc_state.transform)
# npc_tf = self.get_npc_tf_in_cam_space(npc_state.transform, tf_mat)
npc_tf = self.get_npc_tf_in_cam_space(npc.transform, tf_mat)
location = self.get_location(npc_tf)
rotation_y = self.get_rotation_y(npc_tf)
height, width, length = self.get_dimension(npc.bounding_box)
alpha = self.get_alpha(location, rotation_y)
corners_3D = self.get_corners_3D(location, rotation_y,
(height, width, length))
corners_2D = self.project_3D_to_2D(corners_3D)
p_min, p_max = corners_2D[:, 0], corners_2D[:, 0]
for i in range(corners_2D.shape[1]):
p_min = np.minimum(p_min, corners_2D[:, i])
p_max = np.maximum(p_max, corners_2D[:, i])
left, top = p_min
right, bottom = p_max
if npc.name in [
'Sedan', 'SUV', 'Jeep', 'Hatchback', 'BoxTruck', 'SchoolBus'
]:
label = "Car -1 -1 {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f}" \
.format(alpha, left, top, right, bottom, height, width, length, location[0], location[1], location[2], rotation_y)
else:
# if height > 10.0:
# height = 1.70
label = "Pedestrian -1 -1 {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f} {:.2f}" \
.format(alpha, left, top, right, bottom, height, width, length, location[0], location[1], location[2], rotation_y)
print("{}: {}".format(npc.name, height))
labels.append(label)
return labels
def get_npc_tf_in_cam_space(self, npc_transform, tf_mat):
npc_tf = np.dot(transform_to_matrix(npc_transform), tf_mat)
return npc_tf
#!/usr/bin/env python3
#
# Copyright (c) 2019 LG Electronics, Inc.
#
# This software contains code licensed as described in LICENSE.
#
from lgsvl import Transform, Vector
from lgsvl.utils import (
transform_to_matrix,
matrix_inverse,
matrix_multiply,
vector_multiply,
)
# global "world" transform (for example, car position)
world = Transform(Vector(10, 20, 30), Vector(11, 22, 33))
# "local" transform, relative to world transform (for example, Lidar sensor)
local = Transform(Vector(0, 2, -0.5), Vector(0, 0, 0))
# combine them in one transform matrix
mtx = matrix_multiply(transform_to_matrix(local), transform_to_matrix(world))
# compute inverse for transforming points
mtx = matrix_inverse(mtx)
# now transform some point from world position into coordinate system of local transform of Lidar
pt = vector_multiply(Vector(15, 20, 30), mtx)
print(pt)
def get_npc_tf_in_cam_space(self, npc_transform, tf_mat):
"""Converts the world space position of the NPC into the EGO camera space."""
npc_tf = np.dot(transform_to_matrix(npc_transform), tf_mat)
return npc_tf
