sravan/polex
1
0
Fork 0
mirror of https://github.com/Mobile-Robotics-W20-Team-9/polex.git synced 2025-08-28 17:52:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1 from Mobile-Robotics-W20-Team-9/small

Browse Source 
makes it work with current dataset
This commit is contained in:
Sabrina
2020-04-28 15:03:00 -04:00
committed by GitHub
parent 1d1d181957 d0ced52afe
commit d3d6d1cd82
1 changed files with 34 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
poles/pynclt.py
View File

@@ -11,6 +11,7 @@ import pyquaternion as pq
import transforms3d as t3 import transforms3d as t3
import util import util
import time
T_w_o = np.identity(4) T_w_o = np.identity(4)
@@ -19,39 +20,40 @@ T_o_w = util.invert_ht(T_w_o)
eulerdef = 'sxyz' eulerdef = 'sxyz'
csvdelimiter = ',' csvdelimiter = ','
datadir = '/mnt/data/datasets/nclt' datadir = '/mnt/c/Users/SabrinaB/Desktop/nclt'
resultdir = 'nclt' resultdir = 'nclt'
snapshotfile = 'snapshot.npz' snapshotfile = 'snapshot.npz'
sessionfile = 'sessiondata.npz' sessionfile = 'sessiondata.npz'
sessions = [ sessions = [
'2012-01-08', #'2012-01-08',
'2012-01-15', '2012-01-15',
'2012-01-22', #'2012-01-22',
'2012-02-02', #'2012-02-02',
'2012-02-04', #'2012-02-04',
'2012-02-05', #'2012-02-05',
'2012-02-12', #'2012-02-12',
'2012-02-18', #'2012-02-18',
'2012-02-19', #'2012-02-19',
'2012-03-17', #'2012-03-17',
'2012-03-25', #'2012-03-25',
'2012-03-31', #'2012-03-31',
'2012-04-29', #'2012-04-29',
'2012-05-11', #'2012-05-11',
'2012-05-26', #'2012-05-26',
'2012-06-15', #'2012-06-15',
'2012-08-04', #'2012-08-04',
'2012-08-20', #'2012-08-20',
'2012-09-28', #'2012-09-28',
'2012-10-28', #'2012-10-28',
'2012-11-04', #'2012-11-04',
'2012-11-16', #'2012-11-16',
'2012-11-17', #'2012-11-17',
'2012-12-01', #'2012-12-01',
'2013-01-10', #'2013-01-10',
'2013-02-23', #'2013-02-23',
'2013-04-05'] '2013-04-05']
lat0 = np.radians(42.293227) lat0 = np.radians(42.293227)
lon0 = np.radians(-83.709657) lon0 = np.radians(-83.709657)
re = 6378135.0 re = 6378135.0
@@ -159,7 +161,8 @@ class session:
self.gps = data['gps'] self.gps = data['gps']
except: except:
velodir = os.path.join(datadir, 'velodyne_data', velodir = os.path.join(datadir, 'velodyne_data',
self.session + '_vel', 'velodyne_sync') self.session, 'velodyne_sync')
#self.session + '_vel', 'velodyne_sync')
self.velofiles = [os.path.join(velodir, file) \ self.velofiles = [os.path.join(velodir, file) \
for file in os.listdir(velodir) \ for file in os.listdir(velodir) \
if os.path.splitext(file)[1] == '.bin'] if os.path.splitext(file)[1] == '.bin']
@@ -169,7 +172,8 @@ class session:
for velofile in self.velofiles]) for velofile in self.velofiles])
self.velorawfile = os.path.join(datadir, 'velodyne_data', self.velorawfile = os.path.join(datadir, 'velodyne_data',
self.session + '_vel', 'velodyne_hits.bin') self.session, 'velodyne_hits.bin')
#self.session + '_vel', 'velodyne_hits.bin')
self.t_rawvelo = [] self.t_rawvelo = []
self.i_rawvelo = [] self.i_rawvelo = []
with open(self.velorawfile, 'rb') as file: with open(self.velorawfile, 'rb') as file:
@@ -182,6 +186,7 @@ class session:
self.i_rawvelo.append(file.tell() - veloheadersize) self.i_rawvelo.append(file.tell() - veloheadersize)
file.seek(header['count'] * velodatasize, os.SEEK_CUR) file.seek(header['count'] * velodatasize, os.SEEK_CUR)
data = np.array(file.read(veloheadersize)) data = np.array(file.read(veloheadersize))
print(time.time())
self.t_rawvelo = np.array(self.t_rawvelo) self.t_rawvelo = np.array(self.t_rawvelo)
self.i_rawvelo = np.array(self.i_rawvelo) self.i_rawvelo = np.array(self.i_rawvelo)
@@ -210,7 +215,7 @@ class session:
]], [6, 6]) for roc in cov_gt]) ]], [6, 6]) for roc in cov_gt])
sensordir = os.path.join( sensordir = os.path.join(
datadir, 'sensor_data', self.session + '_sen') datadir, 'sensor_data', self.session) # + '_sen')
odofile = os.path.join(sensordir, 'odometry_mu_100hz.csv') odofile = os.path.join(sensordir, 'odometry_mu_100hz.csv')
ododata = np.genfromtxt(odofile, delimiter=csvdelimiter) ododata = np.genfromtxt(odofile, delimiter=csvdelimiter)
self.t_odo = ododata[:, 0] self.t_odo = ododata[:, 0]