From 17877ef19830e94047a7f01c57fd3d8b5bca53a0 Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Thu, 25 Jul 2019 10:28:19 +0200 Subject: [PATCH] Add code. --- poles/__init__.py | 1 + poles/cluster.py | 68 ++++++ poles/cuboid.py | 21 ++ poles/kittidrives.py | 160 ++++++++++++ poles/kittipoles.py | 372 ++++++++++++++++++++++++++++ poles/kittiwrapper.py | 28 +++ poles/mapping.py | 94 ++++++++ poles/ncltpoles.py | 521 ++++++++++++++++++++++++++++++++++++++++ poles/ndshow.py | 81 +++++++ poles/particlefilter.py | 85 +++++++ poles/poles.py | 134 +++++++++++ poles/pynclt.py | 320 ++++++++++++++++++++++++ poles/util.py | 89 +++++++ 13 files changed, 1974 insertions(+) create mode 100644 poles/__init__.py create mode 100644 poles/cluster.py create mode 100644 poles/cuboid.py create mode 100644 poles/kittidrives.py create mode 100644 poles/kittipoles.py create mode 100644 poles/kittiwrapper.py create mode 100644 poles/mapping.py create mode 100644 poles/ncltpoles.py create mode 100644 poles/ndshow.py create mode 100644 poles/particlefilter.py create mode 100644 poles/poles.py create mode 100644 poles/pynclt.py create mode 100644 poles/util.py diff --git a/poles/__init__.py b/poles/__init__.py new file mode 100644 index 0000000..72005db --- /dev/null +++ b/poles/__init__.py @@ -0,0 +1 @@ +from poles import * diff --git a/poles/cluster.py b/poles/cluster.py new file mode 100644 index 0000000..f46c23a --- /dev/null +++ b/poles/cluster.py @@ -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) diff --git a/poles/cuboid.py b/poles/cuboid.py new file mode 100644 index 0000000..417bff5 --- /dev/null +++ b/poles/cuboid.py @@ -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') diff --git a/poles/kittidrives.py b/poles/kittidrives.py new file mode 100644 index 0000000..14aaa25 --- /dev/null +++ b/poles/kittidrives.py @@ -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'} +] diff --git a/poles/kittipoles.py b/poles/kittipoles.py new file mode 100644 index 0000000..2326803 --- /dev/null +++ b/poles/kittipoles.py @@ -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) diff --git a/poles/kittiwrapper.py b/poles/kittiwrapper.py new file mode 100644 index 0000000..37d451f --- /dev/null +++ b/poles/kittiwrapper.py @@ -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 diff --git a/poles/mapping.py b/poles/mapping.py new file mode 100644 index 0000000..2442701 --- /dev/null +++ b/poles/mapping.py @@ -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 diff --git a/poles/ncltpoles.py b/poles/ncltpoles.py new file mode 100644 index 0000000..b0f2a0a --- /dev/null +++ b/poles/ncltpoles.py @@ -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() diff --git a/poles/ndshow.py b/poles/ndshow.py new file mode 100644 index 0000000..a363fe5 --- /dev/null +++ b/poles/ndshow.py @@ -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)) diff --git a/poles/particlefilter.py b/poles/particlefilter.py new file mode 100644 index 0000000..e8d1854 --- /dev/null +++ b/poles/particlefilter.py @@ -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 diff --git a/poles/poles.py b/poles/poles.py new file mode 100644 index 0000000..ca5ab6f --- /dev/null +++ b/poles/poles.py @@ -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 + \ No newline at end of file diff --git a/poles/pynclt.py b/poles/pynclt.py new file mode 100644 index 0000000..eff1fef --- /dev/null +++ b/poles/pynclt.py @@ -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': ('