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)