def init_normalize_factors(self, train_seqs): if os.path.exists(self.path_normalize_factors): mondict = pload(self.path_normalize_factors) return mondict['mean_u'], mondict['std_u'] path = os.path.join(self.predata_dir, train_seqs[0] + '.p') if not os.path.exists(path): print("init_normalize_factors not computed") return 0, 0 print('Start computing normalizing factors ...') cprint("Do it only on training sequences, it is vital!", 'yellow') # first compute mean num_data = 0 for i, sequence in enumerate(train_seqs): pickle_dict = pload(self.predata_dir, sequence + '.p') us = pickle_dict['us'] sms = pickle_dict['xs'] if i == 0: mean_u = us.sum(dim=0) num_positive = sms.sum(dim=0) num_negative = sms.shape[0] - sms.sum(dim=0) else: mean_u += us.sum(dim=0) num_positive += sms.sum(dim=0) num_negative += sms.shape[0] - sms.sum(dim=0) num_data += us.shape[0] mean_u = mean_u / num_data pos_weight = num_negative / num_positive # second compute standard deviation for i, sequence in enumerate(train_seqs): pickle_dict = pload(self.predata_dir, sequence + '.p') us = pickle_dict['us'] if i == 0: std_u = ((us - mean_u)**2).sum(dim=0) else: std_u += ((us - mean_u)**2).sum(dim=0) std_u = (std_u / num_data).sqrt() normalize_factors = { 'mean_u': mean_u, 'std_u': std_u, } print('... ended computing normalizing factors') print('pos_weight:', pos_weight) print('This values most be a training parameters !') print('mean_u :', mean_u) print('std_u :', std_u) print('num_data :', num_data) pdump(normalize_factors, self.path_normalize_factors) return mean_u, std_u
def loop_test(self, dataset, criterion): """Forward loop over test data""" self.net.eval() for i in range(len(dataset)): seq = dataset.sequences[i] us, xs = dataset[i] with torch.no_grad(): hat_xs = self.net(us.cuda().unsqueeze(0)) loss = criterion(xs.cuda().unsqueeze(0), hat_xs) mkdir(self.address, seq) mondict = { 'hat_xs': hat_xs[0].cpu(), 'loss': loss.cpu().item(), } pdump(mondict, self.address, seq, 'results.p')
def __init__(self, res_dir, tb_dir, net_class, net_params, address, dt): self.res_dir = res_dir self.tb_dir = tb_dir self.net_class = net_class self.net_params = net_params self._ready = False self.train_params = {} self.figsize = (20, 12) self.dt = dt # (s) self.address, self.tb_address = self.find_address(address) if address is None: # create new address pdump(self.net_params, self.address, 'net_params.p') ydump(self.net_params, self.address, 'net_params.yaml') else: # pick the network parameters self.net_params = pload(self.address, 'net_params.p') self.train_params = pload(self.address, 'train_params.p') self._ready = True self.path_weights = os.path.join(self.address, 'weights.pt') self.net = self.net_class(**self.net_params) if self._ready: # fill network parameters self.load_weights()
def dump(self, address, seq, zupts, covs): # turn cov J = torch.eye(9).repeat(self.Ps.shape[0], 1, 1) J[:, 3:6, :3] = SO3.wedge(self.vs) J[:, 6:9, :3] = SO3.wedge(self.ps) #self.Ps = axat(J, self.Ps[:, :9, :9]) path = os.path.join(address, seq, 'iekf.p') mondict = { 'Rots': self.Rots, 'vs': self.vs, 'ps': self.ps, 'b_omegas': self.b_omegas, 'b_accs': self.b_accs, 'rs': self.rs, 'Ps': self.Ps.diagonal(dim1=1, dim2=2), 'zupts': zupts, 'covs': covs, } for k, v in mondict.items(): mondict[k] = v.float().detach().cpu() pdump(mondict, path)
def train(self, dataset_class, dataset_params, train_params): """train the neural network. GPU is assumed""" self.train_params = train_params pdump(self.train_params, self.address, 'train_params.p') ydump(self.train_params, self.address, 'train_params.yaml') hparams = self.get_hparams(dataset_class, dataset_params, train_params) ydump(hparams, self.address, 'hparams.yaml') # define datasets dataset_train = dataset_class(**dataset_params, mode='train') dataset_train.init_train() dataset_val = dataset_class(**dataset_params, mode='val') dataset_val.init_val() # get class Optimizer = train_params['optimizer_class'] Scheduler = train_params['scheduler_class'] Loss = train_params['loss_class'] # get parameters dataloader_params = train_params['dataloader'] optimizer_params = train_params['optimizer'] scheduler_params = train_params['scheduler'] loss_params = train_params['loss'] # define optimizer, scheduler and loss dataloader = DataLoader(dataset_train, **dataloader_params) optimizer = Optimizer(self.net.parameters(), **optimizer_params) scheduler = Scheduler(optimizer, **scheduler_params) criterion = Loss(**loss_params) # remaining training parameters freq_val = train_params['freq_val'] n_epochs = train_params['n_epochs'] # init net w.r.t dataset self.net = self.net.cuda() mean_u, std_u = dataset_train.mean_u, dataset_train.std_u self.net.set_normalized_factors(mean_u, std_u) # start tensorboard writer writer = SummaryWriter(self.tb_address) start_time = time.time() best_loss = torch.Tensor([float('Inf')]) # define some function for seeing evolution of training def write(epoch, loss_epoch): writer.add_scalar('loss/train', loss_epoch.item(), epoch) writer.add_scalar('lr', optimizer.param_groups[0]['lr'], epoch) print('Train Epoch: {:2d} \tLoss: {:.4f}'.format( epoch, loss_epoch.item())) scheduler.step(epoch) def write_time(epoch, start_time): delta_t = time.time() - start_time print("Amount of time spent for epochs " + "{}-{}: {:.1f}s\n".format(epoch - freq_val, epoch, delta_t)) writer.add_scalar('time_spend', delta_t, epoch) def write_val(loss, best_loss): if 0.5*loss <= best_loss: msg = 'validation loss decreases! :) ' msg += '(curr/prev loss {:.4f}/{:.4f})'.format(loss.item(), best_loss.item()) cprint(msg, 'green') best_loss = loss self.save_net() else: msg = 'validation loss increases! :( ' msg += '(curr/prev loss {:.4f}/{:.4f})'.format(loss.item(), best_loss.item()) cprint(msg, 'yellow') writer.add_scalar('loss/val', loss.item(), epoch) return best_loss # training loop ! for epoch in range(1, n_epochs + 1): loss_epoch = self.loop_train(dataloader, optimizer, criterion) write(epoch, loss_epoch) scheduler.step(epoch) if epoch % freq_val == 0: loss = self.loop_val(dataset_val, criterion) write_time(epoch, start_time) best_loss = write_val(loss, best_loss) start_time = time.time() # training is over ! # test on new data dataset_test = dataset_class(**dataset_params, mode='test') self.load_weights() test_loss = self.loop_val(dataset_test, criterion) dict_loss = { 'final_loss/val': best_loss.item(), 'final_loss/test': test_loss.item() } writer.add_hparams(hparams, dict_loss) ydump(dict_loss, self.address, 'final_loss.yaml') writer.close()
def read_data(self, data_dir): r"""Read the data from the dataset""" # threshold for ZUPT ground truth sm_velocity_max_threshold = 0.004 # m/s f = os.path.join(self.predata_dir, 'urban06.p') if True and os.path.exists(f): return print("Start read_data, be patient please") def set_path(seq): path_imu = os.path.join(data_dir, seq, "sensor_data", "xsens_imu.csv") path_gt = os.path.join(data_dir, seq, "global_pose.csv") # path_odo = os.path.join(data_dir, seq, "encoder.csv") return path_imu, path_gt time_factor = 1e9 # ns -> s def interpolate(x, t, t_int, angle=False): """ Interpolate ground truth with sensors """ x_int = np.zeros((t_int.shape[0], x.shape[1])) for i in range(x.shape[1]): if angle: x[:, i] = np.unwrap(x[:, i]) x_int[:, i] = np.interp(t_int, t, x[:, i]) return x_int sequences = os.listdir(data_dir) # read each sequence for sequence in sequences: print("\nSequence name: " + sequence) path_imu, path_gt = set_path(sequence) imu = np.genfromtxt(path_imu, delimiter=",") # Urban00-05 and campus00 have only quaternion and Euler data if not imu.shape[1] > 10: cprint("No IMU data for dataset " + sequence, 'yellow') continue gt = np.genfromtxt(path_gt, delimiter=",") # time synchronization between IMU and ground truth t0 = np.max([gt[0, 0], imu[0, 0]]) t_end = np.min([gt[-1, 0], imu[-1, 0]]) # start index idx0_imu = np.searchsorted(imu[:, 0], t0) idx0_gt = np.searchsorted(gt[:, 0], t0) # end index idx_end_imu = np.searchsorted(imu[:, 0], t_end, 'right') idx_end_gt = np.searchsorted(gt[:, 0], t_end, 'right') # subsample imu = imu[idx0_imu:idx_end_imu] gt = gt[idx0_gt:idx_end_gt] t = imu[:, 0] # take ground truth position p_gt = gt[:, [4, 8, 12]] p_gt = p_gt - p_gt[0] # take ground matrix pose Rot_gt = torch.Tensor(gt.shape[0], 3, 3) for j in range(3): Rot_gt[:, j] = torch.Tensor(gt[:, 1 + 4 * j:1 + 4 * j + 3]) q_gt = SO3.to_quaternion(Rot_gt) # convert to angle orientation rpys = SO3.to_rpy(Rot_gt) t_gt = gt[:, 0] # interpolate ground-truth p_gt = interpolate(p_gt, t_gt, t) rpys = interpolate(rpys.numpy(), t_gt, t, angle=True) # convert from numpy ts = (t - t0) / time_factor p_gt = torch.Tensor(p_gt) rpys = torch.Tensor(rpys).float() q_gt = SO3.to_quaternion( SO3.from_rpy(rpys[:, 0], rpys[:, 1], rpys[:, 2])) imu = torch.Tensor(imu).float() # take IMU gyro and accelerometer and magnetometer imu = imu[:, 8:17] dt = ts[1:] - ts[:-1] # compute speed ground truth (apply smoothing) v_gt = torch.zeros(p_gt.shape[0], 3) for j in range(3): p_gt_smooth = savgol_filter(p_gt[:, j], 11, 1) v_j = (p_gt_smooth[1:] - p_gt_smooth[:-1]) / dt v_j_smooth = savgol_filter(v_j, 11, 0) v_gt[1:, j] = torch.Tensor(v_j_smooth) # ground truth specific motion measurement (binary) zupts = v_gt.norm(dim=1, keepdim=True) < sm_velocity_max_threshold zupts = zupts.float() # set ground truth consistent with ZUPT v_gt[zupts.squeeze() == 1] = 0 # save for all training mondict = { 'xs': zupts.float(), 'us': imu.float(), } pdump(mondict, self.predata_dir, sequence + ".p") # save ground truth mondict = { 'ts': ts, 'qs': q_gt.float(), 'vs': v_gt.float(), 'ps': p_gt.float(), } pdump(mondict, self.predata_dir, sequence + "_gt.p")
def read_data(self, data_dir): r"""Read the data from the dataset""" f = os.path.join(self.predata_dir, 'dataset-room1_512_16_gt.p') if True and os.path.exists(f): return print("Start read_data, be patient please") def set_path(seq): path_imu = os.path.join(data_dir, seq, "mav0", "imu0", "data.csv") path_gt = os.path.join(data_dir, seq, "mav0", "mocap0", "data.csv") return path_imu, path_gt sequences = os.listdir(data_dir) # read each sequence for sequence in sequences: print("\nSequence name: " + sequence) if 'room' not in sequence: continue path_imu, path_gt = set_path(sequence) imu = np.genfromtxt(path_imu, delimiter=",", skip_header=1) gt = np.genfromtxt(path_gt, delimiter=",", skip_header=1) # time synchronization between IMU and ground truth t0 = np.max([gt[0, 0], imu[0, 0]]) t_end = np.min([gt[-1, 0], imu[-1, 0]]) # start index idx0_imu = np.searchsorted(imu[:, 0], t0) idx0_gt = np.searchsorted(gt[:, 0], t0) # end index idx_end_imu = np.searchsorted(imu[:, 0], t_end, 'right') idx_end_gt = np.searchsorted(gt[:, 0], t_end, 'right') # subsample imu = imu[idx0_imu:idx_end_imu] gt = gt[idx0_gt:idx_end_gt] ts = imu[:, 0] / 1e9 # interpolate t_gt = gt[:, 0] / 1e9 gt = self.interpolate(gt, t_gt, ts) # take ground truth position p_gt = gt[:, 1:4] p_gt = p_gt - p_gt[0] # take ground true quaternion pose q_gt = SO3.qnorm(torch.Tensor(gt[:, 4:8]).double()) Rot_gt = SO3.from_quaternion(q_gt.cuda(), ordering='wxyz').cpu() # convert from numpy p_gt = torch.Tensor(p_gt).double() v_gt = torch.zeros_like(p_gt).double() v_gt[1:] = (p_gt[1:] - p_gt[:-1]) / self.dt imu = torch.Tensor(imu[:, 1:]).double() # compute pre-integration factors for all training mtf = self.min_train_freq dRot_ij = bmtm(Rot_gt[:-mtf], Rot_gt[mtf:]) dRot_ij = SO3.dnormalize(dRot_ij.cuda()) dxi_ij = SO3.log(dRot_ij).cpu() # masks with 1 when ground truth is available, 0 otherwise masks = dxi_ij.new_ones(dxi_ij.shape[0]) tmp = np.searchsorted(t_gt, ts[:-mtf]) diff_t = ts[:-mtf] - t_gt[tmp] masks[np.abs(diff_t) > 0.01] = 0 # save all the sequence mondict = { 'xs': torch.cat((dxi_ij, masks.unsqueeze(1)), 1).float(), 'us': imu.float(), } pdump(mondict, self.predata_dir, sequence + ".p") # save ground truth mondict = { 'ts': ts, 'qs': q_gt.float(), 'vs': v_gt.float(), 'ps': p_gt.float(), } pdump(mondict, self.predata_dir, sequence + "_gt.p")
def read_data(self, data_dir): r"""Read the data from the dataset""" f = os.path.join(self.predata_dir, 'MH_01_easy.p') if True and os.path.exists(f): return print("Start read_data, be patient please") def set_path(seq): path_imu = os.path.join(data_dir, seq, "mav0", "imu0", "data.csv") path_gt = os.path.join(data_dir, seq, "mav0", "state_groundtruth_estimate0", "data.csv") return path_imu, path_gt sequences = os.listdir(data_dir) # read each sequence for sequence in sequences: print("\nSequence name: " + sequence) path_imu, path_gt = set_path(sequence) imu = np.genfromtxt(path_imu, delimiter=",", skip_header=1) gt = np.genfromtxt(path_gt, delimiter=",", skip_header=1) # time synchronization between IMU and ground truth t0 = np.max([gt[0, 0], imu[0, 0]]) t_end = np.min([gt[-1, 0], imu[-1, 0]]) # start index idx0_imu = np.searchsorted(imu[:, 0], t0) idx0_gt = np.searchsorted(gt[:, 0], t0) # end index idx_end_imu = np.searchsorted(imu[:, 0], t_end, 'right') idx_end_gt = np.searchsorted(gt[:, 0], t_end, 'right') # subsample imu = imu[idx0_imu:idx_end_imu] gt = gt[idx0_gt:idx_end_gt] ts = imu[:, 0] / 1e9 # interpolate gt = self.interpolate(gt, gt[:, 0] / 1e9, ts) # take ground truth position p_gt = gt[:, 1:4] p_gt = p_gt - p_gt[0] # take ground true quaternion pose q_gt = torch.Tensor(gt[:, 4:8]).double() q_gt = q_gt / q_gt.norm(dim=1, keepdim=True) Rot_gt = SO3.from_quaternion(q_gt.cuda(), ordering='wxyz').cpu() # convert from numpy p_gt = torch.Tensor(p_gt).double() v_gt = torch.tensor(gt[:, 8:11]).double() imu = torch.Tensor(imu[:, 1:]).double() # compute pre-integration factors for all training mtf = self.min_train_freq dRot_ij = bmtm(Rot_gt[:-mtf], Rot_gt[mtf:]) dRot_ij = SO3.dnormalize(dRot_ij.cuda()) dxi_ij = SO3.log(dRot_ij).cpu() # save for all training mondict = { 'xs': dxi_ij.float(), 'us': imu.float(), } pdump(mondict, self.predata_dir, sequence + ".p") # save ground truth mondict = { 'ts': ts, 'qs': q_gt.float(), 'vs': v_gt.float(), 'ps': p_gt.float(), } pdump(mondict, self.predata_dir, sequence + "_gt.p")
parser.add_argument('--type', choices=['conll03', 'ontonotes', 'ptb'], help='multi task data type') parser.add_argument('--out', type=str, default='data', help='processed data output dir') # fmt: on args = parser.parse_args() assert args.pos is not None return args if __name__ == "__main__": args = get_args() set_seed(1) parse_table = { "conll03": prepare_conll03, "ontonotes": prepare_ontonotes, "ptb": prepare_ptb, } logger.info(args) assert args.type in parse_table task_lst, vocabs = parse_table[args.type](args) os.makedirs(args.out, exist_ok=True) data_summary(task_lst, vocabs) path = os.path.join(args.out, args.type + ".pkl") logger.info("saving data to " + path) pdump({"task_lst": task_lst, "vocabs": vocabs}, path)