Merge branch 'planning' into cleanup

src/Dockerfile

@@ -0,0 +1,20 @@
+FROM python:3
+
+RUN apt-get update && \
+    apt-get install -y \
+        build-essential \
+        python-opencv \
+        libpcl-dev
+
+RUN pip install -U pip && \
+    pip install -U \
+        cython \
+        numpy \
+        scipy \
+        matplotlib \
+        nltk \
+        setuptools \
+        pylint \
+        pickle-mixin
+
+CMD ["/bin/bash"]

src/dataset/LICENSE.md

@@ -0,0 +1,9 @@
+The NCLT Dataset is made available under the Open Database License [available here](https://opendatacommons.org/licenses/odbl/1.0/). Any rights in individual contents of the database are licensed under the Database Contents License [available here](https://opendatacommons.org/licenses/dbcl/1.0/).
+
+In short, this means that you are free to use this dataset, share, create derivative works, or adapt it, as long as you credit our work, offer any publically used adapted version of this dataset under the same license, and keep any redistribution of this dataset open.
+
+# Citation
+Nicholas Carlevaris-Bianco, Arash K. Ushani, and Ryan M. Eustice, University of Michigan North Campus Long-Term Vision and Lidar Dataset, International Journal of Robotics Research, 2016.
+
+# Website
+http://robots.engin.umich.edu/nclt/

src/dataset/ManageDataset/project_vel_to_cam.py

@@ -0,0 +1,166 @@
+# !/usr/bin/python
+#
+# Demonstrates how to project velodyne points to camera imagery. Requires a binary
+# velodyne sync file, undistorted image, and assumes that the calibration files are
+# in the directory.
+#
+# To use:
+#
+#    python project_vel_to_cam.py vel img cam_num
+#
+#       vel:  The velodyne binary file (timestamp.bin)
+#       img:  The undistorted image (timestamp.tiff)
+#   cam_num:  The index (0 through 5) of the camera
+#
+
+import sys
+import struct
+import matplotlib.pyplot as plt
+import matplotlib.image as mpimg
+import numpy as np
+
+#from undistort import *
+
+def convert(x_s, y_s, z_s):
+
+    scaling = 0.005 # 5 mm
+    offset = -100.0
+
+    x = x_s * scaling + offset
+    y = y_s * scaling + offset
+    z = z_s * scaling + offset
+
+    return x, y, z
+
+def load_vel_hits(filename):
+
+    f_bin = open(filename, "r")
+
+    hits = []
+
+    while True:
+
+        x_str = f_bin.read(2)
+        if x_str == '': # eof
+            break
+
+        x = struct.unpack('<H', x_str)[0]
+        y = struct.unpack('<H', f_bin.read(2))[0]
+        z = struct.unpack('<H', f_bin.read(2))[0]
+        i = struct.unpack('B', f_bin.read(1))[0]
+        l = struct.unpack('B', f_bin.read(1))[0]
+
+        x, y, z = convert(x, y, z)
+
+        # Load in homogenous
+        hits += [[x, y, z, 1]]
+
+    f_bin.close()
+    hits = np.asarray(hits)
+
+    return hits.transpose()
+
+def ssc_to_homo(ssc):
+
+    # Convert 6-DOF ssc coordinate transformation to 4x4 homogeneous matrix
+    # transformation
+
+    sr = np.sin(np.pi/180.0 * ssc[3])
+    cr = np.cos(np.pi/180.0 * ssc[3])
+
+    sp = np.sin(np.pi/180.0 * ssc[4])
+    cp = np.cos(np.pi/180.0 * ssc[4])
+
+    sh = np.sin(np.pi/180.0 * ssc[5])
+    ch = np.cos(np.pi/180.0 * ssc[5])
+
+    H = np.zeros((4, 4))
+
+    H[0, 0] = ch*cp
+    H[0, 1] = -sh*cr + ch*sp*sr
+    H[0, 2] = sh*sr + ch*sp*cr
+    H[1, 0] = sh*cp
+    H[1, 1] = ch*cr + sh*sp*sr
+    H[1, 2] = -ch*sr + sh*sp*cr
+    H[2, 0] = -sp
+    H[2, 1] = cp*sr
+    H[2, 2] = cp*cr
+
+    H[0, 3] = ssc[0]
+    H[1, 3] = ssc[1]
+    H[2, 3] = ssc[2]
+
+    H[3, 3] = 1
+
+    return H
+
+def project_vel_to_cam(hits, cam_num):
+
+    # Load camera parameters
+    K = np.loadtxt('K_cam%d.csv' % (cam_num), delimiter=',')
+    x_lb3_c = np.loadtxt('x_lb3_c%d.csv' % (cam_num), delimiter=',')
+
+    # Other coordinate transforms we need
+    x_body_lb3 = [0.035, 0.002, -1.23, -179.93, -0.23, 0.50]
+
+    # Now do the projection
+    T_lb3_c = ssc_to_homo(x_lb3_c)
+    T_body_lb3 = ssc_to_homo(x_body_lb3)
+
+    T_lb3_body = np.linalg.inv(T_body_lb3)
+    T_c_lb3 = np.linalg.inv(T_lb3_c)
+
+    T_c_body = np.matmul(T_c_lb3, T_lb3_body)
+
+    hits_c = np.matmul(T_c_body, hits)
+    hits_im = np.matmul(K, hits_c[0:3, :])
+
+    return hits_im
+
+def main(args):
+
+    if len(args)<4:
+        print  """Incorrect usage.
+
+To use:
+
+   python project_vel_to_cam.py vel img cam_num
+
+      vel:  The velodyne binary file (timestamp.bin)
+      img:  The undistorted image (timestamp.tiff)
+  cam_num:  The index (0 through 5) of the camera
+"""
+        return 1
+
+
+    # Load velodyne points
+    hits_body = load_vel_hits(args[1])
+
+    # Load image
+    image = mpimg.imread(args[2])
+
+    cam_num = int(args[3])
+
+    hits_image = project_vel_to_cam(hits_body, cam_num)
+
+    x_im = hits_image[0, :]/hits_image[2, :]
+    y_im = hits_image[1, :]/hits_image[2, :]
+    z_im = hits_image[2, :]
+
+    idx_infront = z_im>0
+    x_im = x_im[idx_infront]
+    y_im = y_im[idx_infront]
+    z_im = z_im[idx_infront]
+
+    plt.figure(1)
+    plt.imshow(image)
+    plt.hold(True)
+    plt.scatter(x_im, y_im, c=z_im, s=5, linewidths=0)
+    plt.xlim(0, 1616)
+    plt.ylim(0, 1232)
+    plt.show()
+
+    return 0
+
+if __name__ == '__main__':
+    sys.exit(main(sys.argv))

src/dataset/ManageDataset/read_gps.py

@@ -0,0 +1,55 @@
+# !/usr/bin/python
+#
+# Example code to read and plot the gps data.
+#
+# To call:
+#
+#   python read_gps.py gps.csv
+#
+
+import sys
+import matplotlib.pyplot as plt
+import numpy as np
+
+def main(args):
+
+    if len(sys.argv) < 2:
+        print('Please specify gps file')
+        return 1
+
+    gps = np.loadtxt(sys.argv[1], delimiter = ",")
+
+    num_sats = gps[:, 2]
+    lat = gps[:, 3]
+    lng = gps[:, 4]
+    alt = gps[:, 5]
+
+    lat0 = lat[0]
+    lng0 = lng[0]
+
+    dLat = lat - lat0
+    dLng = lng - lng0
+
+    r = 6400000 # approx. radius of earth (m)
+    x = r * np.cos(lat0) * np.sin(dLng)
+    y = r * np.sin(dLat)
+
+    plt.figure()
+    plt.subplot(1, 2, 1)
+    plt.scatter(x, y, 1, c=alt, linewidth=0)
+    plt.axis('equal')
+    plt.title('By altitude')
+    plt.colorbar()
+
+    plt.subplot(1, 2, 2)
+    plt.scatter(x, y, c=num_sats, linewidth=0)
+    plt.axis('equal')
+    plt.title('By number of satellites')
+    plt.colorbar()
+
+    plt.show()
+
+    return 0
+
+if __name__ == '__main__':
+    sys.exit(main(sys.argv))

src/dataset/ManageDataset/read_ground_truth.py

@@ -0,0 +1,61 @@
+# !/usr/bin/python
+#
+# Example code to read and plot the ground truth data.
+#
+# Note: The ground truth data is provided at a high rate of about 100 Hz. To
+# generate this high rate ground truth, a SLAM solution was used. Nodes in the
+# SLAM graph were not added at 100 Hz, but rather about every 8 meters. In
+# between the nodes in the SLAM graph, the odometry was used to interpolate and
+# provide a high rate ground truth. If precise pose is desired (e.g., for
+# accumulating point clouds), then we recommend using only the ground truth
+# poses that correspond to the nodes in the SLAM graph. This can be found by
+# inspecting the timestamps in the covariance file.
+#
+# To call:
+#
+#   python read_ground_truth.py groundtruth.csv covariance.csv
+#
+
+import sys
+import matplotlib.pyplot as plt
+import numpy as np
+import scipy.interpolate
+
+def main(args):
+
+    if len(sys.argv) < 3:
+        print('Please specify ground truth and covariance files')
+        return 1
+
+    gt = np.loadtxt(sys.argv[1], delimiter = ",")
+    cov = np.loadtxt(sys.argv[2], delimiter = ",")
+
+    t_cov = cov[:, 0]
+
+    # Note: Interpolation is not needed, this is done as a convience
+    interp = scipy.interpolate.interp1d(gt[:, 0], gt[:, 1:], kind='nearest', axis=0, fill_value="extrapolate")
+    pose_gt = interp(t_cov)
+
+    # NED (North, East Down)
+    x = pose_gt[:, 0]
+    y = pose_gt[:, 1]
+    z = pose_gt[:, 2]
+
+    r = pose_gt[:, 3]
+    p = pose_gt[:, 4]
+    h = pose_gt[:, 5]
+
+    plt.figure()
+    plt.scatter(y, x, 1, c=-z, linewidth=0)    # Note Z points down
+    plt.axis('equal')
+    plt.title('Ground Truth Position of Nodes in SLAM Graph')
+    plt.xlabel('East (m)')
+    plt.ylabel('North (m)')
+    plt.colorbar()
+
+    plt.show()
+
+    return 0
+
+if __name__ == '__main__':
+    sys.exit(main(sys.argv))

src/dataset/ManageDataset/read_hokuyo_30m.py

@@ -0,0 +1,72 @@
+# !/usr/bin/python
+#
+# Example code to go through the hokuyo_30m.bin file, read timestamps and the hits
+# in each packet, and plot them.
+#
+# To call:
+#
+#   python read_hokuyo_30m.py hokuyo_30m.bin
+#
+
+import sys
+import struct
+import numpy as np
+import matplotlib.pyplot as plt
+
+def convert(x_s):
+
+    scaling = 0.005 # 5 mm
+    offset = -100.0
+
+    x = x_s * scaling + offset
+
+    return x
+
+def main(args):
+
+    if len(sys.argv) < 2:
+        print("Please specifiy input bin file")
+        return 1
+
+    # hokuyo_30m always has 1081 hits
+    num_hits = 1081
+
+    # angles for each range observation
+    rad0 = -135 * (np.pi/180.0)
+    radstep = 0.25 * (np.pi/180.0)
+    angles = np.linspace(rad0, rad0 + (num_hits-1)*radstep, num_hits)
+
+    f_bin = open(sys.argv[1], "r")
+
+    plt.ion()
+
+    while True:
+
+        # Read timestamp
+        utime = struct.unpack('<Q', f_bin.read(8))[0]
+
+        print('Timestamp', utime)
+
+        r = np.zeros(num_hits)
+
+        for i in range(num_hits):
+
+            s = struct.unpack('<H', f_bin.read(2))[0]
+            r[i] = convert(s)
+
+            #print s
+
+        x = r * np.cos(angles)
+        y = r * np.sin(angles)
+
+        plt.clf()
+        plt.plot(x, y, '.')
+        plt.title(utime)
+        plt.draw()
+
+    f_bin.close()
+
+    return 0
+
+if __name__ == '__main__':
+    sys.exit(main(sys.argv))

src/dataset/ManageDataset/read_hokuyo_4m.py

@@ -0,0 +1,72 @@
+# !/usr/bin/python
+#
+# Example code to go through the hokuyo_4m.bin file, read timestamps and the hits
+# in each packet, and plot them.
+#
+# To call:
+#
+#   python read_hokuyo_4m.py hokuyo_4m.bin
+#
+
+import sys
+import struct
+import numpy as np
+import matplotlib.pyplot as plt
+
+def convert(x_s):
+
+    scaling = 0.005 # 5 mm
+    offset = -100.0
+
+    x = x_s * scaling + offset
+
+    return x
+
+def main(args):
+
+    if len(sys.argv) < 2:
+        print("Please specifiy input bin file")
+        return 1
+
+    # hokuyo_4 always has 726 hits
+    num_hits = 726
+
+    # angles for each range observation
+    rad0 = -2.0862138
+    radstep = 0.0061359233
+    angles = np.linspace(rad0, rad0 + (num_hits-1)*radstep, num_hits)
+
+    f_bin = open(sys.argv[1], "r")
+
+    plt.ion()
+
+    while True:
+
+        # Read timestamp
+        utime = struct.unpack('<Q', f_bin.read(8))[0]
+
+        print('Timestamp', utime)
+
+        r = np.zeros(num_hits)
+
+        for i in range(num_hits):
+
+            s = struct.unpack('<H', f_bin.read(2))[0]
+            r[i] = convert(s)
+
+            #print s
+
+        x = r * np.cos(angles)
+        y = r * np.sin(angles)
+
+        plt.clf()
+        plt.plot(x, y, '.')
+        plt.title(utime)
+        plt.draw()
+
+    f_bin.close()
+
+    return 0
+
+if __name__ == '__main__':
+    sys.exit(main(sys.argv))

src/dataset/ManageDataset/read_ms25.py

@@ -0,0 +1,71 @@
+# !/usr/bin/python
+#
+# Example code to read and plot the microstrain data.
+#
+# To call:
+#
+#   python read_ms25.py ms25.csv
+#
+
+import sys
+import matplotlib.pyplot as plt
+import numpy as np
+
+def main(args):
+
+    if len(sys.argv) < 2:
+        print('Please specify microstrain file')
+        return 1
+
+    ms25 = np.loadtxt(sys.argv[1], delimiter = ",")
+
+    t = ms25[:, 0]
+
+    mag_x = ms25[:, 1]
+    mag_y = ms25[:, 2]
+    mag_z = ms25[:, 3]
+
+    accel_x = ms25[:, 4]
+    accel_y = ms25[:, 5]
+    accel_z = ms25[:, 6]
+
+    rot_r = ms25[:, 7]
+    rot_p = ms25[:, 8]
+    rot_h = ms25[:, 9]
+
+    plt.figure()
+
+    plt.subplot(1, 3, 1)
+    plt.plot(t, mag_x, 'r')
+    plt.plot(t, mag_y, 'g')
+    plt.plot(t, mag_z, 'b')
+    plt.legend(['X', 'Y', 'Z'])
+    plt.title('Magnetic Field')
+    plt.xlabel('utime (us)')
+    plt.ylabel('Gauss')
+
+
+    plt.subplot(1, 3, 2)
+    plt.plot(t, accel_x, 'r')
+    plt.plot(t, accel_y, 'g')
+    plt.plot(t, accel_z, 'b')
+    plt.legend(['X', 'Y', 'Z'])
+    plt.title('Acceleration')
+    plt.xlabel('utime (us)')
+    plt.ylabel('m/s^2')
+
+    plt.subplot(1, 3, 3)
+    plt.plot(t, rot_r, 'r')
+    plt.plot(t, rot_p, 'g')
+    plt.plot(t, rot_h, 'b')
+    plt.legend(['r', 'p', 'h'])
+    plt.title('Angular Rotation Rate')
+    plt.xlabel('utime (us)')
+    plt.ylabel('rad/s')
+
+    plt.show()
+
+    return 0
+
+if __name__ == '__main__':
+    sys.exit(main(sys.argv))

src/dataset/ManageDataset/read_ms25_euler.py

@@ -0,0 +1,43 @@
+# !/usr/bin/python
+#
+# Example code to read and plot the microstrain euler angles data.
+#
+# To call:
+#
+#   python read_ms25_euler.py ms25_euler.csv
+#
+
+import sys
+import matplotlib.pyplot as plt
+import numpy as np
+
+def main(args):
+
+    if len(sys.argv) < 2:
+        print('Please specify microstrain file')
+        return 1
+
+    euler = np.loadtxt(sys.argv[1], delimiter = ",")
+
+    t = euler[:, 0]
+
+    r = euler[:, 1]
+    p = euler[:, 2]
+    h = euler[:, 3]
+
+    plt.figure()
+
+    plt.plot(t, r, 'r')
+    plt.plot(t, p, 'g')
+    plt.plot(t, h, 'b')
+    plt.legend(['r', 'p', 'h'])
+    plt.title('Euler Angles')
+    plt.xlabel('utime (us)')
+    plt.ylabel('rad')
+
+    plt.show()
+
+    return 0
+
+if __name__ == '__main__':
+    sys.exit(main(sys.argv))

src/dataset/ManageDataset/read_odom.py

@@ -0,0 +1,50 @@
+# !/usr/bin/python
+#
+# Example code to read and plot the odometry data.
+#
+# To call:
+#
+#   python read_odom.py odometry.csv
+#
+
+import sys
+import matplotlib.pyplot as plt
+import numpy as np
+
+def main(args):
+
+    if len(sys.argv) < 2:
+        print('Please specify odometry file')
+        return 1
+
+    odom = np.loadtxt(sys.argv[1], delimiter = ",")
+
+    t = odom[:, 0]
+
+    x = odom[:, 1]
+    y = odom[:, 2]
+    z = odom[:, 3]
+
+    r = odom[:, 4]
+    p = odom[:, 5]
+    h = odom[:, 6]
+
+    plt.figure()
+    plt.subplot(1, 2, 1)
+    plt.scatter(x, y, 1, c=z, linewidth=0)
+    plt.axis('equal')
+    plt.title('Odometry position')
+    plt.colorbar()
+
+    plt.subplot(1, 2, 2)
+    plt.plot(t, r, 'r')
+    plt.plot(t, p, 'g')
+    plt.plot(t, h, 'b')
+    plt.legend(['Roll', 'Pitch', 'Heading'])
+    plt.title('Odometry rph')
+    plt.show()
+
+    return 0
+
+if __name__ == '__main__':
+    sys.exit(main(sys.argv))

src/dataset/ManageDataset/undistort.py

@@ -0,0 +1,88 @@
+"""
+Demonstrating how to undistort images.
+
+Reads in the given calibration file, parses it, and uses it to undistort the given
+image. Then display both the original and undistorted images.
+
+To use:
+
+    python undistort.py image calibration_file
+"""
+
+import numpy as np
+import cv2
+import matplotlib.pyplot as plt
+import argparse
+import re
+
+class Undistort(object):
+
+    def __init__(self, fin, scale=1.0, fmask=None):
+        self.fin = fin
+        # read in distort
+        with open(fin, 'r') as f:
+            #chunks = f.readline().rstrip().split(' ')
+            header = f.readline().rstrip()
+            chunks = re.sub(r'[^0-9,]', '', header).split(',')
+            self.mapu = np.zeros((int(chunks[1]),int(chunks[0])),
+                    dtype=np.float32)
+            self.mapv = np.zeros((int(chunks[1]),int(chunks[0])),
+                    dtype=np.float32)
+            for line in f.readlines():
+                chunks = line.rstrip().split(' ')
+                self.mapu[int(chunks[0]),int(chunks[1])] = float(chunks[3])
+                self.mapv[int(chunks[0]),int(chunks[1])] = float(chunks[2])
+        # generate a mask
+        self.mask = np.ones(self.mapu.shape, dtype=np.uint8)
+        self.mask = cv2.remap(self.mask, self.mapu, self.mapv, cv2.INTER_LINEAR)
+        kernel = np.ones((30,30),np.uint8)
+        self.mask = cv2.erode(self.mask, kernel, iterations=1)
+
+    """
+    Optionally, define a mask
+    """
+    def set_mask(fmask):
+        # add in the additional mask passed in as fmask
+        if fmask:
+            mask = cv2.cvtColor(cv2.imread(fmask), cv2.COLOR_BGR2GRAY)
+            self.mask = self.mask & mask
+        new_shape = (int(self.mask.shape[1]*scale), int(self.mask.shape[0]*scale))
+        self.mask = cv2.resize(self.mask, new_shape,
+                               interpolation=cv2.INTER_CUBIC)
+        #plt.figure(1)
+        #plt.imshow(self.mask, cmap='gray')
+        #plt.show()
+
+    """
+    Use OpenCV to undistorted the given image
+    """
+    def undistort(self, img):
+        return cv2.resize(cv2.remap(img, self.mapu, self.mapv, cv2.INTER_LINEAR),
+                          (self.mask.shape[1], self.mask.shape[0]),
+                          interpolation=cv2.INTER_CUBIC)
+
+def main():
+    parser = argparse.ArgumentParser(description="Undistort images")
+    parser.add_argument('image', metavar='img', type=str, help='image to undistort')
+    parser.add_argument('map', metavar='map', type=str, help='undistortion map')
+
+    args = parser.parse_args()
+
+    undistort = Undistort(args.map)
+    print('Loaded camera calibration')
+
+    im = cv2.imread(args.image)
+
+    cv2.namedWindow('Image', cv2.WINDOW_NORMAL)
+    cv2.imshow('Image', im)
+
+    im_undistorted = undistort.undistort(im)
+
+    cv2.namedWindow('Undistorted Image', cv2.WINDOW_NORMAL)
+    cv2.imshow('Undistorted Image', im_undistorted)
+
+    cv2.waitKey(0)
+    cv2.destroyAllWindows()
+
+if __name__ == "__main__":
+    main()

src/dataset/data/downloader.py

@@ -9,7 +9,7 @@ import argparse
 
 base_dir = 'http://robots.engin.umich.edu/nclt'
 
-dates = [];
+dates = []
 dates.append('2012-01-08')
 dates.append('2012-01-15')
 dates.append('2012-01-22')

src/dataset/dataManipulation/GPSmanip.py

@@ -0,0 +1,25 @@
+import sys
+import numpy as np
+
+# Usage: changes lat, long, alt into xyz local frame
+#           This takes in lat, long, and alt and calculates xyz
+# Example: print(gpstoLocalFrame(42.29360387311647,-83.71222615242006,272))
+# Returns: array of XYZ coordinates in radians
+def gpstoLocalFrame(lat, lng, alt):
+    lat0 = 0.7381566413
+    lng0 = -1.4610097151
+    alt0 = 265.8
+
+    print(np.deg2rad(lng))
+    dLat = np.deg2rad(lat) - lat0
+    print(dLat)
+    dLng = np.deg2rad(lng) - lng0
+    dAlt = alt - alt0
+
+    r = 6400000 # approx. radius of earth (m)
+    # WARNING: x and y may need to be flipped. Paper and example files from NCLT have contradictory usages
+    y = r * np.cos(lat0) * np.sin(dLng)
+    x = r * np.sin(dLat)
+    z = dAlt
+
+    return [x,y,z]

src/dataset/dataManipulation/findBuildingCoord.py

@@ -0,0 +1,79 @@
+import sys
+import numpy as np
+import pickle
+import math  
+
+# Example usage of overall file to find xyz coordinates of two buildings given name as string
+'''
+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)
+'''
+
+# Usage: finds building coordinates in lat, long, and alt in degrees
+#           This takes in two building names as strings and returns 
+#           closest two entrances 
+# Example: GPScoords = findClosestEntrance("BBB", "EECS")
+#           print(GPScoords[0][0]) #returns latitutde of first building
+# Returns: 2x2 array of two GPS coordinates in lat, long, and alt in degrees
+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]]
+
+# Usage: finds building coordinates in lat, long, and alt in degrees
+#           This takes in a building name and looks up coordinates in pickle file
+# Example: buildingsGPScoords = buildingtoGPS(building1)
+#           print(buildingGPScoords[0]) #returns latitutde of the building
+# Returns: array of GPS coordinates (lat, long, and alt) in degrees
+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
+
+# Usage: changes lat, long, alt into xyz local frame
+#           This takes in lat, long, and alt and calculates xyz
+# Example: print(gpstoLocalFrame(42.29360387311647,-83.71222615242006,272))
+# Returns: array of XYZ coordinates in radians
+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)
+    # WARNING: x and y may need to be flipped. Paper and example files from NCLT have contradictory usages
+    y = r * np.cos(lat0) * np.sin(dLng) 
+    x = r * np.sin(dLat)
+    z = dAlt
+
+    return [x,y,z]
+
+# Usage: Euclidean distance calculator - helper function
+def calculateDistance(x1,y1,x2,y2):  
+     dist = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)  
+     return dist  
+
+     

