From 363cc06818eacf28eaf99750a84dfade06917bda Mon Sep 17 00:00:00 2001
From: Sravan Balaji <balajsra@umich.edu>
Date: Wed, 15 Apr 2020 17:37:28 -0400
Subject: [PATCH] gitignore & Read Velodyne Data

- Add gitignore so VS Code and dataset files are not committed
- Add python functions provided by NCLT dataset for reading velodyne
  data
---
 .gitignore                             |   6 +
 src/localization/localization.py       |  16 +++
 src/localization/project_vel_to_cam.py | 166 +++++++++++++++++++++++++
 src/localization/read_vel_hits.py      |  90 ++++++++++++++
 src/localization/read_vel_sync.py      |  81 ++++++++++++
 5 files changed, 359 insertions(+)
 create mode 100644 .gitignore
 create mode 100644 src/localization/localization.py
 create mode 100644 src/localization/project_vel_to_cam.py
 create mode 100644 src/localization/read_vel_hits.py
 create mode 100644 src/localization/read_vel_sync.py

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..ad13b3f
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,6 @@
+# Visual Studio Code
+.vscode
+
+# Ignore downloaded dataset files
+src/dataset/*
+!src/dataset/*.py
diff --git a/src/localization/localization.py b/src/localization/localization.py
new file mode 100644
index 0000000..6f19cb4
--- /dev/null
+++ b/src/localization/localization.py
@@ -0,0 +1,16 @@
+# localization.py
+
+import numpy as np
+import matplotlib.pyplot as plt
+import csv
+import os
+from PoseList import PoseList
+from scipy.spatial import distance
+
+
+def main():
+    pass
+
+
+if __name__ == '__main__':
+    main()
diff --git a/src/localization/project_vel_to_cam.py b/src/localization/project_vel_to_cam.py
new file mode 100644
index 0000000..0356d4e
--- /dev/null
+++ b/src/localization/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/localization/read_vel_hits.py b/src/localization/read_vel_hits.py
new file mode 100644
index 0000000..de1e6e1
--- /dev/null
+++ b/src/localization/read_vel_hits.py
@@ -0,0 +1,90 @@
+# !/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 specifiy input bin file"
+        return 1
+
+    f_bin = open(sys.argv[1], "r")
+
+    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))
diff --git a/src/localization/read_vel_sync.py b/src/localization/read_vel_sync.py
new file mode 100644
index 0000000..3ed3288
--- /dev/null
+++ b/src/localization/read_vel_sync.py
@@ -0,0 +1,81 @@
+# !/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 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))