From 340b8744a3496808303c0ceb1d7ed828c22d75b0 Mon Sep 17 00:00:00 2001
From: snbenge <snbenge@umich.edu>
Date: Mon, 20 Apr 2020 14:57:55 -0400
Subject: [PATCH 1/9] adding files from dataset library including ways to
 manage the data itself. This includes citations and licensing on the datset.

---
 src/dataset/LICENSE.md                        |   9 +
 .../ManageDataset/project_vel_to_cam.py       | 166 ++++++++++++++++++
 src/dataset/ManageDataset/read_gps.py         |  55 ++++++
 .../ManageDataset/read_ground_truth.py        |  61 +++++++
 src/dataset/ManageDataset/read_hokuyo_30m.py  |  72 ++++++++
 src/dataset/ManageDataset/read_hokuyo_4m.py   |  72 ++++++++
 src/dataset/ManageDataset/read_ms25.py        |  71 ++++++++
 src/dataset/ManageDataset/read_ms25_euler.py  |  43 +++++
 src/dataset/ManageDataset/read_odom.py        |  50 ++++++
 src/dataset/ManageDataset/undistort.py        |  88 ++++++++++
 src/dataset/downloader.py                     |   2 +-
 11 files changed, 688 insertions(+), 1 deletion(-)
 create mode 100644 src/dataset/LICENSE.md
 create mode 100644 src/dataset/ManageDataset/project_vel_to_cam.py
 create mode 100644 src/dataset/ManageDataset/read_gps.py
 create mode 100644 src/dataset/ManageDataset/read_ground_truth.py
 create mode 100644 src/dataset/ManageDataset/read_hokuyo_30m.py
 create mode 100644 src/dataset/ManageDataset/read_hokuyo_4m.py
 create mode 100644 src/dataset/ManageDataset/read_ms25.py
 create mode 100644 src/dataset/ManageDataset/read_ms25_euler.py
 create mode 100644 src/dataset/ManageDataset/read_odom.py
 create mode 100644 src/dataset/ManageDataset/undistort.py

diff --git a/src/dataset/LICENSE.md b/src/dataset/LICENSE.md
new file mode 100644
index 0000000..39a0f1f
--- /dev/null
+++ b/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/
\ No newline at end of file
diff --git a/src/dataset/ManageDataset/project_vel_to_cam.py b/src/dataset/ManageDataset/project_vel_to_cam.py
new file mode 100644
index 0000000..0356d4e
--- /dev/null
+++ b/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))
diff --git a/src/dataset/ManageDataset/read_gps.py b/src/dataset/ManageDataset/read_gps.py
new file mode 100644
index 0000000..169ab79
--- /dev/null
+++ b/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))
diff --git a/src/dataset/ManageDataset/read_ground_truth.py b/src/dataset/ManageDataset/read_ground_truth.py
new file mode 100644
index 0000000..eaac65c
--- /dev/null
+++ b/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))
diff --git a/src/dataset/ManageDataset/read_hokuyo_30m.py b/src/dataset/ManageDataset/read_hokuyo_30m.py
new file mode 100644
index 0000000..04bf11b
--- /dev/null
+++ b/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))
diff --git a/src/dataset/ManageDataset/read_hokuyo_4m.py b/src/dataset/ManageDataset/read_hokuyo_4m.py
new file mode 100644
index 0000000..1fcfff6
--- /dev/null
+++ b/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))
diff --git a/src/dataset/ManageDataset/read_ms25.py b/src/dataset/ManageDataset/read_ms25.py
new file mode 100644
index 0000000..6eb66b0
--- /dev/null
+++ b/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))
diff --git a/src/dataset/ManageDataset/read_ms25_euler.py b/src/dataset/ManageDataset/read_ms25_euler.py
new file mode 100644
index 0000000..bb76c5c
--- /dev/null
+++ b/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))
diff --git a/src/dataset/ManageDataset/read_odom.py b/src/dataset/ManageDataset/read_odom.py
new file mode 100644
index 0000000..cb22559
--- /dev/null
+++ b/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))
diff --git a/src/dataset/ManageDataset/undistort.py b/src/dataset/ManageDataset/undistort.py
new file mode 100644
index 0000000..b795e8d
--- /dev/null
+++ b/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()
diff --git a/src/dataset/downloader.py b/src/dataset/downloader.py
index fa89da1..147818d 100644
--- a/src/dataset/downloader.py
+++ b/src/dataset/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')

From 6b483c793b0cc2fbc6fa9be1773a2a2ecbb22001 Mon Sep 17 00:00:00 2001
From: snbenge <snbenge@umich.edu>
Date: Tue, 21 Apr 2020 22:37:43 -0400
Subject: [PATCH 2/9] 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

