Add Gitignore, Folder Renaming, Add Velodyne Data Handlers

- Add gitignore so downloaded data, vscode files, and pycache are not included
- Move downloader.py to src/dataset/data folder
- Rename src/dataset/dataManipulation to src/dataset/data_manip
- Rename src/dataset/ManageDataset to src/dataset/read_dataset
- Remove gps.csv and read_gps.py from visualization folder
- Add read_vel_hits.py and read_vel_sync.py (with python 3 fixes) for reading velodyne data
- Add point_cloud_vis.py to create map of entire dataset pointclouds

.gitignore

@@ -0,0 +1,4 @@
+src/dataset/data/*
+!src/dataset/data/downloader.py
+.vscode
+__pycache__/

src/dataset/read_dataset/read_vel_hits.py

@@ -0,0 +1,89 @@
+# !/usr/bin/python
+#
+# Example code to go through the velodyne_hits.bin
+# file and read timestamps, number of hits, and the
+# hits in each packet.
+#
+#
+# To call:
+#
+#   python read_vel_hits.py velodyne.bin
+#
+
+import sys
+import struct
+
+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 verify_magic(s):
+
+    magic = 44444
+
+    m = struct.unpack('<HHHH', s)
+
+    return len(m)>=4 and m[0] == magic and m[1] == magic and m[2] == magic and m[3] == magic
+
+def main(args):
+
+    if len(sys.argv) < 2:
+        print("Please specify input bin file")
+        return 1
+
+    f_bin = open(sys.argv[1], "rb")
+
+    total_hits = 0
+    first_utime = -1
+    last_utime = -1
+
+    while True:
+        magic = f_bin.read(8)
+        if magic == '': # eof
+            break
+
+        if not verify_magic(magic):
+            print("Could not verify magic")
+
+        num_hits = struct.unpack('<I', f_bin.read(4))[0]
+        utime = struct.unpack('<Q', f_bin.read(8))[0]
+
+        padding = f_bin.read(4) # padding
+
+        print("Have %d hits for utime %ld" % (num_hits, utime))
+
+        total_hits += num_hits
+        if first_utime == -1:
+            first_utime = utime
+        last_utime = utime
+
+        for i in range(num_hits):
+
+            x = struct.unpack('<H', f_bin.read(2))[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)
+            s = "%5.3f, %5.3f, %5.3f, %d, %d" % (x, y, z, i, l)
+
+            print(s)
+
+        raw_input("Press enter to continue...")
+
+    f_bin.close()
+
+    print("Read %d total hits from %ld to %ld" % (total_hits, first_utime, last_utime))
+
+    return 0
+
+if __name__ == '__main__':
+    sys.exit(main(sys.argv))

src/dataset/read_dataset/read_vel_sync.py

@@ -0,0 +1,123 @@
+# !/usr/bin/python
+#
+# Example code to read a velodyne_sync/[utime].bin file
+# Plots the point cloud using matplotlib. Also converts
+# to a CSV if desired.
+#
+# To call:
+#
+#   python read_vel_sync.py velodyne.bin [out.csv]
+#
+
+import sys
+import struct
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+
+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 generate_plot(bin_file):
+    f_bin = None
+
+    try:
+        f_bin = open(bin_file, "rb")
+    except IOError:
+        print('Failed to open velodyne file')
+        return 1
+
+    hits = []
+
+    while True:
+        x_str = f_bin.read(2)
+        if x_str == b'': # 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)
+
+        s = "%5.3f, %5.3f, %5.3f, %d, %d" % (x, y, z, i, l)
+
+        if f_csv:
+            f_csv.write('%s\n' % s)
+
+        hits += [[x, y, z]]
+
+    f_bin.close()
+
+    hits = np.asarray(hits)
+
+    fig = plt.figure()
+    ax = fig.add_subplot(111, projection='3d')
+    ax.scatter(hits[:, 0], hits[:, 1], -hits[:, 2], c=-hits[:, 2], s=5, linewidths=0)
+    plt.show()
+
+    return 0    
+
+def main(args):
+
+    if len(sys.argv) < 2:
+        print('Please specify velodyne file')
+        return 1
+
+    f_bin = open(sys.argv[1], "rb")
+
+    if len(sys.argv) >= 3:
+        print('Writing to ', sys.argv[2])
+        f_csv = open(sys.argv[2], "w")
+    else:
+        f_csv = None
+
+    hits = []
+
+    while True:
+
+        x_str = f_bin.read(2)
+        if x_str == b'': # 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)
+
+        s = "%5.3f, %5.3f, %5.3f, %d, %d" % (x, y, z, i, l)
+
+        if f_csv:
+            f_csv.write('%s\n' % s)
+
+        hits += [[x, y, z]]
+
+    f_bin.close()
+
+    if f_csv:
+        f_csv.close()
+
+    hits = np.asarray(hits)
+
+    fig = plt.figure()
+    ax = fig.add_subplot(111, projection='3d')
+    ax.scatter(hits[:, 0], hits[:, 1], -hits[:, 2], c=-hits[:, 2], s=5, linewidths=0)
+    plt.show()
+
+    return 0
+
+if __name__ == '__main__':
+    sys.exit(main(sys.argv))

src/visualization/point_cloud_vis.py

@@ -0,0 +1,125 @@
+# !/usr/bin/python
+#
+#   python point_cloud_vis.py \
+#          /PATH/TO/ground_truth.csv \
+#          /PATH/TO/velodyne_sync
+
+import sys
+import os
+import struct
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+
+class GTPose:
+    def __init__(self, time, x, y, z, r, p, h):
+        self.time = time
+        self.x = x
+        self.y = y
+        self.z = z
+        self.r = r
+        self.p = p
+        self.h = h
+
+class PointCloud:
+    def __init__(self, time):
+        self.time = time
+        self.x_list = []
+        self.y_list = []
+        self.z_list = []
+    
+    def add_point(self, x, y, z):
+        self.time += time
+        self.x_list += x
+        self.y_list += y
+        self.z_list += z
+
+
+def read_gt(file):
+    gt = np.loadtxt(file, delimeter=",")
+    
+    time = gt[:, 0]
+    x = gt[:, 1]
+    y = gt[:, 2]
+    z = gt[:, 3]
+    r = gt[:, 4]
+    p = gt[:, 5]
+    h = gt[:, 6]
+
+    return GTPose(time, x, y, z, r, p, h)
+
+
+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 read_vel(file):
+    time = os.path.splittext(os.path.basename(file))[0]
+    pc = PointCloud(time)
+    f_bin = open(file, "rb")
+    
+    while True:
+        x_str = f_bin.read(2)
+
+        if x_str == b'': # 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]
+
+        # TODO: Be careful about z being flipped when plotting the velodyne data
+        x, y, z = convert(x, y, z)
+        pc.add_point(x, y, -z)
+    
+    return pc
+
+
+def r_to_g_frame(gt, pc):
+    # TODO: Interpolate gt to find corresponding pose for pc
+
+    # TODO: Transform pc from robot frame to global frame
+    pass
+
+
+def main(args):
+    if len(sys.argv) != 2:
+        print("Expecting 2 arguments: ground truth filepath and data folder")
+        return 1
+    
+    ground_truth_file = sys.arv[1]
+    data_path = sys.argv[2]
+
+    x = []
+    y = []
+    z = []
+    gt = read_gt(ground_truth_file)
+    
+    fig = plt.figure()
+    ax = fig.add_subplot(111, projection='3d')
+
+    for filename in os.listdir(data_path):
+        pc = read_vel(ground_truth_file, data_path + '/' + filename)
+        pc = r_to_g_frame(gt, pc)
+        
+        x += pc.x_list
+        y += pc.y_list
+        z += pc.z_list
+
+    ax.scatter(x, y, z, c=z, s=5, linewidths=0)
+    plt.show()
+
+    return 0
+
+
+if __name__ == '__main__':
+    sys.exit(main(sys.argv))

src/visualization/read_gps.py

@@ -1,60 +0,0 @@
-# !/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()
-
-    fig = plt.figure()
-    ax = fig.gca(projection='3d')
-    ax.scatter(x, y, alt, 'b.')
-    plt.show()
-
-    return 0
-
-if __name__ == '__main__':
-    sys.exit(main(sys.argv))