File size: 6,136 Bytes
663494c |
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 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 |
from typing import Type
import numpy as np
import utils.transform as box_transform
import utils.rotation as object_rotation
class Box3D:
def __init__(
self,
x=None,
y=None,
z=None,
h=None,
w=None,
l=None,
pitch=None,
yaw=None,
roll=None,
s=None,
):
self.x = x # center x
self.y = y # center y
self.z = z # center z
self.h = h # height
self.w = w # width
self.l = l # length
self.pitch = pitch # orientation
self.yaw = yaw # orientation
self.roll = roll # orientation
self.s = s # detection score
self.corners_3d_cam = None
def __str__(self):
return "x: {}, y: {}, z: {}, heading: {}, length: {}, width: {}, height: {}, score: {}".format(
self.x, self.y, self.z, self.yaw, self.l, self.w, self.h, self.s
)
@classmethod
def bbox2dict(cls, bbox):
return {
"center_x": bbox.x,
"center_y": bbox.y,
"center_z": bbox.z,
"height": bbox.h,
"width": bbox.w,
"length": bbox.l,
"yaw": bbox.yaw,
}
@classmethod
def bbox2array(cls, bbox):
if bbox.s is None:
return np.array([bbox.x, bbox.y, bbox.z, bbox.yaw, bbox.l, bbox.w, bbox.h])
else:
return np.array(
[bbox.x, bbox.y, bbox.z, bbox.yaw, bbox.l, bbox.w, bbox.h, bbox.s]
)
@classmethod
def bbox2array_raw(cls, bbox):
if bbox.s is None:
return np.array([bbox.h, bbox.w, bbox.l, bbox.x, bbox.y, bbox.z, bbox.yaw])
else:
return np.array(
[bbox.h, bbox.w, bbox.l, bbox.x, bbox.y, bbox.z, bbox.yaw, bbox.s]
)
@classmethod
def array2bbox_xyzlwhyaw(cls, data):
# take the format of data of [x,y,z,l,w,h,yaw]
bbox = Box3D()
bbox.x, bbox.y, bbox.z, bbox.l, bbox.w, bbox.h, bbox.yaw = data[:7]
if len(data) == 8:
bbox.s = data[-1]
return bbox
@classmethod
def array2bbox_raw(cls, data):
# take the format of data of [h,w,l,x,y,z,yaw]
bbox = Box3D()
bbox.h, bbox.w, bbox.l, bbox.x, bbox.y, bbox.z, bbox.yaw = data[:7]
if len(data) == 8:
bbox.s = data[-1]
return bbox
@classmethod
def array2bbox(cls, data):
# take the format of data of [x,y,z,yaw,l,w,h]
bbox = Box3D()
bbox.x, bbox.y, bbox.z, bbox.yaw, bbox.l, bbox.w, bbox.h = data[:7]
if len(data) == 8:
bbox.s = data[-1]
return bbox
@classmethod
def array2bbox_9dof(
cls, location: np.ndarray, rotation: np.ndarray, size: np.ndarray
):
# take the format of data of [h,w,l,x,y,z,yaw]
bbox = Box3D()
bbox.x, bbox.y, bbox.z = location
bbox.pitch, bbox.ry, bbox.roll = rotation
bbox.l, bbox.w, bbox.h = size
# TODO: change to world
bbox.corners_3d_cam: np.ndarray = box_transform.get_box_in_world(
location, rotation, size
) # (3, )
return bbox
@classmethod
def box2corners3d_lidar(cls, bbox):
"""Convert the box to the 8 corners in the lidar coordinate
Note that carla lidar coordinate is
+x -> left
+y -> front
+z -> up
4 -------- 5
/| /|
7 -------- 6 .
| | | |
. 0 -------- 1
|/ |/
3 -------- 2
"""
# canonical box
size = np.array([bbox.l, bbox.w, bbox.h]) # (3, )
box3d: np.ndarray = box_transform.create_bb_points(size) # 8 x 4
# print(box3d)
# zxc
# with rotation
R: np.ndarray = object_rotation.rotz(bbox.yaw) # 3 x 3
# print(R)
box3d = np.dot(R, box3d[:, :3].transpose()) # 3 x 8
# with translation
box3d[0, :] = box3d[0, :] + bbox.x
box3d[1, :] = box3d[1, :] + bbox.y
box3d[2, :] = box3d[2, :] + bbox.z
# homogeneous
box3d = box3d.transpose() # 8 x 3
box3d = np.concatenate((box3d, np.ones((8, 1))), axis=1) # 8 x 4
return box3d
# @classmethod
# def box2corners3d_camcoord(cls, bbox):
# """Takes an object's 3D box with the representation of [x,y,z,theta,l,w,h] and
# convert it to the 8 corners of the 3D box, the box is in the camera coordinate
# with right x, down y, front z
# Returns:
# corners_3d: (8,3) array in in rect camera coord
# box corner order is like follows
# 1 -------- 0 top is bottom because y direction is negative
# /| /|
# 2 -------- 3 .
# | | | |
# . 5 -------- 4
# |/ |/
# 6 -------- 7
# rect/ref camera coord:
# right x, down y, front z
# x -> w, z -> l, y -> h
# """
# # if already computed before, then skip it
# if bbox.corners_3d_cam is not None:
# return bbox.corners_3d_cam
# # compute rotational matrix around yaw axis
# # -1.57 means straight, so there is a rotation here
# R = roty(bbox.ry)
# # 3d bounding box dimensions
# l, w, h = bbox.l, bbox.w, bbox.h
# # 3d bounding box corners
# x_corners = [l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2]
# y_corners = [0, 0, 0, 0, -h, -h, -h, -h]
# z_corners = [w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2]
# # rotate and translate 3d bounding box
# corners_3d = np.dot(R, np.vstack([x_corners, y_corners, z_corners]))
# corners_3d[0, :] = corners_3d[0, :] + bbox.x
# corners_3d[1, :] = corners_3d[1, :] + bbox.y
# corners_3d[2, :] = corners_3d[2, :] + bbox.z
# corners_3d = np.transpose(corners_3d)
# bbox.corners_3d_cam = corners_3d
# return corners_3d
|