adding files from dataset library including ways to manage the data itself. This includes citations and licensing on the datset.

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/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')