---
 .../dataManipulation/findBuildingCoord.py     | 65 +++++++++++++++++++
 .../pickles/BuildingMappings.pkl              | 57 ++++++++++++++++
 .../pickles/BuildingMappings.txt              |  5 ++
 .../dataManipulation/pickles/pickleManage.py  | 30 +++++++++
 4 files changed, 157 insertions(+)
 create mode 100644 src/dataset/dataManipulation/findBuildingCoord.py
 create mode 100644 src/dataset/dataManipulation/pickles/BuildingMappings.pkl
 create mode 100644 src/dataset/dataManipulation/pickles/BuildingMappings.txt
 create mode 100644 src/dataset/dataManipulation/pickles/pickleManage.py

diff --git a/src/dataset/dataManipulation/findBuildingCoord.py b/src/dataset/dataManipulation/findBuildingCoord.py
new file mode 100644
index 0000000..4134ffc
--- /dev/null
+++ b/src/dataset/dataManipulation/findBuildingCoord.py
@@ -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)
+'''
\ No newline at end of file
diff --git a/src/dataset/dataManipulation/pickles/BuildingMappings.pkl b/src/dataset/dataManipulation/pickles/BuildingMappings.pkl
new file mode 100644
index 0000000..501a791
--- /dev/null
+++ b/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.
\ No newline at end of file
diff --git a/src/dataset/dataManipulation/pickles/BuildingMappings.txt b/src/dataset/dataManipulation/pickles/BuildingMappings.txt
new file mode 100644
index 0000000..f5ca3fa
--- /dev/null
+++ b/src/dataset/dataManipulation/pickles/BuildingMappings.txt
@@ -0,0 +1,5 @@
+EECS
+Duderstadt
+Pierpont
+BBB
+FXB
diff --git a/src/dataset/dataManipulation/pickles/pickleManage.py b/src/dataset/dataManipulation/pickles/pickleManage.py
new file mode 100644
index 0000000..17f0a6f
--- /dev/null
+++ b/src/dataset/dataManipulation/pickles/pickleManage.py
@@ -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")

From 2ca6881cea236bbcbb74bb9a78f00c214eb7bc9f Mon Sep 17 00:00:00 2001
From: snbenge <snbenge@umich.edu>
Date: Tue, 21 Apr 2020 22:51:03 -0400
Subject: [PATCH 3/9] adding file to change standard lat long alt to xyz coords

---
 src/dataset/dataManipulation/GPSmanip.py | 24 ++++++++++++++++++++++++
 1 file changed, 24 insertions(+)
 create mode 100644 src/dataset/dataManipulation/GPSmanip.py

diff --git a/src/dataset/dataManipulation/GPSmanip.py b/src/dataset/dataManipulation/GPSmanip.py
new file mode 100644
index 0000000..7ee0f2f
--- /dev/null
+++ b/src/dataset/dataManipulation/GPSmanip.py
@@ -0,0 +1,24 @@
+import sys
+import numpy as np
+
+
+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)
+    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)

From 6bc969822bc91afd2ce5172e2ada47a0bfdf6656 Mon Sep 17 00:00:00 2001
From: snbenge <snbenge@umich.edu>
Date: Wed, 22 Apr 2020 11:08:39 -0400
Subject: [PATCH 4/9] added usage documentation in each file for clarity

---
 src/dataset/dataManipulation/GPSmanip.py      |  9 +-
 .../dataManipulation/findBuildingCoord.py     | 86 +++++++++++--------
 .../dataManipulation/pickles/pickleManage.py  | 18 ++--
 .../pickles/referenceBuildings4Pickle.txt     | 29 +++++++
 4 files changed, 94 insertions(+), 48 deletions(-)
 create mode 100644 src/dataset/dataManipulation/pickles/referenceBuildings4Pickle.txt

diff --git a/src/dataset/dataManipulation/GPSmanip.py b/src/dataset/dataManipulation/GPSmanip.py
index 7ee0f2f..0ed691a 100644
--- a/src/dataset/dataManipulation/GPSmanip.py
+++ b/src/dataset/dataManipulation/GPSmanip.py
@@ -1,7 +1,10 @@
 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
@@ -14,11 +17,9 @@ def gpstoLocalFrame(lat, lng, alt):
     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]
-#Example
-# x = gpstoLocalFrame(42.29360387311647,-83.71222615242006,272)
-# print(x)
diff --git a/src/dataset/dataManipulation/findBuildingCoord.py b/src/dataset/dataManipulation/findBuildingCoord.py
index 4134ffc..a2c3e11 100644
--- a/src/dataset/dataManipulation/findBuildingCoord.py
+++ b/src/dataset/dataManipulation/findBuildingCoord.py
@@ -3,33 +3,21 @@ 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
+# 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)
@@ -48,18 +36,44 @@ def findClosestEntrance(building1, building2):
         return [gps1[1],gps2[0]]
     else:
         return [gps1[1],gps2[1]]
-# Example
-# print(findClosestEntrance("BBB", "EECS"))
 
+# 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  
 
-# 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)
-'''
\ No newline at end of file
+     
\ No newline at end of file
diff --git a/src/dataset/dataManipulation/pickles/pickleManage.py b/src/dataset/dataManipulation/pickles/pickleManage.py
index 17f0a6f..fe93892 100644
--- a/src/dataset/dataManipulation/pickles/pickleManage.py
+++ b/src/dataset/dataManipulation/pickles/pickleManage.py
@@ -1,7 +1,9 @@
 import pickle
 
