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)
