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 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 __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 to_open_vins(self, dataset):
"""
Export results to Open-VINS format. Use them eval toolbox available
at https://github.com/rpng/open_vins/
"""
for i, seq in enumerate(dataset.sequences):
self.seq = seq
# get ground truth
self.gt = dataset.load_gt(i)
raw_us, _ = dataset[i]
net_us = pload(self.address, seq, 'results.p')['hat_xs']
N = net_us.shape[0]
net_qs, imu_Rots, net_Rots = self.integrate_with_quaternions_superfast(N, raw_us, net_us)
path = os.path.join(self.address, seq + '.txt')
header = "timestamp(s) tx ty tz qx qy qz qw"
x = np.zeros((net_qs.shape[0], 8))
x[:, 0] = self.gt['ts'][:net_qs.shape[0]]
x[:, [7, 4, 5, 6]] = net_qs
np.savetxt(path, x[::10], header=header, delimiter=" ",
fmt='%1.9f')
def load_gt(self, i):
return pload(self.predata_dir, self.sequences[i] + '_gt.p')
def load_seq(self, i):
return pload(self.predata_dir, self.sequences[i] + '.p')
def get_iekf_results(self, seq):
return pload(self.address, seq, 'iekf.p')
def get_results(self, seq):
return pload(self.address, seq, 'results.p')['hat_xs']