src/dataset/dataManipulation/pickles/BuildingMappings.pkl

@@ -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.

src/dataset/dataManipulation/pickles/BuildingMappings.txt

@@ -0,0 +1,5 @@
+EECS
+Duderstadt
+Pierpont
+BBB
+FXB

src/dataset/dataManipulation/pickles/pickleManage.py

@@ -0,0 +1,32 @@
+import pickle
+
+# Usage: file to print current pickle files to text file
+#           this allows us to monitor current dictionaries
+# Example: printPickle("BuildingMappings")
+# Output: Text file of dictonary keys
+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()
+
+# Usage: creates pickle files from given dictionaries
+# Example: createPickle('test', {'Bart', 'Lisa', 'Milhouse', 'Nelson'})
+# Output: new Pickle file
+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()
+
+# Usage: updates pickle files from given dictionaries
+# Example: updatePickle('test', {'Bart', 'Lisa', 'Milhouse', 'Nelson'})
+# Output: Pickle file
+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()
+

src/dataset/dataManipulation/pickles/referenceBuildings4Pickle.txt

@@ -0,0 +1,29 @@
+Reference file for coordinates found for google maps
+
+(latitude, longitude, altitude)
+FXB 
+	Beal Ave -  42.29360387311647 -83.71222615242006 272 
+	Hayward -   42.29359196883519 -83.71161460876466 274
+	
+EECS
+	Beal Ave -  42.29259200117389 -83.71376574039459 266 
+	Grove - 42.29250866981882 -83.71487081050874 264 
+
+BBB
+	Grove - 42.292667396114446 -83.71626019477846 264
+	Hayward - 42.2933737232794 -83.71622264385225 271
+	
+Pierpont
+	Grove - 42.291536462529756 -83.71705412864686 261
+	Bonisteel - 42.29065947899958  -83.7178158760071 258
+	
+Duderstadt
+	Bonisteel - 42.29067138383511 -83.7162870168686 261
+	Grover - 42.29158011297468 -83.71514439582826 263
+
+Example for making and changing buildings from pickle file
+
+from pickleManage import *
+createPickle("BuildingMappings", {'Duderstadt':[[42.29067138383511, -83.7162870168686, 261],[42.29158011297468, -83.71514439582826, 263]], 'Pierpont':[[42.291536462529756, -83.71705412864686, 261],[42.29065947899958,  -83.7178158760071, 258]], 'BBB':[[42.292667396114446, -83.71626019477846, 264],[42.2933737232794, -83.71622264385225, 271]], 'EECS':[[42.29259200117389, -83.71376574039459, 266],[42.29250866981882, -83.71487081050874, 264]],'FXB':[[42.29360387311647, -83.71222615242006, 272],[42.29359196883519, -83.71161460876466, 274]]})
+updatePickle("BuildingMappings", {'Duderstadt':[[42.29067138383511, -83.7162870168686, 261],[42.29158011297468, -83.71514439582826, 263]]})
+printPickle("BuildingMappings")

