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 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 load_snapshot(sessionname):
cloud = o3.PointCloud()
trajectory = o3.LineSet()
with np.load(os.path.join(resultdir, sessionname, snapshotfile)) as data:
cloud.points = o3.Vector3dVector(data['points'])
cloud.colors = o3.Vector3dVector(
util.intensity2color(data['intensities'] / 255.0))
trajectory.points = o3.Vector3dVector(data['trajectory'])
lines = np.reshape(range(data['trajectory'].shape[0] - 1), [-1, 1]) \
+ [0, 1]
trajectory.lines = o3.Vector2iVector(lines)
trajectory.colors = o3.Vector3dVector(
np.tile([0.0, 0.5, 0.0], [lines.shape[0], 1]))
return cloud, trajectory
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)