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