def view_local_maps(sessionname): sessiondir = os.path.join(pynclt.resultdir, sessionname) session = pynclt.session(sessionname) maps = np.load(os.path.join(sessiondir, get_localmapfile()))['maps'] for i, map in enumerate(maps): print('Map #{}'.format(i)) mapboundsvis = util.create_wire_box(mapextent, [0.0, 0.0, 1.0]) mapboundsvis.transform(map['T_w_m']) polevis = [] for poleparams in map['poleparams']: x, y, zs, ze, a = poleparams[: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(map['T_w_m'].dot(T_m_p)) polevis.append(pole) accucloud = o3.geometry.PointCloud() for j in range(map['istart'], map['iend']): points, intensities = session.get_velo(j) cloud = o3.geometry.PointCloud() cloud.points = o3.utility.Vector3dVector(points) cloud.colors = o3.utility.Vector3dVector( util.intensity2color(intensities / 255.0)) cloud.transform(session.T_w_r_odo_velo[j]) accucloud.points.extend(cloud.points) accucloud.colors.extend(cloud.colors) o3.visualization.draw_geometries([accucloud, mapboundsvis] + polevis)
def view_local_maps(seq): sequence = dataset.sequence(seq) seqdir = os.path.join(result_dir, '{:03d}'.format(seq)) with np.load(os.path.join(seqdir, localmapfile), allow_pickle=True) as data: for i, map in enumerate(data['maps']): T_w_m = sequence.poses[map['imid']].dot(T_cam0_mc).dot(T_mc_m) mapboundsvis = util.create_wire_box(mapextent, [0.0, 0.0, 1.0]) mapboundsvis.transform(T_w_m) polemap = [] for poleparams in map['poleparams']: x, y, zs, ze, a, _ = poleparams 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)) polemap.append(pole) accucloud = o3.PointCloud() for j in range(map['istart'], map['iend']): velo = sequence.get_velo(j) cloud = o3.PointCloud() cloud.points = o3.Vector3dVector(velo[:, :3]) cloud.colors = o3.Vector3dVector( util.intensity2color(velo[:, 3])) cloud.transform(sequence.poses[j].dot( sequence.calib.T_cam0_velo)) accucloud.points.extend(cloud.points) accucloud.colors.extend(cloud.colors) o3.draw_geometries([accucloud, mapboundsvis] + polemap)
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)