src/planning/Astar.py

@@ -0,0 +1,70 @@
+import PriorityQueue as pq
+import numpy as np
+import scipy.interpolate
+
+class Astar :
+  # This class implements A* search along a network defined by several points
+  # Poses is an array of coordinates
+  # k defines how many nearest neighbors to look at during A* search
+  # The primary usage of this class is the find_path function:
+  #   Required parameters:
+  #     start_idx:  
+  #     goal_idx    
+
+  def __init__(self, poses, k=20) :
+    self.poses = poses
+    self.k = k
+
+  def _extract_path(self, cur_node, parent_idx, start_idx):
+    next_idx = cur_node
+    path = [next_idx]
+
+    while next_idx != start_idx:
+      next_idx = parent_idx[next_idx]
+      path.append(next_idx)
+
+    return path[::-1]
+
+  def find_path(self, start_idx, goal_idx):
+    visit_queue = pq.PriorityQueue()
+    visited_flag, queueed_flag = np.zeros(self.poses.shape[0]), np.zeros(self.poses.shape[0])
+    g_score, h_score = np.full(self.poses.shape[0], np.inf), np.full(self.poses.shape[0], np.inf)
+    parent_idx = np.zeros(self.poses.shape[0], dtype='int')
+
+    test_tree = scipy.spatial.KDTree(self.poses)
+
+    # initialize
+    goal = self.poses[goal_idx]
+    g_score[start_idx] = 0
+    visit_queue.put(start_idx, np.inf)
+    queueed_flag[start_idx] = 1
+    optimal = False
+
+    while not visit_queue.empty():
+      cur_node = visit_queue.get()
+      visited_flag[cur_node] = 1
+
+      if cur_node == goal_idx:
+        optimal = True
+        break
+
+      # find neighbours
+      neighbors = test_tree.query(self.poses[cur_node], k=self.k)
+
+      for nb_cur_dist, nb_idx in zip(neighbors[0][1:], neighbors[1][1:]):
+        if visited_flag[nb_idx] == 1:
+          continue
+
+        temp_dist = g_score[cur_node] + np.linalg.norm(self.poses[cur_node] - self.poses[nb_idx])
+        # temp_dist = g_score[cur_node] + nb_cur_dist     ## this not work
+        if g_score[nb_idx] > temp_dist:
+          g_score[nb_idx] = temp_dist
+          parent_idx[nb_idx] = cur_node
+          f_score = g_score[nb_idx] + np.linalg.norm(self.poses[nb_idx] - goal)
+        
+        # put into queen
+        if queueed_flag[nb_idx] == 0:
+          visit_queue.put(nb_idx, f_score)
+          queueed_flag[nb_idx] = 1
+
+    return self_.extract_path(cur_node, parent_idx, start_idx), optimal

