env_type = "perlin"
func = gen_env_fn_3d(env_type, seed=seed)
directory = Path(__file__).parents[0] / "results"
if not os.path.exists(directory):
os.makedirs(directory)
directory /= "incremental"
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)
submaps = dict()
for m in map_dirs:
map_dir = os.path.join(f, m)
map_names = [name for name in os.listdir(map_dir) if name.endswith(".p")]
if len(map_names) == 0:
raise ValueError("No submaps found in folders")
map_names.sort()
root_logger.debug(map_names)
# 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)
submap = RelativeSubmap(depth)
submap.surfel_dict = submap_surf_dict
submap.surfel_ids = submap_surf_dict.keys()
key = ast.literal_eval(m)
submaps[key] = submap
# 3 Fuse the local models in each submap
root_logger.debug("Fusing submaps' surfels")
for f in ds_folders[1:]:
for m in submaps.keys():
map_dir = os.path.join(f, str(m))
if not os.path.isdir(map_dir):
root_logger.debug(
f"No matching submap region found for {m} in {f}")
continue
ch.setLevel(level) ch.setFormatter(formatter) logger.addHandler(ch) # Set noise seed 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(
batch_pts = batch_pts[:b_index, :, :]
batch_covs = batch_covs[:b_index, :, :]
# Associate points
selections = find_approx_associations(triangulation, batch_pts)
log_like_stm_batch = np.zeros(train_repetitions, dtype=float)
log_like_elev_batch = np.zeros(train_repetitions, dtype=float)
for n_rep in range(train_repetitions):
# Create submaps for each LTR
root_logger.debug(f"Starting repetition {n_rep+1}")
N_regions = simplices.shape[0]
stm_submaps = np.empty(N_regions, dtype=RelativeSubmap)
elev_submaps = np.empty(N_regions, dtype=ElevationMap3D)
for i in range(N_regions):
stm_submaps[i] = RelativeSubmap(grid_depth)
elev_submaps[i] = ElevationMap3D(grid_depth)
stm_submaps_dict_arr = np.empty(N_regions, dtype=dict)
elev_submaps_dict_arr = np.empty(N_regions, dtype=dict)
# Convert to relative IRF and store in ltr
for i, b in selections.items():
N_ltr = np.sum(b)
if N_ltr > 0:
lms_ltr = lms[simplices[i]]
pts_ltr = batch_pts[b, :, :]
covs_ltr = batch_covs[b, :, :]
# Split into training and test datasets
N_test = int(holdout_ratio * N_ltr)
occl_stop,
color="g",
linestyle=":",
fill=True,
alpha=0.3,
label="Observed region")
N_corrs = 7
corr = 0
corr_range = [0]
for i_plot in range(1, N_corrs + 1):
# Correlated prior
print("Corr:", corr)
submap = RelativeSubmap(max_depth=depth,
dim=2,
tolerance=1e-8,
max_iterations=100,
prior_corr=corr)
submap.insert_measurements(m_z, C_z)
submap.update()
# Plot resulting map
col = cmap.to_rgba(corr, norm=False)
# col = cmap.to_rgba(corr)
for i, surfel in enumerate(submap):
edges = surfel.corners.reshape(2)
heights = surfel.bel_h_m.reshape(2)
variation = surfel.bel_v_b / surfel.bel_v_a
stds = np.ones(2) * variation**0.5
tmp = plt.plot(edges, heights, "-o", color=col, markersize=2)
if m_z_batch.size == 0:
m_z_batch = np.copy(m_z)
C_z_batch = np.copy(C_z)
else:
m_z_batch = np.vstack((m_z_batch, m_z))
C_z_batch = np.vstack((C_z_batch, C_z))
scans_indices.append(m_z.shape[0])
# # XXX Plot measurements in hexbin
# plt.figure(constrained_layout=True)
# plt.hexbin(*m_z_batch[:, :2, 0].T, gridsize=50)
# 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)
if not load:
for batch in trange(n_batches, desc="Batch"):
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
# env_type = 'perlin'
env_type = "simonsberg"
env_func = gen_env_fn_2d(env_type)
# XXX: Flags
N = 1000 # Number of measurements
m_z, C_z = gen_meas_2d(env_func, N, scale=0.5)
depth = 4
# XXX Occlude some measurements
valid = m_z[:, 0, 0] > 0.5
m_z = m_z[valid]
C_z = C_z[valid]
submap = RelativeSubmap(max_depth=depth, dim=2, tolerance=1e-3, max_iterations=10)
submap.insert_measurements(m_z, C_z)
submap.update()
# XXX: Analysis
plt.figure(constrained_layout=True)
ax = plt.gca()
submap.plot_map_indices(ax)
# Plot resulting map
for i, surfel in enumerate(submap):
edges = surfel.corners.reshape(2)
heights = surfel.bel_h_m.reshape(2)
variation = surfel.bel_v_b / surfel.bel_v_a
# m_joint = np.vstack((h_m, v_m))
# C_joint = block_diag(h_C, v_var)
# corr_joint = cov_corrcoef(C_joint)
# root_logger.debug(m_joint.flatten())
# root_logger.debug(C_joint)
# root_logger.debug(corr_joint)
# m_approx.append(m_joint)
# C_approx.append(C_joint)
# v_a_approx.append(v_a)
# v_b_approx.append(v_b)
# corr_approx.append(corr_joint)
root_logger.debug("VMP Structured")
approx_labels.append("VMP structured")
submap = RelativeSubmap(0,
tolerance=1e-8,
dim=2,
projection_method=VMPStructured)
submap.insert_measurements(1 * z_m, 1 * z_C)
submap.update()
surfel = [s for s in submap][0]
h_m = surfel.bel_h_m
h_C = surfel.bel_h_C
v_a = surfel.bel_v_a.item()
v_b = surfel.bel_v_b.item()
# Gaussian projection
v_m = v_b / v_a
v_var = v_b**2 / ((v_a - 1)**2 * (v_a - 2))
m_joint = np.vstack((h_m, v_m))
C_joint = block_diag(h_C, v_var)
corr_joint = cov_corrcoef(C_joint)