def get_bbox3dcorners(self):
'''
get the 8 corners of the bbox3d in camera frame.
1.--.2
| |
| |
4.--.3 (bottom)
5.--.6
| |
| |
8.--.7 (top)
Camera Frame:
^z
|
y (x)----->x
'''
# lwh <-> xzy
l, w, h = self.l, self.w, self.h
x, z, y = self.x, self.z, self.y
bottom = np.array([
[-l/2, 0, w/2],
[l/2, 0, w/2],
[l/2, 0, -w/2],
[-l/2, 0, -w/2],
])
bottom = utils.apply_R(bottom, utils.roty(self.ry))
bottom = utils.apply_tr(bottom, np.array([x, y, z]).reshape(-1, 3))
top = utils.apply_tr(bottom, np.array([0, -h, 0]))
return np.vstack([bottom, top])
def get_bbox3dcorners(self):
'''
get the 8 corners of the bbox3d in imu frame.
1.--.2
| |
| |
4.--.3 (bottom)
5.--.6
| |
| |
8.--.7 (top)
IMU Frame:
^x
|
y<----.z
'''
# lwh <-> yxz (imu) # lwh <-> xyz
# (origin comment) # (new comment)
l, w, h = self.l, self.w, self.h
x, z, y = self.x, self.z, self.y
bottom = np.array([
[-l/2, w/2, 0],
[ l/2, w/2, 0],
[ l/2, -w/2, 0],
[-l/2, -w/2, 0],
])
bottom = utils.apply_R(bottom, utils.rotz(self.ry))
bottom = utils.apply_tr(bottom, np.array([x, y, z]).reshape(-1, 3))
top = utils.apply_tr(bottom, np.array([0, 0, h]))
return np.vstack([bottom, top])
def _apply_tr(self, obj, objpc, calib, tr):
# Note: After apply tr, the obj.bbox2d will not coincide with obj.bbox3d
bcenter_Fcam = np.array([obj.x, obj.y, obj.z]).reshape(1, -1)
bcenter_Flidar = calib.leftcam2lidar(bcenter_Fcam)
bcenter_Flidar = apply_tr(bcenter_Flidar, np.array(tr))
bcenter_Fcam = calib.lidar2leftcam(bcenter_Flidar)
obj.x, obj.y, obj.z = bcenter_Fcam.flatten()
objpc[:, :3] = apply_tr(objpc[:, :3], np.array(tr))
def _check_valid(self, obj, objpc, calib, tr):
bcenter_Fcam = np.array([obj.x, obj.y, obj.z]).reshape(1, -1)
bcenter_Flidar = calib.leftcam2lidar(bcenter_Fcam)
bcenter_Flidar = apply_tr(bcenter_Flidar, np.array(tr))
mean_Flidar = apply_tr(objpc[:, :3].mean(axis=0).reshape(1, -1),
np.array(tr))
dis = np.linalg.norm(mean_Flidar[:2] - bcenter_Flidar[:2], ord=2)
x, y = bcenter_Flidar[0, :2]
return dis < 5 and (y < x and -y < x)
def _apply_dry(self, obj, objpc, calib, dry):
if dry == 0:
return
bottom_Fcam = np.array([obj.x, obj.y, obj.z]).reshape(1, -1)
bottom_Flidar = calib.leftcam2lidar(bottom_Fcam)
objpc[:, :3] = apply_tr(objpc[:, :3], -bottom_Flidar)
# obj.ry += dry is correspond to rotz(-dry)
# since obj is in cam frame
# pc_ is in LiDAR frame
objpc[:, :3] = apply_R(objpc[:, :3], rotz(-dry))
objpc[:, :3] = apply_tr(objpc[:, :3], bottom_Flidar)
# modify obj
obj.ry += dry
def get_corner_box_2drot_np(boxes):
'''
get corners of 2d rotated boxes.
@boxes: np.ndarray (M, 5)
[[x, y, l, w, theta]...] (x, y) is the center coordinate;
l and w are the scales along x- and y- axes.
theta is the rotation angle along the z-axis (counter-clockwise).
2.--.3
| | ^x
| | |
1.--.4 (bottom) y<----.z
-> corners: np.ndarray (M, 4, 2)
Note: 35 ms for 100 times (30 boxes)
'''
M = boxes.shape[0]
l = boxes[:, 2:3]
w = boxes[:, 3:4]
theta = boxes[:, 4:5]
p1 = np.hstack((-l / 2.0, w / 2.0, np.zeros_like(l)))
p2 = np.hstack((l / 2.0, w / 2.0, np.zeros_like(l)))
p3 = np.hstack((l / 2.0, -w / 2.0, np.zeros_like(l)))
p4 = np.hstack((-l / 2.0, -w / 2.0, np.zeros_like(l)))
pts = np.stack((p1, p2, p3, p4), axis=0)
pts = np.transpose(pts, (1, 0, 2))
tr_vecs = boxes[:, :2]
tr_vecs = np.hstack((tr_vecs, np.zeros((M, 1))))
for i, (tr_, ry_) in enumerate(zip(tr_vecs, theta)):
pts[i] = apply_R(pts[i], rotz(ry_))
pts[i] = apply_tr(pts[i], tr_)
return pts[:, :, :2]
def get_pts_idx(self, pc, calib=None):
'''
get points from pc
inputs:
pc: (np.array) [#pts, 3]
in imu frame
'''
bottom_Fimu = np.array([self.x, self.y, self.z]).reshape(1, 3)
center_Fimu = bottom_Fimu + np.array([0, 0, self.h/2.0]).reshape(1, 3)
pc_ = utils.apply_tr(pc, -center_Fimu)
pc_ = utils.apply_R(pc_, utils.rotz(-self.ry))
idx_x = np.logical_and(pc_[:, 0] <= self.l/2.0, pc_[:, 0] >= -self.l/2.0)
idx_y = np.logical_and(pc_[:, 1] <= self.w/2.0, pc_[:, 1] >= -self.w/2.0)
idx_z = np.logical_and(pc_[:, 2] <= self.h/2.0, pc_[:, 2] >= -self.h/2.0)
idx = np.logical_and(idx_x, np.logical_and(idx_y, idx_z))
return idx
def get_pts_idx(self, pc, calib):
'''
get points from pc
inputs:
pc: (np.array) [#pts, 3]
point cloud in velodyne frame
calib (KittiCalib)
'''
bottom_Fcam = np.array([self.x, self.y, self.z]).reshape(1, 3)
center_Fcam = bottom_Fcam + np.array([0, -self.h/2.0, 0]).reshape(1, 3)
center_Flidar = calib.leftcam2lidar(center_Fcam)
pc_ = utils.apply_tr(pc, -center_Flidar)
pc_ = utils.apply_R(pc_, utils.rotz(self.ry+np.pi/2))
idx_x = np.logical_and(pc_[:, 0] <= self.l/2.0, pc_[:, 0] >= -self.l/2.0)
idx_y = np.logical_and(pc_[:, 1] <= self.w/2.0, pc_[:, 1] >= -self.w/2.0)
idx_z = np.logical_and(pc_[:, 2] <= self.h/2.0, pc_[:, 2] >= -self.h/2.0)
idx = np.logical_and(idx_x, np.logical_and(idx_y, idx_z))
return idx
def test_applyT(self):
pts = np.array([[-0.6, 0., 0.24], [0.6, 0., 0.24], [0.6, 0., -0.24],
[-0.6, 0., -0.24]])
t = np.array([1, 2, 3]).reshape(1, 3)
ans = pts + t
self.assertTrue(np.allclose(apply_tr(pts, t), ans, rtol=1e-5))
def rotate_obj(self, label: KittiLabel, pc: np.array, calib: KittiCalib,
dry_range: List[float]) -> (KittiLabel, np.array):
'''
rotate object along the z axis in the LiDAR frame
inputs:
label: gt
pc: [#pts, >= 3]
calib:
dry_range: [dry_min, dry_max] in radius
returns:
label_rot
pc_rot
Note: If the attemp times is over max_attemp, it will return the original copy
'''
assert istype(label, "KittiLabel")
dry_min, dry_max = dry_range
max_attemp = 10
num_attemp = 0
calib_is_iter = isinstance(calib, list)
while True:
num_attemp += 1
# copy pc & label
pc_ = pc.copy()
label_ = KittiLabel()
label_.data = []
for obj in label.data:
label_.data.append(KittiObj(str(obj)))
if num_attemp > max_attemp:
print(
"KittiAugmentor.rotate_obj: Warning: the attemp times is over {} times,"
"will return the original copy".format(max_attemp))
break
if not calib_is_iter:
for obj in label_.data:
# generate random number
dry = np.random.rand() * (dry_max - dry_min) + dry_min
# modify pc_
idx = obj.get_pts_idx(pc_[:, :3], calib)
bottom_Fcam = np.array([obj.x, obj.y,
obj.z]).reshape(1, -1)
bottom_Flidar = calib.leftcam2lidar(bottom_Fcam)
pc_[idx, :3] = apply_tr(pc_[idx, :3], -bottom_Flidar)
# obj.ry += dry is correspond to rotz(-dry)
# since obj is in cam frame
# pc_ is in LiDAR frame
pc_[idx, :3] = apply_R(pc_[idx, :3], rotz(-dry))
pc_[idx, :3] = apply_tr(pc_[idx, :3], bottom_Flidar)
# modify obj
obj.ry += dry
else:
for obj, calib_ in zip(label_.data, calib):
# generate random number
dry = np.random.rand() * (dry_max - dry_min) + dry_min
# modify pc_
idx = obj.get_pts_idx(pc_[:, :3], calib_)
bottom_Fcam = np.array([obj.x, obj.y,
obj.z]).reshape(1, -1)
bottom_Flidar = calib_.leftcam2lidar(bottom_Fcam)
pc_[idx, :3] = apply_tr(pc_[idx, :3], -bottom_Flidar)
# obj.ry += dry is correspond to rotz(-dry)
# since obj is in cam frame
# pc_ is in LiDAR frame
pc_[idx, :3] = apply_R(pc_[idx, :3], rotz(-dry))
pc_[idx, :3] = apply_tr(pc_[idx, :3], bottom_Flidar)
# modify obj
obj.ry += dry
if self.check_overlap(label_):
break
res_label = KittiLabel()
for obj in label_.data:
res_label.add_obj(obj)
res_label.current_frame = "Cam2"
return res_label, pc_
def tr_obj(self, label: WaymoLabel, pc: np.array, calib: WaymoCalib,
dx_range: List[float], dy_range: List[float],
dz_range: List[float]) -> (WaymoLabel, np.array):
'''
translate object in the IMU frame
inputs:
label: gt
pc: [#pts, >= 3] in imu frame
calib:
dx_range: [dx_min, dx_max] in imu frame
dy_range: [dy_min, dy_max] in imu frame
dz_range: [dz_min, dz_max] in imu frame
returns:
label_tr
pc_tr
Note: If the attemp times is over max_attemp, it will return the original copy
'''
assert istype(label, "WaymoLabel")
dx_min, dx_max = dx_range
dy_min, dy_max = dy_range
dz_min, dz_max = dz_range
max_attemp = 10
num_attemp = 0
calib_is_iter = isinstance(calib, list)
while True:
num_attemp += 1
# copy pc & label
if isinstance(pc, dict):
pc_ = {k: v.copy() for k, v in pc.items()}
else:
pc_ = pc.copy()
label_ = WaymoLabel()
label_.data = []
for obj in label.data:
label_.data.append(WaymoObj(str(obj)))
if num_attemp > max_attemp:
print(
"WaymoAugmentor.tr_obj: Warning: the attemp times is over {} times,"
"will return the original copy".format(max_attemp))
break
if not calib_is_iter:
for obj in label_.data:
dx = np.random.rand() * (dx_max - dx_min) + dx_min
dy = np.random.rand() * (dy_max - dy_min) + dy_min
dz = np.random.rand() * (dz_max - dz_min) + dz_min
# modify pc
dtr = np.array([dx, dy, dz]).reshape(1, -1)
if isinstance(pc_, dict):
for velo in pc_.keys():
idx = obj.get_pts_idx(pc_[velo][:, :3], calib)
pc_[velo][idx, :3] = apply_tr(
pc_[velo][idx, :3], dtr)
else:
idx = obj.get_pts_idx(pc_[:, :3], calib)
pc_[idx, :3] = apply_tr(pc_[idx, :3], dtr)
# modify obj
obj.x += dx
obj.y += dy
obj.z += dz
else:
for obj, calib_ in zip(label_.data, calib):
dx = np.random.rand() * (dx_max - dx_min) + dx_min
dy = np.random.rand() * (dy_max - dy_min) + dy_min
dz = np.random.rand() * (dz_max - dz_min) + dz_min
# modify pc
dtr = np.array([dx, dy, dz]).reshape(1, -1)
if isinstance(pc_, dict):
for velo in pc_.keys():
idx = obj.get_pts_idx(pc_[velo][:, :3], calib_)
pc_[velo][idx, :3] = apply_tr(
pc_[velo][idx, :3], dtr)
else:
idx = obj.get_pts_idx(pc_[:, :3], calib_)
pc_[idx, :3] = apply_tr(pc_[idx, :3], dtr)
# modify obj
obj.x += dx
obj.y += dy
obj.z += dz
if self.check_overlap(label_):
break
res_label = WaymoLabel()
for obj in label_.data:
res_label.add_obj(obj)
res_label.current_frame = "IMU"
return res_label, pc_
def rotate_obj(self, label: WaymoLabel, pc: np.array, calib: WaymoCalib,
dry_range: List[float]) -> (WaymoLabel, np.array):
'''
rotate object along the z axis in the LiDAR frame
inputs:
label: gt
pc: [#pts, >= 3] in IMU frame
calib:
dry_range: [dry_min, dry_max] in radius
returns:
label_rot
pc_rot
Note: If the attemp times is over max_attemp, it will return the original copy
'''
assert istype(label, "WaymoLabel")
dry_min, dry_max = dry_range
max_attemp = 10
num_attemp = 0
calib_is_iter = isinstance(calib, list)
while True:
num_attemp += 1
# copy pc & label
if isinstance(pc, dict):
pc_ = {k: v.copy() for k, v in pc.items()}
else:
pc_ = pc.copy()
label_ = WaymoLabel()
label_.data = []
for obj in label.data:
label_.data.append(WaymoObj(str(obj)))
if num_attemp > max_attemp:
print(
"WaymoAugmentor.rotate_obj: Warning: the attemp times is over {} times,"
"will return the original copy".format(max_attemp))
break
if not calib_is_iter:
for obj in label_.data:
dry = np.random.rand() * (dry_max - dry_min) + dry_min
# modify pc
bottom_Fimu = np.array([obj.x, obj.y,
obj.z]).reshape(1, -1)
if isinstance(pc_, dict):
for velo in pc_.keys():
idx = obj.get_pts_idx(pc_[velo][:, :3], calib)
pc_[velo][idx, :3] = apply_tr(
pc_[velo][idx, :3], -bottom_Fimu)
pc_[velo][idx, :3] = apply_R(
pc_[velo][idx, :3], rotz(dry))
pc_[velo][idx, :3] = apply_tr(
pc_[velo][idx, :3], bottom_Fimu)
else:
idx = obj.get_pts_idx(pc_[:, :3], calib)
pc_[idx, :3] = apply_tr(pc_[idx, :3], -bottom_Fimu)
pc_[idx, :3] = apply_R(pc_[idx, :3], rotz(dry))
pc_[idx, :3] = apply_tr(pc_[idx, :3], bottom_Fimu)
# modify obj
obj.ry += dry
else:
for obj, calib_ in zip(label_.data, calib):
dry = np.random.rand() * (dry_max - dry_min) + dry_min
# modify pc
bottom_Fimu = np.array([obj.x, obj.y,
obj.z]).reshape(1, -1)
if isinstance(pc_, dict):
for velo in pc_.keys():
idx = obj.get_pts_idx(pc_[velo][:, :3], calib_)
pc_[velo][idx, :3] = apply_tr(
pc_[velo][idx, :3], -bottom_Fimu)
pc_[velo][idx, :3] = apply_R(
pc_[velo][idx, :3], rotz(dry))
pc_[velo][idx, :3] = apply_tr(
pc_[velo][idx, :3], bottom_Fimu)
else:
idx = obj.get_pts_idx(pc_[:, :3], calib_)
pc_[idx, :3] = apply_tr(pc_[idx, :3], -bottom_Fimu)
pc_[idx, :3] = apply_R(pc_[idx, :3], rotz(dry))
pc_[idx, :3] = apply_tr(pc_[idx, :3], bottom_Fimu)
# modify obj
obj.ry += dry
if self.check_overlap(label_):
break
res_label = WaymoLabel()
for obj in label_.data:
res_label.add_obj(obj)
res_label.current_frame = "IMU"
return res_label, pc_
def tr_obj(self, label: KittiLabel, pc: np.array, calib: KittiCalib,
dx_range: List[float], dy_range: List[float],
dz_range: List[float]) -> (KittiLabel, np.array):
'''
translate object in the LiDAR frame
inputs:
label: gt
pc: [#pts, >= 3]
calib:
dx_range: [dx_min, dx_max] in LiDAR frame
dy_range: [dy_min, dy_max] in LiDAR frame
dz_range: [dz_min, dz_max] in LiDAR frame
returns:
label_tr
pc_tr
Note: If the attemp times is over max_attemp, it will return the original copy
'''
assert istype(label, "KittiLabel")
dx_min, dx_max = dx_range
dy_min, dy_max = dy_range
dz_min, dz_max = dz_range
max_attemp = 10
num_attemp = 0
calib_is_iter = isinstance(calib, list)
while True:
num_attemp += 1
# copy pc & label
pc_ = pc.copy()
label_ = KittiLabel()
label_.data = []
for obj in label.data:
label_.data.append(KittiObj(str(obj)))
if num_attemp > max_attemp:
print(
"KittiAugmentor.tr_obj: Warning: the attemp times is over {} times,"
"will return the original copy".format(max_attemp))
break
if not calib_is_iter:
for obj in label_.data:
# gennerate ramdom number
dx = np.random.rand() * (dx_max - dx_min) + dx_min
dy = np.random.rand() * (dy_max - dy_min) + dy_min
dz = np.random.rand() * (dz_max - dz_min) + dz_min
# modify pc_
idx = obj.get_pts_idx(pc_[:, :3], calib)
dtr = np.array([dx, dy, dz]).reshape(1, -1)
pc_[idx, :3] = apply_tr(pc_[idx, :3], dtr)
# modify obj
bottom_Fcam = np.array([obj.x, obj.y,
obj.z]).reshape(1, -1)
bottom_Flidar = calib.leftcam2lidar(bottom_Fcam)
bottom_Flidar = apply_tr(bottom_Flidar, dtr)
bottom_Fcam = calib.lidar2leftcam(bottom_Flidar)
obj.x, obj.y, obj.z = bottom_Fcam.flatten()
else:
for obj, calib_ in zip(label_.data, calib):
# gennerate ramdom number
dx = np.random.rand() * (dx_max - dx_min) + dx_min
dy = np.random.rand() * (dy_max - dy_min) + dy_min
dz = np.random.rand() * (dz_max - dz_min) + dz_min
# modify pc_
idx = obj.get_pts_idx(pc_[:, :3], calib_)
dtr = np.array([dx, dy, dz]).reshape(1, -1)
pc_[idx, :3] = apply_tr(pc_[idx, :3], dtr)
# modify obj
bottom_Fcam = np.array([obj.x, obj.y,
obj.z]).reshape(1, -1)
bottom_Flidar = calib_.leftcam2lidar(bottom_Fcam)
bottom_Flidar = apply_tr(bottom_Flidar, dtr)
bottom_Fcam = calib_.lidar2leftcam(bottom_Flidar)
obj.x, obj.y, obj.z = bottom_Fcam.flatten()
if self.check_overlap(label_):
break
res_label = KittiLabel()
for obj in label_.data:
res_label.add_obj(obj)
res_label.current_frame = "Cam2"
return res_label, pc_