def main(args):
# load reference data
ref_poses, ref_descriptors, _ = utils.import_reference_map(
args.reference_traverse)
# localize all selected query traverses
pbar = tqdm(args.query_traverses)
for traverse in pbar:
pbar.set_description(traverse)
# savepath
save_path = os.path.join(utils.results_path, traverse)
# load query data
query_poses, _, _, query_descriptors, _ = utils.import_query_traverse(
traverse)
# regular traverse with VO
pbar = tqdm(args.descriptors, leave=False)
for desc in pbar:
pbar.set_description(desc)
# one folder per descriptor
save_path1 = os.path.join(save_path, desc)
if not os.path.exists(save_path1):
os.makedirs(save_path1)
model = SeqMatching(
ref_poses,
ref_descriptors[desc],
args.wContrast,
args.numVel,
args.vMin,
args.vMax,
args.matchWindow,
args.enhance,
)
proposals, scores, times, query_gt = utils.localize_traverses_matching(
model,
query_poses,
query_descriptors[desc][:, :args.seq_len, :],
desc="Seq Match",
)
utils.save_obj(
save_path1 + "/SeqMatch.pickle",
model="Seq Match",
query_gt=query_gt,
proposals=proposals,
scores=scores,
times=times,
L=args.seq_len,
)
return None
def main(args):
# load reference data
ref_poses, ref_descriptors, _ = utils.import_reference_map(
args.reference_traverse)
# localize all selected query traverses
pbar = tqdm(args.query_traverses)
for traverse in pbar:
pbar.set_description(traverse)
# savepath
save_path = os.path.join(utils.results_path, traverse)
# load query data
query_poses, _, _, query_descriptors, _ = utils.import_query_traverse(
traverse)
# regular traverse with VO
pbar = tqdm(args.descriptors, leave=False)
for desc in pbar:
pbar.set_description(desc)
# one folder per descriptor
save_path1 = os.path.join(save_path, desc)
if not os.path.exists(save_path1):
os.makedirs(save_path1)
model = TopologicalFilter(
ref_poses,
ref_descriptors[desc],
args.delta,
window_lower=args.window_lower,
window_upper=args.window_upper,
)
proposals, scores, times = utils.localize_traverses_filter(
model, query_descriptors[desc], vo=None, desc="Topological")
utils.save_obj(
save_path1 + "/Topological.pickle",
model="Topological",
query_gt=query_poses,
proposals=proposals,
scores=scores,
times=times,
)
return None
def main(args):
# load reference data
ref_poses, ref_descriptors, _ = utils.import_reference_map(
args.reference_traverse)
# localize all selected query traverses
pbar = tqdm(args.query_traverses)
for traverse in pbar:
pbar.set_description(traverse)
# savepath
save_path = os.path.join(utils.results_path, traverse)
# load query data
query_poses, _, _, query_descriptors, _ = utils.import_query_traverse(
traverse)
# regular traverse with VO
pbar = tqdm(args.descriptors, leave=False)
for desc in pbar:
pbar.set_description(desc)
# one folder per descriptor
save_path1 = os.path.join(save_path, desc)
if not os.path.exists(save_path1):
os.makedirs(save_path1)
L = len(query_descriptors[desc][0])
model = GraphMatching(ref_poses, ref_descriptors[desc], L,
args.exp_rate, args.fan_out)
proposals, scores, times, query_gt = utils.localize_traverses_graph(
model, query_poses, query_descriptors[desc], desc="Graph")
utils.save_obj(
save_path1 + "/Graph.pickle",
model="Graph",
query_gt=query_gt,
proposals=proposals,
scores=scores,
times=times,
)
return None
def main(args):
# load reference data
ref_poses, ref_descriptors, _ = utils.import_reference_map(
args.reference_traverse)
# NN search tree for poses
# 2 times is b/c of rotation angle representation in library
ref_tree = Nigh.SE3Tree(2 * args.attitude_weight)
ref_tree.insert(ref_poses.t(), ref_poses.R().as_quat())
# localize all selected query traverses
pbar = tqdm(args.query_traverses)
for traverse in pbar:
pbar.set_description(traverse)
# savepath
save_path = os.path.join(utils.results_path, traverse)
# load query data
query_poses, vo, rtk_motion, query_descriptors, _ = utils.import_query_traverse(
traverse)
# regular traverse with VO
pbar = tqdm(args.descriptors, leave=False)
for desc in pbar:
pbar.set_description(desc)
save_path1 = os.path.join(save_path,
desc) # one folder per descriptor
if not os.path.exists(save_path1):
os.makedirs(save_path1)
# save results from all trials
proposals_all, scores_all, times_all = [], [], []
for _ in trange(args.ntrials, leave=False, desc="Trials"):
model = ParticleFilter(
ref_tree,
ref_poses,
ref_descriptors[desc],
args.nparticles,
args.lambda2,
args.k_pose,
args.delta,
args.attitude_weight,
params.sigma_init,
params.sigma_vo[traverse],
)
proposals, scores, times = utils.localize_traverses_filter(
model,
query_descriptors[desc],
gt=query_poses,
vo=vo,
desc="Regular VO",
)
proposals_all.append(proposals)
scores_all.append(scores)
times_all.append(times)
utils.save_obj(
save_path1 + "/MCL.pickle",
model="MCL",
query_gt=query_poses,
proposals=proposals_all,
scores=scores_all,
times=times_all,
)
# RTK motion ablation
if not args.regular_only:
pbar = tqdm(args.descriptors, leave=False)
for desc in pbar:
pbar.set_description(desc)
save_path1 = os.path.join(save_path, desc)
proposals_all, scores_all, times_all = [], [], []
for _ in trange(args.ntrials, leave=False, desc="Trials"):
model = ParticleFilter(
ref_tree,
ref_poses,
ref_descriptors[desc],
args.nparticles,
args.lambda2,
args.k_pose,
args.delta,
args.attitude_weight,
params.sigma_init,
params.sigma_vo[traverse],
)
proposals, scores, times = utils.localize_traverses_filter(
model,
query_descriptors[desc],
vo=rtk_motion,
desc="RTK motion")
proposals_all.append(proposals)
scores_all.append(scores)
times_all.append(times)
utils.save_obj(
save_path1 + "/MCL_RTK_motion.pickle",
model="MCL RTK motion",
query_gt=query_poses,
proposals=proposals_all,
scores=scores_all,
times=times_all,
)
return None
def main(args):
# import reference and query traverses
ref_poses, ref_descriptors, _ = utils.import_reference_map(args.reference_traverse)
query_poses, vo, _, query_descriptors, _ = utils.import_query_traverse(
args.query_traverse
)
# shortcuts used when plotting
ref_poses_x, ref_poses_y = ref_poses.t()[:, 1], ref_poses.t()[:, 0]
query_descriptors = query_descriptors[args.descriptor][args.nloc]
query_poses = query_poses[args.nloc]
query_poses_x, query_poses_y = query_poses.t()[:, 1], query_poses.t()[:, 0]
L = len(vo[args.nloc]) + 1 # max sequence length
vo = vo[args.nloc]
# setup topological filter
topofilter = TopologicalFilter(
ref_poses,
ref_descriptors[args.descriptor],
args.delta_topo,
window_lower=args.window_lower,
window_upper=args.window_upper,
)
topofilter.initialize_model(query_descriptors[0, :])
# setup single image matching
single = SingleImageMatching(ref_poses, ref_descriptors[args.descriptor])
# initialize plots
fig = plt.figure(figsize=(14, 4))
gs = fig.add_gridspec(1, 2, wspace=0, hspace=0)
axs = []
for i in range(2):
axs.append(fig.add_subplot(gs[:, i]))
for i in range(len(axs)):
axs[i].set_xticks([])
axs[i].set_yticks([])
axs[i].set_aspect("equal")
axs[i].set_xlim(xlims[0], xlims[1])
axs[i].set_ylim(ylims[0], ylims[1])
for t in range(L):
if t > 0:
topofilter.update(query_descriptors[t, :])
proposalT, scoreT = topofilter.localize()
proposalS, scoreS = single.localize_descriptor(query_descriptors[t, :])
# visualize initial
if t == 0:
axs[0].scatter(
ref_poses_x,
ref_poses_y,
c=topofilter.belief,
cmap="OrRd",
s=12,
vmin=0,
vmax=3e-4,
)
axs[0].set_title(
r"t = {}, $\tau_t$ = {:.2f}".format(t, scoreT), fontsize=36
)
axs[1].scatter(
[proposalT.t()[1]],
[proposalT.t()[0]],
color="fuchsia",
s=400,
marker="X",
)
axs[0].scatter(
[query_poses_x[t]],
[query_poses_y[t]],
color="limegreen",
s=600,
marker="*",
)
# visualize intermediate
if t == args.niter:
axs[1].scatter(
ref_poses_x,
ref_poses_y,
c=topofilter.belief,
cmap="OrRd",
s=12,
vmin=0,
vmax=4e-3,
)
axs[1].set_title(
r"t = {}, $\tau_t$ = {:.2f}".format(t, scoreT), fontsize=36
)
axs[1].scatter(
[proposalT.t()[1]],
[proposalT.t()[0]],
color="fuchsia",
s=400,
marker="X",
alpha=1.0,
)
axs[1].scatter(
[query_poses_x[t]],
[query_poses_y[t]],
color="limegreen",
s=600,
marker="*",
alpha=0.8,
)
fig.tight_layout()
plt.subplots_adjust(wspace=0, hspace=0)
plt.savefig(utils.figures_path + "/confidence_fig.png")
def main(args):
# import reference and query traverses
ref_poses, ref_descriptors, ref_tstamps = utils.import_reference_map(args.reference_traverse)
query_poses, vo, _, query_descriptors, query_tstamps = utils.import_query_traverse(args.query_traverse)
# shortcuts used when plotting
ref_poses_x, ref_poses_y = ref_poses.t()[:, 1], ref_poses.t()[:, 0]
query_descriptors = query_descriptors[args.descriptor][args.nloc]
query_poses = query_poses[args.nloc]
query_poses_x, query_poses_y = query_poses.t()[:, 1], query_poses.t()[:, 0]
query_tstamps = query_tstamps[args.nloc]
L = len(vo[args.nloc]) + 1 # max sequence length
vo = vo[args.nloc]
# import all images
query_images = []
nearest_ref_images = []
for t in range(len(query_tstamps)):
# load query images
imgFolderQ = os.path.join(READY_PATH, params.traverses[args.query_traverse], 'stereo/left')
imgPathQ = os.path.join(imgFolderQ, str(query_tstamps[t]) + '.png')
imgQ = plt.imread(imgPathQ)
query_images.append(imgQ)
distrel = geometry.metric(query_poses[t], ref_poses, args.attitude_weight)
idx = np.argmin(distrel)
imgFolderR = os.path.join(READY_PATH, params.traverses[args.reference_traverse], 'stereo/left')
imgPathR = os.path.join(imgFolderR, str(ref_tstamps[idx]) + '.png')
imgR = plt.imread(imgPathR)
nearest_ref_images.append(imgR)
# setup particle filter
ref_tree = Nigh.SE3Tree(2 * args.attitude_weight) # 2 times is b/c of rotation angle representation in library
ref_tree.insert(ref_poses.t(), ref_poses.R().as_quat())
particlefilter = ParticleFilter(ref_tree, ref_poses, ref_descriptors[args.descriptor], args.nparticles, args.lambda2, args.k_pose,
args.delta_particles, args.attitude_weight, params.sigma_init, params.sigma_vo[args.query_traverse])
particlefilter.initialize_model(query_descriptors[0, :])
# setup topological filter
topofilter = TopologicalFilter(ref_poses, ref_descriptors[args.descriptor], args.delta_topo, window_lower=args.window_lower, window_upper=args.window_upper)
topofilter.initialize_model(query_descriptors[0, :])
# setup and localize SeqSLAM
seqmatch = SeqMatching(ref_poses, ref_descriptors[args.descriptor], args.wContrast, args.numVel, args.vMin, args.vMax, args.matchWindow, args.enhance)
proposalSS, scoreSS = seqmatch.localize(query_descriptors)
# setup single image matching
single = SingleImageMatching(ref_poses, ref_descriptors[args.descriptor])
# initialize over figure and subplots
fig, axs = plt.subplots(2, 3, figsize=(20, 10))
# initialize specific particle/belief plots
belief_plots = []
proposal_plots = []
true_query_plots = []
single_plots = []
for i in range(axs.shape[0]):
for j in range(axs.shape[1] - 1):
if (i == 0 and j == 0) or (i == 1 and j == 0):
axs[i, j].scatter(ref_poses_x, ref_poses_y, color='black', s=8) # reference map plots (Topo)
belief_plots.append(axs[i, j].scatter([], [], cmap='jet', c=[], vmin=vmins[j], vmax=vmaxs[j]))
else:
belief_plots.append(axs[i, j].scatter(ref_poses_x, ref_poses_y, s=8, cmap='jet', c=topofilter.belief, vmin=vmins[j], vmax=vmaxs[j]))
proposal_plots.append(axs[i, j].scatter([], [], color='gold', marker="*", s=75))
true_query_plots.append(axs[i, j].scatter([], [], color='lawngreen', marker="X", s=50))
single_plots.append(axs[i, j].scatter([], [], color='orange', marker="*", s=50))
# plot seqslam proposals once
axs[i, j].scatter([proposalSS.t()[1]], [proposalSS.t()[0]], color='magenta', marker="*", s=50)
axs[i, j].scatter([query_poses_x[0]], [query_poses_y[0]], color='magenta', marker="X", s=50)
# fixed axes limits for global view
axs[i, j].set_aspect('equal', adjustable='datalim')
if (i == 0 and j == 1) or (i == 1 and j == 1):
axs[i, j].set_xlim(np.min(ref_poses_x) - 100, np.max(ref_poses_x) + 100)
axs[i, j].set_ylim(np.min(ref_poses_y) - 100, np.max(ref_poses_y) + 100)
axs[i, j].set_xticks([])
axs[i, j].set_yticks([])
# image plots
axs[0, -1].set_xticks([])
axs[0, -1].set_yticks([])
axs[1, -1].set_xticks([])
axs[1, -1].set_yticks([])
img_plots = [axs[0, -1].imshow(np.zeros_like(query_images[0])), axs[1, -1].imshow(np.zeros_like(query_images[0]))]
axs[0, 2].set_title("Query image")
axs[1, 2].set_title("Closest reference image")
def animate(t):
R = args.radius
# update belief
if t > 0:
particlefilter.update(vo[t-1], query_descriptors[t, :])
topofilter.update(query_descriptors[t, :])
proposalP, scoreP = particlefilter.localize()
proposalT, scoreT = topofilter.localize()
proposalS, scoreS = single.localize_descriptor(query_descriptors[t, :])
# plot beliefs for all models
particles = particlefilter.particles
belief = topofilter.belief
belief_plots[0].set_offsets(particles.t()[:, 1::-1]) # particle filter plots
belief_plots[2].set_offsets(particles.t()[:, 1::-1])
belief_plots[0].set_array(particlefilter.weights)
belief_plots[2].set_array(particlefilter.weights)
belief_plots[1].set_array(belief)
belief_plots[3].set_array(belief)
# plot is zoomed around gt, adj limits
for j in range(axs.shape[1] - 1):
axs[1, j].set_xlim(query_poses_x[t] - R, query_poses_x[t] + R)
axs[1, j].set_ylim(query_poses_y[t] - R, query_poses_y[t] + R)
# compute proposal distance from truth
relP = proposalP.inv() * query_poses[t]
relT = proposalT.inv() * query_poses[t]
tdistP = np.linalg.norm(relP.t())
tdistT = np.linalg.norm(relT.t())
RdistP = relP.R().magnitude() * 180 / np.pi
RdistT = relT.R().magnitude() * 180 / np.pi
axs[0, 0].set_title(r"MCL: $\tau_t = {:.1f}$, t: {:.1f} R: {:.1f}".format(scoreP, tdistP, RdistP))
axs[1, 0].set_title(r"MCL: Local view. $\tau_t = {:.1f}$, t: {:.1f} R: {:.1f}".format(scoreP, tdistP, RdistP))
axs[0, 1].set_title(r"Topological: Global view. $\tau_t = {:.2f}$, t: {:.1f} R: {:.1f}".format(scoreT, tdistT, RdistT))
axs[1, 1].set_title(r"Topological: Local view. $\tau_t = {:.2f}$, t: {:.1f} R: {:.1f}".format(scoreT, tdistT, RdistT))
# plot proposals from each model
proposal_plots[0].set_offsets([proposalP.t()[1::-1]]) # particles
proposal_plots[2].set_offsets([proposalP.t()[1::-1]])
proposal_plots[1].set_offsets([proposalT.t()[1::-1]]) # topological
proposal_plots[3].set_offsets([proposalT.t()[1::-1]])
single_plots[0].set_offsets([proposalS.t()[1::-1]]) # particles
single_plots[2].set_offsets([proposalS.t()[1::-1]])
single_plots[1].set_offsets([proposalS.t()[1::-1]]) # topological
single_plots[3].set_offsets([proposalS.t()[1::-1]])
# plot ground truth pose for each panel
for plot in true_query_plots:
plot.set_offsets([query_poses_x[t], query_poses_y[t]])
# image plots
img_plots[0].set_data(query_images[t])
img_plots[1].set_data(nearest_ref_images[t])
return belief_plots + proposal_plots + true_query_plots
fig.tight_layout()
ani = animation.FuncAnimation(fig, animate, np.arange(L), blit=False, interval=args.delay, repeat=True)
Writer = animation.writers['ffmpeg']
writer = Writer(fps=2, metadata=dict(artist='Me'), bitrate=1800)
ani.save(utils.figures_path + '/visualization.mp4', writer=writer)