mirror of
https://github.com/Mobile-Robotics-W20-Team-9/polex.git
synced 2025-08-28 17:52:45 +00:00
Add code.
This commit is contained in:
Alexander Schaefer
1
poles/__init__.py
Normal file
1
poles/__init__.py
Normal file
@@ -0,0 +1 @@
|
||||
from poles import *
|
||||
68
poles/cluster.py
Normal file
68
poles/cluster.py
Normal file
@@ -0,0 +1,68 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import networkx as nx
|
||||
import numpy as np
|
||||
|
||||
|
||||
def cluster_boxes(boxes):
|
||||
graph = nx.Graph()
|
||||
for i in range(boxes.shape[0]):
|
||||
ioverlap = np.where(np.logical_and(
|
||||
np.all(boxes[i, :2] <= boxes[i+1:, 2:], axis=1),
|
||||
np.all(boxes[i, 2:] >= boxes[i+1:, :2], axis=1)))[0] + i + 1
|
||||
if ioverlap.size > 0:
|
||||
ebunch = np.stack(
|
||||
[i * np.ones(ioverlap.size, dtype=np.int), ioverlap]).T
|
||||
graph.add_edges_from(ebunch)
|
||||
else:
|
||||
graph.add_node(i)
|
||||
|
||||
return nx.connected_components(graph)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
boxes = np.array([[0, 0, 5, 6],
|
||||
[1, 1, 3, 3],
|
||||
[2, 2, 4, 4],
|
||||
[4, 5, 6, 7],
|
||||
[8, 1, 10, 4],
|
||||
[9, 3, 11, 5],
|
||||
[11, 2, 13, 4]])
|
||||
clusters = list(cluster_boxes(boxes))
|
||||
print(clusters)
|
||||
|
||||
boxes = np.array([[14.79661574, 21.21601612, 15.30176893, 21.72116931],
|
||||
[18.48462587, 7.41137572, 19.03767023, 7.96442009],
|
||||
[2.69673224, 9.59842375, 3.32057219, 10.22226371]])
|
||||
clusters = list(cluster_boxes(boxes))
|
||||
print(clusters)
|
||||
|
||||
boxes = np.array([[6.81938108, 18.65041178, 7.52721282, 19.35824351],
|
||||
[10.47957221, 4.74881386, 10.97182461, 5.24106625],
|
||||
[13.85465225, 10.67964592, 14.85465225, 11.67964592],
|
||||
[ 7.64280886, 18.18693742, 8.34956098, 18.89368953],
|
||||
[ 8.59207879, 18.34769795, 9.21301121, 18.96863037],
|
||||
[12.26176147, 4.42352969, 12.86341253, 5.02518075],
|
||||
[ 9.15 , 18.63943225, 9.75556467, 19.24499692],
|
||||
[14.10533103, 5.69170303, 14.68470125, 6.27107324],
|
||||
[ 9.97261293, 19.30506776, 10.55050568, 19.88296051],
|
||||
[10.80585378, 19.7812637 , 11.28636705, 20.26177698],
|
||||
[24.60318075, 11.85 , 25.01565066, 12.26246991],
|
||||
[14.78231664, 6.11960893, 15.29557712, 6.63286942],
|
||||
[11.95292709, 20.39982668, 12.55003767, 20.99693726],
|
||||
[17.46145317, 7.17423254, 17.89978823, 7.6125676 ],
|
||||
[14.79661574, 21.21601612, 15.30176893, 21.72116931],
|
||||
[18.48462587, 7.41137572, 19.03767023, 7.96442009],
|
||||
[ 2.69673224, 9.59842375, 3.32057219, 10.22226371]])
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import matplotlib.patches as pat
|
||||
fig = plt.figure()
|
||||
ax = fig.add_subplot(111, aspect='equal')
|
||||
for b in boxes:
|
||||
ax.add_patch(pat.Rectangle(b[:2], b[2]-b[0], b[3]-b[1]))
|
||||
plt.xlim([0, 30])
|
||||
plt.ylim([0, 30])
|
||||
plt.show()
|
||||
clusters = list(cluster_boxes(boxes))
|
||||
print(clusters)
|
||||
21
poles/cuboid.py
Normal file
21
poles/cuboid.py
Normal file
@@ -0,0 +1,21 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
import numpy as np
|
||||
|
||||
def cuboid(ax, lower, upper):
|
||||
xl, yl, zl = lower
|
||||
xu, yu, zu = upper
|
||||
faces = np.array([
|
||||
[[[xl,xu],[xl,xu]], [[yl,yl],[yl,yl]], [[zl,zl],[zu,zu]]],
|
||||
[[[xl,xu],[xl,xu]], [[yu,yu],[yu,yu]], [[zl,zl],[zu,zu]]],
|
||||
[[[xl,xl],[xl,xl]], [[yl,yu],[yl,yu]], [[zl,zl],[zu,zu]]],
|
||||
[[[xu,xu],[xu,xu]], [[yl,yu],[yl,yu]], [[zl,zl],[zu,zu]]],
|
||||
[[[xl,xu],[xl,xu]], [[yl,yl],[yu,yu]], [[zl,zl],[zl,zl]]],
|
||||
[[[xl,xu],[xl,xu]], [[yl,yl],[yu,yu]], [[zu,zu],[zu,zu]]]
|
||||
])
|
||||
|
||||
for face in faces:
|
||||
ax.plot_surface(face[0], face[1], face[2], alpha=0.5, color='b')
|
||||
ax.plot_wireframe(face[0], face[1], face[2], color='k')
|
||||
160
poles/kittidrives.py
Normal file
160
poles/kittidrives.py
Normal file
@@ -0,0 +1,160 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
drives = [
|
||||
{'date': '2011_09_26', 'drive': '0002'},
|
||||
{'date': '2011_09_26', 'drive': '0005'},
|
||||
{'date': '2011_09_26', 'drive': '0009'},
|
||||
{'date': '2011_09_26', 'drive': '0011'},
|
||||
{'date': '2011_09_26', 'drive': '0001'},
|
||||
{'date': '2011_09_26', 'drive': '0013'},
|
||||
{'date': '2011_09_26', 'drive': '0014'},
|
||||
{'date': '2011_09_26', 'drive': '0015'},
|
||||
{'date': '2011_09_26', 'drive': '0017'},
|
||||
{'date': '2011_09_26', 'drive': '0018'},
|
||||
{'date': '2011_09_26', 'drive': '0019'},
|
||||
{'date': '2011_09_26', 'drive': '0020'},
|
||||
{'date': '2011_09_26', 'drive': '0022'},
|
||||
{'date': '2011_09_26', 'drive': '0023'},
|
||||
{'date': '2011_09_26', 'drive': '0027'},
|
||||
{'date': '2011_09_26', 'drive': '0028'},
|
||||
{'date': '2011_09_26', 'drive': '0029'},
|
||||
{'date': '2011_09_26', 'drive': '0032'},
|
||||
{'date': '2011_09_26', 'drive': '0035'},
|
||||
{'date': '2011_09_26', 'drive': '0036'},
|
||||
{'date': '2011_09_26', 'drive': '0039'},
|
||||
{'date': '2011_09_26', 'drive': '0046'},
|
||||
{'date': '2011_09_26', 'drive': '0048'},
|
||||
{'date': '2011_09_26', 'drive': '0051'},
|
||||
{'date': '2011_09_26', 'drive': '0052'},
|
||||
{'date': '2011_09_26', 'drive': '0056'},
|
||||
{'date': '2011_09_26', 'drive': '0057'},
|
||||
{'date': '2011_09_26', 'drive': '0059'},
|
||||
{'date': '2011_09_26', 'drive': '0060'},
|
||||
{'date': '2011_09_26', 'drive': '0061'},
|
||||
{'date': '2011_09_26', 'drive': '0064'},
|
||||
{'date': '2011_09_26', 'drive': '0070'},
|
||||
{'date': '2011_09_26', 'drive': '0079'},
|
||||
{'date': '2011_09_26', 'drive': '0084'},
|
||||
{'date': '2011_09_26', 'drive': '0086'},
|
||||
{'date': '2011_09_26', 'drive': '0087'},
|
||||
{'date': '2011_09_26', 'drive': '0091'},
|
||||
{'date': '2011_09_26', 'drive': '0093'},
|
||||
{'date': '2011_09_26', 'drive': '0095'},
|
||||
{'date': '2011_09_26', 'drive': '0096'},
|
||||
{'date': '2011_09_26', 'drive': '0101'},
|
||||
{'date': '2011_09_26', 'drive': '0104'},
|
||||
{'date': '2011_09_26', 'drive': '0106'},
|
||||
{'date': '2011_09_26', 'drive': '0113'},
|
||||
{'date': '2011_09_26', 'drive': '0117'},
|
||||
{'date': '2011_09_26', 'drive': '0119'},
|
||||
{'date': '2011_09_28', 'drive': '0001'},
|
||||
{'date': '2011_09_28', 'drive': '0002'},
|
||||
{'date': '2011_09_28', 'drive': '0016'},
|
||||
{'date': '2011_09_28', 'drive': '0021'},
|
||||
{'date': '2011_09_28', 'drive': '0034'},
|
||||
{'date': '2011_09_28', 'drive': '0035'},
|
||||
{'date': '2011_09_28', 'drive': '0037'},
|
||||
{'date': '2011_09_28', 'drive': '0038'},
|
||||
{'date': '2011_09_28', 'drive': '0039'},
|
||||
{'date': '2011_09_28', 'drive': '0043'},
|
||||
{'date': '2011_09_28', 'drive': '0045'},
|
||||
{'date': '2011_09_28', 'drive': '0047'},
|
||||
{'date': '2011_09_28', 'drive': '0053'},
|
||||
{'date': '2011_09_28', 'drive': '0054'},
|
||||
{'date': '2011_09_28', 'drive': '0057'},
|
||||
{'date': '2011_09_28', 'drive': '0065'},
|
||||
{'date': '2011_09_28', 'drive': '0066'},
|
||||
{'date': '2011_09_28', 'drive': '0068'},
|
||||
{'date': '2011_09_28', 'drive': '0070'},
|
||||
{'date': '2011_09_28', 'drive': '0071'},
|
||||
{'date': '2011_09_28', 'drive': '0075'},
|
||||
{'date': '2011_09_28', 'drive': '0077'},
|
||||
{'date': '2011_09_28', 'drive': '0078'},
|
||||
{'date': '2011_09_28', 'drive': '0080'},
|
||||
{'date': '2011_09_28', 'drive': '0082'},
|
||||
{'date': '2011_09_28', 'drive': '0086'},
|
||||
{'date': '2011_09_28', 'drive': '0087'},
|
||||
{'date': '2011_09_28', 'drive': '0089'},
|
||||
{'date': '2011_09_28', 'drive': '0090'},
|
||||
{'date': '2011_09_28', 'drive': '0094'},
|
||||
{'date': '2011_09_28', 'drive': '0095'},
|
||||
{'date': '2011_09_28', 'drive': '0096'},
|
||||
{'date': '2011_09_28', 'drive': '0098'},
|
||||
{'date': '2011_09_28', 'drive': '0100'},
|
||||
{'date': '2011_09_28', 'drive': '0102'},
|
||||
{'date': '2011_09_28', 'drive': '0103'},
|
||||
{'date': '2011_09_28', 'drive': '0104'},
|
||||
{'date': '2011_09_28', 'drive': '0106'},
|
||||
{'date': '2011_09_28', 'drive': '0108'},
|
||||
{'date': '2011_09_28', 'drive': '0110'},
|
||||
{'date': '2011_09_28', 'drive': '0113'},
|
||||
{'date': '2011_09_28', 'drive': '0117'},
|
||||
{'date': '2011_09_28', 'drive': '0119'},
|
||||
{'date': '2011_09_28', 'drive': '0121'},
|
||||
{'date': '2011_09_28', 'drive': '0122'},
|
||||
{'date': '2011_09_28', 'drive': '0125'},
|
||||
{'date': '2011_09_28', 'drive': '0126'},
|
||||
{'date': '2011_09_28', 'drive': '0128'},
|
||||
{'date': '2011_09_28', 'drive': '0132'},
|
||||
{'date': '2011_09_28', 'drive': '0134'},
|
||||
{'date': '2011_09_28', 'drive': '0135'},
|
||||
{'date': '2011_09_28', 'drive': '0136'},
|
||||
{'date': '2011_09_28', 'drive': '0138'},
|
||||
{'date': '2011_09_28', 'drive': '0141'},
|
||||
{'date': '2011_09_28', 'drive': '0143'},
|
||||
{'date': '2011_09_28', 'drive': '0145'},
|
||||
{'date': '2011_09_28', 'drive': '0146'},
|
||||
{'date': '2011_09_28', 'drive': '0149'},
|
||||
{'date': '2011_09_28', 'drive': '0153'},
|
||||
{'date': '2011_09_28', 'drive': '0154'},
|
||||
{'date': '2011_09_28', 'drive': '0155'},
|
||||
{'date': '2011_09_28', 'drive': '0156'},
|
||||
{'date': '2011_09_28', 'drive': '0160'},
|
||||
{'date': '2011_09_28', 'drive': '0161'},
|
||||
{'date': '2011_09_28', 'drive': '0162'},
|
||||
{'date': '2011_09_28', 'drive': '0165'},
|
||||
{'date': '2011_09_28', 'drive': '0166'},
|
||||
{'date': '2011_09_28', 'drive': '0167'},
|
||||
{'date': '2011_09_28', 'drive': '0168'},
|
||||
{'date': '2011_09_28', 'drive': '0171'},
|
||||
{'date': '2011_09_28', 'drive': '0174'},
|
||||
{'date': '2011_09_28', 'drive': '0177'},
|
||||
{'date': '2011_09_28', 'drive': '0179'},
|
||||
{'date': '2011_09_28', 'drive': '0183'},
|
||||
{'date': '2011_09_28', 'drive': '0184'},
|
||||
{'date': '2011_09_28', 'drive': '0185'},
|
||||
{'date': '2011_09_28', 'drive': '0186'},
|
||||
{'date': '2011_09_28', 'drive': '0187'},
|
||||
{'date': '2011_09_28', 'drive': '0191'},
|
||||
{'date': '2011_09_28', 'drive': '0192'},
|
||||
{'date': '2011_09_28', 'drive': '0195'},
|
||||
{'date': '2011_09_28', 'drive': '0198'},
|
||||
{'date': '2011_09_28', 'drive': '0199'},
|
||||
{'date': '2011_09_28', 'drive': '0201'},
|
||||
{'date': '2011_09_28', 'drive': '0204'},
|
||||
{'date': '2011_09_28', 'drive': '0205'},
|
||||
{'date': '2011_09_28', 'drive': '0208'},
|
||||
{'date': '2011_09_28', 'drive': '0209'},
|
||||
{'date': '2011_09_28', 'drive': '0214'},
|
||||
{'date': '2011_09_28', 'drive': '0216'},
|
||||
{'date': '2011_09_28', 'drive': '0220'},
|
||||
{'date': '2011_09_28', 'drive': '0222'},
|
||||
{'date': '2011_09_28', 'drive': '0225'},
|
||||
{'date': '2011_09_29', 'drive': '0004'},
|
||||
{'date': '2011_09_29', 'drive': '0026'},
|
||||
{'date': '2011_09_29', 'drive': '0071'},
|
||||
{'date': '2011_09_29', 'drive': '0108'},
|
||||
{'date': '2011_09_30', 'drive': '0016'},
|
||||
{'date': '2011_09_30', 'drive': '0018'},
|
||||
{'date': '2011_09_30', 'drive': '0020'},
|
||||
{'date': '2011_09_30', 'drive': '0027'},
|
||||
{'date': '2011_09_30', 'drive': '0028'},
|
||||
{'date': '2011_09_30', 'drive': '0033'},
|
||||
{'date': '2011_09_30', 'drive': '0034'},
|
||||
{'date': '2011_09_30', 'drive': '0072'},
|
||||
{'date': '2011_10_03', 'drive': '0027'},
|
||||
{'date': '2011_10_03', 'drive': '0034'},
|
||||
{'date': '2011_10_03', 'drive': '0042'},
|
||||
{'date': '2011_10_03', 'drive': '0047'},
|
||||
{'date': '2011_10_03', 'drive': '0058'}
|
||||
]
|
||||
372
poles/kittipoles.py
Normal file
372
poles/kittipoles.py
Normal file
@@ -0,0 +1,372 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import datetime
|
||||
import os
|
||||
|
||||
import arrow
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import open3d as o3
|
||||
import progressbar
|
||||
import scipy.interpolate
|
||||
|
||||
import cluster
|
||||
import kittiwrapper
|
||||
import mapping
|
||||
import particlefilter
|
||||
import poles
|
||||
import util
|
||||
|
||||
|
||||
dataset = kittiwrapper.kittiwrapper('/mnt/data/datasets/kitti')
|
||||
|
||||
mapextent = np.array([30.0, 30.0, 3.0])
|
||||
mapsize = np.full(3, 0.1)
|
||||
mapshape = np.array(mapextent / mapsize, dtype=np.int)
|
||||
mapinterval = 3.0
|
||||
mapdistance = 5.0
|
||||
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(np.linalg.norm(
|
||||
np.diff(sequence.poses[:, :3, 3], axis=0), axis=1))])
|
||||
istart = []
|
||||
imid = []
|
||||
iend = []
|
||||
i = 0
|
||||
j = 0
|
||||
k = 0
|
||||
for id, d in enumerate(distance):
|
||||
if d >= i * mapinterval:
|
||||
istart.append(id)
|
||||
i += 1
|
||||
if d >= j * mapinterval + 0.5 * mapdistance:
|
||||
imid.append(id)
|
||||
j += 1
|
||||
if d > k * mapinterval + mapdistance:
|
||||
iend.append(id)
|
||||
k += 1
|
||||
return istart, imid, iend
|
||||
|
||||
|
||||
def save_local_maps(seq):
|
||||
print(seq)
|
||||
sequence = dataset.sequence(seq)
|
||||
seqdir = os.path.join('kitti', '{: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.PointCloud()
|
||||
scan.points = o3.Vector3dVector(velo[:, :3])
|
||||
scan.colors = o3.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 view_local_maps(seq):
|
||||
sequence = dataset.sequence(seq)
|
||||
seqdir = os.path.join('kitti', '{:03d}'.format(seq))
|
||||
with np.load(os.path.join(seqdir, localmapfile)) 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_global_map(seq):
|
||||
sequence = dataset.sequence(seq)
|
||||
seqdir = os.path.join('kitti', '{:03d}'.format(seq))
|
||||
util.makedirs(seqdir)
|
||||
istart, imid, iend = get_map_indices(sequence)
|
||||
poleparams = np.empty([0, 6])
|
||||
with np.load(os.path.join(seqdir, localmapfile)) as data:
|
||||
for i, map in enumerate(data['maps']):
|
||||
T_w_m = sequence.poses[map['imid']].dot(T_cam0_mc).dot(T_mc_m)
|
||||
localpoleparams = map['poleparams']
|
||||
h = np.diff(localpoleparams[:, 2:4], axis=1).squeeze()
|
||||
npoles = localpoleparams.shape[0]
|
||||
polepos_m = np.hstack(
|
||||
[localpoleparams[:, :3], np.ones([npoles, 1])]).T
|
||||
polepos_w = np.matmul(T_w_m, polepos_m)[:3].T
|
||||
localpoleparams[:, :3] = polepos_w
|
||||
localpoleparams[:, 3] = polepos_w[:, 2] + h
|
||||
poleparams = np.vstack([poleparams, localpoleparams])
|
||||
|
||||
xy = poleparams[:, :2]
|
||||
a = poleparams[:, [4]]
|
||||
boxes = np.hstack([xy - 0.5 * a, xy + 0.5 * a])
|
||||
clustermeans = np.zeros([0, 5])
|
||||
clustercovs = np.zeros([0, 5, 5])
|
||||
clusterviz = []
|
||||
for ci in cluster.cluster_boxes(boxes):
|
||||
ci = list(ci)
|
||||
if len(ci) < 1:
|
||||
continue
|
||||
clustermeans = np.vstack([clustermeans, np.average(
|
||||
poleparams[ci, :-1], axis=0, weights=poleparams[ci, -1])])
|
||||
np.savez(os.path.join(seqdir, globalmapfile), polemeans=clustermeans,
|
||||
polecovs=clustercovs)
|
||||
|
||||
|
||||
def view_global_map(seq):
|
||||
seqdir = '{:03d}'.format(seq)
|
||||
with np.load(os.path.join('kitti', seqdir, globalmapfile)) as data:
|
||||
xy = data['polemeans'][:, :2]
|
||||
plt.scatter(xy[:, 0], xy[:, 1], s=5, c='b', marker='s')
|
||||
plt.show()
|
||||
|
||||
|
||||
def localize(seq, visualize=False):
|
||||
print(seq)
|
||||
sequence = dataset.sequence(seq)
|
||||
seqdir = os.path.join('kitti', '{:03d}'.format(seq))
|
||||
mapdata = np.load(os.path.join(seqdir, globalmapfile))
|
||||
polemap = mapdata['polemeans']
|
||||
polemap = np.hstack([polemap[:, :2], np.diff(polemap[:, 2:4], axis=1)])
|
||||
locdata = np.load(os.path.join(seqdir, localmapfile))['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=5, 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, 3.0, 0.0, length_includes_head=True,
|
||||
head_width=2.1, head_length=3.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 = 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)))
|
||||
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)
|
||||
|
||||
|
||||
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)))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
save_local_maps(13)
|
||||
save_global_map(13)
|
||||
localize(13, visualize=True)
|
||||
evaluate(13)
|
||||
28
poles/kittiwrapper.py
Normal file
28
poles/kittiwrapper.py
Normal file
@@ -0,0 +1,28 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import os
|
||||
|
||||
import numpy
|
||||
import pykitti
|
||||
|
||||
import kittidrives
|
||||
import util
|
||||
|
||||
|
||||
class kittiwrapper:
|
||||
def __init__(self, datadir):
|
||||
self.ododir = os.path.join(datadir, 'odometry')
|
||||
self.rawdir = os.path.join(datadir, 'raw_data')
|
||||
|
||||
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 - 11]
|
||||
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
|
||||
94
poles/mapping.py
Normal file
94
poles/mapping.py
Normal file
@@ -0,0 +1,94 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import copy
|
||||
import os
|
||||
import psutil
|
||||
import shutil
|
||||
import subprocess
|
||||
|
||||
import numpy as np
|
||||
import open3d as o3
|
||||
import scipy.special
|
||||
|
||||
import raytracing as rt
|
||||
|
||||
|
||||
tmpdir = 'tmp'
|
||||
scanformat = 'ply'
|
||||
occupancythreshold = 0.05
|
||||
|
||||
|
||||
# Scan points are specified with respect to the sensor coordinate frame.
|
||||
# Poses are specified with respect to the map coordinate frame.
|
||||
def occupancymap(scans, poses, mapshape, mapsize):
|
||||
map = rt.gridmap(mapshape, mapsize)
|
||||
for scan, pose in zip(scans, poses):
|
||||
scan_map = copy.copy(scan)
|
||||
scan_map.transform(pose)
|
||||
points = np.asarray(scan_map.points)
|
||||
rt.trace3d(np.tile(pose[:3, 3], [points.shape[0], 1]), points, map)
|
||||
|
||||
reflectionmap = map.reflectionmap()
|
||||
reflectivity = reflectionmap[np.isfinite(reflectionmap)]
|
||||
if reflectivity.size == 0:
|
||||
occupancymap = np.zeros(map.shape)
|
||||
else:
|
||||
mean = np.mean(reflectivity)
|
||||
var = np.var(reflectivity)
|
||||
alpha = -mean * ((mean**2.0 - mean) / var + 1.0)
|
||||
beta = mean - 1.0 + (mean - 2.0 * mean**2.0 + mean**3.0) / var
|
||||
prior = 1.0 - scipy.special.betainc(alpha, beta, occupancythreshold)
|
||||
occupancymap = 1.0 - scipy.special.betainc(
|
||||
map.hits + alpha, map.misses + beta, occupancythreshold)
|
||||
|
||||
return occupancymap
|
||||
|
||||
|
||||
# Scan points are specified with respect to the sensor coordinate frame.
|
||||
def export(scans, poses, dir):
|
||||
scannames = []
|
||||
if os.path.exists(dir):
|
||||
shutil.rmtree(dir)
|
||||
os.mkdir(dir)
|
||||
for i in range(len(scans)):
|
||||
scanname = 'scan{:03d}'.format(i)
|
||||
scannames.append(scanname)
|
||||
o3.write_point_cloud(
|
||||
os.path.join(dir, scanname + '.' + scanformat), scans[i])
|
||||
|
||||
rlconversion = np.array([1, -1, 1])
|
||||
trans = poses[i, :3, 3] * rlconversion
|
||||
rot = np.degrees(t3.euler.mat2euler(poses[i, :3, :3])) * -rlconversion
|
||||
pose = np.vstack([trans, rot])
|
||||
np.savetxt(
|
||||
os.path.join(dir, scanname + '.pose'), pose, delimiter=' ')
|
||||
|
||||
process = subprocess.Popen(['pose2frames', '.'], cwd=dir)
|
||||
if process.wait() != 0:
|
||||
raise Exception('Module \"pose2frames\" failed.')
|
||||
|
||||
|
||||
# Scan points are specified with respect to the sensor coordinate frame.
|
||||
def detect_dynamic_points(scans, poses):
|
||||
export(scans, poses, tmpdir)
|
||||
|
||||
maskdir = 'mask'
|
||||
process = subprocess.Popen(['peopleremover',
|
||||
'-f', scanformat,
|
||||
'--voxel-size', '0.02',
|
||||
'--fuzz', '0.1',
|
||||
'--min-cluster-size', '5',
|
||||
'--maxrange-method', 'normals',
|
||||
'--maskdir', maskdir,
|
||||
'-j', '{}'.format(psutil.cpu_count()),
|
||||
'.'], cwd=tmpdir)
|
||||
if process.wait() != 0:
|
||||
raise Exception('Module \"peopleremover\" failed.')
|
||||
|
||||
dynamic = []
|
||||
for i in range(len(scans)):
|
||||
scanname = 'scan{:03d}'.format(i)
|
||||
maskfilename = os.path.join(tmpdir, maskdir, scanname + '.mask')
|
||||
dynamic.append(np.loadtxt(maskfilename, dtype=np.bool))
|
||||
|
||||
return dynamic
|
||||
521
poles/ncltpoles.py
Normal file
521
poles/ncltpoles.py
Normal file
@@ -0,0 +1,521 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import copy
|
||||
import datetime
|
||||
import multiprocessing
|
||||
import os
|
||||
import shutil
|
||||
|
||||
import matplotlib.patches
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import open3d as o3
|
||||
import progressbar
|
||||
import scipy.interpolate
|
||||
import scipy.special
|
||||
|
||||
import cluster
|
||||
import mapping
|
||||
import ndshow
|
||||
import particlefilter
|
||||
import poles
|
||||
import pynclt
|
||||
import util
|
||||
|
||||
|
||||
plt.rcParams['text.latex.preamble']=[r'\usepackage{lmodern}']
|
||||
params = {'text.usetex': True,
|
||||
'font.size': 16,
|
||||
'font.family': 'lmodern',
|
||||
'text.latex.unicode': True}
|
||||
|
||||
mapextent = np.array([30.0, 30.0, 5.0])
|
||||
mapsize = np.full(3, 0.2)
|
||||
mapshape = np.array(mapextent / mapsize, dtype=np.int)
|
||||
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, 10 * poles.minscore, poles.polesides[-1])
|
||||
|
||||
|
||||
def get_localmapfile():
|
||||
return 'localmaps_{:.0f}_{:.0f}.npz'.format(
|
||||
10 * poles.minscore, poles.polesides[-1])
|
||||
|
||||
|
||||
def get_evalfile():
|
||||
return 'evaluation_{:.0f}_{:.0f}.npz'.format(
|
||||
10 * poles.minscore, poles.polesides[-1])
|
||||
|
||||
|
||||
def get_map_indices(session):
|
||||
distance = np.hstack([0.0, np.cumsum(np.linalg.norm(
|
||||
np.diff(session.T_w_r_gt_velo[:, :3, 3], axis=0), axis=1))])
|
||||
istart = []
|
||||
imid = []
|
||||
iend = []
|
||||
i = 0
|
||||
j = 0
|
||||
k = 0
|
||||
for id, d in enumerate(distance):
|
||||
if d >= i * mapinterval:
|
||||
istart.append(id)
|
||||
i += 1
|
||||
if d >= j * mapinterval + 0.5 * mapdistance:
|
||||
imid.append(id)
|
||||
j += 1
|
||||
if d > k * mapinterval + mapdistance:
|
||||
iend.append(id)
|
||||
k += 1
|
||||
return istart[:len(iend)], imid[:len(iend)], iend
|
||||
|
||||
|
||||
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('nclt', get_globalmapname() + '.npz')
|
||||
np.savez(globalmapfile,
|
||||
polemeans=clustermeans, mapfactors=mapfactors, mappos=globalmappos)
|
||||
plot_global_map(globalmapfile)
|
||||
|
||||
|
||||
def plot_global_map(globalmapfile):
|
||||
data = np.load(globalmapfile)
|
||||
x, y = data['polemeans'][:, :2].T
|
||||
plt.clf()
|
||||
plt.rcParams.update(params)
|
||||
plt.scatter(x, y, s=1, c='b', marker='.')
|
||||
plt.xlabel('x [m]')
|
||||
plt.ylabel('y [m]')
|
||||
plt.savefig(globalmapfile[:-4] + '.svg')
|
||||
plt.savefig(globalmapfile[:-4] + '.pgf')
|
||||
print(data['mapfactors'])
|
||||
|
||||
|
||||
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.PointCloud()
|
||||
scan.points = o3.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.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.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 view_local_maps(sessionname):
|
||||
sessiondir = os.path.join('nclt', 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.PointCloud()
|
||||
for j in range(map['istart'], map['iend']):
|
||||
points, intensities = session.get_velo(j)
|
||||
cloud = o3.PointCloud()
|
||||
cloud.points = o3.Vector3dVector(points)
|
||||
cloud.colors = o3.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.draw_geometries([accucloud, mapboundsvis] + polevis)
|
||||
|
||||
|
||||
def evaluate_matches():
|
||||
mapdata = np.load(os.path.join('nclt', get_globalmapname() + '.npz'))
|
||||
polemap = mapdata['polemeans'][:, :2]
|
||||
kdtree = scipy.spatial.cKDTree(polemap[:, :2], leafsize=10)
|
||||
maxdist = 0.5
|
||||
n_matches = np.zeros(len(pynclt.sessions))
|
||||
n_all = np.zeros(len(pynclt.sessions))
|
||||
for i, sessionname in enumerate(pynclt.sessions):
|
||||
sessiondir = os.path.join('nclt', sessionname)
|
||||
session = pynclt.session(sessionname)
|
||||
maps = np.load(os.path.join(sessiondir, get_localmapfile()))['maps']
|
||||
for map in maps:
|
||||
n = map['poleparams'].shape[0]
|
||||
n_all[i] += n
|
||||
polepos_m = np.hstack(
|
||||
[map['poleparams'][:, :2], np.zeros([n, 1]), np.ones([n, 1])]).T
|
||||
T_w_m = session.T_w_r_gt_velo[map['imid']].dot(T_r_m)
|
||||
polepos_w = T_w_m.dot(polepos_m)
|
||||
|
||||
plt.scatter(polemap[:, 0], polemap[:, 1], color='k')
|
||||
plt.scatter(polepos_w[0,:], polepos_w[1,:], color='r')
|
||||
plt.show()
|
||||
|
||||
dist, _ = kdtree.query(
|
||||
polepos_w[:2].T, k=1, distance_upper_bound=maxdist)
|
||||
n_matches[i] += np.sum(np.isfinite(dist))
|
||||
print('{}: {}'.format(
|
||||
sessionname, np.true_divide(n_matches[i], n_all[i])))
|
||||
|
||||
|
||||
def localize(sessionname, visualize=False):
|
||||
print(sessionname)
|
||||
mapdata = np.load(os.path.join('nclt', get_globalmapname() + '.npz'))
|
||||
polemap = mapdata['polemeans'][:, :2]
|
||||
polevar = 1.50
|
||||
session = pynclt.session(sessionname)
|
||||
locdata = np.load(os.path.join(session.dir, get_localmapfile()))['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)
|
||||
|
||||
|
||||
def plot_trajectories():
|
||||
trajectorydir = os.path.join(
|
||||
pynclt.resultdir, 'trajectories_est_{:.0f}_{:.0f}_{:.0f}'.format(
|
||||
n_mapdetections, 10 * poles.minscore, poles.polesides[-1]))
|
||||
pgfdir = os.path.join(trajectorydir, 'pgf')
|
||||
util.makedirs(trajectorydir)
|
||||
util.makedirs(pgfdir)
|
||||
mapdata = np.load(os.path.join('nclt', get_globalmapname() + '.npz'))
|
||||
polemap = mapdata['polemeans']
|
||||
plt.rcParams.update(params)
|
||||
for sessionname in pynclt.sessions:
|
||||
try:
|
||||
session = pynclt.session(sessionname)
|
||||
files = [file for file \
|
||||
in os.listdir(os.path.join(pynclt.resultdir, sessionname)) \
|
||||
if file.startswith('localization_3_6_7_2019-07')]
|
||||
# if file.startswith(get_locfileprefix())]
|
||||
for file in files:
|
||||
T_w_r_est = np.load(os.path.join(
|
||||
pynclt.resultdir, sessionname, file))['T_w_r_est']
|
||||
plt.clf()
|
||||
plt.scatter(polemap[:, 0], polemap[:, 1],
|
||||
s=1, c='b', marker='.')
|
||||
plt.plot(session.T_w_r_gt[::20, 0, 3],
|
||||
session.T_w_r_gt[::20, 1, 3], color=(0.5, 0.5, 0.5))
|
||||
plt.plot(T_w_r_est[::20, 0, 3], T_w_r_est[::20, 1, 3], 'r')
|
||||
plt.xlabel('x [m]')
|
||||
plt.ylabel('y [m]')
|
||||
plt.gcf().subplots_adjust(
|
||||
bottom=0.13, top=0.98, left=0.145, right=0.98)
|
||||
filename = sessionname + file[18:-4]
|
||||
plt.savefig(os.path.join(trajectorydir, filename + '.svg'))
|
||||
plt.savefig(os.path.join(pgfdir, filename + '.pgf'))
|
||||
except:
|
||||
pass
|
||||
|
||||
|
||||
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_3_6_7_2019-07')]
|
||||
# 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('nclt', 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']))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
poles.minscore = 0.6
|
||||
poles.polesides = range(1, 7+1)
|
||||
save_global_map()
|
||||
session = '2012-01-08'
|
||||
save_local_maps(session)
|
||||
localize(session)
|
||||
plot_trajectories()
|
||||
evaluate()
|
||||
81
poles/ndshow.py
Normal file
81
poles/ndshow.py
Normal file
@@ -0,0 +1,81 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.widgets import Slider
|
||||
import numpy as np
|
||||
import torch
|
||||
|
||||
|
||||
def _take(data, index):
|
||||
slice = data
|
||||
for i in range(len(index)):
|
||||
slice = np.take(slice, min(slice.shape[0]-1, index[i]), axis=0)
|
||||
|
||||
return slice
|
||||
|
||||
|
||||
def matshow(data, matnames=[], dimnames=[]):
|
||||
if not isinstance(data, list):
|
||||
data = [data]
|
||||
|
||||
for i in range(len(data)):
|
||||
if type(data[i]) is torch.Tensor:
|
||||
data[i] = data[i].numpy()
|
||||
|
||||
ndim = max([d.ndim for d in data])
|
||||
|
||||
for i in range(len(data)):
|
||||
while data[i].ndim < ndim:
|
||||
data[i] = np.expand_dims(data[i], axis=0)
|
||||
|
||||
shape = []
|
||||
for dim in range(ndim - 2):
|
||||
shape.append(max(d.shape[dim] for d in data))
|
||||
|
||||
figure, axes = plt.subplots(
|
||||
1, len(data), sharex=True, sharey=True, squeeze=False)
|
||||
|
||||
for i in range(len(data)):
|
||||
axes[0,i].imshow(_take(data[i], [0] * (ndim-2)),
|
||||
vmin=np.amin(data[i]), vmax=np.amax(data[i]),
|
||||
interpolation=None, origin='lower',
|
||||
extent=[0.0, data[i].shape[-1], 0.0, data[i].shape[-2]])
|
||||
|
||||
for i in range(min(len(data), len(matnames))):
|
||||
axes[0,i].set_title(matnames[i])
|
||||
|
||||
sliders = []
|
||||
updatefuncs = []
|
||||
bottom = np.linspace(0.0, 0.1, ndim)[1:-1]
|
||||
for i in range(len(shape)):
|
||||
sliderax = plt.axes([0.2, bottom[i], 0.6, 0.02],
|
||||
facecolor='lightgoldenrodyellow')
|
||||
|
||||
if i < len(dimnames):
|
||||
label = dimnames[i]
|
||||
else:
|
||||
label = 'Axis {}'.format(i)
|
||||
sliders.append(Slider(sliderax, label=label,
|
||||
valmin=0, valmax=shape[i]-1, valinit=0, valstep=1))
|
||||
|
||||
def update(val):
|
||||
indices = [int(slider.val) for slider in sliders]
|
||||
for j in range(axes.size):
|
||||
axes[0,j].images[0].set_array(_take(data[j], indices))
|
||||
figure.canvas.draw_idle()
|
||||
updatefuncs.append(update)
|
||||
|
||||
sliders[i].on_changed(updatefuncs[i])
|
||||
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
matshow(np.random.rand(100, 15, 25, 64, 64))
|
||||
matshow(np.random.rand(100, 15, 25, 64, 64),
|
||||
matnames=['Matrix A', 'Matrix B'],
|
||||
dimnames=['depth', 'height', 'width'])
|
||||
matshow([np.random.rand(64, 64, 64), np.random.rand(10, 10, 64)])
|
||||
matshow([np.random.rand(9, 11), np.random.rand(12, 12, 6)],
|
||||
matnames=['9x11', '12x12x6'])
|
||||
matshow(np.random.rand(100, 100, 100))
|
||||
85
poles/particlefilter.py
Normal file
85
poles/particlefilter.py
Normal file
@@ -0,0 +1,85 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import warnings
|
||||
|
||||
import numpy as np
|
||||
import scipy
|
||||
|
||||
import util
|
||||
|
||||
|
||||
class particlefilter:
|
||||
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
|
||||
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)
|
||||
|
||||
@property
|
||||
def neff(self):
|
||||
return 1.0 / (np.sum(self.weights**2.0) * self.count)
|
||||
|
||||
def update_motion(self, mean, cov):
|
||||
T_r0_r1 = util.xyp2ht(
|
||||
np.random.multivariate_normal(mean, cov, self.count))
|
||||
self.particles = np.matmul(self.particles, T_r0_r1)
|
||||
|
||||
def update_measurement(self, poleparams, resample=True):
|
||||
n = poleparams.shape[0]
|
||||
polepos_r = np.hstack(
|
||||
[poleparams[:, :2], np.zeros([n, 1]), np.ones([n, 1])]).T
|
||||
for i in range(self.count):
|
||||
polepos_w = self.particles[i].dot(polepos_r)
|
||||
d, _ = self.kdtree.query(
|
||||
polepos_w[:2].T, k=1, distance_upper_bound=self.d_max)
|
||||
self.weights[i] *= np.prod(
|
||||
self.poledist.pdf(np.clip(d, 0.0, self.d_max)) + 0.1)
|
||||
self.weights /= np.sum(self.weights)
|
||||
|
||||
if resample and self.neff < self.minneff:
|
||||
self.resample()
|
||||
|
||||
def estimate_pose(self):
|
||||
if self.estimatetype == 'mean':
|
||||
xyp = util.ht2xyp(np.matmul(self.T_o_w, self.particles))
|
||||
mean = np.hstack(
|
||||
[np.average(xyp[:, :2], axis=0, weights=self.weights),
|
||||
util.average_angles(xyp[:, 2], weights=self.weights)])
|
||||
return self.T_w_o.dot(util.xyp2ht(mean))
|
||||
if self.estimatetype == 'max':
|
||||
return self.particles[np.argmax(self.weights)]
|
||||
if self.estimatetype == 'best':
|
||||
i = np.argsort(self.weights)[-int(0.1 * self.count):]
|
||||
xyp = util.ht2xyp(np.matmul(self.T_o_w, self.particles[i]))
|
||||
mean = np.hstack(
|
||||
[np.average(xyp[:, :2], axis=0, weights=self.weights[i]),
|
||||
util.average_angles(xyp[:, 2], weights=self.weights[i])])
|
||||
return self.T_w_o.dot(util.xyp2ht(mean))
|
||||
|
||||
def resample(self):
|
||||
cumsum = np.cumsum(self.weights)
|
||||
pos = np.random.rand() / self.count
|
||||
idx = np.empty(self.count, dtype=np.int)
|
||||
ics = 0
|
||||
for i in range(self.count):
|
||||
while cumsum[ics] < pos:
|
||||
ics += 1
|
||||
idx[i] = ics
|
||||
pos += 1.0 / self.count
|
||||
self.particles = self.particles[idx]
|
||||
self.weights[:] = 1.0 / self.count
|
||||
134
poles/poles.py
Normal file
134
poles/poles.py
Normal file
@@ -0,0 +1,134 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import numpy as np
|
||||
import scipy.stats
|
||||
import skimage.feature
|
||||
import skimage.measure
|
||||
import torch
|
||||
|
||||
|
||||
device = torch.device('cuda')
|
||||
polesides = range(1, 5+1)
|
||||
minscore = 0.6
|
||||
minheight = 1.0
|
||||
freelength = 0.2
|
||||
dstop = 1.0e-3
|
||||
normstd = 0.2
|
||||
|
||||
|
||||
def detect_poles(occupancymap, mapsize):
|
||||
f = int(np.round(freelength / mapsize[0]))
|
||||
polemapshape = occupancymap.shape - np.array([2*f, 2*f, 0])
|
||||
|
||||
ogm = torch.tensor(
|
||||
occupancymap, device=device).permute([2, 0, 1]).unsqueeze(1)
|
||||
accuscores = torch.zeros(
|
||||
tuple(np.hstack([len(polesides), polemapshape[[2, 0, 1]]])),
|
||||
dtype=torch.float64, device=device)
|
||||
for ia, a in enumerate(polesides):
|
||||
af = a + 2 * f
|
||||
xmax = torch.nn.functional.max_pool2d(
|
||||
ogm, kernel_size=[af, f], stride=1)
|
||||
xmax = torch.max(xmax[..., :-a-f], xmax[..., a+f:])
|
||||
|
||||
ymax = torch.nn.functional.max_pool2d(
|
||||
ogm, kernel_size=[f, af], stride=1)
|
||||
ymax = torch.max(ymax[..., :-a-f, :], ymax[..., a+f:, :])
|
||||
|
||||
kernel = torch.ones([1, 1, a, a], dtype=torch.float64, device=device) \
|
||||
/ (a**2)
|
||||
|
||||
score = (torch.nn.functional.conv2d(ogm[..., f:-f, f:-f], kernel) \
|
||||
- torch.max(xmax, ymax)).squeeze()
|
||||
|
||||
accuscores[ia] = torch.nn.functional.max_pool2d(
|
||||
torch.nn.functional.pad(score, [a-1] * 4, 'constant', -1.0),
|
||||
kernel_size=a, stride=1) / 2.0 + 0.5
|
||||
accuscore = torch.max(accuscores, 0)[0]
|
||||
|
||||
h = torch.zeros(
|
||||
tuple(polemapshape[:2]), dtype=torch.uint8, device=device)
|
||||
hmax = h.clone()
|
||||
z = h.clone()
|
||||
zmax = z.clone()
|
||||
for im, m in enumerate(accuscore):
|
||||
ispole = m >= minscore
|
||||
h += ispole
|
||||
ih = h > hmax
|
||||
hmax[ih] = h[ih]
|
||||
zmax[ih] = z[ih]
|
||||
h[ispole == False] = 0
|
||||
z[ispole == False] = im + 1
|
||||
|
||||
accuscores = accuscores.cpu().numpy()
|
||||
accuscore = accuscore.cpu().numpy()
|
||||
hmax = hmax.cpu().numpy()
|
||||
zmax = zmax.cpu().numpy()
|
||||
|
||||
ix, iy = np.where(hmax >= minheight / mapsize[2])
|
||||
poleness = np.zeros(polemapshape[:2])
|
||||
if ix.size > 0:
|
||||
for iix, iiy in zip(ix, iy):
|
||||
h = hmax[iix, iiy]
|
||||
z = zmax[iix, iiy]
|
||||
poleness[iix, iiy] = np.mean(accuscore[z:z+h, iix, iiy])
|
||||
|
||||
peaks = skimage.feature.peak_local_max(
|
||||
poleness, min_distance=f, exclude_border=False, indices=False)
|
||||
label = skimage.measure.label(peaks, neighbors=8, background=False)
|
||||
regions = skimage.measure.regionprops(label, coordinates='rc')
|
||||
centroids = np.array([r.centroid for r in regions]) + 0.5
|
||||
|
||||
normdist = scipy.stats.norm(0.0, normstd / mapsize[0])
|
||||
cellcoords = np.meshgrid(
|
||||
range(polemapshape[0]), range(polemapshape[1]), indexing='ij')
|
||||
cellcoords = np.stack(cellcoords, axis=2).astype(np.float) + 0.5
|
||||
|
||||
poleparams = np.empty([centroids.shape[0], 6])
|
||||
optcentroids = centroids.copy()
|
||||
for ic in range(optcentroids.shape[0]):
|
||||
lastcentroid = np.full(2, np.inf)
|
||||
while np.linalg.norm(lastcentroid - optcentroids[ic]) \
|
||||
> dstop / mapsize[0]:
|
||||
lastcentroid = optcentroids[ic]
|
||||
d = np.linalg.norm(cellcoords - optcentroids[ic], axis=2)
|
||||
weights = np.tile(np.expand_dims(poleness * normdist.pdf(d), 2),
|
||||
[1, 1, 2])
|
||||
optcentroids[ic] = np.average(
|
||||
cellcoords, weights=weights, axis=(0, 1))
|
||||
|
||||
ix, iy = np.floor(optcentroids[ic]).astype(np.int)
|
||||
if hmax[ix, iy] < minheight / mapsize[2]:
|
||||
optcentroids[ic] = centroids[ic]
|
||||
ix, iy = np.floor(centroids[ic]).astype(np.int)
|
||||
|
||||
h = hmax[ix, iy]
|
||||
z = zmax[ix, iy]
|
||||
zstart = 0.0
|
||||
if z > 0:
|
||||
zstart = mapsize[2] * (np.interp(minscore,
|
||||
accuscore[z-1:z+1, ix, iy], [z-1, z]) + 0.5)
|
||||
zend = occupancymap.shape[2] * mapsize[2]
|
||||
if z + h < polemapshape[2]:
|
||||
zend = mapsize[2] * (np.interp(minscore,
|
||||
accuscore[z+h-1:z+h+1, ix, iy], [z+h-1, z+h]) + 0.5)
|
||||
|
||||
sideweights = np.mean(accuscores[:, z:z+h, ix, iy], axis=1)
|
||||
sidelength = np.average(polesides, weights=sideweights) * mapsize[0]
|
||||
score = np.mean(np.average(
|
||||
accuscores[:, z:z+h, ix, iy], weights=sideweights, axis=0))
|
||||
|
||||
x, y = mapsize[:2] * (optcentroids[ic] + f)
|
||||
poleparams[ic] = [x, y, zstart, zend, sidelength, score]
|
||||
|
||||
poleparams = poleparams[np.flip(np.argsort(poleparams[:, -1]), axis=0), :]
|
||||
ip = 0
|
||||
while ip < poleparams.shape[0]:
|
||||
d = np.linalg.norm(poleparams[ip, :2] - poleparams[ip+1:, :2], axis=1) \
|
||||
- 0.5 * (poleparams[ip, 4] + poleparams[ip+1:, 4])
|
||||
poleparams = np.delete(
|
||||
poleparams, np.where(d < freelength)[0] + ip + 1, axis=0)
|
||||
ip += 1
|
||||
|
||||
return poleparams
|
||||
|
||||
320
poles/pynclt.py
Normal file
320
poles/pynclt.py
Normal file
@@ -0,0 +1,320 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
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']
|
||||
|
||||
lat0 = np.radians(42.293227)
|
||||
lon0 = np.radians(-83.709657)
|
||||
re = 6378135.0
|
||||
rp = 6356750
|
||||
rns = (re * rp)**2.0 \
|
||||
/ ((re * np.cos(lat0))**2.0 + (rp * np.sin(lat0))**2.0)**1.5
|
||||
rew = re**2.0 / np.sqrt((re * np.cos(lat0))**2.0 + (rp * np.sin(lat0))**2.0)
|
||||
|
||||
veloheadertype = np.dtype({
|
||||
'magic': ('<u8', 0),
|
||||
'count': ('<u4', 8),
|
||||
'utime': ('<u8', 12),
|
||||
'pad': ('V4', 20)})
|
||||
veloheadersize = 24
|
||||
|
||||
velodatatype = np.dtype({
|
||||
'x': ('<u2', 0),
|
||||
'y': ('<u2', 2),
|
||||
'z': ('<u2', 4),
|
||||
'i': ('u1', 6),
|
||||
'l': ('u1', 7)})
|
||||
velodatasize = 8
|
||||
|
||||
|
||||
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 view_snapshot(sessionname):
|
||||
cloud, trajectory = load_snapshot(sessionname)
|
||||
o3.draw_geometries([cloud, trajectory])
|
||||
|
||||
|
||||
def pose2ht(pose):
|
||||
r, p, y = pose[3:]
|
||||
return t3.affines.compose(
|
||||
pose[:3], t3.euler.euler2mat(r, p, y, eulerdef), np.ones(3))
|
||||
|
||||
|
||||
def latlon2xy(latlon):
|
||||
lat = latlon[:, [0]]
|
||||
lon = latlon[:, [1]]
|
||||
return np.hstack([np.sin(lat - lat0) * rns,
|
||||
np.sin(lon - lon0) * rew * np.cos(lat0)])
|
||||
|
||||
|
||||
def data2xyzi(data):
|
||||
xyzil = data.view(velodatatype)
|
||||
xyz = np.hstack(
|
||||
[xyzil[axis].reshape([-1, 1]) for axis in ['x', 'y', 'z']])
|
||||
xyz = xyz * 0.005 - 100.0
|
||||
return xyz, xyzil['i']
|
||||
|
||||
|
||||
def save_trajectories():
|
||||
trajectorydir = os.path.join(resultdir, 'trajectories_gt')
|
||||
util.makedirs(trajectorydir)
|
||||
|
||||
trajectories = [session(s).T_w_r_gt[::20, :2, 3] for s in sessions]
|
||||
for i in range(len(trajectories)):
|
||||
plt.clf()
|
||||
[plt.plot(t[:, 0], t[:, 1], color=(0.5, 0.5, 0.5)) \
|
||||
for t in trajectories]
|
||||
plt.plot(trajectories[i][:, 0], trajectories[i][:, 1], color='y')
|
||||
plt.savefig(os.path.join(trajectorydir, sessions[i] + '.svg'))
|
||||
|
||||
|
||||
class session:
|
||||
def __init__(self, session):
|
||||
self.session = session
|
||||
self.dir = os.path.join(resultdir, self.session)
|
||||
|
||||
try:
|
||||
data = np.load(os.path.join(self.dir, sessionfile))
|
||||
self.velofiles = data['velofiles']
|
||||
self.t_velo = data['t_velo']
|
||||
self.velorawfile = data['velorawfile']
|
||||
self.t_rawvelo = data['t_rawvelo']
|
||||
self.i_rawvelo = data['i_rawvelo']
|
||||
self.t_gt = data['t_gt']
|
||||
self.T_w_r_gt = data['T_w_r_gt']
|
||||
self.T_w_r_gt_velo = data['T_w_r_gt_velo']
|
||||
self.t_cov_gt = data['t_cov_gt']
|
||||
self.cov_gt = data['cov_gt']
|
||||
self.t_odo = data['t_odo']
|
||||
self.T_w_r_odo = data['T_w_r_odo']
|
||||
self.T_w_r_odo_velo = data['T_w_r_odo_velo']
|
||||
self.t_relodo = data['t_relodo']
|
||||
self.relodo = data['relodo']
|
||||
self.relodocov = data['relodocov']
|
||||
self.t_gps = data['t_gps']
|
||||
self.gps = data['gps']
|
||||
except:
|
||||
velodir = os.path.join(datadir, 'velodyne_data',
|
||||
self.session + '_vel', 'velodyne_sync')
|
||||
self.velofiles = [os.path.join(velodir, file) \
|
||||
for file in os.listdir(velodir) \
|
||||
if os.path.splitext(file)[1] == '.bin']
|
||||
self.velofiles.sort()
|
||||
self.t_velo = np.array([
|
||||
int(os.path.splitext(os.path.basename(velofile))[0]) \
|
||||
for velofile in self.velofiles])
|
||||
|
||||
self.velorawfile = os.path.join(datadir, 'velodyne_data',
|
||||
self.session + '_vel', 'velodyne_hits.bin')
|
||||
self.t_rawvelo = []
|
||||
self.i_rawvelo = []
|
||||
with open(self.velorawfile, 'rb') as file:
|
||||
data = np.array(file.read(veloheadersize))
|
||||
while data:
|
||||
header = data.view(veloheadertype)
|
||||
if header['magic'] != 0xad9cad9cad9cad9c:
|
||||
break
|
||||
self.t_rawvelo.append(header['utime'])
|
||||
self.i_rawvelo.append(file.tell() - veloheadersize)
|
||||
file.seek(header['count'] * velodatasize, os.SEEK_CUR)
|
||||
data = np.array(file.read(veloheadersize))
|
||||
self.t_rawvelo = np.array(self.t_rawvelo)
|
||||
self.i_rawvelo = np.array(self.i_rawvelo)
|
||||
|
||||
posefile = os.path.join(
|
||||
datadir, 'ground_truth', 'groundtruth_' + self.session + '.csv')
|
||||
posedata = np.genfromtxt(posefile, delimiter=csvdelimiter)
|
||||
posedata = posedata[np.logical_not(np.any(np.isnan(posedata), 1))]
|
||||
self.t_gt = posedata[:, 0]
|
||||
self.T_w_r_gt = np.stack([T_w_o.dot(pose2ht(pose_o_r)) \
|
||||
for pose_o_r in posedata[:, 1:]])
|
||||
self.T_w_r_gt_velo = np.stack(
|
||||
[self.get_T_w_r_gt(t) for t in self.t_velo])
|
||||
|
||||
cov_gtfile = os.path.join(
|
||||
datadir, 'ground_truth_cov', 'cov_' + self.session + '.csv')
|
||||
cov_gt = np.genfromtxt(cov_gtfile, delimiter=csvdelimiter)
|
||||
self.t_cov_gt = cov_gt[:, 0]
|
||||
self.cov_gt = np.stack(
|
||||
[np.reshape(roc[[
|
||||
1, 2, 3, 4, 5, 6,
|
||||
2, 7, 8, 9, 10, 11,
|
||||
3, 8, 12, 13, 14, 15,
|
||||
4, 9, 13, 16, 17, 18,
|
||||
5, 10, 14, 17, 19, 20,
|
||||
6, 11, 15, 18, 20, 21
|
||||
]], [6, 6]) for roc in cov_gt])
|
||||
|
||||
sensordir = os.path.join(
|
||||
datadir, 'sensor_data', self.session + '_sen')
|
||||
odofile = os.path.join(sensordir, 'odometry_mu_100hz.csv')
|
||||
ododata = np.genfromtxt(odofile, delimiter=csvdelimiter)
|
||||
self.t_odo = ododata[:, 0]
|
||||
self.T_w_r_odo = np.stack([T_w_o.dot(pose2ht(pose_o_r)) \
|
||||
for pose_o_r in ododata[:, 1:]])
|
||||
self.T_w_r_odo_velo = np.stack(
|
||||
[self.get_T_w_r_odo(t) for t in self.t_velo])
|
||||
|
||||
relodofile = os.path.join(sensordir, 'odometry_mu.csv')
|
||||
relodo = np.genfromtxt(relodofile, delimiter=csvdelimiter)
|
||||
self.t_relodo = relodo[:, 0]
|
||||
self.relodo = relodo[:, [1, 2, 6]]
|
||||
|
||||
relodocovfile = os.path.join(sensordir, 'odometry_cov.csv')
|
||||
relodocov = np.genfromtxt(relodocovfile, delimiter=csvdelimiter)
|
||||
self.relodocov = np.stack(
|
||||
[np.reshape(roc[[
|
||||
1, 2, 3, 4, 5, 6,
|
||||
2, 7, 8, 9, 10, 11,
|
||||
3, 8, 12, 13, 14, 15,
|
||||
4, 9, 13, 16, 17, 18,
|
||||
5, 10, 14, 17, 19, 20,
|
||||
6, 11, 15, 18, 20, 21
|
||||
]], [6, 6]) for roc in relodocov])
|
||||
|
||||
gpsfile = os.path.join(sensordir, 'gps.csv')
|
||||
gps = np.genfromtxt(gpsfile, delimiter=csvdelimiter)[:, [0, 3, 4]]
|
||||
self.t_gps = gps[:, 0]
|
||||
self.gps = latlon2xy(gps[:, 1:])
|
||||
|
||||
util.makedirs(self.dir)
|
||||
np.savez(os.path.join(self.dir, sessionfile),
|
||||
velofiles=self.velofiles,
|
||||
t_velo=self.t_velo,
|
||||
velorawfile=self.velorawfile,
|
||||
t_rawvelo=self.t_rawvelo,
|
||||
i_rawvelo=self.i_rawvelo,
|
||||
t_gt=self.t_gt,
|
||||
T_w_r_gt=self.T_w_r_gt,
|
||||
T_w_r_gt_velo=self.T_w_r_gt_velo,
|
||||
t_cov_gt=self.t_cov_gt,
|
||||
cov_gt=self.cov_gt,
|
||||
t_odo=self.t_odo,
|
||||
T_w_r_odo=self.T_w_r_odo,
|
||||
T_w_r_odo_velo=self.T_w_r_odo_velo,
|
||||
t_relodo=self.t_relodo,
|
||||
relodo=self.relodo,
|
||||
relodocov=self.relodocov,
|
||||
t_gps=self.t_gps,
|
||||
gps=self.gps)
|
||||
|
||||
def get_velo(self, i):
|
||||
return data2xyzi(np.fromfile(self.velofiles[i]))
|
||||
|
||||
def get_velo_raw(self, i):
|
||||
with open(self.velorawfile, 'rb') as file:
|
||||
data = np.array(file.read(veloheadersize))
|
||||
header = data.view(veloheadertype)
|
||||
data = np.fromfile(file, count=header['count']).view(velodatatype)
|
||||
xyz = np.empty([data.shape[0], 3])
|
||||
intensities = np.empty([data.shape[0], 1])
|
||||
for i in range(data.shape[0]):
|
||||
xyz[i], intensities[i] = data2xyzi(data[i])
|
||||
return xyz, intensities
|
||||
|
||||
def get_T_w_r_gt(self, t):
|
||||
i = np.clip(np.searchsorted(self.t_gt, t), 1, self.t_gt.size - 1) \
|
||||
+ np.array([-1, 0])
|
||||
return util.interpolate_ht(self.T_w_r_gt[i], self.t_gt[i], t)
|
||||
|
||||
def get_T_w_r_odo(self, t):
|
||||
i = np.clip(np.searchsorted(self.t_odo, t), 1, self.t_odo.size - 1) \
|
||||
+ np.array([-1, 0])
|
||||
return util.interpolate_ht(self.T_w_r_odo[i], self.t_odo[i], t)
|
||||
|
||||
def save_snapshot(self):
|
||||
print(self.session)
|
||||
naccupoints = int(3e7)
|
||||
nscans = len(self.velofiles)
|
||||
nmaxpoints = naccupoints / nscans
|
||||
accupoints = np.full([naccupoints, 3], np.nan)
|
||||
accuintensities = np.empty([naccupoints, 1])
|
||||
|
||||
ipstart = 0
|
||||
with progressbar.ProgressBar(max_value=nscans) as bar:
|
||||
for i in range(nscans):
|
||||
points, intensities = self.get_velo(i)
|
||||
npoints = min(points.shape[0], nmaxpoints)
|
||||
ip = np.random.choice(points.shape[0], npoints, replace=False)
|
||||
points = np.hstack([points[ip], np.ones([npoints, 1])]).T
|
||||
points = self.T_w_r_gt_velo[i].dot(points)[:3].T
|
||||
accupoints[ipstart:ipstart+npoints] = points
|
||||
intensities = intensities[ip].reshape([-1, 1])
|
||||
accuintensities[ipstart:ipstart+npoints] = intensities
|
||||
ipstart += npoints
|
||||
bar.update(i)
|
||||
trajectory = self.T_w_r_gt[:, :3, 3]
|
||||
|
||||
util.makedirs(self.dir)
|
||||
np.savez(os.path.join(self.dir, snapshotfile),
|
||||
points=accupoints, intensities=accuintensities,
|
||||
trajectory=trajectory)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
for s in sessions:
|
||||
session(s)
|
||||
89
poles/util.py
Normal file
89
poles/util.py
Normal file
@@ -0,0 +1,89 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import os
|
||||
|
||||
import numpy as np
|
||||
import open3d as o3
|
||||
import transforms3d as t3
|
||||
import pyquaternion as pq
|
||||
|
||||
|
||||
def invert_ht(ht):
|
||||
ht = np.tile(ht, [1, 1, 1])
|
||||
iht = np.tile(np.identity(4), [ht.shape[0], 1, 1])
|
||||
iht[..., :3, :3] = ht[..., :3, :3].transpose(0, 2, 1)
|
||||
iht[..., :3, [3]] = -np.matmul(iht[..., :3, :3], ht[..., :3, [3]])
|
||||
return iht.squeeze()
|
||||
|
||||
|
||||
def create_wire_box(edgelengths, color=[0.0, 0.0, 0.0]):
|
||||
lineset = o3.LineSet()
|
||||
x, y, z = edgelengths
|
||||
lineset.points = o3.Vector3dVector([[0, 0, 0], [x, 0, 0], [0, y, 0],
|
||||
[x, y, 0], [0, 0, z], [x, 0, z], [0, y, z], [x, y, z]])
|
||||
lineset.lines = o3.Vector2iVector([[0, 1], [1, 3], [3, 2], [2, 0],
|
||||
[0, 4], [1, 5], [3, 7], [2, 6],
|
||||
[4, 5], [5, 7], [7, 6], [6, 4]])
|
||||
lineset.colors = o3.Vector3dVector(np.tile(color, [len(lineset.lines), 1]))
|
||||
return lineset
|
||||
|
||||
|
||||
def intensity2color(intensity):
|
||||
return np.tile(np.reshape(intensity * 0.8, [-1, 1]), [1, 3])
|
||||
|
||||
|
||||
def xyp2ht(xyp):
|
||||
ht = np.tile(np.identity(4), [xyp.size / 3, 1, 1])
|
||||
cp = np.cos(xyp[..., 2])
|
||||
sp = np.sin(xyp[..., 2])
|
||||
ht[..., :2, 3] = xyp[..., :2]
|
||||
ht[..., 0, 0] = cp
|
||||
ht[..., 0, 1] = -sp
|
||||
ht[..., 1, 0] = sp
|
||||
ht[..., 1, 1] = cp
|
||||
return ht.squeeze()
|
||||
|
||||
|
||||
def ht2xyp(ht):
|
||||
ht = np.tile(ht, [1, 1, 1])
|
||||
p = np.arctan2(ht[..., 1, 0], ht[..., 0, 0])
|
||||
return np.hstack([ht[..., :2, 3], np.reshape(p, [-1, 1])]).squeeze()
|
||||
|
||||
|
||||
def interpolate_ht(ht, t, tq):
|
||||
amount = np.clip((tq - t[0]) / np.diff(t), 0.0, 1.0)
|
||||
pos = ht[0, :3, 3] + amount * np.diff(ht[:, :3, 3], axis=0).squeeze()
|
||||
q = [pq.Quaternion(matrix=m) for m in ht]
|
||||
qq = pq.Quaternion.slerp(q[0], q[1], amount=amount)
|
||||
return t3.affines.compose(pos, qq.rotation_matrix, np.ones(3))
|
||||
|
||||
|
||||
def project_xy(ht):
|
||||
htp = np.identity(4)
|
||||
htp[:2, 0] = ht[:2, 0] / np.linalg.norm(ht[:2, 0])
|
||||
htp[:2, 1] = [-htp[1, 0], htp[0, 0]]
|
||||
htp[:2, 3] = ht[:2, 3]
|
||||
return htp
|
||||
|
||||
|
||||
def xyzi2pc(xyz, intensities=None):
|
||||
pc = o3.PointCloud()
|
||||
pc.points = o3.Vector3dVector(xyz)
|
||||
if intensities is not None:
|
||||
pc.colors = o3.Vector3dVector(intensity2color(intensities / 255.0))
|
||||
return pc
|
||||
|
||||
|
||||
def average_angles(angles, weights=None):
|
||||
if weights is None:
|
||||
weights = np.ones(angles.shape[0])
|
||||
x = np.cos(angles) * weights
|
||||
y = np.sin(angles) * weights
|
||||
return np.arctan2(np.sum(y), np.sum(x))
|
||||
|
||||
|
||||
def makedirs(dir):
|
||||
try:
|
||||
os.makedirs(dir)
|
||||
except:
|
||||
pass
|
||||
Reference in New Issue
Block a user