mirror of
https://github.com/Mobile-Robotics-W20-Team-9/UMICH-NCLT-SLAP.git
synced 2025-09-08 04:03:14 +00:00
Adding files necessary to find building coordinates in both XYZ and GPS coordinates and adding python file to manipulate various pickle files used in semantic aspects of code and BuildingMappings pickles
This commit is contained in:
65
src/dataset/dataManipulation/findBuildingCoord.py
Normal file
65
src/dataset/dataManipulation/findBuildingCoord.py
Normal file
@@ -0,0 +1,65 @@
|
||||
import sys
|
||||
import numpy as np
|
||||
import pickle
|
||||
import math
|
||||
|
||||
def gpstoLocalFrame(lat, lng, alt):
|
||||
lat0 = 0.7381566413
|
||||
lng0 = -1.4610097151
|
||||
alt0 = 265.8
|
||||
|
||||
dLat = np.deg2rad(lat) - lat0
|
||||
dLng = np.deg2rad(lng) - lng0
|
||||
dAlt = alt - alt0
|
||||
|
||||
r = 6400000 # approx. radius of earth (m)
|
||||
y = r * np.cos(lat0) * np.sin(dLng)
|
||||
x = r * np.sin(dLat)
|
||||
z = dAlt
|
||||
|
||||
return [x,y,z]
|
||||
#Example
|
||||
# x = gpstoLocalFrame(42.29360387311647,-83.71222615242006,272)
|
||||
# print(x)
|
||||
|
||||
def buildingtoGPS(building):
|
||||
pickle_in = open('pickles/BuildingMappings.pkl',"rb")
|
||||
currDict = pickle.load(pickle_in)
|
||||
for place in currDict:
|
||||
if place == building:
|
||||
return currDict.get(building)
|
||||
return 0
|
||||
|
||||
def findClosestEntrance(building1, building2):
|
||||
gps1 = buildingtoGPS(building1)
|
||||
gps2 = buildingtoGPS(building2)
|
||||
|
||||
x = [0,0,0,0]
|
||||
x[0] = calculateDistance(gps1[0][0],gps1[0][1],gps2[0][0],gps2[0][1])
|
||||
x[1] = calculateDistance(gps1[0][0],gps1[0][1],gps2[1][0],gps2[1][1])
|
||||
x[2] = calculateDistance(gps1[1][0],gps1[1][1],gps2[0][0],gps2[0][1])
|
||||
x[3] = calculateDistance(gps1[1][0],gps1[1][1],gps2[1][0],gps2[1][1])
|
||||
index = np.argmin(x)
|
||||
if index == 0:
|
||||
return [gps1[0],gps2[0]]
|
||||
elif index == 1:
|
||||
return [gps1[0],gps2[1]]
|
||||
elif index == 2:
|
||||
return [gps1[1],gps2[0]]
|
||||
else:
|
||||
return [gps1[1],gps2[1]]
|
||||
# Example
|
||||
# print(findClosestEntrance("BBB", "EECS"))
|
||||
|
||||
def calculateDistance(x1,y1,x2,y2):
|
||||
dist = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
|
||||
return dist
|
||||
|
||||
# Example usage of overall file
|
||||
'''
|
||||
GPScoords = findClosestEntrance("BBB", "EECS")
|
||||
Building1 = gpstoLocalFrame(GPScoords[0][0], GPScoords[0][1], GPScoords[0][2])
|
||||
Building2 = gpstoLocalFrame(GPScoords[1][0], GPScoords[1][1], GPScoords[1][2])
|
||||
print(Building1)
|
||||
print(Building2)
|
||||
'''
|
57
src/dataset/dataManipulation/pickles/BuildingMappings.pkl
Normal file
57
src/dataset/dataManipulation/pickles/BuildingMappings.pkl
Normal file
@@ -0,0 +1,57 @@
|
||||
(dp0
|
||||
S'EECS'
|
||||
p1
|
||||
(lp2
|
||||
(lp3
|
||||
F42.29259200117389
|
||||
aF-83.71376574039459
|
||||
aI266
|
||||
aa(lp4
|
||||
F42.29250866981882
|
||||
aF-83.71487081050874
|
||||
aI264
|
||||
aasS'Duderstadt'
|
||||
p5
|
||||
(lp6
|
||||
(lp7
|
||||
F42.29067138383511
|
||||
aF-83.7162870168686
|
||||
aI261
|
||||
aa(lp8
|
||||
F42.29158011297468
|
||||
aF-83.71514439582826
|
||||
aI263
|
||||
aasS'FXB'
|
||||
p9
|
||||
(lp10
|
||||
(lp11
|
||||
F42.29360387311647
|
||||
aF-83.71222615242006
|
||||
aI272
|
||||
aa(lp12
|
||||
F42.29359196883519
|
||||
aF-83.71161460876466
|
||||
aI274
|
||||
aasS'Pierpont'
|
||||
p13
|
||||
(lp14
|
||||
(lp15
|
||||
F42.291536462529756
|
||||
aF-83.71705412864686
|
||||
aI261
|
||||
aa(lp16
|
||||
F42.29065947899958
|
||||
aF-83.7178158760071
|
||||
aI258
|
||||
aasS'BBB'
|
||||
p17
|
||||
(lp18
|
||||
(lp19
|
||||
F42.292667396114446
|
||||
aF-83.71626019477846
|
||||
aI264
|
||||
aa(lp20
|
||||
F42.2933737232794
|
||||
aF-83.71622264385225
|
||||
aI271
|
||||
aas.
|
@@ -0,0 +1,5 @@
|
||||
EECS
|
||||
Duderstadt
|
||||
Pierpont
|
||||
BBB
|
||||
FXB
|
30
src/dataset/dataManipulation/pickles/pickleManage.py
Normal file
30
src/dataset/dataManipulation/pickles/pickleManage.py
Normal file
@@ -0,0 +1,30 @@
|
||||
import pickle
|
||||
|
||||
# file to print current pickle files to text file
|
||||
# this allows us to monitor current dictionaries
|
||||
def printPickle(filename):
|
||||
pickle_in = open(filename + '.pkl',"rb")
|
||||
currDict = pickle.load(pickle_in)
|
||||
f = open(filename + '.txt',"w")
|
||||
for x in currDict:
|
||||
f.write('%s\n' % x )
|
||||
f.close()
|
||||
|
||||
# update pickle files to update dictionaries
|
||||
# example: grades = {'Bart', 'Lisa', 'Milhouse', 'Nelson'}
|
||||
def createPickle(filename, pklList):
|
||||
f = open(filename + '.pkl', 'wb') # Pickle file is newly created where foo1.py is
|
||||
pickle.dump(pklList, f) # dump data to f
|
||||
f.close()
|
||||
|
||||
def updatePickle(filename, pklList):
|
||||
pickle_in = open(filename + '.pkl',"rb")
|
||||
currDict = pickle.load(pickle_in)
|
||||
f = open(filename + '.pkl', 'wb') # Pickle file is newly created where foo1.py is
|
||||
pickle.dump(currDict + pklList, f) # dump data to f
|
||||
f.close()
|
||||
|
||||
# Example usage
|
||||
# createPickle('test', {'Bart', 'Lisa', 'Milhouse', 'Nelson'})
|
||||
# updatePickle('test', {'Theo'})
|
||||
# printPickle("test")
|
Reference in New Issue
Block a user