def flip_pc(self, label: WaymoLabel, pc: np.array,
calib: WaymoCalib) -> (WaymoLabel, np.array):
'''
flip point cloud along the y axis of the Kitti Lidar frame
inputs:
label: ground truth
pc: point cloud
calib:
'''
assert istype(label, "WaymoLabel")
# 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)))
# flip point cloud
if isinstance(pc_, dict):
for velo in pc_.keys():
pc_[velo][:, 1] *= -1
else:
pc_[:, 1] *= -1
# modify gt
for obj in label_.data:
obj.y *= -1
obj.ry *= -1
res_label = WaymoLabel()
for obj in label_.data:
res_label.add_obj(obj)
res_label.current_frame = "IMU"
return res_label, pc_
def write_clusters(clusters: np.array, cluster_dir: str):
"""Writes all clusters to individual numpy files."""
for center, vertices in clusters.items():
out_path = os.path.join(cluster_dir, '%f_%f_%f' % center)
out_data = np.vstack(vertices)
assert out_data.shape[1] > 3
np.save(out_path, out_data)
def projected_gradient(x_k: np.array, epsilon: float, delta: float, opt):
k = 1
while True:
# print('\nIteração {}...'.format(k),end='')
prev_x = x_k.copy()
# calcular gradiente
d_k = opt.max_likelihood_grad(x_k)
# projetar lambdas
d_norm = np.linalg.norm(np.array(list(d_k.values())))
for bairro in opt.lista_bairros:
x = x_k[bairro]
d = d_k[bairro]
x_k[bairro] = max(x - (2 / k) * (d / d_norm), epsilon)
# print('ok')
# print('Norma do gradiente:',d_norm)
# print('Função de custo:',sum(list(opt.loss.values())))
if next_is_close_to_prev(prev_x, x_k, delta):
opt.max_likelihood_grad(x_k)
break
else:
k += 1
return pd.DataFrame(list(opt.loss.items()),
columns=['bairro_id', 'valor_custo']), pd.DataFrame(
list(x_k.items()), columns=['bairro_id', 'lambda'])
def _get_function(linear: np.array, quadratic: np.array, constant: int) -> \
Dict[Union[int, Tuple[int, int]], int]:
"""Convert the problem to a dictionary format."""
func = {-1: int(constant)}
for i, v in enumerate(linear):
func[i] = int(v)
for (i, j), v in quadratic.items():
if i != j:
func[(i, j)] = int(quadratic[(i, j)])
else:
func[i] += int(v)
return func
def fig_rewards(fout_name: str,
rewards: np.array,
t_max: int,
points_in_bin=10):
""" 報酬の推移を作図する。
"""
plt.figure()
x = np.array([x + 1 for x in range(t_max)])
for algo_type, rewards_ in rewards.items():
plt.plot(x[::points_in_bin],
rewards_[::points_in_bin],
label=algo_type)
plt.legend()
plt.savefig(fout_name)
def keep(self, label: WaymoLabel, pc: np.array,
calib: WaymoCalib) -> (WaymoLabel, np.array):
assert istype(label, "WaymoLabel")
# 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)))
res_label = WaymoLabel()
for obj in label_.data:
res_label.add_obj(obj)
res_label.current_frame = "IMU"
return res_label, pc_
def get_mean_positions(face_features: np.array):
mean_positions = {}
for key, val in face_features.items():
mean_positions[key] = np.mean(np.array(val), axis=0)
return mean_positions
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_
