def display_test(self, dataset, mode): self.roes = { 'Rots': [], 'yaws': [], } self.to_open_vins(dataset) for i, seq in enumerate(dataset.sequences): print('\n', 'Results for sequence ' + seq ) self.seq = seq # get ground truth self.gt = dataset.load_gt(i) Rots = SO3.from_quaternion(self.gt['qs'].cuda()) self.gt['Rots'] = Rots.cpu() self.gt['rpys'] = SO3.to_rpy(Rots).cpu() # get data and estimate self.net_us = pload(self.address, seq, 'results.p')['hat_xs'] self.raw_us, _ = dataset[i] N = self.net_us.shape[0] self.gyro_corrections = (self.raw_us[:, :3] - self.net_us[:N, :3]) self.ts = torch.linspace(0, N*self.dt, N) self.convert() self.plot_gyro() self.plot_gyro_correction() plt.show()
def integrate_with_quaternions_superfast(self, N, raw_us, net_us): imu_qs = SO3.qnorm(SO3.qexp(raw_us[:, :3].cuda().double()*self.dt)) net_qs = SO3.qnorm(SO3.qexp(net_us[:, :3].cuda().double()*self.dt)) Rot0 = SO3.qnorm(self.gt['qs'][:2].cuda().double()) imu_qs[0] = Rot0[0] net_qs[0] = Rot0[0] N = np.log2(imu_qs.shape[0]) for i in range(int(N)): k = 2**i imu_qs[k:] = SO3.qnorm(SO3.qmul(imu_qs[:-k], imu_qs[k:])) net_qs[k:] = SO3.qnorm(SO3.qmul(net_qs[:-k], net_qs[k:])) if int(N) < N: k = 2**int(N) k2 = imu_qs[k:].shape[0] imu_qs[k:] = SO3.qnorm(SO3.qmul(imu_qs[:k2], imu_qs[k:])) net_qs[k:] = SO3.qnorm(SO3.qmul(net_qs[:k2], net_qs[k:])) imu_Rots = SO3.from_quaternion(imu_qs).float() net_Rots = SO3.from_quaternion(net_qs).float() return net_qs.cpu(), imu_Rots, net_Rots
def display_test(self, dataset_class, dataset_params, mode): dataset = dataset_class(**dataset_params, mode=mode) for i, seq in enumerate(dataset.sequences): print('\n', '--------- Result for sequence ' + seq + ' ---------') self.seq = seq # get ground truth pose self.gt = dataset.load_gt(i) self.gt['Rots'] = SO3.from_quaternion(self.gt['qs'].cuda()).cpu() # get data and estimate self.us, self.zupt = dataset[i] self.iekf = self.get_iekf_results(seq) self.N = self.iekf['ps'].shape[0] N0 = self.us.shape[0] - self.N self.us = self.us[N0:] self.zupt = self.zupt[N0:] for key, val in self.gt.items(): self.gt[key] = val[N0:] self.ts = torch.linspace(0, self.N * self.dt, self.N) self.align_traj() self.convert() self.plot_orientation() self.plot_velocity() self.plot_velocity_in_body_frame() self.plot_position() self.plot_horizontal_position() self.plot_bias_gyro() self.plot_bias_acc() self.plot_gyro() self.plot_acc() self.plot_zupt() self.plot_orientation_err() self.plot_velocity_err() self.plot_body_velocity_err() self.plot_position_err()
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")