-
Notifications
You must be signed in to change notification settings - Fork 0
/
client_bounding_boxes.py
161 lines (128 loc) · 5.44 KB
/
client_bounding_boxes.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
#Append path of carla egg file to sys
import os
import sys
import settings
sys.path.append(settings.CARLA_EGG_PATH)
#Imports
import carla
import numpy as np
import time
import pandas as pd
vehicle_list_before = []
vehicle_list_after = []
vehicle_list_temp = []
# ==============================================================================
# -- ClientSideBoundingBoxes ---------------------------------------------------
# ==============================================================================
class ClientSideBoundingBoxes(object):
"""
This is a module responsible for creating 3D bounding boxes and drawing them
client-side on pygame surface.
"""
@staticmethod
def get_bounding_boxes(vehicles, camera):
df_1 = pd.DataFrame(columns=['vehiclesandwalkers', 'boundingboxes'])
"""
Creates 3D bounding boxes based on carla vehicle list and camera.
"""
bounding_boxes = [ClientSideBoundingBoxes.get_bounding_box(vehicle, camera) for vehicle in vehicles]
df_1.vehiclesandwalkers = [u for u in vehicles]
df_1.boundingboxes = [u for u in bounding_boxes]
timestamp = round(time.time() * 1000.0)
# filter objects behind camera
bounding_boxes = [bb for bb in bounding_boxes if all(bb[:, 2] > 0)]
bounding_boxes = [bb for bb in bounding_boxes]
timestamp = round(time.time() * 1000.0)
vehicles_dataframe_before = pd.DataFrame(vehicle_list_before)
vehicles_dataframe_after = pd.DataFrame(vehicle_list_after)
vehicle_list_before.clear()
vehicle_list_after.clear()
return bounding_boxes, df_1
@staticmethod
def get_bounding_box(vehicle, camera):
"""
Returns 3D bounding box for a vehicle based on camera view.
"""
vehicle_list_before.append(vehicle)
bb_cords = ClientSideBoundingBoxes._create_bb_points(vehicle)
cords_x_y_z = ClientSideBoundingBoxes._vehicle_to_sensor(bb_cords, vehicle, camera)[:3, :]
cords_y_minus_z_x = np.concatenate([cords_x_y_z[1, :], -cords_x_y_z[2, :], cords_x_y_z[0, :]])
bbox = np.transpose(np.dot(camera.calibration, cords_y_minus_z_x))
camera_bbox = np.concatenate([bbox[:, 0] / bbox[:, 2], bbox[:, 1] / bbox[:, 2], bbox[:, 2]], axis=1)
vehicle_list_after.append(vehicle)
if camera_bbox.any():
pass
else:
vehicle_list_temp.append(vehicle)
return camera_bbox
@staticmethod
def _create_bb_points(vehicle):
"""
Returns 3D bounding box for a vehicle.
"""
cords = np.zeros((8, 4))
extent = vehicle.bounding_box.extent
cords[0, :] = np.array([extent.x, extent.y, -extent.z, 1])
cords[1, :] = np.array([-extent.x, extent.y, -extent.z, 1])
cords[2, :] = np.array([-extent.x, -extent.y, -extent.z, 1])
cords[3, :] = np.array([extent.x, -extent.y, -extent.z, 1])
cords[4, :] = np.array([extent.x, extent.y, extent.z, 1])
cords[5, :] = np.array([-extent.x, extent.y, extent.z, 1])
cords[6, :] = np.array([-extent.x, -extent.y, extent.z, 1])
cords[7, :] = np.array([extent.x, -extent.y, extent.z, 1])
return cords
@staticmethod
def _vehicle_to_sensor(cords, vehicle, sensor):
"""
Transforms coordinates of a vehicle bounding box to sensor.
"""
world_cord = ClientSideBoundingBoxes._vehicle_to_world(cords, vehicle)
sensor_cord = ClientSideBoundingBoxes._world_to_sensor(world_cord, sensor)
return sensor_cord
@staticmethod
def _vehicle_to_world(cords, vehicle):
"""
Transforms coordinates of a vehicle bounding box to world.
"""
bb_transform = carla.Transform(vehicle.bounding_box.location)
bb_vehicle_matrix = ClientSideBoundingBoxes.get_matrix(bb_transform)
vehicle_world_matrix = ClientSideBoundingBoxes.get_matrix(vehicle.get_transform())
bb_world_matrix = np.dot(vehicle_world_matrix, bb_vehicle_matrix)
world_cords = np.dot(bb_world_matrix, np.transpose(cords))
return world_cords
@staticmethod
def _world_to_sensor(cords, sensor):
"""
Transforms world coordinates to sensor.
"""
sensor_world_matrix = ClientSideBoundingBoxes.get_matrix(sensor.get_transform())
world_sensor_matrix = np.linalg.inv(sensor_world_matrix)
sensor_cords = np.dot(world_sensor_matrix, cords)
return sensor_cords
@staticmethod
def get_matrix(transform):
"""
Creates matrix from carla transform.
"""
rotation = transform.rotation
location = transform.location
c_y = np.cos(np.radians(rotation.yaw))
s_y = np.sin(np.radians(rotation.yaw))
c_r = np.cos(np.radians(rotation.roll))
s_r = np.sin(np.radians(rotation.roll))
c_p = np.cos(np.radians(rotation.pitch))
s_p = np.sin(np.radians(rotation.pitch))
matrix = np.matrix(np.identity(4))
matrix[0, 3] = location.x
matrix[1, 3] = location.y
matrix[2, 3] = location.z
matrix[0, 0] = c_p * c_y
matrix[0, 1] = c_y * s_p * s_r - s_y * c_r
matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r
matrix[1, 0] = s_y * c_p
matrix[1, 1] = s_y * s_p * s_r + c_y * c_r
matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r
matrix[2, 0] = s_p
matrix[2, 1] = -c_p * s_r
matrix[2, 2] = c_p * c_r
return matrix