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)