def load_vio(self, dataset, args):
"""
load ts, p, q, v from vio states, load ba and bg from calibration states
"""
logging.info("loading vio states from " +
osp.join(args.root_dir, dataset, "evolving_state.txt"))
vio_states = np.loadtxt(osp.join(args.root_dir, dataset,
"evolving_state.txt"),
delimiter=",")
vio_calibs = np.loadtxt(osp.join(args.root_dir, dataset,
"calib_state.txt"),
delimiter=",")
self.vio_ts = vio_states[:, 0] * 1e-6
self.vio_p = vio_states[:, 5:8]
self.vio_v = vio_states[:, 8:11]
self.vio_rq = vio_states[:, 1:5]
vio_r = Rotation.from_quat(
np.concatenate([
self.vio_rq[:, 1:4],
np.expand_dims(self.vio_rq[:, 0], axis=1)
],
axis=1))
self.vio_eul = vio_r.as_euler("xyz", degrees=True)
self.vio_R = vio_r.as_matrix()
self.vio_calib_ts = vio_calibs[:, 0] * 1e-6
self.vio_ba = vio_calibs[:, 28:31]
self.vio_bg = vio_calibs[:, 31:34]
self.vio_accelScaleInv = vio_calibs[:, 1:10].reshape((-1, 3, 3))
self.vio_gyroScaleInv = vio_calibs[:, 10:19].reshape((-1, 3, 3))
self.vio_gyroGSense = vio_calibs[:, 19:28].reshape((-1, 3, 3))
def run_on_each_dataset_and_gather_metrics(args, data_names):
all_metrics = {}
# retrieve metric from old, logs, they would be erased if necessary
# I use the presence of .svg or .png as the flag for something necessary
if osp.exists(args.log_dir + "/metrics.json"):
with open(args.log_dir + "/metrics.json", "r") as f:
logging.info(f"Loading old metric file at {args.log_dir + '/metrics.json'}")
all_metrics = json.load(f)
for dataset in progressbar.progressbar(data_names, redirect_stdout=True):
logging.info(f"Plotting dataset {dataset}")
try:
results_folder = os.path.join(args.log_dir, dataset)
# if osp.exists(osp.join(results_folder, "position-3d.png")):
# logging.info(f"Skipping {dataset} because alraedy processed")
# continue
metric_map = run(args, dataset)
all_metrics[dataset] = metric_map
with open(args.log_dir + "/metrics.json", "w") as f:
json.dump(all_metrics, f, indent=1)
except ValueError as e:
raise e
except OSError as e:
print(e)
continue
except Exception as e:
raise e
def do_synthesis(self, input_text):
input_text = self.tts_pause.add_pause(input_text)
print("input_text>>>>", input_text)
logging.info(
"[TTSModel] [do_synthesis] input_text:{}".format(input_text))
input_ids = self.processor.text_to_sequence(input_text, inference=True)
# nput_ids = np.concatenate([input_ids, [219 - 1]], -1)
self.interpreter.resize_tensor_input(self.input_details[0]['index'],
[1, len(input_ids)])
self.interpreter.allocate_tensors()
input_data = self.prepare_input(input_ids)
for i, detail in enumerate(self.input_details):
input_shape = detail['shape']
self.interpreter.set_tensor(detail['index'], input_data[i])
# self.interpreter.invoke()
decoder_output_tflite, mel_outputs = self.interpreter.get_tensor(
self.output_details[0]['index']), interpreter.get_tensor(
self.output_details[1]['index'])
remove_end = 1024
audio = self.mb_melgan.inference(mel_outputs)[0, :-remove_end, 0]
return mel_outputs.numpy(), decoder_output_tflite.numpy(), audio.numpy(
)
def __init__(self, model_path, force_cpu=False):
# load trained network model
if not torch.cuda.is_available() or force_cpu:
self.device = torch.device("cpu")
self.net = torch.jit.load(model_path, map_location="cpu")
else:
self.device = torch.device("cuda:0")
self.net = torch.jit.load(model_path)
self.net.to(self.device)
logging.info("Model {} loaded to device {}.".format(
model_path, self.device))
def load_sim_data(self, args):
"""
This loads simulation data from an imu.csv file containing
perfect imu data.
"""
logging.info("loading simulation data from " + args.sim_data_path)
sim_data = np.loadtxt(
args.sim_data_path,
delimiter=",",
usecols=(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16),
)
ts_all = sim_data[:, 0]
vio_p = sim_data[:, 1:4]
vio_rq = sim_data[:, 4:8]
acc_all = sim_data[:, 8:11]
vio_v = sim_data[:, 11:14]
gyr_all = sim_data[:, 14:17]
# add sim noise and bias
if args.add_sim_imu_noise:
wa = args.sim_sigma_na * np.random.normal(0, 1, acc_all.shape)
wg = args.sim_sigma_ng * np.random.normal(0, 1, gyr_all.shape)
acc_all = acc_all + wa
gyr_all = gyr_all + wg
sim_ba = np.array([0.3, -0.2, 0.4])
sim_bg = np.array([0.0005, 0.002, -0.001])
acc_all = acc_all + sim_ba
gyr_all = gyr_all + sim_bg
if args.start_from_ts is not None:
idx_start = np.where(ts_all >= args.start_from_ts * 1e-6)[0][0]
else:
idx_start = 50
self.ts_all = ts_all[idx_start:] * 1e6
self.acc_all = 0.5 * (acc_all[idx_start:, :] +
acc_all[idx_start - 1:-1, :])
self.gyr_all = 0.5 * (gyr_all[idx_start:, :] +
gyr_all[idx_start - 1:-1, :])
# self.acc_all = acc_all[idx_start:,:]
# self.gyr_all = gyr_all[idx_start:,:]
self.vio_ts = ts_all[idx_start - 1:]
self.vio_p = vio_p[idx_start - 1:, :]
self.vio_v = vio_v[idx_start - 1:, :]
self.vio_rq = vio_rq[idx_start - 1:, :]
vio_r = Rotation.from_quat(self.vio_rq)
self.vio_eul = vio_r.as_euler("xyz", degrees=True)
self.vio_R = vio_r.as_matrix()
self.dataset_size = self.ts_all.shape[0]
self.init_ts = self.ts_all[0]
def save_model(args, epoch, network, optimizer, interrupt=False):
if interrupt:
model_path = osp.join(args.out_dir, "checkpoints", "checkpoint_latest.pt")
else:
model_path = osp.join(args.out_dir, "checkpoints", "checkpoint_%d.pt" % epoch)
state_dict = {
"model_state_dict": network.state_dict(),
"epoch": epoch,
"optimizer_state_dict": optimizer.state_dict(),
"args": vars(args),
}
torch.save(state_dict, model_path)
logging.info(f"Model saved to {model_path}")
def compare_biases(args):
with open(args.data_list) as f:
data_names = [
s.strip().split("," or " ")[0]
for s in f.readlines()
if len(s) > 0 and s[0] != "#"
]
# get headset name
headset_names = set()
for dataset in data_names:
headset_name = dataset.split("_")[1].strip("hidacori")
headset_names.add(headset_name)
logging.info(f"Got {len(headset_names)} different hw")
# assign color to each
headset_names_colors = {}
for i, hw in enumerate(headset_names):
headset_names_colors[hw] = "C" + str(i)
for dataset in data_names:
headset_name = dataset.split("_")[1].strip("hidacori")
results_folder = os.path.join(args.log_dir, dataset)
states = np.load(os.path.join(results_folder, args.log_filename + ".npy"))
ts = states[:, 27]
ts = ts - ts[0]
ba = states[:, 15:18]
bg = states[:, 18:21]
sigma_bg = np.sqrt(states[:, 37:40])
sigma_ba = np.sqrt(states[:, 40:43])
plot_state_euclidean(
"accel bias",
dataset,
["ba_x", "ba_y", "ba_z"],
ts,
ba,
sigma_ba,
color=headset_names_colors[headset_name],
)
plt.legend([])
plot_state_euclidean(
"gyro bias",
dataset,
["bg_x", "bg_y", "bg_z"],
ts,
np.rad2deg(bg) * 3600,
np.rad2deg(sigma_bg) * 3600,
color=headset_names_colors[headset_name],
)
plt.legend([])
plt.show()
def post(self):
if not request.json or 'content' not in request.json :
res = { "code": "400", "data": {}, "message": "request is not json or content not in json" }
return jsonify ( res )
else:
logging.info( "[QDNetInference] [post] request.json:{}".format( request.json ) )
url = request.json["content"]
logging.info( "[QDNetInference] [post] url:{}".format( url ) )
data = url2imgcv2(url)
pre = qdnet_model.predict(data)
res = { "code": "200", "data": pre, "message": "" }
return jsonify ( res )
def text_to_pinyin_sequence(self, text):
# pinyin = self.processor.pinyin_parser(text, style=Style.TONE3, errors="ignore")
pinyin, text = self.tts_py.get_pyin(text)
new_pinyin = []
for x in str(pinyin).split(" "):
if "#" not in x:
new_pinyin.append(x)
phonemes = self.processor.get_phoneme_from_char_and_pinyin(
text, new_pinyin)
text = " ".join(phonemes)
print("phoneme seq: {}".format(text))
logging.info(
"[TTSModel] [text_to_pinyin_sequence] phoneme seq:{}".format(text))
input_ids = self.processor.text_to_sequence(text, inference=False)
return input_ids
def do_synthesis(self, input_text):
input_text = self.tts_pause.add_pause(input_text)
print("input_text>>>>", input_text)
logging.info(
"[TTSModel] [do_synthesis] input_text:{}".format(input_text))
input_ids = self.processor.text_to_sequence(input_text, inference=True)
_, mel_outputs, stop_token_prediction, alignment_history = self.tacotron2.inference(
tf.expand_dims(tf.convert_to_tensor(input_ids, dtype=tf.int32), 0),
tf.convert_to_tensor([len(input_ids)], tf.int32),
tf.convert_to_tensor([0], dtype=tf.int32))
remove_end = 1024
audio = self.mb_melgan.inference(mel_outputs)[0, :-remove_end, 0]
return mel_outputs.numpy(), alignment_history.numpy(), audio.numpy()
def __init__(self, model_path, arch, net_config, force_cpu=False):
# network
self.net = get_model(arch, net_config, 6, 3)
# load trained network model
if not torch.cuda.is_available() or force_cpu:
self.device = torch.device("cpu")
checkpoint = torch.load(
model_path, map_location=lambda storage, location: storage)
else:
self.device = torch.device("cuda:0")
checkpoint = torch.load(model_path)
self.net.load_state_dict(checkpoint["model_state_dict"])
self.net.eval().to(self.device)
logging.info("Model {} loaded to device {}.".format(
model_path, self.device))
def post(self):
if not request.json or 'content' not in request.json:
res = {
"code": "400",
"data": {},
"message": "request is not json or content not in json"
}
return jsonify(res)
else:
logging.info("[TTSInference] [post] request.json:{}".format(
request.json))
text = request.json["content"]
logging.info("[TTSInference] [post] text:{}".format(text))
mels, alignment_history, audios = tts_model.do_synthesis(text)
res = {"code": "200", "data": audios.tolist(), "message": ""}
return jsonify(res)
def write_summary(summary_writer, attr_dict, epoch, optimizer, mode):
""" Given the attr_dict write summary and log the losses """
mse_loss = np.mean((attr_dict["targets"] - attr_dict["preds"]) ** 2, axis=0)
ml_loss = np.average(attr_dict["losses"])
sigmas = np.exp(attr_dict["preds_cov"])
summary_writer.add_scalar(f"{mode}_loss/loss_x", mse_loss[0], epoch)
summary_writer.add_scalar(f"{mode}_loss/loss_y", mse_loss[1], epoch)
summary_writer.add_scalar(f"{mode}_loss/loss_z", mse_loss[2], epoch)
summary_writer.add_scalar(f"{mode}_loss/avg", np.mean(mse_loss), epoch)
summary_writer.add_scalar(f"{mode}_dist/loss_full", ml_loss, epoch)
summary_writer.add_histogram(f"{mode}_hist/sigma_x", sigmas[:, 0], epoch)
summary_writer.add_histogram(f"{mode}_hist/sigma_y", sigmas[:, 1], epoch)
summary_writer.add_histogram(f"{mode}_hist/sigma_z", sigmas[:, 2], epoch)
if epoch > 0:
summary_writer.add_scalar(
"optimizer/lr", optimizer.param_groups[0]["lr"], epoch - 1
)
logging.info(
f"{mode}: average ml loss: {ml_loss}, average mse loss: {mse_loss}/{np.mean(mse_loss)}"
)
def on_imu_measurement(self, t_us, gyr_raw, acc_raw):
assert isinstance(t_us, int)
if self.filter.initialized:
return self._on_imu_measurement_after_init(t_us, gyr_raw, acc_raw)
else:
logging.info(f"Initializing filter at time {t_us*1e-6}")
if self.icalib:
logging.info(f"Using bias from initial calibration")
init_ba = self.icalib.accelBias
init_bg = self.icalib.gyroBias
# calibrate raw imu data
acc_biascpst, gyr_biascpst = self.icalib.calibrate_raw(
acc_raw, gyr_raw) # removed offline bias and scaled
else:
logging.info(f"Using zero bias")
init_ba = np.zeros((3, 1))
init_bg = np.zeros((3, 1))
acc_biascpst, gyr_biascpst = acc_raw, gyr_raw
self.filter.initialize(acc_biascpst, t_us, init_ba, init_bg)
self.next_interp_t_us = t_us
self.next_aug_t_us = t_us
self._add_interpolated_imu_to_buffer(acc_biascpst, gyr_biascpst,
t_us)
self.next_aug_t_us = t_us + self.dt_update_us
self.last_t_us, self.last_acc, self.last_gyr = (
t_us,
acc_biascpst,
gyr_biascpst,
)
return False
def arg_conversion(args):
""" Conversions from time arguments to data size """
if not (args.past_time * args.imu_freq).is_integer():
raise ValueError(
"past_time cannot be represented by integer number of IMU data.")
if not (args.window_time * args.imu_freq).is_integer():
raise ValueError(
"window_time cannot be represented by integer number of IMU data.")
if not (args.future_time * args.imu_freq).is_integer():
raise ValueError(
"future_time cannot be represented by integer number of IMU data.")
if not (args.imu_freq / args.sample_freq).is_integer():
raise ValueError("sample_freq must be divisible by imu_freq.")
data_window_config = dict([
("past_data_size", int(args.past_time * args.imu_freq)),
("window_size", int(args.window_time * args.imu_freq)),
("future_data_size", int(args.future_time * args.imu_freq)),
("step_size", int(args.imu_freq / args.sample_freq)),
])
net_config = {
"in_dim": (data_window_config["past_data_size"] +
data_window_config["window_size"] +
data_window_config["future_data_size"]) // 32 + 1
}
# Display
np.set_printoptions(formatter={"all": "{:.6f}".format})
logging.info(f"Training/testing with {args.imu_freq} Hz IMU data")
logging.info("Size: " + str(data_window_config["past_data_size"]) + "+" +
str(data_window_config["window_size"]) + "+" +
str(data_window_config["future_data_size"]) + ", " +
"Time: " + str(args.past_time) + "+" + str(args.window_time) +
"+" + str(args.future_time))
logging.info("Perturb on bias: %s" % args.do_bias_shift)
logging.info("Perturb on gravity: %s" % args.perturb_gravity)
logging.info("Sample frequency: %s" % args.sample_freq)
return data_window_config, net_config
def pose_integrate(args, dataset, preds):
"""
Concatenate predicted velocity to reconstruct sequence trajectory
"""
dp_t = args.window_time
pred_vels = preds / dp_t
ind = np.array([i[1] for i in dataset.index_map], dtype=np.int)
delta_int = int(args.window_time * args.imu_freq /
2.0) # velocity as the middle of the segment
if not (args.window_time * args.imu_freq / 2.0).is_integer():
logging.info("Trajectory integration point is not centered.")
ind_intg = ind + delta_int # the indices of doing integral
ts = dataset.ts[0]
dts = np.mean(ts[ind_intg[1:]] - ts[ind_intg[:-1]])
pos_intg = np.zeros([pred_vels.shape[0] + 1, args.output_dim])
pos_intg[0] = dataset.gt_pos[0][ind_intg[0], :]
pos_intg[1:] = np.cumsum(pred_vels[:, :] * dts, axis=0) + pos_intg[0]
ts_intg = np.append(ts[ind_intg], ts[ind_intg[-1]] + dts)
ts_in_range = ts[ind_intg[0]:ind_intg[-1]] # s
pos_pred = interp1d(ts_intg, pos_intg, axis=0)(ts_in_range)
pos_gt = dataset.gt_pos[0][ind_intg[0]:ind_intg[-1], :]
ori_pred = dataset.orientations[0][ind_intg[0]:ind_intg[-1], :]
ori_gt = dataset.gt_ori[0][ind_intg[0]:ind_intg[-1], :]
eul_pred = Rotation.from_quat(ori_pred).as_euler("xyz", degrees=True)
eul_gt = Rotation.from_quat(ori_gt).as_euler("xyz", degrees=True)
traj_attr_dict = {
"ts": ts_in_range,
"pos_pred": pos_pred,
"pos_gt": pos_gt,
"eul_pred": eul_pred,
"eul_gt": eul_gt,
}
return traj_attr_dict
def plot_autocorellation(args, log_folder, n, dataset):
results_folder = log_folder + "/" + dataset
filename = os.path.join(results_folder, args.log_filename + ".npy")
try:
logging.info(f"Loading {filename}")
states = np.load(filename)
except:
logging.error(
f"{filename}.npy was not found. Surely means filter did not finish"
)
raise FileNotFoundError
# load all states
meas = states[:, 46:49]
if os.path.exists(os.path.join(results_folder, "vio_states.npy")):
vio_states = np.load(os.path.join(results_folder, "vio_states.npy"))
ref_disp = vio_states[:, 15:18]
else:
logging.error(
"vio_states.npy was not found. you shoud create it with plot_state.py before... sorry :("
)
raise FileNotFoundError
fig = plt.figure("autocorellation " + dataset)
meas_err = meas - ref_disp
meas_err_update = meas_err[~np.isnan(meas_err).any(axis=1)]
logging.warning("We assume update frequency at 20hz for autocorrelation")
for i in range(3):
plt.subplot(3, 1, i + 1)
plt.acorr(meas_err_update[:, i], maxlags=100, lw=2, usevlines=False, label=n)
locs, labels = plt.xticks() # Get locations and labels
for (l, t) in zip(locs, labels):
t.set_text(str(l / 20.0) + "s")
plt.xticks(locs, labels) # Set locations and labels
plt.xlim(left=0)
plt.grid()
plt.legend()
def compute_rpe_ronin(ns_rpe):
rpe_rmse, rpe_rmse_z, relative_yaw_rmse = compute_rpe(
ns_rpe, ps_ronin, ps_gt, euls_aekf[:, [2]], euls_gt[:, [2]]
)
logging.info(f"RPE RMSE of ronin over 1s: {rpe_rmse}")
logging.info(f"RPE RMSE Z of ronin over 1s: {rpe_rmse_z}")
logging.info(f"RPE RMSE Yaw of ronin over 1s: {relative_yaw_rmse}")
metric_map["ronin"]["rpe_rmse_" + str(ns_rpe)] = rpe_rmse
metric_map["ronin"]["rpe_rmse_z_" + str(ns_rpe)] = rpe_rmse_z
metric_map["ronin"]["relative_yaw_rmse_" + str(ns_rpe)] = relative_yaw_rmse
def compute_rpe_filter(ns_rpe):
rpe_rmse, rpe_rmse_z, relative_yaw_rmse = compute_rpe_distance(
ns_rpe, ps_filter, ps_gt, euls_filter[:, [2]], euls_gt[:, [2]]
)
logging.info(f"RPE RMSE of filter over {1e-3*ns_rpe}s: {rpe_rmse}")
logging.info(f"RPE RMSE Z of filter over {1e-3*ns_rpe}s: {rpe_rmse_z}")
logging.info(f"RPE RMSE Yaw of filter over {1e-3*ns_rpe}s: {relative_yaw_rmse}")
metric_map["filter"]["rpe_rmse_" + str(ns_rpe)] = rpe_rmse
metric_map["filter"]["rpe_rmse_z_" + str(ns_rpe)] = rpe_rmse_z
metric_map["filter"]["relative_yaw_rmse_" + str(ns_rpe)] = relative_yaw_rmse
help="Path to dataset directory",
)
io_groups.add_argument("--save_plot", action="store_true")
io_groups.add_argument("--concatenate", action="store_true")
args = parser.parse_args()
all_models = list(Path.cwd().glob(args.model_globbing))
def save_plot_arg(yes):
if yes:
return f"--save_plot"
else:
return f"--no-save_plot"
logging.info(f"Found {len(all_models)} models")
logging.info(f"Found {all_models}")
for m in all_models:
base_folder = Path(m).parent
logging.info(base_folder)
name_run = str(Path(m).parents[1].name) + "-" + str(
Path(m).parents[0].name)
if not osp.exists(f"./{args.out_dir}/{name_run}/"):
os.mkdir(f"./{args.out_dir}/{name_run}/")
# read yaml
with open(str(base_folder) + "/parameters.json", "r") as f:
conf = json.load(f)
print(conf)
sample_freq = 20.0 # no concatenation
if args.concatenate:
def run(args, dataset):
plt.close("all")
if args.dir is not None:
results_folder = args.dir
else:
results_folder = os.path.join(args.log_dir, dataset)
try:
print("here wtf")
print(os.path.join(results_folder, args.log_filename + ".npy"))
states = np.load(os.path.join(results_folder, args.log_filename + ".npy"))
save_vio_states = True # traj is done
except:
logging.warning(
"Relying on .txt file because .npy was not found. Surely means filter did not finish"
)
states = np.loadtxt(
os.path.join(results_folder, args.log_filename), delimiter=","
) # traj is still processing
save_vio_states = False # traj is done
R_init = states[0, :9].reshape(-1, 3, 3)
r_init = Rotation.from_matrix(R_init)
Rs = states[:, :9].reshape(-1, 3, 3)
rs = Rotation.from_matrix(Rs)
euls = rs.as_euler("xyz", degrees=True)
vs = states[:, 9:12]
ps = states[:, 12:15]
ps_dr = np.cumsum(
states[:, 9:12] * np.diff(states[:, 27:28], prepend=states[0, 27], axis=0),
axis=0,
)
ba = states[:, 15:18]
bg = states[:, 18:21]
accs = states[:, 21:24] # offline calib compensated, scale+bias
gyrs = states[:, 24:27] # offline calib compensated, scale+bias
ts = states[:, 27]
sigma_r = np.sqrt(states[:, 28:31]) * 180.0 / np.pi
sigma_v = np.sqrt(states[:, 31:34])
sigma_p = np.sqrt(states[:, 34:37])
sigma_bg = np.sqrt(states[:, 37:40])
sigma_ba = np.sqrt(states[:, 40:43])
innos = states[:, 43:46]
meas = states[:, 46:49]
pred = states[:, 49:52]
meas_sigma = states[:, 52:55]
inno_sigma = states[:, 55:58]
nobs_sigma = states[:, 58 : 58 + 16]
N = ts.shape[0]
# get RoNIN concatenation results
if args.ronin_dir is not None:
try:
ronin = np.loadtxt(
osp.join(args.ronin_dir, dataset + ".txt"), delimiter=","
)
logging.info(
f"Reading ronin data from {osp.join(args.ronin_dir, dataset)}.txt"
)
except:
ronin = np.loadtxt(
osp.join(args.ronin_dir, dataset, "trajectory.txt"), delimiter=","
)
logging.info(
f"Reading ronin data from {osp.join(args.ronin_dir, dataset, 'trajectory.txt')}"
)
ronin_ts = ronin[:, 0]
ronin_p = ronin[:, 1:4]
if ronin_ts[0] > ts[0]:
ronin_ts = np.insert(ronin_ts, 0, ts[0])
ronin_p = np.concatenate([ronin_p[0].reshape(1, 3), ronin_p], axis=0)
if ronin_ts[-1] < ts[-1]:
ronin_ts = np.insert(ronin_ts, -1, ts[-1])
ronin_p = np.concatenate([ronin_p, ronin_p[-1].reshape(1, 3)], axis=0)
ronin_p = interp1d(ronin_ts, ronin_p, axis=0)(ts)
# get vio states
if args.plot_sim is False:
if os.path.exists(os.path.join(results_folder, "vio_states.npy")):
vio_states = np.load(os.path.join(results_folder, "vio_states.npy"))
vio_euls = vio_states[:, :3]
vio_p = vio_states[:, 3:6]
vio_v = vio_states[:, 6:9]
vio_ba = vio_states[:, 9:12]
vio_bg = vio_states[:, 12:15]
vio_disp = vio_states[:, 15:18]
vio_ba_b = vio_states[:, 18:21]
vio_bg_b = vio_states[:, 21:24]
vio_accelScaleInv_flat = vio_states[:, 24:33]
vio_gyroScaleInv_flat = vio_states[:, 33:42]
vio_accelScaleInv = vio_accelScaleInv_flat.reshape((-1, 3, 3))
vio_gyroScaleInv = vio_gyroScaleInv_flat.reshape((-1, 3, 3))
else:
vio_states = np.loadtxt(
os.path.join(args.root_dir, dataset, "evolving_state.txt"),
delimiter=",",
)
vio_calibs = np.loadtxt(
os.path.join(args.root_dir, dataset, "calib_state.txt"), delimiter=","
)
vio_ts = vio_states[:, 0] * 1e-6
vio_rq = vio_states[:, 1:5]
vio_p = vio_states[:, 5:8]
vio_v = vio_states[:, 8:11]
vio_r = Rotation.from_quat(
np.concatenate(
[vio_rq[:, 1:4], np.expand_dims(vio_rq[:, 0], axis=1)], axis=1
)
)
vio_euls = vio_r.as_euler("xyz", degrees=True)
vio_calib_ts = vio_calibs[:, 0] * 1e-6
vio_accelScaleInv = vio_calibs[:, 1:10].reshape((-1, 3, 3))
vio_gyroScaleInv = vio_calibs[:, 10:19].reshape((-1, 3, 3))
vio_gyroGSense = vio_calibs[:, 19:28].reshape((-1, 3, 3))
vio_ba = vio_calibs[:, 28:31]
vio_bg = vio_calibs[:, 31:34]
vio_pj_idx = np.searchsorted(vio_ts, ts) - 1
vio_pi_idx = np.searchsorted(vio_ts, ts - args.displacement_time) - 1
vio_pj = vio_p[vio_pj_idx, :]
vio_pi = vio_p[vio_pi_idx, :]
vio_disp = vio_pj - vio_pi
vio_Ri = vio_r[vio_pi_idx].as_matrix()
ri_z = Rotation.from_matrix(vio_Ri).as_euler("xyz")[:, 2]
vio_Riz = Rotation.from_euler("z", ri_z).as_matrix()
vio_Rizt = np.transpose(vio_Riz, (0, 2, 1))
vio_disp = np.squeeze(
np.matmul(vio_Rizt, np.expand_dims(vio_disp, axis=-1))
)
vio_uw_euls = unwrap_rpy(vio_euls)
if vio_ts[0] > ts[0]:
vio_ts = np.insert(vio_ts, 0, ts[0])
vio_uw_euls = np.concatenate(
[vio_uw_euls[0].reshape(1, 3), vio_uw_euls], axis=0
)
vio_p = np.concatenate([vio_p[0].reshape(1, 3), vio_p], axis=0)
vio_v = np.concatenate([vio_v[0].reshape(1, 3), vio_v], axis=0)
if vio_ts[-1] < ts[-1]:
vio_ts = np.insert(vio_ts, -1, ts[-1])
vio_uw_euls = np.concatenate(
[vio_uw_euls, vio_uw_euls[-1].reshape(1, 3)], axis=0
)
vio_p = np.concatenate([vio_p, vio_p[-1].reshape(1, 3)], axis=0)
vio_v = np.concatenate([vio_v, vio_v[-1].reshape(1, 3)], axis=0)
vio_uw_euls_interp = interp1d(vio_ts, vio_uw_euls, axis=0)(ts)
vio_euls = wrap_rpy(vio_uw_euls_interp)
vio_p = interp1d(vio_ts, vio_p, axis=0)(ts)
vio_v = interp1d(vio_ts, vio_v, axis=0)(ts)
# This compute bias in non scaled sensor frame (I think)
vio_accelScaleInv_flat = vio_accelScaleInv.reshape((-1, 9))
vio_gyroScaleInv_flat = vio_gyroScaleInv.reshape((-1, 9))
if vio_calib_ts[0] > ts[0]:
vio_calib_ts = np.insert(vio_calib_ts, 0, ts[0])
vio_ba = np.concatenate([vio_ba[0].reshape(1, 3), vio_ba], axis=0)
vio_bg = np.concatenate([vio_bg[0].reshape(1, 3), vio_bg], axis=0)
vio_accelScaleInv_flat = np.concatenate(
[vio_accelScaleInv_flat[0].reshape(1, 9), vio_accelScaleInv_flat],
axis=0,
)
vio_gyroScaleInv_flat = np.concatenate(
[vio_gyroScaleInv_flat[0].reshape(1, 9), vio_gyroScaleInv_flat],
axis=0,
)
if vio_calib_ts[-1] < ts[-1]:
vio_calib_ts = np.insert(vio_calib_ts, -1, ts[-1])
vio_ba = np.concatenate([vio_ba, vio_ba[-1].reshape(1, 3)], axis=0)
vio_bg = np.concatenate([vio_bg, vio_bg[-1].reshape(1, 3)], axis=0)
vio_accelScaleInv_flat = np.concatenate(
[vio_accelScaleInv_flat, vio_accelScaleInv_flat[-1].reshape(1, 9)],
axis=0,
)
vio_gyroScaleInv_flat = np.concatenate(
[vio_gyroScaleInv_flat, vio_gyroScaleInv_flat[-1].reshape(1, 9)],
axis=0,
)
vio_ba = interp1d(vio_calib_ts, vio_ba, axis=0)(ts)
vio_bg = interp1d(vio_calib_ts, vio_bg, axis=0)(ts)
vio_accelScaleInv_flat = interp1d(
vio_calib_ts, vio_accelScaleInv_flat, axis=0
)(ts)
vio_gyroScaleInv_flat = interp1d(
vio_calib_ts, vio_gyroScaleInv_flat, axis=0
)(ts)
vio_accelScaleInv = vio_accelScaleInv_flat.reshape((-1, 3, 3))
vio_gyroScaleInv = vio_gyroScaleInv_flat.reshape((-1, 3, 3))
vio_ba_temp = np.expand_dims(vio_ba, axis=-1)
vio_bg_temp = np.expand_dims(vio_bg, axis=-1)
vio_ba_b = np.squeeze(
np.matmul(np.linalg.inv(vio_accelScaleInv), vio_ba_temp)
)
vio_bg_b = np.squeeze(
np.matmul(np.linalg.inv(vio_gyroScaleInv), vio_bg_temp)
)
if save_vio_states:
vio_states = np.concatenate(
[
vio_euls,
vio_p,
vio_v,
vio_ba,
vio_bg,
vio_disp,
vio_ba_b,
vio_bg_b,
vio_accelScaleInv_flat,
vio_gyroScaleInv_flat,
],
axis=1,
)
np.save(os.path.join(results_folder, "vio_states.npy"), vio_states)
else:
logging.warning(
"Not saving vio_states.npy because traj is still processing"
)
# get simulation states
if args.plot_sim:
sim_data = np.loadtxt(
args.sim_data_path,
delimiter=",",
usecols=(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16),
)
sim_ts = sim_data[:, 0]
sim_p = sim_data[:, 1:4]
sim_rq = sim_data[:, 4:8]
sim_v = sim_data[:, 11:14]
# acc_all = sim_data[:,8:11]
# gyr_all = sim_data[:,14:17]
sim_r = Rotation.from_quat(sim_rq)
sim_euls = sim_r.as_euler("xyz", degrees=True)
sim_pj_idx = np.searchsorted(sim_ts, ts) - 1
sim_pi_idx = np.searchsorted(sim_ts, ts - args.displacement_time) - 1
sim_pj = sim_p[sim_pj_idx, :]
sim_pi = sim_p[sim_pi_idx, :]
sim_disp = sim_pj - sim_pi
ri_z = sim_r[sim_pi_idx].as_matrix().as_euler("xyz")[:, 2]
sim_Riz = Rotation.from_euler("z", ri_z).as_matrix()
sim_Rizt = np.transpose(sim_Riz, (0, 2, 1))
sim_disp = np.squeeze(np.matmul(sim_Rizt, np.expand_dims(sim_disp, axis=-1)))
sim_uw_euls = unwrap_rpy(sim_euls)
if sim_ts[0] > ts[0]:
sim_ts = np.insert(sim_ts, 0, ts[0])
sim_uw_euls = np.concatenate(
[sim_uw_euls[0].reshape(1, 3), sim_uw_euls], axis=0
)
sim_p = np.concatenate([sim_p[0].reshape(1, 3), sim_p], axis=0)
sim_v = np.concatenate([sim_v[0].reshape(1, 3), sim_v], axis=0)
if sim_ts[-1] < ts[-1]:
sim_ts = np.insert(sim_ts, -1, ts[-1])
sim_uw_euls = np.concatenate(
[sim_uw_euls, sim_uw_euls[-1].reshape(1, 3)], axis=0
)
sim_p = np.concatenate([sim_p, sim_p[-1].reshape(1, 3)], axis=0)
sim_v = np.concatenate([sim_v, sim_v[-1].reshape(1, 3)], axis=0)
sim_uw_euls_interp = interp1d(sim_ts, sim_uw_euls, axis=0)(ts)
sim_euls = wrap_rpy(sim_uw_euls_interp)
sim_p = interp1d(sim_ts, sim_p, axis=0)(ts)
sim_v = interp1d(sim_ts, sim_v, axis=0)(ts)
sim_ba = np.zeros((ts.shape[0], 3)) + np.array([0.3, -0.2, 0.4])
sim_bg = np.zeros((ts.shape[0], 3)) + np.array([0.0005, 0.002, -0.001])
if args.plot_sim:
ref_type = "sim"
ref_p = sim_p
ref_v = sim_v
ref_bg = sim_bg
ref_ba = sim_ba
ref_euls = sim_euls
ref_disp = sim_disp
else:
ref_type = "vio"
ref_p = vio_p
ref_v = vio_v
ref_bg = vio_bg
ref_ba = vio_ba
ref_euls = vio_euls
ref_disp = vio_disp
# obtain biases in the body frame in the same unit
attitude_filter_path = osp.join(args.root_dir, dataset, "calib_state.txt")
(
init_gyroScaleInv,
init_gyroBias,
init_gyroGSense,
init_accelScaleInv,
init_accelBias,
) = load_aekf_calibration(attitude_filter_path)
if args.body_bias:
ba_temp = np.expand_dims(ba, axis=-1)
bg_temp = np.expand_dims(bg, axis=-1)
ba_b = np.squeeze(np.matmul(np.linalg.inv(init_accelScaleInv), ba_temp))
bg_b = np.squeeze(np.matmul(np.linalg.inv(init_gyroScaleInv), bg_temp))
ba = ba_b
bg = bg_b
# load aekf rotation
aekf_ts, aekf_R = load_aekf_rotation(attitude_filter_path)
aekf_euls = unwrap_rpy(aekf_R.as_euler("xyz", degrees=True))
# plotting
N = ts.shape[0]
start_idx = 2000 # 2 s
end_idx = N - 1
start_ts = ts[start_idx]
end_ts = ts[end_idx]
# align diverse trajectory sources
ts = ts - start_ts
aekf_ts = aekf_ts - start_ts
try:
# align at start_ts
aekf_euls[:, 2] -= (
interp1d(aekf_ts, aekf_euls[:, 2])(ts[start_idx]) - ref_euls[start_idx, 2]
)
except:
# if we can't align at first aekf timestamp
aekf_euls[:, 2] -= aekf_euls[0, 2] - interp1d(ts, ref_euls[:, 2])(aekf_ts[0])
aekf_euls_time_aligned = interp1d(aekf_ts, aekf_euls, axis=0, fill_value=np.nan)(ts)
aekf_euls_time_aligned = wrap_rpy(aekf_euls_time_aligned)
if args.ronin_dir is not None:
ronin_p = ronin_p - (ronin_p[start_idx, :] - ref_p[start_idx, :])
ps = ps - (ps[start_idx, :] - ref_p[start_idx, :])
euls[:, 2] = euls[:, 2] - (euls[start_idx, 2] - ref_euls[start_idx, 2])
euls = wrap_rpy(euls)
ps_gt = ref_p[start_idx:end_idx, :]
euls_gt = ref_euls[start_idx:end_idx, :]
euls_aekf = aekf_euls_time_aligned[start_idx:end_idx, :]
# metrics computation
metric_map = {"filter": {}, "ronin": {}}
# align on first error, WHY IS THAT?
ps_filter = ps[start_idx:end_idx, :]
euls_filter = euls[start_idx:end_idx, :]
ps_filter = ps_filter - (ps_filter[0, :] - ps_gt[0, :])
# get drift and ATE
ps_diff = ps_gt[1:, :] - ps_gt[:-1, :]
traj_length = np.sum(np.linalg.norm(ps_diff, axis=1))
drift_filter = np.linalg.norm(ps_filter[-1, :] - ps_gt[-1, :])
angular_drift_filter = np.linalg.norm(euls[-1, 2] - ref_euls[-1, 2])
filter_heading_error = wrap_rpy(euls - ref_euls)[:, 2]
ate_filter = np.sqrt(np.mean(np.linalg.norm(ps_filter - ps_gt, axis=1) ** 2))
metric_map["filter"]["drift_ratio"] = drift_filter / traj_length
metric_map["filter"]["ate"] = ate_filter
metric_map["filter"]["mhe"] = np.sqrt(
np.nansum(filter_heading_error ** 2)
/ np.count_nonzero(~(np.isnan(filter_heading_error)))
)
metric_map["filter"]["angular_drift_deg_hour"] = (
angular_drift_filter / ts.max() * 3600
)
logging.info(f"drift of filter {metric_map['filter']['drift_ratio']}")
logging.info(f"ATE of filter {metric_map['filter']['ate']}")
logging.info(f"Mean Heading error of filter {metric_map['filter']['mhe']}")
def compute_rpe_filter(ns_rpe):
rpe_rmse, rpe_rmse_z, relative_yaw_rmse = compute_rpe_distance(
ns_rpe, ps_filter, ps_gt, euls_filter[:, [2]], euls_gt[:, [2]]
)
logging.info(f"RPE RMSE of filter over {1e-3*ns_rpe}s: {rpe_rmse}")
logging.info(f"RPE RMSE Z of filter over {1e-3*ns_rpe}s: {rpe_rmse_z}")
logging.info(f"RPE RMSE Yaw of filter over {1e-3*ns_rpe}s: {relative_yaw_rmse}")
metric_map["filter"]["rpe_rmse_" + str(ns_rpe)] = rpe_rmse
metric_map["filter"]["rpe_rmse_z_" + str(ns_rpe)] = rpe_rmse_z
metric_map["filter"]["relative_yaw_rmse_" + str(ns_rpe)] = relative_yaw_rmse
if args.rpe_1 is True:
compute_rpe_filter(1000) # 1 s
if args.rpe_10 is True:
compute_rpe_filter(10000) # 10 s
if args.rpe_100 is True:
compute_rpe_filter(100000) # 100 s
if args.ronin_dir is not None:
ps_ronin = ronin_p[start_idx:end_idx, :]
ps_ronin = ps_ronin - (ps_ronin[0, :] - ps_gt[0, :])
drift_ronin = np.linalg.norm(ps_ronin[-1, :] - ps_gt[-1, :])
ate_ronin = np.sqrt(np.mean(np.linalg.norm(ps_ronin - ps_gt, axis=1) ** 2))
angular_drift_ronin = np.linalg.norm(
aekf_euls_time_aligned[-1, 2] - ref_euls[-1, 2]
)
heading_error_ronin = wrap_rpy(aekf_euls_time_aligned - ref_euls)[:, 2]
metric_map["ronin"]["drift_ratio"] = drift_ronin / traj_length
metric_map["ronin"]["ate"] = ate_ronin
metric_map["ronin"]["mhe"] = np.sqrt(
np.nansum(heading_error_ronin ** 2)
/ np.count_nonzero(~(np.isnan(heading_error_ronin)))
)
metric_map["ronin"]["angular_drift_deg_hour"] = (
angular_drift_ronin / ts.max() * 3600
)
logging.info(f"drift of ronin {metric_map['ronin']['drift_ratio']}")
logging.info(f"ATE of ronin {metric_map['ronin']['ate']}")
logging.info(f"Mean Heading error of ronin {metric_map['ronin']['mhe']}")
def compute_rpe_ronin(ns_rpe):
rpe_rmse, rpe_rmse_z, relative_yaw_rmse = compute_rpe(
ns_rpe, ps_ronin, ps_gt, euls_aekf[:, [2]], euls_gt[:, [2]]
)
logging.info(f"RPE RMSE of ronin over 1s: {rpe_rmse}")
logging.info(f"RPE RMSE Z of ronin over 1s: {rpe_rmse_z}")
logging.info(f"RPE RMSE Yaw of ronin over 1s: {relative_yaw_rmse}")
metric_map["ronin"]["rpe_rmse_" + str(ns_rpe)] = rpe_rmse
metric_map["ronin"]["rpe_rmse_z_" + str(ns_rpe)] = rpe_rmse_z
metric_map["ronin"]["relative_yaw_rmse_" + str(ns_rpe)] = relative_yaw_rmse
if args.rpe_1 is True:
compute_rpe_ronin(1000)
if args.rpe_10 is True:
compute_rpe_ronin(10000)
if args.rpe_100 is True:
compute_rpe_ronin(100000)
if args.make_plots is False:
return metric_map
# Plot results
idxs = slice(start_idx, end_idx)
if not args.plot_sim:
plt.figure("Calibration accelerometer vio vs init")
plt.plot(ts, vio_accelScaleInv[:, 0, 0], label="1")
plt.plot(ts, vio_accelScaleInv[:, 1, 1], label="2")
plt.plot(ts, vio_accelScaleInv[:, 2, 2], label="3")
plt.plot(ts, vio_accelScaleInv[:, 0, 1], label="12")
plt.plot(ts, vio_accelScaleInv[:, 0, 2], label="13")
plt.plot(ts, vio_accelScaleInv[:, 1, 2], label="23")
plt.plot(
ts,
np.ones_like(vio_accelScaleInv[:, 0, 0]) * init_accelScaleInv[0, 0],
label="1",
)
plt.plot(
ts,
np.ones_like(vio_accelScaleInv[:, 1, 1]) * init_accelScaleInv[1, 1],
label="2",
)
plt.plot(
ts,
np.ones_like(vio_accelScaleInv[:, 2, 2]) * init_accelScaleInv[2, 2],
label="2",
)
plt.plot(
ts,
np.ones_like(vio_accelScaleInv[:, 0, 1]) * init_accelScaleInv[0, 1],
label="12",
)
plt.plot(
ts,
np.ones_like(vio_accelScaleInv[:, 0, 2]) * init_accelScaleInv[0, 2],
label="13",
)
plt.plot(
ts,
np.ones_like(vio_accelScaleInv[:, 1, 2]) * init_accelScaleInv[1, 2],
label="23",
)
plt.grid(True)
plt.legend(loc="upper center")
plt.figure()
g = np.einsum("kip,p->ki", vio_accelScaleInv, [0, 9.81, 0])
plt.plot(ts, g[:, 1])
plt.figure("Calibration gyrometer vio vs init")
plt.plot(ts, vio_gyroScaleInv[:, 0, 0], label="1")
plt.plot(ts, vio_gyroScaleInv[:, 1, 1], label="2")
plt.plot(ts, vio_gyroScaleInv[:, 2, 2], label="3")
plt.plot(ts, vio_gyroScaleInv[:, 0, 1], label="12")
plt.plot(ts, vio_gyroScaleInv[:, 0, 2], label="13")
plt.plot(ts, vio_gyroScaleInv[:, 1, 2], label="23")
plt.plot(
ts,
np.ones_like(vio_gyroScaleInv[:, 0, 0]) * init_gyroScaleInv[0, 0],
label="1",
)
plt.plot(
ts,
np.ones_like(vio_gyroScaleInv[:, 1, 1]) * init_gyroScaleInv[1, 1],
label="2",
)
plt.plot(
ts,
np.ones_like(vio_gyroScaleInv[:, 2, 2]) * init_gyroScaleInv[2, 2],
label="2",
)
plt.plot(
ts,
np.ones_like(vio_gyroScaleInv[:, 0, 1]) * init_gyroScaleInv[0, 1],
label="12",
)
plt.plot(
ts,
np.ones_like(vio_gyroScaleInv[:, 0, 2]) * init_gyroScaleInv[0, 2],
label="13",
)
plt.plot(
ts,
np.ones_like(vio_gyroScaleInv[:, 1, 2]) * init_gyroScaleInv[1, 2],
label="23",
)
plt.grid(True)
plt.legend(loc="upper center")
fig14 = plt.figure("position-2d")
plt.plot(ps[idxs, 0], ps[idxs, 1], label="filter", color=color_filter)
# plt.plot(ps_dr[idxs, 0], ps_dr[idxs, 1], label="filter_dr")
plt.plot(ref_p[idxs, 0], ref_p[idxs, 1], label=ref_type, color=color_vio)
if args.ronin_dir is not None:
plt.plot(ronin_p[idxs, 0], ronin_p[idxs, 1], label="ronin", color=color_ronin)
plt.legend(loc="upper center")
plt.xlabel("x")
plt.ylabel("y")
plt.grid(True)
plt.title("position-2d")
plt.gca().set_aspect("equal")
fig15 = plt.figure("position-3d")
ax = fig15.add_subplot(111, projection="3d")
plt.plot(ps[idxs, 0], ps[idxs, 1], ps[idxs, 2], label="filter", color=color_filter)
# plt.plot(ps_dr[idxs, 0], ps_dr[idxs, 1], ps_dr[idxs, 2], label="filter_dr")
plt.plot(
ref_p[idxs, 0], ref_p[idxs, 1], ref_p[idxs, 2], label="vio", color=color_vio
)
if args.ronin_dir is not None:
plt.plot(
ronin_p[idxs, 0],
ronin_p[idxs, 1],
ronin_p[idxs, 2],
label="ronin",
color=color_ronin,
)
set_axes_equal(ax)
fig1 = plot_state_euclidean(
"position",
"filter",
["p_x", "p_y", "p_z"],
ts[idxs],
ps[idxs, :],
sigma_p[idxs, :],
color=color_filter,
)
fig2 = plot_state_euclidean(
"velocity",
"filter",
["v_x", "v_y", "v_z"],
ts[idxs],
vs[idxs, :],
sigma_v[idxs, :],
color=color_filter,
)
fig3 = plot_state_euclidean(
"attitude",
"filter",
["eul_x", "eul_y", "eul_z"],
ts[idxs],
euls[idxs, :],
sigma_r[idxs, :],
color=color_filter,
)
fig4 = plot_state_euclidean(
"accel bias",
"filter",
["ba_x", "ba_y", "ba_z"],
ts[idxs],
ba[idxs, :],
sigma_ba[idxs, :],
color=color_filter,
)
fig5 = plot_state_euclidean(
"gyro bias",
"filter",
["bg_x", "bg_y", "bg_z"],
ts[idxs],
bg[idxs, :],
sigma_bg[idxs, :],
color=color_filter,
)
# plot integrated speed output
# plot_state_euclidean(
# "position",
# "filter_dr",
# ["p_x", "p_y", "p_z"],
# ts[idxs],
# ps_dr[idxs, :]
# )
# plot reference traj
plot_state_euclidean(
"position",
ref_type,
["p_x", "p_y", "p_z"],
ts[idxs],
ref_p[idxs, :],
color=color_vio,
)
plot_state_euclidean(
"velocity",
ref_type,
["v_x", "v_y", "v_z"],
ts[idxs],
ref_v[idxs, :],
color=color_vio,
)
plot_state_euclidean(
"attitude",
ref_type,
["eul_x", "eul_y", "eul_z"],
ts[idxs],
ref_euls[idxs, :],
color=color_vio,
)
plot_state_euclidean(
"accel bias",
ref_type,
["ba_x", "ba_y", "ba_z"],
ts[idxs],
ref_ba[idxs, :],
color=color_vio,
)
plot_state_euclidean(
"accel bias",
ref_type + "b",
["ba_x", "ba_y", "ba_z"],
ts[idxs],
vio_ba_b[idxs, :],
color=color_vio,
)
plot_state_euclidean(
"gyro bias",
ref_type,
["bg_x", "bg_y", "bg_z"],
ts[idxs],
ref_bg[idxs, :],
color=color_vio,
)
plot_state_euclidean(
"gyro bias",
ref_type + "b",
["bg_x", "bg_y", "bg_z"],
ts[idxs],
vio_bg_b[idxs, :],
color=color_vio,
)
# plot RONIN
if args.ronin_dir is not None:
plot_state_euclidean(
"position",
"ronin",
["p_x", "p_y", "p_z"],
ts[idxs],
ronin_p[idxs, :],
color=color_ronin,
)
plot_state_euclidean(
"attitude",
"aekf",
["eul_x", "eul_y", "eul_z"],
ts[idxs],
aekf_euls_time_aligned[idxs, :],
color=color_ronin,
)
# compute values at update rate
idx_update = ~np.isnan(innos[idxs, :]).any(axis=1)
ts_update = ts[idxs][idx_update]
innos_update = innos[idxs, :][idx_update, :]
innos_sigma_update = inno_sigma[idxs, :][idx_update, :]
meas_update = meas[idxs, :][idx_update, :]
meas_sigma_update = meas_sigma[idxs, :][idx_update, :]
pred_update = pred[idxs, :][idx_update, :]
ref_disp_update = ref_disp[idxs, :][idx_update, :]
fig6 = plot_error_euclidean(
"innovation (m)",
"filter",
["x", "y", "z"],
ts_update,
innos_update,
sigma=innos_sigma_update,
)
# plot measurement
# compute norm for visualization
meas_plus_nom = np.hstack(
(meas_update, np.atleast_2d(np.linalg.norm(meas_update, axis=1)).T)
)
pred_plus_nom = np.hstack(
(pred_update, np.atleast_2d(np.linalg.norm(pred_update, axis=1)).T)
)
ref_disp_plus_norm = np.hstack(
(ref_disp_update, np.atleast_2d(np.linalg.norm(ref_disp_update, axis=1)).T)
)
meas_sigma_plus_norm = np.hstack(
(meas_sigma_update, np.zeros((meas_update.shape[0], 1)) * np.nan)
)
fig7 = plot_state_euclidean(
"displacement measurement vs target",
"meas",
["x", "y", "z", "norm"],
ts_update,
meas_plus_nom,
sigma=meas_sigma_plus_norm,
color=color_ronin,
)
plot_state_euclidean(
"displacement measurement vs target",
"pred",
["x", "y", "z", "norm"],
ts_update,
pred_plus_nom,
color=color_filter,
)
plot_state_euclidean(
"displacement measurement vs target",
ref_type,
["x", "y", "z", "norm"],
ts_update,
ref_disp_plus_norm,
color=color_vio,
)
# Plot the error in covariance
meas_err_update = meas_update - ref_disp_update
fig8 = plot_error_euclidean(
"displace measurement errors",
"meas-err",
["x", "y", "z"],
ts_update,
meas_err_update,
meas_sigma_update,
)
plot_error_euclidean(
"displace measurement errors",
"pred-err",
["x", "y", "z"],
ts_update,
pred_update - ref_disp_update,
meas_sigma_update,
)
for ax in fig8.axes:
ax.set_ylim([-0.5, 0.5])
# display measurement error autocorellation (SLOW)
fig8b = plt.figure("autocorellation of measurement error")
logging.warning("We assume update frequency at 20hz for autocorrelation")
for i in range(3):
plt.subplot(3, 1, i + 1)
plt.acorr(meas_err_update[:, i], maxlags=100, lw=2)
locs, labels = plt.xticks() # Get locations and labels
for (l, t) in zip(locs, labels):
t.set_text(str(l / 20.0) + "s")
plt.xticks(locs, labels) # Set locations and labels
plt.xlim(left=0)
plt.grid()
fig9 = plot_error_euclidean(
"position error",
ref_type,
["p_x", "p_y", "p_z"],
ts[idxs],
ps[idxs, :] - ref_p[idxs, :],
sigma_p[idxs, :],
)
fig10 = plot_error_euclidean(
"velocity error",
ref_type,
["v_x", "v_y", "v_z"],
ts[idxs],
vs[idxs, :] - ref_v[idxs, :],
sigma_v[idxs, :],
)
euls_err = wrap_rpy(euls - ref_euls)
fig11 = plot_error_euclidean(
"attitude error",
ref_type,
["eulx", "euly", "eulz"],
ts[idxs],
euls_err[idxs, :],
sigma_r[idxs, :],
)
fig12 = plot_error_euclidean(
"accel bias error",
ref_type,
["ba_x", "ba_y", "ba_z"],
ts[idxs],
ba[idxs, :] - ref_ba[idxs, :],
sigma_ba[idxs, :],
)
fig13 = plot_error_euclidean(
"gyros bias error",
ref_type,
["bg_x", "bg_y", "bg_z"],
ts[idxs],
bg[idxs, :] - ref_bg[idxs, :],
sigma_bg[idxs, :],
)
if args.save_fig:
if not save_vio_states:
logging.warning("Not saving figure because, the filter did not finish")
else:
logging.info("Saving figures")
for save_as in ["png", "svg"]:
fig1.savefig(osp.join(results_folder, "position." + save_as))
fig2.savefig(osp.join(results_folder, "velocity." + save_as))
fig3.savefig(osp.join(results_folder, "rotation." + save_as))
fig4.savefig(osp.join(results_folder, "acc-bias." + save_as))
fig5.savefig(osp.join(results_folder, "gyr-bias." + save_as))
fig6.savefig(osp.join(results_folder, "innovation." + save_as))
fig7.savefig(osp.join(results_folder, "displacement." + save_as))
fig8.savefig(osp.join(results_folder, "displacement-err." + save_as))
fig9.savefig(osp.join(results_folder, "position-err." + save_as))
fig10.savefig(osp.join(results_folder, "velocity-err." + save_as))
fig11.savefig(osp.join(results_folder, "rotation-err." + save_as))
fig12.savefig(osp.join(results_folder, "acc-bias-err." + save_as))
fig13.savefig(osp.join(results_folder, "gyr-bias-err." + save_as))
fig14.savefig(osp.join(results_folder, "position-2d." + save_as))
fig15.savefig(osp.join(results_folder, "position-3d." + save_as))
# # also save as pickle just in case (TOO LARGE)
# import pickle as pl
# pl.dump(fig1, open(osp.join(results_folder, "position.pickle"),"wb"))
# pl.dump(fig2, open(osp.join(results_folder, "velocity.pickle"),"wb"))
# pl.dump(fig3, open(osp.join(results_folder, "rotation.pickle"),"wb"))
# pl.dump(fig4, open(osp.join(results_folder, "acc-bias.pickle"),"wb"))
# pl.dump(fig5, open(osp.join(results_folder, "gyr-bias.pickle"),"wb"))
# pl.dump(fig6, open(osp.join(results_folder, "innovation.pickle"),"wb"))
# pl.dump(fig7, open(osp.join(results_folder, "displacement.pickle"),"wb"))
# pl.dump(fig8, open(osp.join(results_folder, "displacement-err.pickle"),"wb"))
# pl.dump(fig9, open(osp.join(results_folder, "position-err.pickle"),"wb"))
# pl.dump(fig10, open(osp.join(results_folder, "velocity-err.pickle"),"wb"))
# pl.dump(fig11, open(osp.join(results_folder, "rotation-err.pickle"),"wb"))
# pl.dump(fig12, open(osp.join(results_folder, "acc-bias-err.pickle"),"wb"))
# pl.dump(fig13, open(osp.join(results_folder, "gyr-bias-err.pickle"),"wb"))
# pl.dump(fig14, open(osp.join(results_folder, "position-2d.pickle"),"wb"))
# pl.dump(fig15, open(osp.join(results_folder, "position-3d.pickle"),"wb"))
else:
logging.warning("Not saving figure")
if args.display_fig:
fig_state = [fig1, fig2, fig3, fig4, fig5, fig14, fig15]
fig_innovation = [fig6, fig7, fig8]
fig_err = [fig9, fig10, fig11, fig12, fig13]
# [plt.close(f) for f in fig_state]
# [plt.close(f) for f in fig_innovation]
# [plt.close(f) for f in fig_err]
import matplotlib
fig_button = plt.figure("continue")
axcontinue = plt.axes([0, 0.0, 0.5, 0.5])
bcontinue = matplotlib.widgets.Button(axcontinue, "Continue")
bcontinue.on_clicked(lambda d: plt.close("all"))
ax3d = plt.axes([0.5, 0.5, 0.5, 0.5])
b3d = matplotlib.widgets.Button(ax3d, "state")
b3d.on_clicked(lambda d: [f.show() for f in fig_state])
axerror = plt.axes([0, 0.5, 0.5, 0.5])
berror = matplotlib.widgets.Button(axerror, "error")
berror.on_clicked(lambda d: [f.show() for f in fig_err])
axinnov = plt.axes([0.5, 0, 0.5, 0.5])
binnov = matplotlib.widgets.Button(axinnov, "innov")
binnov.on_clicked(lambda d: [f.show() for f in fig_innovation])
fig15.show()
plt.show()
return metric_map
def add_watch(self, dir):
if self.fun is not None:
logging.info('Adding watcher for : %s', dir)
observer = Observer()
observer.schedule(MyHandler(self.fun), path=dir, recursive=True)
self.__observers.append(observer)
parser.add_argument("--rpe_10", type=bool, default=True)
parser.add_argument("--rpe_100", type=bool, default=False)
subparsers = parser.add_subparsers()
parser_bias = subparsers.add_parser("bias")
parser_bias.set_defaults(func=compare_biases)
args = parser.parse_args()
print(vars(args))
try:
args.func(args)
except AttributeError:
pass
# default plot
if args.plot_sim is False:
with open(args.data_list) as f:
data_names = [
s.strip().split("," or " ")[0]
for s in f.readlines()
if len(s) > 0 and s[0] != "#"
]
if args.dataset_number is not None:
logging.info(f"Plotting dataset {data_names[args.dataset_number]}")
run(args, data_names[args.dataset_number])
else:
run_on_each_dataset_and_gather_metrics(args, data_names)
else:
pass
def __init__(
self,
model_path,
model_param_path,
update_freq,
filter_tuning,
imu_calib: Optional[ImuCalib] = None,
force_cpu=False,
):
config_from_network = dotdict({})
with open(model_param_path) as json_file:
data_json = json.load(json_file)
config_from_network["imu_freq_net"] = data_json["imu_freq"]
config_from_network["past_time"] = data_json["past_time"]
config_from_network["window_time"] = data_json["window_time"]
config_from_network["arch"] = data_json["arch"]
# frequencies and sizes conversion
if not (config_from_network.past_time *
config_from_network.imu_freq_net).is_integer():
raise ValueError(
"past_time cannot be represented by integer number of IMU data."
)
if not (config_from_network.window_time *
config_from_network.imu_freq_net).is_integer():
raise ValueError(
"window_time cannot be represented by integer number of IMU data."
)
self.imu_freq_net = (config_from_network.imu_freq_net
) # imu frequency as input to the network
self.past_data_size = int(config_from_network.past_time *
config_from_network.imu_freq_net)
self.disp_window_size = int(config_from_network.window_time *
config_from_network.imu_freq_net)
self.net_input_size = self.disp_window_size + self.past_data_size
# EXAMPLE :
# if using 200 samples with step size 10, inference at 20 hz
# we do update between clone separated by 19=update_distance_num_clone-1 other clone
# if using 400 samples with 200 past data and clone_every_n_netimu_sample 10, inference at 20 hz
# we do update between clone separated by 19=update_distance_num_clone-1 other clone
if not (config_from_network.imu_freq_net / update_freq).is_integer():
raise ValueError("update_freq must be divisible by imu_freq_net.")
if not (config_from_network.window_time * update_freq).is_integer():
raise ValueError(
"window_time cannot be represented by integer number of updates."
)
self.update_freq = update_freq
self.clone_every_n_netimu_sample = int(
config_from_network.imu_freq_net /
update_freq) # network inference/filter update interval
assert (config_from_network.imu_freq_net % update_freq == 0
) # imu frequency must be a multiple of update frequency
self.update_distance_num_clone = int(config_from_network.window_time *
update_freq)
# time
self.dt_interp_us = int(1.0 / self.imu_freq_net * 1e6)
self.dt_update_us = int(1.0 / self.update_freq *
1e6) # multiple of interpolation interval
# logging
logging.info(
f"Network Input Time: {config_from_network.past_time + config_from_network.window_time} = {config_from_network.past_time} + {config_from_network.window_time} (s)"
)
logging.info(
f"Network Input size: {self.net_input_size} = {self.past_data_size} + {self.disp_window_size} (samples)"
)
logging.info("IMU interpolation frequency: %s (Hz)" %
self.imu_freq_net)
logging.info("Measurement update frequency: %s (Hz)" %
self.update_freq)
logging.info("Filter update stride state number: %i" %
self.update_distance_num_clone)
logging.info(
f"Interpolating IMU measurement every {self.dt_interp_us}us for the network input"
)
# IMU initial calibration
self.icalib = imu_calib
# MSCKF
self.filter = ImuMSCKF(filter_tuning)
net_config = {
"in_dim": (self.past_data_size + self.disp_window_size) // 32 + 1
}
self.meas_source = MeasSourceNetwork(model_path,
config_from_network["arch"],
net_config, force_cpu)
# self.meas_source = MeasSourceTorchScript(model_path, force_cpu)
self.imu_buffer = ImuBuffer()
# This callback is called at first update if set
self.callback_first_update = None
# This callback can be use to bypass network use for measurement
self.debug_callback_get_meas = None
# keep track of past timestamp and measurement
self.last_t_us, self.last_acc, self.last_gyr = -1, None, None
self.next_interp_t_us = None
self.next_aug_t_us = None
self.has_done_first_update = False
def stop_signal_handler(args, epoch, network, optimizer, signal, frame):
logging.info("-" * 30)
logging.info("Early terminate")
save_model(args, epoch, network, optimizer, interrupt=True)
sys.exit()
def net_train(args):
"""
Main function for network training
"""
try:
if args.root_dir is None:
raise ValueError("root_dir must be specified.")
if args.train_list is None:
raise ValueError("train_list must be specified.")
if args.out_dir is not None:
if not osp.isdir(args.out_dir):
os.makedirs(args.out_dir)
if not osp.isdir(osp.join(args.out_dir, "checkpoints")):
os.makedirs(osp.join(args.out_dir, "checkpoints"))
if not osp.isdir(osp.join(args.out_dir, "logs")):
os.makedirs(osp.join(args.out_dir, "logs"))
with open(
os.path.join(args.out_dir, "parameters.json"), "w"
) as parameters_file:
parameters_file.write(json.dumps(vars(args), sort_keys=True, indent=4))
logging.info(f"Training output writes to {args.out_dir}")
else:
raise ValueError("out_dir must be specified.")
if args.val_list is None:
logging.warning("val_list is not specified.")
if args.continue_from is not None:
if osp.exists(args.continue_from):
logging.info(
f"Continue training from existing model {args.continue_from}"
)
else:
raise ValueError(
f"continue_from model file path {args.continue_from} does not exist"
)
data_window_config, net_config = arg_conversion(args)
except ValueError as e:
logging.error(e)
return
# Display
np.set_printoptions(formatter={"all": "{:.6f}".format})
logging.info(f"Training/testing with {args.imu_freq} Hz IMU data")
logging.info(
"Size: "
+ str(data_window_config["past_data_size"])
+ "+"
+ str(data_window_config["window_size"])
+ "+"
+ str(data_window_config["future_data_size"])
+ ", "
+ "Time: "
+ str(args.past_time)
+ "+"
+ str(args.window_time)
+ "+"
+ str(args.future_time)
)
logging.info("Perturb on bias: %s" % args.do_bias_shift)
logging.info("Perturb on gravity: %s" % args.perturb_gravity)
logging.info("Sample frequency: %s" % args.sample_freq)
train_loader, val_loader = None, None
start_t = time.time()
train_list = get_datalist(args.train_list)
try:
train_dataset = FbSequenceDataset(
args.root_dir, train_list, args, data_window_config, mode="train"
)
train_loader = DataLoader(
train_dataset, batch_size=args.batch_size, shuffle=True
)
except OSError as e:
logging.error(e)
return
end_t = time.time()
logging.info(f"Training set loaded. Loading time: {end_t - start_t:.3f}s")
logging.info(f"Number of train samples: {len(train_dataset)}")
if args.val_list is not None:
val_list = get_datalist(args.val_list)
try:
val_dataset = FbSequenceDataset(
args.root_dir, val_list, args, data_window_config, mode="val"
)
val_loader = DataLoader(val_dataset, batch_size=512, shuffle=True)
except OSError as e:
logging.error(e)
return
logging.info("Validation set loaded.")
logging.info(f"Number of val samples: {len(val_dataset)}")
device = torch.device(
"cuda:0" if torch.cuda.is_available() and not args.cpu else "cpu"
)
network = get_model(args.arch, net_config, args.input_dim, args.output_dim).to(
device
)
total_params = network.get_num_params()
logging.info(f'Network "{args.arch}" loaded to device {device}')
logging.info(f"Total number of parameters: {total_params}")
optimizer = torch.optim.Adam(network.parameters(), args.lr)
scheduler = torch.optim.lr_scheduler.ReduceLROnPlateau(
optimizer, factor=0.1, patience=10, verbose=True, eps=1e-12
)
logging.info(f"Optimizer: {optimizer}, Scheduler: {scheduler}")
start_epoch = 0
if args.continue_from is not None:
checkpoints = torch.load(args.continue_from)
start_epoch = checkpoints.get("epoch", 0)
network.load_state_dict(checkpoints.get("model_state_dict"))
optimizer.load_state_dict(checkpoints.get("optimizer_state_dict"))
logging.info(f"Continue from epoch {start_epoch}")
else:
# default starting from latest checkpoint from interruption
latest_pt = os.path.join(args.out_dir, "checkpoints", "checkpoint_latest.pt")
if os.path.isfile(latest_pt):
checkpoints = torch.load(latest_pt)
start_epoch = checkpoints.get("epoch", 0)
network.load_state_dict(checkpoints.get("model_state_dict"))
optimizer.load_state_dict(checkpoints.get("optimizer_state_dict"))
logging.info(
f"Detected saved checkpoint, starting from epoch {start_epoch}"
)
summary_writer = SummaryWriter(osp.join(args.out_dir, "logs"))
summary_writer.add_text("info", f"total_param: {total_params}")
logging.info(f"-------------- Init, Epoch {start_epoch} --------------")
attr_dict = get_inference(network, train_loader, device, start_epoch)
write_summary(summary_writer, attr_dict, start_epoch, optimizer, "train")
if val_loader is not None:
attr_dict = get_inference(network, val_loader, device, start_epoch)
write_summary(summary_writer, attr_dict, start_epoch, optimizer, "val")
def stop_signal_handler(args, epoch, network, optimizer, signal, frame):
logging.info("-" * 30)
logging.info("Early terminate")
save_model(args, epoch, network, optimizer, interrupt=True)
sys.exit()
best_val_loss = np.inf
for epoch in range(start_epoch + 1, args.epochs):
signal.signal(
signal.SIGINT, partial(stop_signal_handler, args, epoch, network, optimizer)
)
signal.signal(
signal.SIGTERM,
partial(stop_signal_handler, args, epoch, network, optimizer),
)
logging.info(f"-------------- Training, Epoch {epoch} ---------------")
start_t = time.time()
train_attr_dict = do_train(network, train_loader, device, epoch, optimizer)
write_summary(summary_writer, train_attr_dict, epoch, optimizer, "train")
end_t = time.time()
logging.info(f"time usage: {end_t - start_t:.3f}s")
if val_loader is not None:
val_attr_dict = get_inference(network, val_loader, device, epoch)
write_summary(summary_writer, val_attr_dict, epoch, optimizer, "val")
if np.mean(val_attr_dict["losses"]) < best_val_loss:
best_val_loss = np.mean(val_attr_dict["losses"])
save_model(args, epoch, network, optimizer)
else:
save_model(args, epoch, network, optimizer)
logging.info("Training complete.")
return
parser_bias.set_defaults(func=plot_position)
parser_bias = subparsers.add_parser("plot_autocorellation")
parser_bias.set_defaults(func=plot_position)
args = parser.parse_args()
print(vars(args))
with open(args.data_list) as f:
data_names = [
s.strip().split("," or " ")[0]
for s in f.readlines()
if len(s) > 0 and s[0] != "#"
]
if args.dataset_number is not None:
dataset = data_names[args.dataset_number]
logging.info(f"Plotting dataset {dataset}")
log_folders = glob.glob(args.root_dir_regexp)
logging.info(f"{log_folders}")
for f in log_folders:
if f == "./output_b1":
continue
logging.info(f"Processing run {f}")
n = f
try:
args.func(args, f, n, dataset)
except Exception as e:
print(e)
logging.warning(f"Something went wrong with {dataset}")
continue
plt.show()
else:
def plot_position(args, log_folder, n, dataset):
results_folder = log_folder + "/" + dataset
filename = os.path.join(results_folder, args.log_filename + ".npy")
try:
logging.info(f"Loading {filename}")
states = np.load(filename)
except:
logging.error(
f"{filename}.npy was not found. Surely means filter did not finish"
)
raise FileNotFoundError
# load all states
R_init = states[0, :9].reshape(-1, 3, 3)
Rs = states[:, :9].reshape(-1, 3, 3)
rs = Rotation.from_matrix(Rs)
ps = states[:, 12:15]
ts = states[:, 27]
sigma_p = np.sqrt(states[:, 34:37])
if os.path.exists(os.path.join(results_folder, "vio_states.npy")):
vio_states = np.load(os.path.join(results_folder, "vio_states.npy"))
ref_p = vio_states[:, 3:6]
ref_accelScaleInv_flat = vio_states[:, 24:33]
ref_gyroScaleInv_flat = vio_states[:, 33:42]
else:
logging.error(
"vio_states.npy was not found. you shoud create it with plot_state.py before... sorry :("
)
raise FileNotFoundError
fig14 = plt.figure("position-2d " + dataset)
plt.plot(ps[:, 0], ps[:, 1], label=n)
plt.plot(
ref_p[:, 0], ref_p[:, 1], label="vio", color=plot_state.color_vio, linestyle=":"
)
plt.legend(loc="upper center")
plt.xlabel("x")
plt.ylabel("y")
plt.grid(True)
plt.title("position-2d")
plt.gca().set_aspect("equal")
fig1 = plot_state.plot_state_euclidean(
"position " + dataset,
n,
["p_x", "p_y", "p_z"],
ts,
ps,
linestyle=":",
color="black",
)
fig9 = plot_state.plot_error_euclidean(
"position error " + dataset,
n,
["p_x", "p_y", "p_z"],
ts,
ps - ref_p,
sigma=sigma_p,
linestyle=":",
color="black",
)
io_groups.add_argument(
"--ronin_dir",
type=str,
help="folder names should match those in filter_dir")
io_groups.add_argument(
"--window_time",
type=float,
default=1.0,
help="window time when no ronin dir exists",
)
io_groups.add_argument("--no_make_plots", action="store_true")
args = parser.parse_args()
all_outputs_filter = list(Path.cwd().glob(args.filter_dir))
logging.info(f"Found {len(all_outputs_filter)} runs")
for m in all_outputs_filter:
base_folder = Path(m)
print(base_folder)
logging.info(base_folder)
name_run = "/tlio_output/" + base_folder.name
# read parameters
with open(base_folder.joinpath("./tlio_output/parameters.json"),
"r") as f:
conf_filter = json.load(f)
pprint(conf_filter)
if "window_time" in conf_filter.keys():
disp_time = float(conf_filter["window_time"])
else:
disp_time = args.window_time # This is default
io_groups.add_argument("--model_path",
required=True,
type=str,
default=None)
io_groups.add_argument("--model_param_path",
required=True,
type=str,
default=None)
io_groups.add_argument("--out_dir", type=str, default="./")
parser.add_argument("--cpu", type=bool, default=True)
args = parser.parse_args()
# overwrite network model parameter from json file if specified
if args.model_param_path is not None:
with open(args.model_param_path) as json_file:
data_json = json.load(json_file)
args.imu_freq_net = data_json["imu_freq"]
args.past_time = data_json["past_time"]
args.window_time = data_json["window_time"]
args.arch = data_json["arch"]
np.set_printoptions(linewidth=2000)
logging.info("Program options:")
logging.info(pprint(vars(args)))
# run filter
if not os.path.exists(args.out_dir):
os.mkdir(args.out_dir)
load_and_convert(args)
