# Read in latest submap
filename = os.path.join(map_dir, map_names[-1])
submap_surf_dict = dill.load(open(filename, "rb"))
depth = np.log(len(submap_surf_dict)) / np.log(4)
assert (depth >= 0) and ((depth % 1) == 0)
submaps[m].fuse_submaps(submap_surf_dict)
else:
root_logger.debug(f"Empty submap folder for {m} in {f}")
root_logger.debug("Updating fused submaps")
for key, submap in submaps.items():
root_logger.debug(f"Updating submap {key}")
submap.update()
# 4 Save result
root_logger.debug("Saving fused submaps")
for m in submaps.keys():
root_logger.debug(f"Saving {m}")
submap = submaps[m]
save_path = os.path.join(main_path, "submap", sensor_type, str(m))
root_logger.debug(save_path)
if not os.path.exists(save_path):
os.makedirs(save_path)
filename = os.path.join(save_path, "0.p")
dill.dump(dict(submap.surfel_dict), open(filename, "wb"))
directory /= "reobserve"
if not os.path.exists(directory):
os.makedirs(directory)
load_iterations = True
if load_iterations == False:
root_logger.debug("Generating new measurements and updating")
grid = RelativeSubmap(max_depth=grid_depth, tolerance=1e-5)
normalised_iterations = []
iterations = []
# NOTE This is a pseudo update. There is something weird with just the first update. It add more messages than needed I think
C = 10000 * np.eye(3).reshape(1, 3, 3)
grid.insert_measurements(np.zeros((1, 3, 1)), C)
update_iter = grid.update()
for i in range(N_updates):
print("Update number:", i)
# Generate Measurements
print(i, "------- Generating Measurements -------")
m_z_stack, C_z_stack = gen_meas_3d(func, N_z_gen, scale=1)
b = (
(m_z_stack[:, 0, 0] + m_z_stack[:, 1, 0] <= 1)
& (m_z_stack[:, 0, 0] > 0)
& (m_z_stack[:, 0, 0] < 1)
& (m_z_stack[:, 1, 0] > 0)
& (m_z_stack[:, 1, 0] < 1)
)
m_z_stack = m_z_stack[b, :, :]
C_z_stack = C_z_stack[b, :, :]
# plt.xlim(0, 1)
# plt.ylim(0, 1)
grid_iterations = []
grid = RelativeSubmap(max_depth=grid_depth, tolerance=1e-5, prior_corr=0.5)
# grid.plot_map_indices()
# plt.show()
# exit()
grid_iterations.append(copy.deepcopy(grid.surfel_dict))
normalised_iterations = []
iterations = []
# NOTE This is a pseudo update. There is something weird with just the first update. It add more messages than needed I think
C = 10000 * np.eye(3).reshape(1, 3, 3)
grid.insert_measurements(np.zeros((1, 3, 1)), C)
update_iter = grid.update()
# XXX Uncomment if you want update the whole map once before push-broom
# N_z_gen = 10*4**5
# m_z_stack, C_z_stack = gen_meas_3d(func, N_z_gen, scale=1)
# b = (
# (m_z_stack[:, 0, 0] + m_z_stack[:, 1, 0] <= 1)
# & (m_z_stack[:, 0, 0] > 0)
# & (m_z_stack[:, 0, 0] < 1)
# & (m_z_stack[:, 1, 0] > 0)
# & (m_z_stack[:, 1, 0] < 1)
# )
# m_z_stack = m_z_stack[b, :, :]
# C_z_stack = C_z_stack[b, :, :]
# N_z_new = m_z_stack.shape[0]
# print("N: total =", N_z_gen, " remainder = ", N_z_new)
seed = (int)(np.random.rand() * (2.0 ** 30)) seed = 654981589 np.random.seed(seed) random.seed(seed) # env_type = 'perlin' env_type = "perlin" env_func = gen_env_fn_3d(env_type, seed) depth = 4 submap = RelativeSubmap(max_depth=depth, dim=3, tolerance=1e-5, max_iterations=500) N = 500 # Number of measurements m_z, C_z = gen_meas_3d(env_func, N, scale=0.9) submap.insert_measurements(m_z, C_z) msg_counter = submap.update() print(msg_counter) # m_z, C_z = gen_meas_3d(env_func, N, scale=0.9) # submap.insert_measurements(m_z, C_z) # msg_counter = submap.update() # print(msg_counter) # XXX: Analysis # Plot Samples # pts = mlab.points3d( # *m_z[:, :, 0].T, m_z[:, 2, 0], scale_mode="none", colormap="viridis", scale_factor=0.005 # ) # Plot exact surface step = 0.01
env_type = "perlin"
env_func = gen_env_fn_3d(env_type, seed=seed)
test_pts = np.zeros((N_test, 3, 1))
test_pts[:, :2, 0] = xy
test_pts[:, -1] = env_func(test_pts[:, 0], test_pts[:, 1])
m_gen, C_gen = gen_meas_3d(env_func, N, scale=scale)
depth = 3
m_z = np.copy(m_gen)
C_z = np.copy(C_gen)
# STM Map
stm_map = RelativeSubmap(depth, dim=3, tolerance=1e-5, max_iterations=1000)
stm_map.insert_measurements(1 * m_z, 1 * C_z)
stm_map.update()
log_like_stm = stm_map.loglike(1 * test_pts)
mse_stm = stm_map.mean_squared_error(1 * test_pts)
# GP Map
gp_map = GPMap(dim=3)
gp_map.update(1 * m_z, 1 * C_z)
log_like_gp = gp_map.loglike(1 * test_pts)
mse_gp = gp_map.mean_squared_error(1 * test_pts)
log_like_stm_batch[batch] = log_like_stm
log_like_gp_batch[batch] = log_like_gp
mse_stm_batch[batch] = mse_stm
mse_gp_batch[batch] = mse_gp
print("MSE's (gp, stm)", mse_gp, mse_stm)
print("LL's (gp, stm)", log_like_gp, log_like_stm)