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)))