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 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 test_by_visualization(self):
from det3.ops import read_bin
from det3.visualizer.vis import BEVImage, FVImage
from tqdm import tqdm
import numpy as np
from det3.utils.utils import roty, rotz
label_path = f"./unit-test/data/ut_UdiLabel_21_bin.json"
pc_path = f"./unit-test/data/ut_UdiLabel_21.bin"
calib_path = f"./unit-test/data/ut_UdiLabel_21.txt"
pc = read_bin(pc_path, dtype=np.float32).reshape(-1, 4)
label = UdiLabel(label_path).read_label_file()
calib = UdiCalib(calib_path).read_calib_file()
bev_img = BEVImage(x_range=(-100, 100),
y_range=(-50, 50),
grid_size=(0.05, 0.05))
bev_img.from_lidar(pc, scale=1)
for obj in label:
bev_img.draw_box(obj, None, bool_gt=True, text=obj.cls[0])
bev_img.save(f"./unit-test/result/test_UdiLabel_21.png")
fv_img = FVImage()
vcam_T = np.eye(4)
vcam_T[:3, 3] = np.array([-3, 0, 3])
vcam_T[:3, :3] = rotz(0) @ roty(np.pi * 0.1)
calib.vcam_T = vcam_T
fv_img.from_lidar(calib, pc, scale=2)
for obj in label:
fv_img.draw_3dbox(obj, calib, bool_gt=True)
fv_img.save(f"./unit-test/result/test_UdiLabel_21_fv.png")
print("Please manually check ./unit-test/result/test_UdiLabel_21.png")
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 vis_fn(idx):
global dataset, data_dir, output_dir
if dataset == "KITTI":
from det3.dataloader.kittidata import KittiData
calib, img, label, pc = KittiData(data_dir, idx).read_data()
elif dataset == "CARLA":
from det3.dataloader.carladata import CarlaData
import numpy as np
pc_dict, label, calib = CarlaData(data_dir, idx).read_data()
pc = np.vstack([
calib.lidar2imu(v, key="Tr_imu_to_{}".format(k))
for k, v in pc_dict.items()
])
elif dataset == "UDI":
from det3.dataloader.udidata import UdiData, UdiFrame
import numpy as np
udidata = UdiData(data_dir, idx).read_data()
calib = udidata["calib"]
label = udidata["label"]
pc_dict = udidata["lidar"]
for k, v in pc_dict.items():
pc_dict[k] = calib.transform(v[:, :3],
source_frame=UdiFrame(
UdiData.lidar_to_frame(k)),
target_frame=UdiFrame("BASE"))
pc_merge = np.vstack([v for k, v in pc_dict.items()])
bevimg = BEVImage(x_range=(-70, 70),
y_range=(-40, 40),
grid_size=(0.05, 0.05))
bevimg.from_lidar(pc_merge, scale=1)
for obj in label:
bevimg.draw_box(obj, calib, bool_gt=True)
bevimg_img = Image.fromarray(bevimg.data)
bevimg_img.save(os.path.join(output_dir, idx + '.png'))
for k, v in pc_dict.items():
bevimg = BEVImage(x_range=(-70, 70),
y_range=(-40, 40),
grid_size=(0.05, 0.05))
bevimg.from_lidar(v, scale=1)
for obj in label:
bevimg.draw_box(obj, calib, bool_gt=True)
bevimg_img = Image.fromarray(bevimg.data)
bevimg_img.save(os.path.join(output_dir, idx + f'_{k}.png'))
fv_img = FVImage()
vcam_T = np.eye(4)
vcam_T[:3, 3] = np.array([-3, 0, 3])
vcam_T[:3, :3] = rotz(0) @ roty(np.pi * 0.1)
calib.vcam_T = vcam_T
fv_img.from_lidar(calib, pc_merge, scale=2)
for obj in label:
fv_img.draw_3dbox(obj, calib, bool_gt=True)
fv_img.save(os.path.join(output_dir, idx + '_fv.png'))
return
bevimg = BEVImage(x_range=(0, 70),
y_range=(-40, 40),
grid_size=(0.05, 0.05))
bevimg.from_lidar(pc, scale=1)
for obj in label.read_label_file().data:
bevimg.draw_box(obj, calib, bool_gt=True)
bevimg_img = Image.fromarray(bevimg.data)
bevimg_img.save(os.path.join(output_dir, idx + '.png'))
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_