def get_network_input(self, input_mode, input_scales, cov_type="WTV"):
ind_l, ind_u = state_indices(cov_type)
cov_input = np.array([
upper_triangular_list(self.P[ind_l:ind_u, ind_l:ind_u, i],
ret_dim=False) for i in range(self.nposes)
])
Wsb_input = np.vstack([rot.as_rotvec() for rot in self.Rsb])
if input_mode == "cov":
raw_net_input = cov_input
elif input_mode == "gsbcov":
raw_net_input = np.hstack((Wsb_input, self.Tsb.T, cov_input))
elif input_mode == "gsbvcov":
raw_net_input = np.hstack(
(Wsb_input, self.Tsb.T, self.Vsb.T, cov_input))
# Scale the input
assert (raw_net_input.shape[1] == len(input_scales))
scale_matrix = np.tile(np.array(input_scales), (self.nposes, 1))
net_input = raw_net_input / scale_matrix
return net_input
def load_xivo_data(self, results_file, return_est=False):
est = EstimatorData(results_file, adjust_startend_to_samplecov=True)
all_x = np.zeros((est.nposes, input_dim))
all_y = np.zeros((est.nposes, cov_tri_len))
for i in range(est.nposes):
P = est.sample_covWTV[ind_l:ind_u, ind_l:ind_u, i]
P_triangle = upper_triangular_list(P, return_numpy=True, ret_dim=False)
all_y[i,:] = P_triangle
Pest = est.P[ind_l:ind_u, ind_l:ind_u, i]
Pest_triangle = upper_triangular_list(Pest, return_numpy=True, ret_dim=False)
if input_mode=="cov":
all_x[i,:] = Pest_triangle
elif input_mode=="gsbcov":
x = np.concatenate((
est.Rsb[i].as_rotvec(),
est.Tsb[:,i].tolist(),
Pest_triangle
))
all_x[i,:] = x
elif input_mode=="gsbvcov":
x = np.concatenate((
est.Rsb[i].as_rotvec(),
est.Tsb[:,i].tolist(),
est.Vsb[:,i].tolist(),
Pest_triangle
))
all_x[i,:] = x
else:
raise ValueError("Invalid input mode")
if return_est:
return (all_x, all_y, est)
else:
return (all_x, all_y)
def write_sample_cov_to_dataset(self, output_filename):
first_ind = self.half_window_size
last_ind = self.est.nposes - self.half_window_size
for ind in range(self.est.nposes):
if (ind >= first_ind) and (ind < last_ind):
self.est.assign_val(ind, "has_sample_cov", True)
self.est.assign_val(ind, "sample_cov_WTV",
upper_triangular_list(self.sample_covWTV[:,:,ind]))
else:
self.est.assign_val(ind, "has_sample_cov", False)
self.est.assign_val(ind, "sample_cov_WTV", [])
self.est.add_param("sample_cov_window", self.sample_cov_window_size)
self.est.write_json(output_filename)
y_train = np.zeros((num_train_cases*num_timesteps, cov_tri_len))
y_test = np.zeros((num_test_cases*num_timesteps, cov_tri_len))
# Collect training data
idx = 0
for i in range(num_train_cases):
inputs = all_inputs[:,:,i]
simdata = DiscreteTimeSystemSimulation(sys, est, x_init, P_init,
2*len(beacon_x), inputs, nruns)
simdata.sim_all_runs()
simdata.calc_mean()
simdata.calc_true_cov()
train_simdatas.append(simdata)
for j in range(num_timesteps):
Pest_upper_tri = upper_triangular_list(simdata.Pest[0,j,:,:], return_numpy=True, ret_dim=False)
Ptrue_upper_tri = upper_triangular_list(simdata.P[j,:,:], return_numpy=True, ret_dim=False)
xest = simdata.xest[0,j,:]
x_train_cov[idx,:] = Pest_upper_tri
x_train_statecov[idx,:] = np.concatenate((xest, Pest_upper_tri))
y_train[idx,:] = Ptrue_upper_tri
idx += 1
# Collect test data
idx = 0
for i in range(num_test_cases):
case_idx = num_train_cases + i
inputs = all_inputs[:,:,case_idx]
simdata = DiscreteTimeSystemSimulation(sys, est, x_init, P_init,
2*len(beacon_x), inputs, nruns)
# Scale the output
#net_output_train *= np.tile(train_output_maxes, (num_train_pts,1))
#net_output_test *= np.tile(train_output_maxes, (num_test_pts,1))
# put the output back into the simulation data and compute NEES
for iii in range(len(data_dict["train_simdata"])):
simdata = copy.deepcopy(data_dict["train_simdata"][iii])
npts = simdata.nruns * simdata.num_timesteps
all_states = np.reshape(simdata.xest, (npts, state_dim))
all_Pest = np.zeros((npts, cov_tri_len))
idx = 0
for i in range(simdata.nruns):
for j in range(simdata.num_timesteps):
all_Pest[idx, :] = upper_triangular_list(
simdata.Pest[i, j, :, :],
return_numpy=True,
ret_dim=False)
idx += 1
if input_mode == "cov":
net_input = all_Pest
elif input_mode == "statecov":
net_input = np.hstack((all_states, all_Pest))
net_input = net_input / np.tile(train_input_maxes, (npts, 1))
net_output = network.predict(net_input)
idx = 0
for j in range(simdata.num_timesteps):
Q = np.reshape(net_output[idx, :], (cov_dim, cov_dim))
cov = Q @ Q.T
tri_idx = 0
def onVisionUpdate(self, estimator, datum):
ts, content = datum
# Get camera pose
gsb = np.array(estimator.gsb())
gbc = np.array(estimator.gbc())
gsc = np.array(estimator.gsc())
qsb = mat2quat(gsb[:3, :3])
Tsb = gsb[:, 3]
qbc = mat2quat(gbc[:3, :3])
Tbc = gbc[:, 3]
qsc = mat2quat(gsc[:3, :3])
Tsc = gsc[:, 3]
Vsb = np.array(estimator.Vsb())
# Get calibration states
bg = estimator.bg()
ba = estimator.ba()
qg = mat2quat(estimator.Rg())
td = estimator.td()
Ca = estimator.Ca()
Cg = estimator.Cg()
# Get filter covariance
if self.save_full_cov:
Pstate = np.array(estimator.P())
else:
Pstate = np.array(estimator.Pstate())
# Group ID
GaugeGroup = estimator.gauge_group()
# Get filter innovations
if not estimator.MeasurementUpdateInitialized():
inn_Tsb = np.zeros((3, ))
inn_Wsb = np.zeros((3, ))
inn_Vsb = np.zeros((3, ))
else:
inn_Tsb = np.array(estimator.inn_Tsb())
inn_Wsb = np.array(estimator.inn_Wsb())
inn_Vsb = np.array(estimator.inn_Vsb())
# Get features
num_instate_features = estimator.num_instate_features()
if num_instate_features > 0:
feature_positions = estimator.InstateFeaturePositions()
feature_covs = estimator.InstateFeatureCovs()
feature_ids = estimator.InstateFeatureIDs()
feature_sinds = estimator.InstateFeatureSinds()
else:
feature_positions = []
feature_covs = []
feature_ids = []
feature_sinds = []
# Get groups
num_instate_groups = estimator.num_instate_groups()
if num_instate_groups > 0:
group_covs = estimator.InstateGroupCovs()
group_sinds = estimator.InstateGroupSinds()
group_ids = estimator.InstateGroupIDs()
group_poses = estimator.InstateGroupPoses()
else:
group_poses = []
group_covs = []
group_ids = []
group_sinds = []
# Save results
entry = dict()
entry['group'] = GaugeGroup
entry['ImagePath'] = str(content)
entry['Timestamp'] = ts
entry['Tsb_XYZ'] = Tsb.tolist()
entry['qsb_WXYZ'] = qsb.tolist()
entry['Tbc_XYZ'] = Tbc.tolist()
entry['qbc_WXYZ'] = qbc.tolist()
entry['Tsc_XYZ'] = Tsc.tolist()
entry['qsc_WXYZ'] = qsc.tolist()
entry['Vsb_XYZ'] = Vsb.tolist()
entry['Pstate'] = upper_triangular_list(Pstate)
entry[
'MeasurementUpdateInitialized'] = estimator.MeasurementUpdateInitialized(
)
entry['inn_Tsb'] = inn_Tsb.tolist()
entry['inn_Wsb'] = inn_Wsb.tolist()
entry['inn_Vsb'] = inn_Vsb.tolist()
entry['bg'] = bg.tolist()
entry['ba'] = ba.tolist()
entry['qg_WXYZ'] = qg.tolist()
entry['td'] = td
entry['Ca'] = Ca
entry['Cg'] = Cg
entry['num_instate_features'] = num_instate_features
entry['feature_positions'] = feature_positions
entry['feature_covs'] = feature_covs
entry['feature_ids'] = feature_ids
entry['feature_sinds'] = feature_sinds
entry['num_instate_groups'] = num_instate_groups
entry['group_poses'] = group_poses
entry['group_covs'] = group_covs
entry['group_ids'] = group_ids
entry['group_sinds'] = group_sinds
self.results.append(entry)