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_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_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 test_applyR(self):
pts = np.array([[-0.6, 0., 0.24], [0.6, 0., 0.24], [0.6, 0., -0.24],
[-0.6, 0., -0.24]])
ry = 0.01
ans = np.array([[-0.59757004, 0,
0.2459879], [0.60236996, 0, 0.2339881],
[0.59757004, 0, -0.2459879],
[-0.60236996, 0, -0.2339881]])
self.assertTrue(np.allclose(apply_R(pts, roty(ry)), ans, rtol=1e-5))
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_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 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 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_