src/planning/PriorityQueue.py

@@ -0,0 +1,15 @@
+import heapq #https://docs.python.org/3/library/heapq.html
+
+class PriorityQueue:
+    def __init__(self):
+        self.elements = []
+    
+    def empty(self):
+        return len(self.elements) == 0
+    
+    def put(self, item, priority):
+        heapq.heappush(self.elements, (priority, item))
+    
+    def get(self):
+        return heapq.heappop(self.elements)[1]
+

src/planning/astar_driver.py

@@ -0,0 +1,41 @@
+# -*- coding: utf-8 -*-
+"""kdtree_testing
+
+Automatically generated by Colaboratory.
+
+Original file is located at
+    https://colab.research.google.com/drive/15-biioPffqZoK2zW9Fvi9t53y0o14XbZ
+"""
+
+import sys
+import matplotlib.pyplot as plt
+import numpy as np
+import math
+import random
+import time
+import pdb
+
+import Astar
+
+def load_poses(pose_gt_file) :
+    pose_gt = np.loadtxt(pose_gt_file, delimiter = ",")
+    return pose_gt
+
+
+def main() :
+  #poses = load_poses('../dataset/ground_truth/groundtruth_2012-01-08.csv')
+  poses = load_poses('../dataset/ground_truth/debug.csv')
+  sparseness = 10
+  k=50
+
+  sparse_poses = poses[1::sparseness, 1:3]
+  astar = Astar.Astar(poses=sparse_poses, k=k)
+  
+  start_idx = np.random.randint(sparse_poses.shape[0])
+  goal_idx = np.random.randint(sparse_poses.shape[0])
+  path, optimal = astar.find_path(start_idx, goal_idx)
+
+  np.save('optimal_path', path)
+
+if __name__ == '__main__' :
+  main()