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_