def get_corners(self):
'''
corners.T => (8, 3)
(0:4) -> top
(4:8) -> bottom
axis 2: x
axis 0: y
axis 1: z
(top,bottom)
|
V
(1,5)----(2,6)
| |
| |
(0,4)----(3,7)
'''
if self.__corners is None:
# Compute corners
self.__corners = np.array(
[[
-self.l / 2, self.l / 2, self.l / 2, -self.l / 2,
-self.l / 2, self.l / 2, self.l / 2, -self.l / 2
], [0, 0, 0, 0, -self.h, -self.h, -self.h, -self.h],
[
-self.w / 2, -self.w / 2, self.w / 2, self.w / 2,
-self.w / 2, -self.w / 2, self.w / 2, self.w / 2
]],
dtype=np.float32)
H = np.dot(translation_matrix(self.x, self.y, self.z),
rot_y_matrix(self.yaw))
self.__corners = transform(H, self.__corners)
return self.__corners
def get_arrow_pts(self):
if self.__arrow_pts is None:
# Compute arrow from center pointing forward
self.__arrow_pts = np.array([[0, (self.l / 2) + 1],
[-self.h / 2, -self.h / 2], [0, 0]])
H = np.dot(translation_matrix(self.x, self.y, self.z),
rot_y_matrix(self.yaw))
self.__arrow_pts = transform(H, self.__arrow_pts)
return self.__arrow_pts
def get_corners_3D(box):
corners = np.array([[
-box.l / 2, box.l / 2, box.l / 2, -box.l / 2, -box.l / 2, box.l / 2,
box.l / 2, -box.l / 2
], [0, 0, 0, 0, -box.h, -box.h, -box.h, -box.h],
[
-box.w / 2, -box.w / 2, box.w / 2, box.w / 2,
-box.w / 2, -box.w / 2, box.w / 2, box.w / 2
]],
dtype=np.float32)
H = np.dot(translation_matrix(box.x, box.y, box.z), rot_y_matrix(box.yaw))
return transform(H, corners)
def get_velo(path,
calib_path,
workspace_lim=((-40, 40), (-1, 2.5), (0, 70)),
use_fov_filter=True):
velo = np.fromfile(path, dtype=np.float32).reshape((-1, 4)).T
pts = velo[0:3]
reflectance = velo[3:]
# Transform points from velo coordinates to rectified camera coordinates
V2C, R0, P2 = get_calib(calib_path)
pts = transform(np.dot(R0, V2C), pts)
# Remove points out of workspace
pts, reflectance = box_filter(pts, workspace_lim, decorations=reflectance)
# Remove points not projecting onto the image plane
if use_fov_filter:
pts, reflectance = fov_filter(pts,
P=P2,
img_size=(IMG_HEIGHT, IMG_WIDTH),
decorations=reflectance)
return pts, reflectance