def __init__(self,
count,
start,
posrange,
angrange,
polemeans,
polevar,
T_w_o=np.identity(4)):
self.p_min = 0.01
self.d_max = np.sqrt(
-2.0 * polevar *
np.log(np.sqrt(2.0 * np.pi * polevar) * self.p_min))
self.minneff = 0.5
self.estimatetype = 'best'
self.count = count
r = np.random.uniform(low=0.0, high=posrange, size=[self.count, 1])
angle = np.random.uniform(low=-np.pi, high=np.pi, size=[self.count, 1])
xy = r * np.hstack([np.cos(angle), np.sin(angle)])
dxyp = np.hstack([
xy,
np.random.uniform(low=-angrange,
high=angrange,
size=[self.count, 1])
])
self.particles = np.matmul(start, util.xyp2ht(dxyp))
self.weights = np.full(self.count, 1.0 / self.count)
self.polemeans = polemeans # global map data
self.poledist = scipy.stats.norm(loc=0.0, scale=np.sqrt(polevar))
self.kdtree = scipy.spatial.cKDTree(polemeans[:, :2], leafsize=3)
self.T_w_o = T_w_o
self.T_o_w = util.invert_ht(self.T_w_o)
self.Sigma = np.eye(3) * 1e-13
def save_global_map():
globalmappos = np.empty([0, 2])
mapfactors = np.full(len(pynclt.sessions), np.nan)
poleparams = np.empty([0, 6])
for isession, s in enumerate(pynclt.sessions):
print(s)
session = pynclt.session(s)
istart, imid, iend = get_map_indices(session)
localmappos = session.T_w_r_gt_velo[imid, :2, 3]
if globalmappos.size == 0:
imaps = range(localmappos.shape[0])
else:
imaps = []
for imap in range(localmappos.shape[0]):
distance = np.linalg.norm(
localmappos[imap] - globalmappos, axis=1).min()
if distance > remapdistance:
imaps.append(imap)
globalmappos = np.vstack([globalmappos, localmappos[imaps]])
mapfactors[isession] = np.true_divide(len(imaps), len(imid))
with progressbar.ProgressBar(max_value=len(imaps)) as bar:
for iimap, imap in enumerate(imaps):
scans = []
for iscan in range(istart[imap], iend[imap]):
xyz, _ = session.get_velo(iscan)
scan = o3.PointCloud()
scan.points = o3.Vector3dVector(xyz)
scans.append(scan)
T_w_mc = np.identity(4)
T_w_mc[:3, 3] = session.T_w_r_gt_velo[imid[imap], :3, 3]
T_w_m = T_w_mc.dot(T_mc_m)
T_m_w = util.invert_ht(T_w_m)
T_m_r = np.matmul(
T_m_w, session.T_w_r_gt_velo[istart[imap]:iend[imap]])
occupancymap = mapping.occupancymap(
scans, T_m_r, mapshape, mapsize)
localpoleparams = poles.detect_poles(occupancymap, mapsize)
localpoleparams[:, :2] += T_w_m[:2, 3]
poleparams = np.vstack([poleparams, localpoleparams])
bar.update(iimap)
xy = poleparams[:, :2]
a = poleparams[:, [4]]
boxes = np.hstack([xy - 0.5 * a, xy + 0.5 * a])
clustermeans = np.empty([0, 5])
scores = []
for ci in cluster.cluster_boxes(boxes):
ci = list(ci)
if len(ci) < n_mapdetections:
continue
clustermeans = np.vstack([clustermeans, np.average(
poleparams[ci, :-1], axis=0, weights=poleparams[ci, -1])])
scores.append(np.mean(poleparams[ci, -1]))
clustermeans = np.hstack([clustermeans, np.array(scores).reshape([-1, 1])])
globalmapfile = os.path.join(pynclt.resultdir, get_globalmapname() + '.npz')
np.savez(globalmapfile,
polemeans=clustermeans, mapfactors=mapfactors, mappos=globalmappos)
plot_global_map(globalmapfile)
def evaluate():
stats = []
for sessionname in pynclt.sessions:
files = [file for file \
in os.listdir(os.path.join(pynclt.resultdir, sessionname)) \
if file.startswith(localization_name_start)]
# if file.startswith(get_locfileprefix())]
files.sort()
session = pynclt.session(sessionname)
cumdist = np.hstack([0.0, np.cumsum(np.linalg.norm(np.diff(
session.T_w_r_gt[:, :3, 3], axis=0), axis=1))])
t_eval = scipy.interpolate.interp1d(
cumdist, session.t_gt)(np.arange(0.0, cumdist[-1], 1.0))
T_w_r_gt = np.stack([util.project_xy(
session.get_T_w_r_gt(t).dot(T_r_mc)).dot(T_mc_r) \
for t in t_eval])
T_gt_est = []
for file in files:
T_w_r_est = np.load(os.path.join(
pynclt.resultdir, sessionname, file))['T_w_r_est']
T_w_r_est_interp = np.empty([len(t_eval), 4, 4])
iodo = 1
for ieval in range(len(t_eval)):
while session.t_relodo[iodo] < t_eval[ieval]:
iodo += 1
T_w_r_est_interp[ieval] = util.interpolate_ht(
T_w_r_est[iodo-1:iodo+1],
session.t_relodo[iodo-1:iodo+1], t_eval[ieval])
T_gt_est.append(
np.matmul(util.invert_ht(T_w_r_gt), T_w_r_est_interp))
T_gt_est = np.stack(T_gt_est)
lonerror = np.mean(np.mean(np.abs(T_gt_est[..., 0, 3]), axis=-1))
laterror = np.mean(np.mean(np.abs(T_gt_est[..., 1, 3]), axis=-1))
poserrors = np.linalg.norm(T_gt_est[..., :2, 3], axis=-1)
poserror = np.mean(np.mean(poserrors, axis=-1))
posrmse = np.mean(np.sqrt(np.mean(poserrors**2, axis=-1)))
angerrors = np.degrees(np.abs(
np.array([util.ht2xyp(T)[:, 2] for T in T_gt_est])))
angerror = np.mean(np.mean(angerrors, axis=-1))
angrmse = np.mean(np.sqrt(np.mean(angerrors**2, axis=-1)))
stats.append({'session': sessionname, 'lonerror': lonerror,
'laterror': laterror, 'poserror': poserror, 'posrmse': posrmse,
'angerror': angerror, 'angrmse': angrmse, 'T_gt_est': T_gt_est})
np.savez(os.path.join(pynclt.resultdir, get_evalfile()), stats=stats)
mapdata = np.load(os.path.join(pynclt.resultdir, get_globalmapname() + '.npz'))
print('session \t f\te_pos \trmse_pos \te_ang \te_rmse')
row = '{session} \t{f} \t{poserror} \t{posrmse} \t{angerror} \t{angrmse}'
for i, stat in enumerate(stats):
print(row.format(
session=stat['session'],
f=mapdata['mapfactors'][i] * 100.0,
poserror=stat['poserror'],
posrmse=stat['posrmse'],
angerror=stat['angerror'],
angrmse=stat['angrmse']))
def save_local_maps(sessionname, visualize=False):
print(sessionname)
session = pynclt.session(sessionname)
util.makedirs(session.dir)
istart, imid, iend = get_map_indices(session)
maps = []
with progressbar.ProgressBar(max_value=len(iend)) as bar:
for i in range(len(iend)):
scans = []
for idx, val in enumerate(range(istart[i], iend[i])):
xyz, _ = session.get_velo(val)
scan = o3.geometry.PointCloud()
scan.points = o3.utility.Vector3dVector(xyz)
scans.append(scan)
T_w_mc = util.project_xy(
session.T_w_r_odo_velo[imid[i]].dot(T_r_mc))
T_w_m = T_w_mc.dot(T_mc_m)
T_m_w = util.invert_ht(T_w_m)
T_w_r = session.T_w_r_odo_velo[istart[i]:iend[i]]
T_m_r = np.matmul(T_m_w, T_w_r)
occupancymap = mapping.occupancymap(scans, T_m_r, mapshape,
mapsize)
poleparams = poles.detect_poles(occupancymap, mapsize)
if visualize:
cloud = o3.geometry.PointCloud()
for T, scan in zip(T_w_r, scans):
s = copy.copy(scan)
s.transform(T)
cloud.points.extend(s.points)
mapboundsvis = util.create_wire_box(mapextent, [0.0, 0.0, 1.0])
mapboundsvis.transform(T_w_m)
polevis = []
for j in range(poleparams.shape[0]):
x, y, zs, ze, a = poleparams[j, :5]
pole = util.create_wire_box([a, a, ze - zs],
color=[1.0, 1.0, 0.0])
T_m_p = np.identity(4)
T_m_p[:3, 3] = [x - 0.5 * a, y - 0.5 * a, zs]
pole.transform(T_w_m.dot(T_m_p))
polevis.append(pole)
o3.visualization.draw_geometries(polevis +
[cloud, mapboundsvis])
map = {
'poleparams': poleparams,
'T_w_m': T_w_m,
'istart': istart[i],
'imid': imid[i],
'iend': iend[i]
}
maps.append(map)
bar.update(i)
np.savez(os.path.join(session.dir, get_localmapfile()), maps=maps)
def sequence(self, i):
# if 0 <= i < 11:
# sequence = pykitti.odometry(self.ododir, '{:02d}'.format(i))
# sequence.poses = numpy.array(sequence.poses)
# else:
drive = kittidrives.drives[i]
sequence = pykitti.raw(self.rawdir, drive['date'], drive['drive'])
T_imu_cam0 = util.invert_ht(sequence.calib.T_cam0_imu)
sequence.poses = numpy.array(
[numpy.matmul(oxts.T_w_imu, T_imu_cam0) \
for oxts in sequence.oxts])
return sequence
def save_local_maps(seq):
print(seq)
sequence = dataset.sequence(seq)
seqdir = os.path.join(result_dir, '{:03d}'.format(seq))
util.makedirs(seqdir)
istart, imid, iend = get_map_indices(sequence)
maps = []
with progressbar.ProgressBar(max_value=len(iend)) as bar:
for i in range(len(iend)):
scans = []
for idx, val in enumerate(range(istart[i], iend[i])):
velo = sequence.get_velo(val)
scan = o3.geometry.PointCloud()
scan.points = o3.utility.Vector3dVector(velo[:, :3])
scan.colors = o3.utility.Vector3dVector(
util.intensity2color(velo[:, 3]))
scans.append(scan)
T_m_w = T_m_mc.dot(T_mc_cam0).dot(
util.invert_ht(sequence.poses[imid[i]]))
T_w_velo = np.matmul(sequence.poses[istart[i]:iend[i]],
sequence.calib.T_cam0_velo)
T_m_velo = np.matmul(T_m_w, T_w_velo)
occupancymap = mapping.occupancymap(scans, T_m_velo, mapshape,
mapsize)
poleparams = poles.detect_poles(occupancymap, mapsize)
# accuscan = o3.PointCloud()
# for j in range(len(scans)):
# scans[j].transform(T_w_velo[j])
# accuscan.points.extend(scans[j].points)
# o3.draw_geometries([accuscan])
# import ndshow
# ndshow.matshow(occupancymap.transpose([2, 0, 1]))
map = {
'poleparams': poleparams,
'istart': istart[i],
'imid': imid[i],
'iend': iend[i]
}
maps.append(map)
bar.update(i)
np.savez(os.path.join(seqdir, localmapfile), maps=maps)
def __init__(self,
count,
start,
posrange,
angrange,
polemeans,
polevar,
T_w_o=np.identity(4)):
self.p_min = 0.01
self.d_max = np.sqrt(
-2.0 * polevar *
np.log(np.sqrt(2.0 * np.pi * polevar) * self.p_min))
self.minneff = 0.5
self.estimatetype = 'best'
self.count = count
"""
r = #count times a uniformly random distance 0 -> posrange.
angle = #count times a random angle -pi -> pi
xy = the angles multiplied by distance (radius)
dxyp = xy and #count random angles from the anglerange
particles = #count points in 2D-space and an angle phi
weights = starts with uniform weight
"""
r = np.random.uniform(low=0.0, high=posrange, size=[self.count, 1])
angle = np.random.uniform(low=-np.pi, high=np.pi, size=[self.count, 1])
xy = r * np.hstack([np.cos(angle), np.sin(angle)])
dxyp = np.hstack([
xy,
np.random.uniform(low=-angrange,
high=angrange,
size=[self.count, 1])
])
self.particles = np.matmul(start, util.xyp2ht(dxyp))
self.weights = np.full(self.count, 1.0 / self.count)
self.polemeans = polemeans
self.poledist = scipy.stats.norm(loc=0.0, scale=np.sqrt(polevar))
self.kdtree = scipy.spatial.cKDTree(polemeans[:, :2], leafsize=3)
self.T_w_o = T_w_o
self.T_o_w = util.invert_ht(self.T_w_o)
def localize(sessionname, visualize=False):
print(sessionname)
mapdata = np.load(
os.path.join(pynclt.resultdir,
get_globalmapname() + '.npz'))
polemap = mapdata['polemeans'][:, :2]
polevar = 1.50
session = pynclt.session(sessionname)
figuresdir = os.path.join(
pynclt.resultdir, session.dir,
'Figures_{:.0f}_{:.0f}_{:.0f}'.format(n_mapdetections,
10 * poles.minscore,
poles.polesides[-1]))
util.makedirs(figuresdir)
locdata = np.load(os.path.join(session.dir, get_localmapfile()),
allow_pickle=True)['maps']
polepos_m = []
polepos_w = []
for i in range(len(locdata)):
n = locdata[i]['poleparams'].shape[0]
pad = np.hstack([np.zeros([n, 1]), np.ones([n, 1])])
polepos_m.append(np.hstack([locdata[i]['poleparams'][:, :2], pad]).T)
polepos_w.append(locdata[i]['T_w_m'].dot(polepos_m[i]))
istart = 0
T_w_r_start = util.project_xy(
session.get_T_w_r_gt(session.t_relodo[istart]).dot(T_r_mc)).dot(T_mc_r)
filter = particlefilter.particlefilter(5000,
T_w_r_start,
2.5,
np.radians(5.0),
polemap,
polevar,
T_w_o=T_mc_r)
filter.estimatetype = 'best'
filter.minneff = 0.5
if visualize:
plt.ion()
figure = plt.figure()
nplots = 1
mapaxes = figure.add_subplot(nplots, 1, 1)
mapaxes.set_aspect('equal')
mapaxes.scatter(polemap[:, 0], polemap[:, 1], s=10, c='b', marker='s')
x_gt, y_gt = session.T_w_r_gt[::20, :2, 3].T
mapaxes.plot(x_gt, y_gt, 'g')
particles = mapaxes.scatter([], [], s=1, c='r')
arrow = mapaxes.arrow(0.0,
0.0,
4.0,
0.0,
length_includes_head=True,
head_width=1.2,
head_length=1.5,
color='k')
arrowdata = np.hstack(
[arrow.get_xy(), np.zeros([8, 1]),
np.ones([8, 1])]).T
locpoles = mapaxes.scatter([], [], s=30, c='y', marker='^')
viewoffset = 25.0
imap = 0
while imap < locdata.shape[0] - 1 and \
session.t_velo[locdata[imap]['iend']] < session.t_relodo[istart]:
imap += 1
T_w_r_est = np.full([session.t_relodo.size, 4, 4], np.nan)
with progressbar.ProgressBar(max_value=session.t_relodo.size) as bar:
for i in range(istart, session.t_relodo.size):
relodocov = np.empty([3, 3])
relodocov[:2, :2] = session.relodocov[i, :2, :2]
relodocov[:, 2] = session.relodocov[i, [0, 1, 5], 5]
relodocov[2, :] = session.relodocov[i, 5, [0, 1, 5]]
filter.update_motion(session.relodo[i], relodocov * 2.0**2)
T_w_r_est[i] = filter.estimate_pose()
t_now = session.t_relodo[i]
if imap < locdata.shape[0]:
t_end = session.t_velo[locdata[imap]['iend']]
if t_now >= t_end:
imaps = range(imap, np.clip(imap - n_localmaps, -1, None),
-1)
xy = np.hstack([polepos_w[j][:2] for j in imaps]).T
a = np.vstack([ld['poleparams'][:, [4]] \
for ld in locdata[imaps]])
boxes = np.hstack([xy - 0.5 * a, xy + 0.5 * a])
ipoles = set(range(polepos_w[imap].shape[1]))
iactive = set()
for ci in cluster.cluster_boxes(boxes):
if len(ci) >= n_locdetections:
iactive |= set(ipoles) & ci
iactive = list(iactive)
if iactive:
t_mid = session.t_velo[locdata[imap]['imid']]
T_w_r_mid = util.project_xy(
session.get_T_w_r_odo(t_mid).dot(T_r_mc)).dot(
T_mc_r)
T_w_r_now = util.project_xy(
session.get_T_w_r_odo(t_now).dot(T_r_mc)).dot(
T_mc_r)
T_r_now_r_mid = util.invert_ht(T_w_r_now).dot(
T_w_r_mid)
polepos_r_now = T_r_now_r_mid.dot(T_r_m).dot(
polepos_m[imap][:, iactive])
filter.update_measurement(polepos_r_now[:2].T)
T_w_r_est[i] = filter.estimate_pose()
if visualize:
polepos_w_est = T_w_r_est[i].dot(polepos_r_now)
locpoles.set_offsets(polepos_w_est[:2].T)
imap += 1
if visualize:
particles.set_offsets(filter.particles[:, :2, 3])
arrow.set_xy(T_w_r_est[i].dot(arrowdata)[:2].T)
x, y = T_w_r_est[i, :2, 3]
mapaxes.set_xlim(left=x - viewoffset, right=x + viewoffset)
mapaxes.set_ylim(bottom=y - viewoffset, top=y + viewoffset)
# Save figures for generating GIF
if i % 25 == 0:
filename = sessionname + '_' + str(i) + '_'
figure.savefig(os.path.join(figuresdir, filename + '.png'))
figure.canvas.draw_idle()
figure.canvas.flush_events()
bar.update(i)
filename = os.path.join(session.dir, get_locfileprefix() \
+ datetime.datetime.now().strftime('_%Y-%m-%d_%H-%M-%S.npz'))
np.savez(filename, T_w_r_est=T_w_r_est)
import os
import warnings
import open3d as o3
import matplotlib.pyplot as plt
import numpy as np
import progressbar
import pyquaternion as pq
import transforms3d as t3
import util
T_w_o = np.identity(4)
T_w_o[:3, :3] = [[0, 1, 0], [1, 0, 0], [0, 0, -1]]
T_o_w = util.invert_ht(T_w_o)
eulerdef = 'sxyz'
csvdelimiter = ','
datadir = '/mnt/data/datasets/nclt'
resultdir = 'nclt'
snapshotfile = 'snapshot.npz'
sessionfile = 'sessiondata.npz'
sessions = [
'2012-01-08', '2012-01-15', '2012-01-22', '2012-02-02', '2012-02-04',
'2012-02-05', '2012-02-12', '2012-02-18', '2012-02-19', '2012-03-17',
'2012-03-25', '2012-03-31', '2012-04-29', '2012-05-11', '2012-05-26',
'2012-06-15', '2012-08-04', '2012-08-20', '2012-09-28', '2012-10-28',
'2012-11-04', '2012-11-16', '2012-11-17', '2012-12-01', '2013-01-10',
'2013-02-23', '2013-04-05'
]
def evaluate(seq):
sequence = dataset.sequence(seq)
T_w_velo_gt = np.matmul(sequence.poses, sequence.calib.T_cam0_velo)
T_w_velo_gt = np.array([util.project_xy(ht) for ht in T_w_velo_gt])
trajectory_dir = os.path.join(result_dir, '{:03d}'.format(seq),
'trajectory')
util.makedirs(trajectory_dir)
mapdata = np.load(os.path.join(seqdir, globalmapfile), allow_pickle=True)
polemap = mapdata['polemeans']
# plt.scatter(polemap[:, 0], polemap[:, 1], s=1, c='b')
# plt.plot(T_w_velo_gt[:, 0, 3], T_w_velo_gt[:, 1, 3], color=(0, 1, 0), label='Ground Truth', linewidth=3.0)
cumdist = np.hstack([
0.0,
np.cumsum(
np.linalg.norm(np.diff(T_w_velo_gt[:, :2, 3], axis=0), axis=1))
])
timestamps = np.array([arrow.get(timestamp).float_timestamp \
for timestamp in sequence.timestamps])
t_eval = scipy.interpolate.interp1d(cumdist, timestamps)(np.arange(
0.0, cumdist[-1], 1.0))
n = t_eval.size
T_w_velo_gt_interp = np.empty([n, 4, 4])
iodo = 1
for ieval in range(n):
while timestamps[iodo] < t_eval[ieval]:
iodo += 1
T_w_velo_gt_interp[ieval] = util.interpolate_ht(
T_w_velo_gt[iodo - 1:iodo + 1], timestamps[iodo - 1:iodo + 1],
t_eval[ieval])
files = [file for file in os.listdir(seqdir) \
if os.path.basename(file).startswith(locfileprefix)]
poserror = np.full([n, len(files)], np.nan)
laterror = np.full([n, len(files)], np.nan)
lonerror = np.full([n, len(files)], np.nan)
angerror = np.full([n, len(files)], np.nan)
T_gt_est = np.full([n, 4, 4], np.nan)
for ifile in range(len(files)):
T_w_velo_est = np.load(os.path.join(seqdir, files[ifile]),
allow_pickle=True)['T_w_velo_est']
plt.clf()
plt.plot(T_w_velo_gt[:, 0, 3],
T_w_velo_gt[:, 1, 3],
color=(0, 1, 0),
label='Ground Truth',
linewidth=3.0)
landmarks = plt.scatter(polemap[:, 0],
polemap[:, 1],
s=1,
c='m',
marker='*',
label='Landmarks')
iodo = 1
for ieval in range(n):
while timestamps[iodo] < t_eval[ieval]:
iodo += 1
T_w_velo_est_interp = util.interpolate_ht(
T_w_velo_est[iodo - 1:iodo + 1], timestamps[iodo - 1:iodo + 1],
t_eval[ieval])
T_gt_est[ieval] = util.invert_ht(
T_w_velo_gt_interp[ieval]).dot(T_w_velo_est_interp)
lonerror[:, ifile] = T_gt_est[:, 0, 3]
laterror[:, ifile] = T_gt_est[:, 1, 3]
poserror[:, ifile] = np.linalg.norm(T_gt_est[:, :2, 3], axis=1)
angerror[:, ifile] = util.ht2xyp(T_gt_est)[:, 2]
plt.plot(T_w_velo_est[:, 0, 3],
T_w_velo_est[:, 1, 3],
'r',
label='Estimated trajectory')
plt.ylabel('North (Unit:m)')
plt.xlabel('East (Unit:m)')
plt.legend()
plt.gcf().subplots_adjust(bottom=0.13,
top=0.98,
left=0.145,
right=0.98)
plt.grid(color=(0.5, 0.5, 0.5), linestyle='-', linewidth=1)
angerror = np.degrees(angerror)
lonstd = np.std(lonerror, axis=0)
latstd = np.std(laterror, axis=0)
angstd = np.std(angerror, axis=0)
angerror = np.abs(angerror)
laterror = np.mean(np.abs(laterror), axis=0)
lonerror = np.mean(np.abs(lonerror), axis=0)
posrmse = np.sqrt(np.mean(poserror**2, axis=0))
angrmse = np.sqrt(np.mean(angerror**2, axis=0))
poserror = np.mean(poserror, axis=0)
angerror = np.mean(angerror, axis=0)
plt.savefig(os.path.join(trajectory_dir, 'trajectory_est.svg'))
plt.savefig(os.path.join(trajectory_dir, 'trajectory_est.png'))
np.savez(os.path.join(seqdir, evalfile),
poserror=poserror,
angerror=angerror,
posrmse=posrmse,
angrmse=angrmse,
laterror=laterror,
latstd=latstd,
lonerror=lonerror,
lonstd=lonstd)
print('poserror: {}\nposrmse: {}\n'
'laterror: {}\nlatstd: {}\n'
'lonerror: {}\nlonstd: {}\n'
'angerror: {}\nangstd: {}\nangrmse: {}'.format(
np.mean(poserror), np.mean(posrmse), np.mean(laterror),
np.mean(latstd), np.mean(lonerror), np.mean(lonstd),
np.mean(angerror), np.mean(angstd), np.mean(angrmse)))
def localize(seq, visualize=False):
print(seq)
sequence = dataset.sequence(seq)
seqdir = os.path.join(result_dir, '{:03d}'.format(seq))
mapdata = np.load(os.path.join(seqdir, globalmapfile), allow_pickle=True)
polemap = mapdata['polemeans']
polemap = np.hstack([polemap[:, :2], np.diff(polemap[:, 2:4], axis=1)])
figuresdir = os.path.join(seqdir, 'Figures')
util.makedirs(figuresdir)
locdata = np.load(os.path.join(seqdir, localmapfile),
allow_pickle=True)['maps']
T_velo_cam0 = util.invert_ht(sequence.calib.T_cam0_velo)
T_w_velo_gt = np.matmul(sequence.poses, sequence.calib.T_cam0_velo)
i = 0
polecov = 1.0
filter = particlefilter.particlefilter(2000, T_w_velo_gt[i], 3.0,
np.radians(5.0), polemap, polecov)
filter.minneff = 0.5
filter.estimatetype = 'best'
if visualize:
plt.ion()
figure = plt.figure()
nplots = 2
mapaxes = figure.add_subplot(nplots, 1, 1)
mapaxes.set_aspect('equal')
mapaxes.scatter(polemap[:, 0], polemap[:, 1], s=10, c='b', marker='s')
mapaxes.plot(T_w_velo_gt[:, 0, 3], T_w_velo_gt[:, 1, 3], 'g')
particles = mapaxes.scatter([], [], s=1, c='r')
arrow = mapaxes.arrow(0.0,
0.0,
4.0,
0.0,
length_includes_head=True,
head_width=1.2,
head_length=1.5,
color='k')
arrowdata = np.hstack(
[arrow.get_xy(), np.zeros([8, 1]),
np.ones([8, 1])]).T
locpoles = mapaxes.scatter([], [], s=30, c='y', marker='^')
viewoffset = 25.0
# weightaxes = figure.add_subplot(nplots, 1, 2)
# gridsize = 50
# offset = 10.0
# visfilter = particlefilter.particlefilter(gridsize**2,
# np.identity(4), 0.0, 0.0, polemap, polecov)
# gridcoord = np.linspace(-offset, offset, gridsize)
# x, y = np.meshgrid(gridcoord, gridcoord)
# dxy = np.hstack([x.reshape([-1, 1]), y.reshape([-1, 1])])
# weightimage = weightaxes.matshow(np.zeros([gridsize, gridsize]),
# extent=(-offset, offset, -offset, offset))
# histaxes = figure.add_subplot(nplots, 1, 3)
imap = 0
while locdata[imap]['imid'] < i:
imap += 1
T_w_velo_est = np.full(T_w_velo_gt.shape, np.nan)
T_w_velo_est[i] = filter.estimate_pose()
i += 1
with progressbar.ProgressBar(max_value=T_w_velo_est.shape[0] - i) as bar:
while i < T_w_velo_est.shape[0]:
relodo = util.ht2xyp(
util.invert_ht(T_w_velo_gt[i - 1]).dot(T_w_velo_gt[i]))
relodocov = np.diag((0.02 * relodo)**2)
relodo = np.random.multivariate_normal(relodo, relodocov)
filter.update_motion(relodo, relodocov)
T_w_velo_est[i] = filter.estimate_pose()
if imap < locdata.size and i >= locdata[imap]['iend']:
T_w_cam0_mid = sequence.poses[locdata[imap]['imid']]
T_w_cam0_now = sequence.poses[i]
T_cam0_now_cam0_mid \
= util.invert_ht(T_w_cam0_now).dot(T_w_cam0_mid)
poleparams = locdata[imap]['poleparams']
npoles = poleparams.shape[0]
h = np.diff(poleparams[:, 2:4], axis=1)
polepos_m_mid = np.hstack([
poleparams[:, :2],
np.zeros([npoles, 1]),
np.ones([npoles, 1])
]).T
polepos_velo_now = T_velo_cam0.dot(T_cam0_now_cam0_mid).dot(
T_cam0_m).dot(polepos_m_mid)
poleparams = np.hstack([polepos_velo_now[:2].T, h])
filter.update_measurement(poleparams[:, :2])
T_w_velo_est[i] = filter.estimate_pose()
if visualize:
polepos_w = T_w_velo_est[i].dot(polepos_velo_now)
locpoles.set_offsets(polepos_w[:2].T)
# particleposes = np.tile(T_w_velo_gt[i], [gridsize**2, 1, 1])
# particleposes[:, :2, 3] += dxy
# visfilter.particles = particleposes
# visfilter.weights[:] = 1.0 / visfilter.count
# visfilter.update_measurement(poleparams[:, :2], resample=False)
# weightimage.set_array(np.flipud(
# visfilter.weights.reshape([gridsize, gridsize])))
# weightimage.autoscale()
imap += 1
if visualize:
particles.set_offsets(filter.particles[:, :2, 3])
arrow.set_xy(T_w_velo_est[i].dot(arrowdata)[:2].T)
x, y = T_w_velo_est[i, :2, 3]
mapaxes.set_xlim(left=x - viewoffset, right=x + viewoffset)
mapaxes.set_ylim(bottom=y - viewoffset, top=y + viewoffset)
# histaxes.cla()
# histaxes.hist(filter.weights,
# bins=50, range=(0.0, np.max(filter.weights)))
if i % 15 == 0:
filename = str(seq) + '_' + str(i) + '_'
figure.savefig(os.path.join(figuresdir, filename + '.png'))
figure.canvas.draw_idle()
figure.canvas.flush_events()
bar.update(i)
i += 1
filename = os.path.join(seqdir, locfileprefix \
+ datetime.datetime.now().strftime('_%Y-%m-%d_%H-%M-%S.npz'))
np.savez(filename, T_w_velo_est=T_w_velo_est)
mapinterval = 1.5
mapdistance = 1.5
remapdistance = 10.0
n_mapdetections = 3
n_locdetections = 2
n_localmaps = 3
poles.minscore = 0.6
poles.minheight = 1.0
poles.freelength = 0.5
poles.polesides = range(1, 11)
T_mc_cam0 = np.identity(4)
T_mc_cam0[:3, :3] \
= [[0.0, 0.0, 1.0], [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0]]
T_cam0_mc = util.invert_ht(T_mc_cam0)
T_m_mc = np.identity(4)
T_m_mc[:3, 3] = np.hstack([0.5 * mapextent[:2], 2.0])
T_mc_m = util.invert_ht(T_m_mc)
T_cam0_m = T_cam0_mc.dot(T_mc_m)
globalmapfile = 'globalmap_3.npz'
localmapfile = 'localmaps_3.npz'
locfileprefix = 'localization'
evalfile = 'evaluation.npz'
def get_map_indices(sequence):
distance = np.hstack([
0.0,
np.cumsum(
def evaluate(seq):
sequence = dataset.sequence(seq)
T_w_velo_gt = np.matmul(sequence.poses, sequence.calib.T_cam0_velo)
T_w_velo_gt = np.array([util.project_xy(ht) for ht in T_w_velo_gt])
seqdir = os.path.join('kitti', '{:03d}'.format(seq))
mapdata = np.load(os.path.join(seqdir, globalmapfile))
polemap = mapdata['polemeans']
plt.scatter(polemap[:, 0], polemap[:, 1], s=1, c='b')
plt.plot(T_w_velo_gt[:, 0, 3], T_w_velo_gt[:, 1, 3], color=(0.5, 0.5, 0.5))
cumdist = np.hstack([
0.0,
np.cumsum(
np.linalg.norm(np.diff(T_w_velo_gt[:, :2, 3], axis=0), axis=1))
])
timestamps = np.array([arrow.get(timestamp).float_timestamp \
for timestamp in sequence.timestamps])
t_eval = scipy.interpolate.interp1d(cumdist, timestamps)(np.arange(
0.0, cumdist[-1], 1.0))
n = t_eval.size
T_w_velo_gt_interp = np.empty([n, 4, 4])
iodo = 1
for ieval in range(n):
while timestamps[iodo] < t_eval[ieval]:
iodo += 1
T_w_velo_gt_interp[ieval] = util.interpolate_ht(
T_w_velo_gt[iodo - 1:iodo + 1], timestamps[iodo - 1:iodo + 1],
t_eval[ieval])
files = [file for file in os.listdir(seqdir) \
if os.path.basename(file).startswith(locfileprefix)]
poserror = np.full([n, len(files)], np.nan)
laterror = np.full([n, len(files)], np.nan)
lonerror = np.full([n, len(files)], np.nan)
angerror = np.full([n, len(files)], np.nan)
T_gt_est = np.full([n, 4, 4], np.nan)
for ifile in range(len(files)):
T_w_velo_est = np.load(os.path.join(seqdir,
files[ifile]))['T_w_velo_est']
iodo = 1
for ieval in range(n):
while timestamps[iodo] < t_eval[ieval]:
iodo += 1
T_w_velo_est_interp = util.interpolate_ht(
T_w_velo_est[iodo - 1:iodo + 1], timestamps[iodo - 1:iodo + 1],
t_eval[ieval])
T_gt_est[ieval] = util.invert_ht(
T_w_velo_gt_interp[ieval]).dot(T_w_velo_est_interp)
lonerror[:, ifile] = T_gt_est[:, 0, 3]
laterror[:, ifile] = T_gt_est[:, 1, 3]
poserror[:, ifile] = np.linalg.norm(T_gt_est[:, :2, 3], axis=1)
angerror[:, ifile] = util.ht2xyp(T_gt_est)[:, 2]
plt.plot(T_w_velo_est[:, 0, 3], T_w_velo_est[:, 1, 3], 'r')
angerror = np.degrees(angerror)
lonstd = np.std(lonerror, axis=0)
latstd = np.std(laterror, axis=0)
angstd = np.std(angerror, axis=0)
angerror = np.abs(angerror)
laterror = np.mean(np.abs(laterror), axis=0)
lonerror = np.mean(np.abs(lonerror), axis=0)
posrmse = np.sqrt(np.mean(poserror**2, axis=0))
angrmse = np.sqrt(np.mean(angerror**2, axis=0))
poserror = np.mean(poserror, axis=0)
angerror = np.mean(angerror, axis=0)
plt.savefig(os.path.join(seqdir, 'trajectory_est.svg'))
np.savez(os.path.join(seqdir, evalfile),
poserror=poserror,
angerror=angerror,
posrmse=posrmse,
angrmse=angrmse,
laterror=laterror,
latstd=latstd,
lonerror=lonerror,
lonstd=lonstd)
print('poserror: {}\nposrmse: {}\n'
'laterror: {}\nlatstd: {}\n'
'lonerror: {}\nlonstd: {}\n'
'angerror: {}\nangstd: {}\nangrmse: {}'.format(
np.mean(poserror), np.mean(posrmse), np.mean(laterror),
np.mean(latstd), np.mean(lonerror), np.mean(lonstd),
np.mean(angerror), np.mean(angstd), np.mean(angrmse)))
def localize(sessionname, visualize=False):
print(sessionname)
mapdata = np.load(
os.path.join(pynclt.resultdir,
get_globalmapname() + '.npz'))
polemap = mapdata['polemeans'][:, :2]
polevar = 1.50
session = pynclt.session(sessionname)
locdata = np.load(os.path.join(session.dir, get_localmapfile()),
allow_pickle=True)['maps']
polepos_m = []
polepos_w = []
for i in range(len(locdata)):
n = locdata[i]['poleparams'].shape[0]
pad = np.hstack([np.zeros([n, 1]), np.ones([n, 1])])
polepos_m.append(np.hstack([locdata[i]['poleparams'][:, :2], pad]).T)
polepos_w.append(locdata[i]['T_w_m'].dot(polepos_m[i]))
istart = 0
# igps = np.searchsorted(session.t_gps, session.t_relodo[istart]) + [-4, 1]
# igps = np.clip(igps, 0, session.gps.shape[0] - 1)
# T_w_r_start = pynclt.T_w_o
# T_w_r_start[:2, 3] = np.mean(session.gps[igps], axis=0)
T_w_r_start = util.project_xy(
session.get_T_w_r_gt(session.t_relodo[istart]).dot(T_r_mc)).dot(T_mc_r)
filter = particlefilter.particlefilter(500,
T_w_r_start,
2.5,
np.radians(5.0),
polemap,
polevar,
T_w_o=T_mc_r)
# Init: particlefilter(count = #particles, start: init pose, posrange: for init, angrange: for init,\
# polemeans: global map data, polevar, T_w_o=np.identity(4))
filter.estimatetype = 'best'
filter.minneff = 0.5
# filter = inEKF.inEKF(T_w_r_start, polemap, polevar, T_w_o = T_mc_r)
if visualize:
plt.ion()
figure = plt.figure()
nplots = 1
mapaxes = figure.add_subplot(nplots, 1, 1)
mapaxes.set_aspect('equal')
mapaxes.scatter(polemap[:, 0], polemap[:, 1], s=5, c='b', marker='s')
x_gt, y_gt = session.T_w_r_gt[::20, :2, 3].T
mapaxes.plot(x_gt, y_gt, 'g')
particles = mapaxes.scatter([], [], s=1, c='r')
arrow = mapaxes.arrow(0.0,
0.0,
1.0,
0.0,
length_includes_head=True,
head_width=0.7,
head_length=1.0,
color='k')
arrowdata = np.hstack(
[arrow.get_xy(), np.zeros([8, 1]),
np.ones([8, 1])]).T
locpoles = mapaxes.scatter([], [], s=30, c='k', marker='x')
viewoffset = 25.0
# weightaxes = figure.add_subplot(nplots, 1, 2)
# gridsize = 50
# offset = 5.0
# visfilter = particlefilter.particlefilter(gridsize**2,
# np.identity(4), 0.0, 0.0, polemap,
# polevar, T_w_o=pynclt.T_w_o)
# gridcoord = np.linspace(-offset, offset, gridsize)
# x, y = np.meshgrid(gridcoord, gridcoord)
# dxy = np.hstack([x.reshape([-1, 1]), y.reshape([-1, 1])])
# weightimage = weightaxes.matshow(np.zeros([gridsize, gridsize]),
# extent=(-offset, offset, -offset, offset))
# histaxes = figure.add_subplot(nplots, 1, 3)
imap = 0
while imap < locdata.shape[0] - 1 and session.t_velo[
locdata[imap]['iend']] < session.t_relodo[istart]:
imap += 1
T_w_r_est = np.full([session.t_relodo.size, 4, 4], np.nan)
#steps = 5000
x_sigma_contour = np.zeros(session.t_relodo.size)
y_sigma_contour = np.zeros(session.t_relodo.size)
p_sigma_contour = np.zeros(session.t_relodo.size)
x_err = np.zeros(session.t_relodo.size)
y_err = np.zeros(session.t_relodo.size)
p_err = np.zeros(session.t_relodo.size)
with progressbar.ProgressBar(max_value=session.t_relodo.size) as bar:
for i in range(istart, session.t_relodo.size):
#for i in range(istart, istart + steps):
relodocov = np.empty([3, 3])
relodocov[:2, :2] = session.relodocov[i, :2, :2]
relodocov[:, 2] = session.relodocov[i, [0, 1, 5], 5]
relodocov[2, :] = session.relodocov[i, 5, [0, 1, 5]]
filter.update_motion(
session.relodo[i], relodocov
) ### relodocov #propagate: session.relodo[i]=[x,y,p] in R^3
T_w_r_est[i] = filter.estimate_pose() ## estimate pose
t_now = session.t_relodo[i]
if imap < locdata.shape[0]:
t_end = session.t_velo[locdata[imap]['iend']]
if t_now >= t_end:
imaps = range(imap, np.clip(imap - n_localmaps, -1, None),
-1)
xy = np.hstack([polepos_w[j][:2] for j in imaps]).T
a = np.vstack([ld['poleparams'][:, [4]] \
for ld in locdata[imaps]])
boxes = np.hstack([xy - 0.5 * a, xy + 0.5 * a])
ipoles = set(range(polepos_w[imap].shape[1]))
iactive = set()
for ci in cluster.cluster_boxes(boxes):
if len(ci) >= n_locdetections:
iactive |= set(ipoles) & ci
iactive = list(iactive)
# print('{}.'.format(
# len(iactive) - polepos_w[imap].shape[1]))
if iactive:
t_mid = session.t_velo[locdata[imap]['imid']]
T_w_r_mid = util.project_xy(
session.get_T_w_r_odo(t_mid).dot(T_r_mc)).dot(
T_mc_r)
T_w_r_now = util.project_xy(
session.get_T_w_r_odo(t_now).dot(T_r_mc)).dot(
T_mc_r)
T_r_now_r_mid = util.invert_ht(T_w_r_now).dot(
T_w_r_mid)
polepos_r_now = T_r_now_r_mid.dot(T_r_m).dot(
polepos_m[imap]
[:, iactive]) # online poles(landmarks): lumbda
filter.update_measurement(
polepos_r_now[:2].T) ### measurement update
T_w_r_est[i] = filter.estimate_pose() ### estimate
if visualize:
polepos_w_est = T_w_r_est[i].dot(polepos_r_now)
locpoles.set_offsets(polepos_w_est[:2].T)
# T_w_r_gt_now = session.get_T_w_r_gt(t_now)
# T_w_r_gt_now = np.tile(
# T_w_r_gt_now, [gridsize**2, 1, 1])
# T_w_r_gt_now[:, :2, 3] += dxy
# visfilter.particles = T_w_r_gt_now
# visfilter.weights[:] = 1.0 / visfilter.count
# visfilter.update_measurement(
# polepos_r_now[:2].T, resample=False)
# weightimage.set_array(np.flipud(
# visfilter.weights.reshape(
# [gridsize, gridsize])))
# weightimage.autoscale()
imap += 1
# estimattion error
T_w_r_gt_i = util.project_xy(
session.get_T_w_r_gt(session.t_relodo[i]).dot(T_r_mc)).dot(
T_mc_r) # ground truth pose
x_err[i] = T_w_r_est[i, 0, 3] - T_w_r_gt_i[0, 3]
y_err[i] = T_w_r_est[i, 1, 3] - T_w_r_gt_i[1, 3]
p_err[i] = np.arctan2(T_w_r_est[i, 1, 0],
T_w_r_est[i, 0, 0]) - np.arctan2(
T_w_r_gt_i[1, 0], T_w_r_gt_i[0, 0])
p_err[i] = np.arctan2(np.sin(p_err[i]),
np.cos(p_err[i])) # wrap to [-pi, pi]
# 3-sigma contour
if isinstance(filter, inEKF.inEKF):
x_sigma_contour[i] = 3 * np.sqrt(filter.Sigma[1, 1])
y_sigma_contour[i] = 3 * np.sqrt(filter.Sigma[2, 2])
p_sigma_contour[i] = 3 * np.sqrt(filter.Sigma[0, 0])
elif isinstance(filter, particlefilter.particlefilter):
x_sigma_contour[i] = 3 * np.sqrt(filter.Sigma[0, 0])
y_sigma_contour[i] = 3 * np.sqrt(filter.Sigma[1, 1])
p_sigma_contour[i] = 3 * np.sqrt(filter.Sigma[2, 2])
if visualize:
## particles.set_offsets(filter.particles[:, :2, 3])
arrow.set_xy(T_w_r_est[i].dot(arrowdata)[:2].T)
x, y = T_w_r_est[i, :2, 3]
mapaxes.set_xlim(left=x - viewoffset, right=x + viewoffset)
mapaxes.set_ylim(bottom=y - viewoffset, top=y + viewoffset)
# histaxes.cla()
# histaxes.hist(filter.weights,
# bins=50, range=(0.0, np.max(filter.weights)))
figure.canvas.draw_idle()
figure.canvas.flush_events()
bar.update(i)
# Plot the NEES (normalized estimation error squared) graph
fig = plt.figure()
ax1 = plt.subplot(311)
plt.plot(x_err, 'r', label="Deviance from Ground Truth")
plt.plot(x_sigma_contour, 'b', label='3-Sigma Contour')
plt.plot(-x_sigma_contour, 'b')
plt.ylabel('x error')
plt.xlabel('step')
# ax.legend(loc='upper right', bbox_to_anchor=(1,0))
ax2 = plt.subplot(312)
plt.plot(y_err, 'r')
plt.plot(y_sigma_contour, 'b')
plt.plot(-y_sigma_contour, 'b')
plt.ylabel('y error')
plt.xlabel('step')
ax3 = plt.subplot(313)
plt.plot(p_err, 'r')
plt.plot(p_sigma_contour, 'b')
plt.plot(-p_sigma_contour, 'b')
plt.ylabel('theta error')
plt.xlabel('step')
handles, labels = ax1.get_legend_handles_labels()
fig.legend(handles, labels, 'upper right')
plt.savefig(os.path.join(pynclt.resultdir, sessionname + '_NEES.png'))
#plt.savefig("NEES.png")
filename = os.path.join(session.dir, get_locfileprefix() \
+ datetime.datetime.now().strftime('_%Y-%m-%d_%H-%M-%S.npz'))
np.savez(filename, T_w_r_est=T_w_r_est)
def localize(sessionname, visualize=False):
print(sessionname)
mapdata = np.load(os.path.join(pynclt.resultdir, get_globalmapname() + '.npz'))
polemap = mapdata['polemeans'][:, :2]
polevar = 1.50
session = pynclt.session(sessionname)
locdata = np.load(os.path.join(session.dir, get_localmapfile()), allow_pickle=True)['maps']
polepos_m = []
polepos_w = []
for i in range(len(locdata)):
n = locdata[i]['poleparams'].shape[0]
pad = np.hstack([np.zeros([n, 1]), np.ones([n, 1])])
polepos_m.append(np.hstack([locdata[i]['poleparams'][:, :2], pad]).T)
polepos_w.append(locdata[i]['T_w_m'].dot(polepos_m[i]))
istart = 0
# igps = np.searchsorted(session.t_gps, session.t_relodo[istart]) + [-4, 1]
# igps = np.clip(igps, 0, session.gps.shape[0] - 1)
# T_w_r_start = pynclt.T_w_o
# T_w_r_start[:2, 3] = np.mean(session.gps[igps], axis=0)
T_w_r_start = util.project_xy(
session.get_T_w_r_gt(session.t_relodo[istart]).dot(T_r_mc)).dot(T_mc_r)
filter = particlefilter.particlefilter(5000,
T_w_r_start, 2.5, np.radians(5.0), polemap, polevar, T_w_o=T_mc_r)
filter.estimatetype = 'best'
filter.minneff = 0.5
if visualize:
plt.ion()
figure = plt.figure()
nplots = 1
mapaxes = figure.add_subplot(nplots, 1, 1)
mapaxes.set_aspect('equal')
mapaxes.scatter(polemap[:, 0], polemap[:, 1], s=5, c='b', marker='s')
x_gt, y_gt = session.T_w_r_gt[::20, :2, 3].T
mapaxes.plot(x_gt, y_gt, 'g')
particles = mapaxes.scatter([], [], s=1, c='r')
arrow = mapaxes.arrow(0.0, 0.0, 1.0, 0.0, length_includes_head=True,
head_width=0.7, head_length=1.0, color='k')
arrowdata = np.hstack(
[arrow.get_xy(), np.zeros([8, 1]), np.ones([8, 1])]).T
locpoles = mapaxes.scatter([], [], s=30, c='k', marker='x')
viewoffset = 25.0
# weightaxes = figure.add_subplot(nplots, 1, 2)
# gridsize = 50
# offset = 5.0
# visfilter = particlefilter.particlefilter(gridsize**2,
# np.identity(4), 0.0, 0.0, polemap,
# polevar, T_w_o=pynclt.T_w_o)
# gridcoord = np.linspace(-offset, offset, gridsize)
# x, y = np.meshgrid(gridcoord, gridcoord)
# dxy = np.hstack([x.reshape([-1, 1]), y.reshape([-1, 1])])
# weightimage = weightaxes.matshow(np.zeros([gridsize, gridsize]),
# extent=(-offset, offset, -offset, offset))
# histaxes = figure.add_subplot(nplots, 1, 3)
imap = 0
while imap < locdata.shape[0] - 1 and \
session.t_velo[locdata[imap]['iend']] < session.t_relodo[istart]:
imap += 1
T_w_r_est = np.full([session.t_relodo.size, 4, 4], np.nan)
with progressbar.ProgressBar(max_value=session.t_relodo.size) as bar:
for i in range(istart, session.t_relodo.size):
relodocov = np.empty([3, 3])
relodocov[:2, :2] = session.relodocov[i, :2, :2]
relodocov[:, 2] = session.relodocov[i, [0, 1, 5], 5]
relodocov[2, :] = session.relodocov[i, 5, [0, 1, 5]]
filter.update_motion(session.relodo[i], relodocov * 2.0**2)
T_w_r_est[i] = filter.estimate_pose()
t_now = session.t_relodo[i]
if imap < locdata.shape[0]:
t_end = session.t_velo[locdata[imap]['iend']]
if t_now >= t_end:
imaps = range(imap, np.clip(imap-n_localmaps, -1, None), -1)
xy = np.hstack([polepos_w[j][:2] for j in imaps]).T
a = np.vstack([ld['poleparams'][:, [4]] \
for ld in locdata[imaps]])
boxes = np.hstack([xy - 0.5 * a, xy + 0.5 * a])
ipoles = set(range(polepos_w[imap].shape[1]))
iactive = set()
for ci in cluster.cluster_boxes(boxes):
if len(ci) >= n_locdetections:
iactive |= set(ipoles) & ci
iactive = list(iactive)
# print('{}.'.format(
# len(iactive) - polepos_w[imap].shape[1]))
if iactive:
t_mid = session.t_velo[locdata[imap]['imid']]
T_w_r_mid = util.project_xy(session.get_T_w_r_odo(
t_mid).dot(T_r_mc)).dot(T_mc_r)
T_w_r_now = util.project_xy(session.get_T_w_r_odo(
t_now).dot(T_r_mc)).dot(T_mc_r)
T_r_now_r_mid = util.invert_ht(T_w_r_now).dot(T_w_r_mid)
polepos_r_now = T_r_now_r_mid.dot(T_r_m).dot(
polepos_m[imap][:, iactive])
filter.update_measurement(polepos_r_now[:2].T)
T_w_r_est[i] = filter.estimate_pose()
if visualize:
polepos_w_est = T_w_r_est[i].dot(polepos_r_now)
locpoles.set_offsets(polepos_w_est[:2].T)
# T_w_r_gt_now = session.get_T_w_r_gt(t_now)
# T_w_r_gt_now = np.tile(
# T_w_r_gt_now, [gridsize**2, 1, 1])
# T_w_r_gt_now[:, :2, 3] += dxy
# visfilter.particles = T_w_r_gt_now
# visfilter.weights[:] = 1.0 / visfilter.count
# visfilter.update_measurement(
# polepos_r_now[:2].T, resample=False)
# weightimage.set_array(np.flipud(
# visfilter.weights.reshape(
# [gridsize, gridsize])))
# weightimage.autoscale()
imap += 1
if visualize:
particles.set_offsets(filter.particles[:, :2, 3])
arrow.set_xy(T_w_r_est[i].dot(arrowdata)[:2].T)
x, y = T_w_r_est[i, :2, 3]
mapaxes.set_xlim(left=x - viewoffset, right=x + viewoffset)
mapaxes.set_ylim(bottom=y - viewoffset, top=y + viewoffset)
# histaxes.cla()
# histaxes.hist(filter.weights,
# bins=50, range=(0.0, np.max(filter.weights)))
figure.canvas.draw_idle()
figure.canvas.flush_events()
bar.update(i)
filename = os.path.join(session.dir, get_locfileprefix() \
+ datetime.datetime.now().strftime('_%Y-%m-%d_%H-%M-%S.npz'))
np.savez(filename, T_w_r_est=T_w_r_est)
mapsize = np.full(3, 0.2)
mapshape = np.array(mapextent / mapsize, dtype=np.int64)
mapinterval = 1.5
mapdistance = 1.5
remapdistance = 10.0
n_mapdetections = 3
n_locdetections = 2
n_localmaps = 3
poles.minscore = 0.6
poles.minheight = 1.0
poles.freelength = 0.5
poles.polesides = range(1, 7 + 1)
T_mc_r = pynclt.T_w_o
T_r_mc = util.invert_ht(T_mc_r)
T_m_mc = np.identity(4)
T_m_mc[:3, 3] = np.hstack([0.5 * mapextent[:2], 0.5])
T_mc_m = util.invert_ht(T_m_mc)
T_m_r = T_m_mc.dot(T_mc_r)
T_r_m = util.invert_ht(T_m_r)
def get_globalmapname():
return 'globalmap_{:.0f}_{:.0f}_{:.0f}'.format(n_mapdetections,
10 * poles.minscore,
poles.polesides[-1])
def get_locfileprefix():
return 'localization_{:.0f}_{:.0f}_{:.0f}'.format(n_mapdetections,
T_w_velo_est = np.full(T_w_velo_gt.shape, np.nan)
"""
Gets the weighted average of the particles poses (in world coordinates)
This is the likeliest position of the car at the current time
"""
T_w_velo_est[i] = filter.estimate_pose() # i = 0
i += 1
# with progressbar.ProgressBar(max_value=T_w_velo_est.shape[0] - i) as bar:
while i < T_w_velo_est.shape[0]:
"""
Change from last to current pose in terms of x, y and phi??
"""
relodo = util.ht2xyp(
util.invert_ht(T_w_velo_gt[i-1]).dot(T_w_velo_gt[i]))
relodocov = np.diag((0.02 * relodo)**2)
"""
Creates random new xyp mean, drawn from a multivariate normal
distribution.
We then update all the particles we have by similarly drawing
new xyp changes using the new mean and the relodocov.
This basically means that we change our particles poses
according to the motion of the vehicle + some randomness.
Then we check again what our likeliest pose is.
"""
relodo = np.random.multivariate_normal(relodo, relodocov)
filter.update_motion(relodo, relodocov)
