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)