-# file to print current pickle files to text file
-# this allows us to monitor current dictionaries
+# 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)
@@ -10,13 +12,17 @@ def printPickle(filename):
         f.write('%s\n' % x )
     f.close()
 
-# update pickle files to update dictionaries
-# example: grades = {'Bart', 'Lisa', 'Milhouse', 'Nelson'}
+# 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)
@@ -24,7 +30,3 @@ def updatePickle(filename, pklList):
     pickle.dump(currDict + pklList, f)          # dump data to f
     f.close()
 
-# Example usage
-#   createPickle('test', {'Bart', 'Lisa', 'Milhouse', 'Nelson'})
-#   updatePickle('test', {'Theo'})
-#   printPickle("test")
diff --git a/src/dataset/dataManipulation/pickles/referenceBuildings4Pickle.txt b/src/dataset/dataManipulation/pickles/referenceBuildings4Pickle.txt
new file mode 100644
index 0000000..07166b1
--- /dev/null
+++ b/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")

From 51a2ff758f910921d5dd6ecc6c5a77faa3a47b8c Mon Sep 17 00:00:00 2001
From: Sabrina <snbenge@umich.edu>
Date: Wed, 22 Apr 2020 11:22:32 -0400
Subject: [PATCH 5/9] Needed dependency for pickle things

---
 src/Dockerfile | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/Dockerfile b/src/Dockerfile
index 1139682..58b7ac4 100644
--- a/src/Dockerfile
+++ b/src/Dockerfile
@@ -14,6 +14,7 @@ RUN pip install -U pip && \
         matplotlib \
         nltk \
         setuptools \
-        pylint
+        pylint \
+        pickle-mixin
 
 CMD ["/bin/bash"]

From 84a7b0ad0d5574030cc0320cbec709708977b995 Mon Sep 17 00:00:00 2001
From: dbrisbin <dbrisbin@umich.edu>
Date: Sat, 25 Apr 2020 13:24:44 -0400
Subject: [PATCH 6/9] initial commit of astar

---
 src/planning/astar.py | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 create mode 100644 src/planning/astar.py

diff --git a/src/planning/astar.py b/src/planning/astar.py
new file mode 100644
index 0000000..e69de29

From 11d481fd0ecd9f6f9224c43ec451844c55602802 Mon Sep 17 00:00:00 2001
From: dbrisbin <dbrisbin@umich.edu>
Date: Sat, 25 Apr 2020 15:49:49 -0400
Subject: [PATCH 7/9] Priority queue class and Astar usage template

---
 src/planning/PriorityQueue.py | 15 +++++++++++++
 src/planning/astar_driver.py  | 41 +++++++++++++++++++++++++++++++++++
 2 files changed, 56 insertions(+)
 create mode 100644 src/planning/PriorityQueue.py
 create mode 100644 src/planning/astar_driver.py

diff --git a/src/planning/PriorityQueue.py b/src/planning/PriorityQueue.py
new file mode 100644
index 0000000..f492273
--- /dev/null
+++ b/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]
+
diff --git a/src/planning/astar_driver.py b/src/planning/astar_driver.py
new file mode 100644
index 0000000..c7f1553
--- /dev/null
+++ b/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()

From b774ffa202e806b007feb41773523489331ff359 Mon Sep 17 00:00:00 2001
From: dbrisbin <dbrisbin@umich.edu>
Date: Sat, 25 Apr 2020 15:53:02 -0400
Subject: [PATCH 8/9] capitalization issue?

---
 src/planning/astar.py | 70 +++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 70 insertions(+)

diff --git a/src/planning/astar.py b/src/planning/astar.py
index e69de29..dec0f74 100644
--- a/src/planning/astar.py
+++ b/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
\ No newline at end of file

From 685bf1f5d7ad690a37046b68119ab5596b0517b1 Mon Sep 17 00:00:00 2001
From: ddbrisbin <35312448+ddbrisbin@users.noreply.github.com>
Date: Sat, 25 Apr 2020 15:54:09 -0400
Subject: [PATCH 9/9] Rename astar.py to Astar.py

---
 src/planning/{astar.py => Astar.py} | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)
 rename src/planning/{astar.py => Astar.py} (99%)

diff --git a/src/planning/astar.py b/src/planning/Astar.py
similarity index 99%
rename from src/planning/astar.py
rename to src/planning/Astar.py
index dec0f74..51c49bc 100644
--- a/src/planning/astar.py
+++ b/src/planning/Astar.py
@@ -67,4 +67,4 @@ class Astar :
           visit_queue.put(nb_idx, f_score)
           queueed_flag[nb_idx] = 1
 
-    return self_.extract_path(cur_node, parent_idx, start_idx), optimal
\ No newline at end of file
+    return self_.extract_path(cur_node, parent_idx, start_idx), optimal