230 lines
8.3 KiB
Python
230 lines
8.3 KiB
Python
|
import numpy as np
|
||
|
|
from scipy.spatial import Delaunay
|
||
|
|
import scipy
|
||
|
|
|
||
|
|
|
||
|
|
def cls_type_to_id(cls_type):
|
||
|
|
type_to_id = {'Car': 1, 'Pedestrian': 2, 'Cyclist': 3, 'Van': 4}
|
||
|
|
if cls_type not in type_to_id.keys():
|
||
|
|
return -1
|
||
|
|
return type_to_id[cls_type]
|
||
|
|
|
||
|
|
|
||
|
|
class Object3d(object):
|
||
|
|
def __init__(self, line):
|
||
|
|
label = line.strip().split(' ')
|
||
|
|
self.src = line
|
||
|
|
self.cls_type = label[0]
|
||
|
|
self.cls_id = cls_type_to_id(self.cls_type)
|
||
|
|
self.trucation = float(label[1])
|
||
|
|
self.occlusion = float(label[2]) # 0:fully visible 1:partly occluded 2:largely occluded 3:unknown
|
||
|
|
self.alpha = float(label[3])
|
||
|
|
self.box2d = np.array((float(label[4]), float(label[5]), float(label[6]), float(label[7])), dtype=np.float32)
|
||
|
|
self.h = float(label[8])
|
||
|
|
self.w = float(label[9])
|
||
|
|
self.l = float(label[10])
|
||
|
|
self.pos = np.array((float(label[11]), float(label[12]), float(label[13])), dtype=np.float32)
|
||
|
|
self.dis_to_cam = np.linalg.norm(self.pos)
|
||
|
|
self.ry = float(label[14])
|
||
|
|
self.score = float(label[15]) if label.__len__() == 16 else -1.0
|
||
|
|
self.level_str = None
|
||
|
|
self.level = self.get_obj_level()
|
||
|
|
|
||
|
|
def get_obj_level(self):
|
||
|
|
height = float(self.box2d[3]) - float(self.box2d[1]) + 1
|
||
|
|
|
||
|
|
if height >= 40 and self.trucation <= 0.15 and self.occlusion <= 0:
|
||
|
|
self.level_str = 'Easy'
|
||
|
|
return 1 # Easy
|
||
|
|
elif height >= 25 and self.trucation <= 0.3 and self.occlusion <= 1:
|
||
|
|
self.level_str = 'Moderate'
|
||
|
|
return 2 # Moderate
|
||
|
|
elif height >= 25 and self.trucation <= 0.5 and self.occlusion <= 2:
|
||
|
|
self.level_str = 'Hard'
|
||
|
|
return 3 # Hard
|
||
|
|
else:
|
||
|
|
self.level_str = 'UnKnown'
|
||
|
|
return 4
|
||
|
|
|
||
|
|
def generate_corners3d(self):
|
||
|
|
"""
|
||
|
|
generate corners3d representation for this object
|
||
|
|
:return corners_3d: (8, 3) corners of box3d in camera coord
|
||
|
|
"""
|
||
|
|
l, h, w = self.l, self.h, self.w
|
||
|
|
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]
|
||
|
|
|
||
|
|
R = np.array([[np.cos(self.ry), 0, np.sin(self.ry)],
|
||
|
|
[0, 1, 0],
|
||
|
|
[-np.sin(self.ry), 0, np.cos(self.ry)]])
|
||
|
|
corners3d = np.vstack([x_corners, y_corners, z_corners]) # (3, 8)
|
||
|
|
corners3d = np.dot(R, corners3d).T
|
||
|
|
corners3d = corners3d + self.pos
|
||
|
|
return corners3d
|
||
|
|
|
||
|
|
def to_str(self):
|
||
|
|
print_str = '%s %.3f %.3f %.3f box2d: %s hwl: [%.3f %.3f %.3f] pos: %s ry: %.3f' \
|
||
|
|
% (self.cls_type, self.trucation, self.occlusion, self.alpha, self.box2d, self.h, self.w, self.l,
|
||
|
|
self.pos, self.ry)
|
||
|
|
return print_str
|
||
|
|
|
||
|
|
def to_kitti_format(self):
|
||
|
|
kitti_str = '%s %.2f %d %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f' \
|
||
|
|
% (self.cls_type, self.trucation, int(self.occlusion), self.alpha, self.box2d[0], self.box2d[1],
|
||
|
|
self.box2d[2], self.box2d[3], self.h, self.w, self.l, self.pos[0], self.pos[1], self.pos[2],
|
||
|
|
self.ry)
|
||
|
|
return kitti_str
|
||
|
|
|
||
|
|
|
||
|
|
def get_calib_from_file(calib_file):
|
||
|
|
with open(calib_file) as f:
|
||
|
|
lines = f.readlines()
|
||
|
|
|
||
|
|
obj = lines[2].strip().split(' ')[1:]
|
||
|
|
P2 = np.array(obj, dtype=np.float32)
|
||
|
|
obj = lines[3].strip().split(' ')[1:]
|
||
|
|
P3 = np.array(obj, dtype=np.float32)
|
||
|
|
obj = lines[4].strip().split(' ')[1:]
|
||
|
|
R0 = np.array(obj, dtype=np.float32)
|
||
|
|
obj = lines[5].strip().split(' ')[1:]
|
||
|
|
Tr_velo_to_cam = np.array(obj, dtype=np.float32)
|
||
|
|
|
||
|
|
return {'P2': P2.reshape(3, 4),
|
||
|
|
'P3': P3.reshape(3, 4),
|
||
|
|
'R0': R0.reshape(3, 3),
|
||
|
|
'Tr_velo2cam': Tr_velo_to_cam.reshape(3, 4)}
|
||
|
|
|
||
|
|
|
||
|
|
class Calibration(object):
|
||
|
|
def __init__(self, calib_file):
|
||
|
|
if isinstance(calib_file, str):
|
||
|
|
calib = get_calib_from_file(calib_file)
|
||
|
|
else:
|
||
|
|
calib = calib_file
|
||
|
|
|
||
|
|
self.P2 = calib['P2'] # 3 x 4
|
||
|
|
self.R0 = calib['R0'] # 3 x 3
|
||
|
|
self.V2C = calib['Tr_velo2cam'] # 3 x 4
|
||
|
|
|
||
|
|
def cart_to_hom(self, pts):
|
||
|
|
"""
|
||
|
|
:param pts: (N, 3 or 2)
|
||
|
|
:return pts_hom: (N, 4 or 3)
|
||
|
|
"""
|
||
|
|
pts_hom = np.hstack((pts, np.ones((pts.shape[0], 1), dtype=np.float32)))
|
||
|
|
return pts_hom
|
||
|
|
|
||
|
|
def lidar_to_rect(self, pts_lidar):
|
||
|
|
"""
|
||
|
|
:param pts_lidar: (N, 3)
|
||
|
|
:return pts_rect: (N, 3)
|
||
|
|
"""
|
||
|
|
pts_lidar_hom = self.cart_to_hom(pts_lidar)
|
||
|
|
pts_rect = np.dot(pts_lidar_hom, np.dot(self.V2C.T, self.R0.T))
|
||
|
|
return pts_rect
|
||
|
|
|
||
|
|
def rect_to_img(self, pts_rect):
|
||
|
|
"""
|
||
|
|
:param pts_rect: (N, 3)
|
||
|
|
:return pts_img: (N, 2)
|
||
|
|
"""
|
||
|
|
pts_rect_hom = self.cart_to_hom(pts_rect)
|
||
|
|
pts_2d_hom = np.dot(pts_rect_hom, self.P2.T)
|
||
|
|
pts_img = (pts_2d_hom[:, 0:2].T / pts_rect_hom[:, 2]).T # (N, 2)
|
||
|
|
pts_rect_depth = pts_2d_hom[:, 2] - self.P2.T[3, 2] # depth in rect camera coord
|
||
|
|
return pts_img, pts_rect_depth
|
||
|
|
|
||
|
|
def lidar_to_img(self, pts_lidar):
|
||
|
|
"""
|
||
|
|
:param pts_lidar: (N, 3)
|
||
|
|
:return pts_img: (N, 2)
|
||
|
|
"""
|
||
|
|
pts_rect = self.lidar_to_rect(pts_lidar)
|
||
|
|
pts_img, pts_depth = self.rect_to_img(pts_rect)
|
||
|
|
return pts_img, pts_depth
|
||
|
|
|
||
|
|
|
||
|
|
def get_objects_from_label(label_file):
|
||
|
|
with open(label_file, 'r') as f:
|
||
|
|
lines = f.readlines()
|
||
|
|
objects = [Object3d(line) for line in lines]
|
||
|
|
return objects
|
||
|
|
|
||
|
|
|
||
|
|
def objs_to_boxes3d(obj_list):
|
||
|
|
boxes3d = np.zeros((obj_list.__len__(), 7), dtype=np.float32)
|
||
|
|
for k, obj in enumerate(obj_list):
|
||
|
|
boxes3d[k, 0:3], boxes3d[k, 3], boxes3d[k, 4], boxes3d[k, 5], boxes3d[k, 6] \
|
||
|
|
= obj.pos, obj.h, obj.w, obj.l, obj.ry
|
||
|
|
return boxes3d
|
||
|
|
|
||
|
|
|
||
|
|
def boxes3d_to_corners3d(boxes3d, rotate=True):
|
||
|
|
"""
|
||
|
|
:param boxes3d: (N, 7) [x, y, z, h, w, l, ry]
|
||
|
|
:param rotate:
|
||
|
|
:return: corners3d: (N, 8, 3)
|
||
|
|
"""
|
||
|
|
boxes_num = boxes3d.shape[0]
|
||
|
|
h, w, l = boxes3d[:, 3], boxes3d[:, 4], boxes3d[:, 5]
|
||
|
|
x_corners = np.array([l / 2., l / 2., -l / 2., -l / 2., l / 2., l / 2., -l / 2., -l / 2.], dtype=np.float32).T # (N, 8)
|
||
|
|
z_corners = np.array([w / 2., -w / 2., -w / 2., w / 2., w / 2., -w / 2., -w / 2., w / 2.], dtype=np.float32).T # (N, 8)
|
||
|
|
|
||
|
|
y_corners = np.zeros((boxes_num, 8), dtype=np.float32)
|
||
|
|
y_corners[:, 4:8] = -h.reshape(boxes_num, 1).repeat(4, axis=1) # (N, 8)
|
||
|
|
|
||
|
|
if rotate:
|
||
|
|
ry = boxes3d[:, 6]
|
||
|
|
zeros, ones = np.zeros(ry.size, dtype=np.float32), np.ones(ry.size, dtype=np.float32)
|
||
|
|
rot_list = np.array([[np.cos(ry), zeros, -np.sin(ry)],
|
||
|
|
[zeros, ones, zeros],
|
||
|
|
[np.sin(ry), zeros, np.cos(ry)]]) # (3, 3, N)
|
||
|
|
R_list = np.transpose(rot_list, (2, 0, 1)) # (N, 3, 3)
|
||
|
|
|
||
|
|
temp_corners = np.concatenate((x_corners.reshape(-1, 8, 1), y_corners.reshape(-1, 8, 1),
|
||
|
|
z_corners.reshape(-1, 8, 1)), axis=2) # (N, 8, 3)
|
||
|
|
rotated_corners = np.matmul(temp_corners, R_list) # (N, 8, 3)
|
||
|
|
x_corners, y_corners, z_corners = rotated_corners[:, :, 0], rotated_corners[:, :, 1], rotated_corners[:, :, 2]
|
||
|
|
|
||
|
|
x_loc, y_loc, z_loc = boxes3d[:, 0], boxes3d[:, 1], boxes3d[:, 2]
|
||
|
|
|
||
|
|
x = x_loc.reshape(-1, 1) + x_corners.reshape(-1, 8)
|
||
|
|
y = y_loc.reshape(-1, 1) + y_corners.reshape(-1, 8)
|
||
|
|
z = z_loc.reshape(-1, 1) + z_corners.reshape(-1, 8)
|
||
|
|
|
||
|
|
corners = np.concatenate((x.reshape(-1, 8, 1), y.reshape(-1, 8, 1), z.reshape(-1, 8, 1)), axis=2)
|
||
|
|
|
||
|
|
return corners.astype(np.float32)
|
||
|
|
|
||
|
|
|
||
|
|
def enlarge_box3d(boxes3d, extra_width):
|
||
|
|
"""
|
||
|
|
:param boxes3d: (N, 7) [x, y, z, h, w, l, ry]
|
||
|
|
"""
|
||
|
|
if isinstance(boxes3d, np.ndarray):
|
||
|
|
large_boxes3d = boxes3d.copy()
|
||
|
|
else:
|
||
|
|
large_boxes3d = boxes3d.clone()
|
||
|
|
large_boxes3d[:, 3:6] += extra_width * 2
|
||
|
|
large_boxes3d[:, 1] += extra_width
|
||
|
|
return large_boxes3d
|
||
|
|
|
||
|
|
|
||
|
|
def in_hull(p, hull):
|
||
|
|
"""
|
||
|
|
:param p: (N, K) test points
|
||
|
|
:param hull: (M, K) M corners of a box
|
||
|
|
:return (N) bool
|
||
|
|
"""
|
||
|
|
try:
|
||
|
|
if not isinstance(hull, Delaunay):
|
||
|
|
hull = Delaunay(hull)
|
||
|
|
flag = hull.find_simplex(p) >= 0
|
||
|
|
except scipy.spatial.qhull.QhullError:
|
||
|
|
print('Warning: not a hull %s' % str(hull))
|
||
|
|
flag = np.zeros(p.shape[0], dtype=np.bool)
|
||
|
|
|
||
|
|
return flag
|