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 get_T_w_r_odo(self, t):
i = np.clip(np.searchsorted(self.t_odo, t), 1, self.t_odo.size - 1) \
+ np.array([-1, 0])
return util.interpolate_ht(self.T_w_r_odo[i], self.t_odo[i], t)
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 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)))
