ready to merge with main

gen_data.py

@@ -4,9 +4,15 @@
 import numpy as np
 import random
 batch_size = 32
-directory = './nclt_new/'
+# shop facade
+directory = './ShopFacade/'
 dataset = 'dataset_train.txt'
 
+# King's College
+#directory = './KingsCollege/'
+#dataset = 'dataset_train.txt'
+
+
 class datasource(object):
 	def __init__(self, images, poses):
 		self.images = images

gen_data_cam.py

@@ -0,0 +1,117 @@
+import pdb
+from tqdm import tqdm
+import cv2, os, sys
+import numpy as np
+import random, imutils
+batch_size = 32
+# test directory
+# directory = '/home/eecs568/Documents/TestImages/'
+# dataset = 'testSet.csv'
+
+# train directory
+directory = './cam4_train/'
+dataset = 'dataset_train.csv'
+
+class datasource(object):
+    def __init__(self, images, poses):
+        self.images = images
+        self.poses = poses
+
+def centeredCrop(img, output_side_length):
+    height, width, depth = img.shape
+    new_height = output_side_length
+    new_width = output_side_length
+    if height > width:
+        new_height = output_side_length * height / width
+    else:
+        new_width = output_side_length * width / height
+    height_offset = (new_height - output_side_length) / 2
+    width_offset = (new_width - output_side_length) / 2
+    cropped_img = img[height_offset:height_offset + output_side_length,
+                      width_offset:width_offset + output_side_length]
+    return cropped_img
+
+def preprocess(images):
+    images_out = [] #final result
+    #Resize and crop and compute mean!
+    images_cropped = []
+    for i in tqdm(range(len(images))):
+        X = cv2.imread(images[i])
+        if X.shape[0] < X.shape[1]:
+            X = imutils.resize(X , height=256)
+        else:
+            X = imutils.resize(X, width=256)
+        X = imutils.rotate(X, angle=270)
+        #X = cv2.resize(X, (455, 256))
+        X = centeredCrop(X, 224)
+        images_cropped.append(X)
+    #compute images mean
+    N = 0
+    mean = np.zeros((1, 3, 224, 224))
+    for X in tqdm(images_cropped):
+        mean[0][0] += X[:,:,0]
+        mean[0][1] += X[:,:,1]
+        mean[0][2] += X[:,:,2]
+        N += 1
+    mean[0] /= N
+    #Subtract mean from all images
+    for X in tqdm(images_cropped):
+        X = np.transpose(X,(2,0,1))
+        X = X - mean
+        X = np.squeeze(X)
+        X = np.transpose(X, (1,2,0))
+        images_out.append(X)
+    return images_out
+
+def get_data():
+    poses = []
+    images = []
+    line_num = 0
+    all_imgs = sorted(os.listdir(directory))[:-1]
+    with open(directory+dataset) as f:
+        #next(f)  # skip the 3 header lines
+        #next(f)
+        #next(f)
+        for line in f:
+            fname, p0,p1,p2,p3,p4,p5 = line.split(',')
+            p0 = float(p0)
+            p1 = float(p1)
+            p2 = float(p2)
+            p3 = float(p3)
+            p4 = float(p4)
+            p5 = float(p5)
+
+            filename = directory+'/'+all_imgs[line_num] #fname+".tiff"
+            if (os.path.isfile(filename)==False):
+                pdb.set_trace()
+                continue
+            else:
+                poses.append((p0,p1,p2,p3,p4,p5))
+                images.append(filename )
+            line_num += 1
+    print("Num of images is "+str(len(images)))
+    images = preprocess(images)
+    return datasource(images, poses)
+
+def gen_data(source):
+    while True:
+        indices = range(len(source.images))
+        random.shuffle(indices)
+        for i in indices:
+            image = source.images[i]
+            pose_x = source.poses[i][0:3]
+            pose_q = source.poses[i][3:]
+            yield image, pose_x, pose_q
+
+def gen_data_batch(source):
+    data_gen = gen_data(source)
+    while True:
+        image_batch = []
+        pose_x_batch = []
+        pose_q_batch = []
+        for _ in range(batch_size):
+            image, pose_x, pose_q = next(data_gen)
+            image_batch.append(image)
+            pose_x_batch.append(pose_x)
+            pose_q_batch.append(pose_q)
+        yield np.array(image_batch), np.array(pose_x_batch), np.array(pose_q_batch)

gen_test_trajectory.py

@@ -0,0 +1,62 @@
+import tensorflow as tf
+import os, sys
+import gen_data
+import train
+import numpy as np
+import pdb
+import math, transforms3d
+import matplotlib
+matplotlib.use('Agg')
+import matplotlib.pyplot as plt
+
+os.environ['CUDA_VISIBLE_DEVICES'] = '0'
+# second to last argument False for nclt True for others
+#weight_path = 'success_models/ShopFacade_weights/model_epoch_3.ckpt'
+#image_path = '.ShopFacade/'
+#fig_name = 'ShopFacade Trajectory.png'
+
+weight_path = 'success_models/KingsCollege/model_epoch_90.ckpt'
+image_path = './KingsCollege/'
+fig_name = 'KingsCollege Trajectory.png'
+trainer = train.trainer(weight_path, image_path, True, True, True)
+datasource = gen_data.get_data()
+
+# initialize plot tool
+fig = plt.figure(1)
+
+error = np.zeros([len(datasource.images),3])
+
+for i in range(len(datasource.images)):
+    np_image = datasource.images[i]
+    feed={tf.get_default_graph().get_tensor_by_name('Placeholder:0'): np.expand_dims(np_image, axis=0) }
+
+    # ground truth x y z
+    pose_x= np.asarray(datasource.poses[i][0:3])
+
+    # ground truth euler angles
+    pose_q= np.asarray(datasource.poses[i][3:7]) 
+    pose_euler_angle = transforms3d.euler.quat2euler(pose_q)
+
+    x_q = trainer.sess.run([tf.get_default_graph().get_tensor_by_name('fc9/fc9:0') ], feed)
+
+    # x y z
+    pred_x = np.squeeze(x_q)[0:3]
+
+    # euler angle
+    pred_q = np.squeeze(x_q)[3:7]
+    pred_euler_angle = transforms3d.euler.quat2euler(pred_q)
+
+    # scatter plot for pose
+    plt.scatter(pose_x[0],pose_x[1],c='g')
+    plt.scatter(pred_x[0],pred_x[1],c='r')
+
+    error[i,:] = np.array([pose_x[0]-pred_x[0],pose_x[1]-pred_x[1],pose_q[-1]-pred_q[-1]])
+
+# save the plot
+plt.legend(['ground truth','prediction'])
+fig.savefig(fig_name)
+
+# calculate stddev and mean error
+meanErr = np.sum(error,axis=0)/len(error)
+stdErr = np.std(error,axis=0)
+print("The mean error is {} and standard deviation is {}.".format(meanErr,stdErr))

gen_test_trajectory_euler_angle.py

@@ -0,0 +1,66 @@
+import tensorflow as tf
+import os, sys
+import gen_data_nclt_new
+import train
+import numpy as np
+import pdb
+import math, transforms3d
+import matplotlib
+matplotlib.use('Agg')
+import matplotlib.pyplot as plt
+
+
+# os.environ['CUDA_VISIBLE_DEVICES'] = '0'
+# second to last argument False for nclt True for others
+weightPath = '/home/eecs568/eecs568/Mobile-Robotics/success_models/nclt_new/20180409-130922model_epoch_4.ckpt'
+#imagePath = './cam4_train/'
+#figname = 'nclt_train.png'
+imagePath = '/home/eecs568/Documents/TestImages\ 2012-01-08/test/'
+figname = 'nclt_test_seq2.png'
+# trainer = train.trainer(weightPath, imagePath, 100, False, False)
+datasource = gen_data_nclt_new.get_data()
+
+# initialize plot tool
+fig = plt.figure(1)
+
+error = np.zeros([len(datasource.images),3])
+
+for i in range(len(datasource.images)):
+    np_image = datasource.images[i]
+    # feed={tf.get_default_graph().get_tensor_by_name('Placeholder:0'): np.expand_dims(np_image, axis=0) }
+
+    # ground truth x y z
+    pose_x= np.asarray(datasource.poses[i][0:2])
+
+    # ground truth euler angles
+    pose_q= np.asarray(datasource.poses[i][3:6]) 
+    # pose_euler_angle = transforms3d.euler.quat2euler(pose_q)
+
+    # x_q = trainer.sess.run([tf.get_default_graph().get_tensor_by_name('fc9/fc9:0') ], feed)
+    # pdb.set_trace()
+
+    # x y z
+    # pred_x = np.squeeze(x_q)[0:3]
+
+    # euler angle
+    # pred_q = np.squeeze(x_q)[3:6]
+    # pred_euler_angle = transforms3d.euler.quat2euler(pred_q)
+
+    # scatter plot for pose
+    plt.scatter(pose_x[0],pose_x[1],c='g')
+    # plt.scatter(pred_x[0],pred_x[1],c='r')
+    # plt.pause(0.01)
+    # plt.draw()
+    # error[i,:] = np.array([pose_x[0]-pred_x[0],pose_x[1]-pred_x[1],pose_q[-1]-pred_q[-1]])
+
+    print("iteration {}\n".format(i))
+
+# save the plot
+plt.legend(['ground truth','prediction'])
+fig.savefig(figname)
+
+# calculate stddev and mean error
+#meanErr = np.sum(error,axis=0)/len(error)
+#stdErr = np.std(error,axis=0)
+#print("The mean error is {} and standard deviation is {}.".format(meanErr,stdErr))
+

subset_gen.py

@@ -0,0 +1,81 @@
+import os, sys, csv, shutil
+from tqdm import tqdm
+from numpy import genfromtxt
+import numpy as np
+import pdb
+class Subset:
+    def __init__(self, folder_name, label_file,  train_freq, test_freq, tail_str):
+        self.imgs = []
+        self.folder_name = folder_name
+        self.train_freq = train_freq
+        self.test_freq = test_freq
+        self.train_folder = folder_name + '/train' 
+        self.test_folder = folder_name  + '/test'
+        self.tail_str = tail_str
+
+        for name in sorted(os.listdir(folder_name)):
+            if name[-len(tail_str):] != tail_str:
+                continue
+            self.imgs.append(name)
+        self.labels = genfromtxt(label_file,  delimiter=',')
+
+        # pointer point to the current timestamp that wait to be matched
+        self.currentMatch = 0
+        # match tolerance in microseconds
+        self.matchTol = 1e5
+
+
+    def gen_subset(self):
+        # self.dump_to_folder( self.train_freq, self.train_folder,'dataset_train.csv',  True)
+        self.dump_to_folder( self.test_freq, self.test_folder,'dataset_test.csv',  False)
+
+    def dump_to_folder(self, freq, new_folder, new_label_file, is_train):
+        table_ind = 0
+        total_num_imgs = len(self.imgs)
+        table = np.zeros((total_num_imgs // freq , self.labels.shape[1]))
+        if not os.path.exists(new_folder):
+            os.makedirs(new_folder)
+        new_labels = open(new_folder + '/' + new_label_file, "a")
+        for i in tqdm(range(total_num_imgs)):
+            if (i % freq == 0):
+                img_i = self.imgs[i][:-len(self.tail_str)]
+                label_i = self.match(img_i)
+                if label_i == -1: continue
+                table[table_ind, :] = self.labels[label_i,:]
+                if is_train:
+                    table[table_ind, 0] = str(int(self.labels[label_i, 0]))[0:12]
+                    img_i = img_i[0:12]
+                shutil.copyfile(self.folder_name +'/' + self.imgs[i], new_folder + '/' + img_i + self.tail_str)
+                to_write = str(int(table[table_ind, 0]))+ ','  + \
+                           str(float(table[table_ind, 1])) + ',' + \
+                           str(float(table[table_ind, 2])) + ',' + \
+                           str(float(table[table_ind, 3])) + ',' + \
+                           str(float(table[table_ind, 4])) + ',' + \
+                           str(float(table[table_ind, 5])) + ',' + \
+                           str(float(table[table_ind, 6])) + '\n'
+                new_labels.write(to_write)
+                table_ind += 1
+                if table_ind == table.shape[0]: break
+        new_labels.close()
+        #np.savetxt( , table, delimiter=",")
+
+
+    def match(self,str_timestamp):
+        timestamp = int(str_timestamp)
+
+        matchId = -1
+        # begin match
+        for i in range(len(self.labels)):
+            if(abs(timestamp-self.labels[self.currentMatch,0])>self.matchTol):
+                if(self.currentMatch>=len(self.labels)-1):
+                    self.currentMatch = 0
+                else:
+                    self.currentMatch += 1
+            else:
+                matchId = self.currentMatch
+                if(self.currentMatch>=len(self.labels)-1):
+                    self.currentMatch = 0
+                else:
+                    self.currentMatch += 1
+
+        return matchId

test_subset.py

@@ -0,0 +1,3 @@
+import subset_gen
+gen = subset_gen.Subset('/home/eecs568/Documents/TestImages 2012-01-08', '/home/eecs568/Documents/groundtruth_2012-01-08.csv', 7, 1, '.tiff')
+gen.gen_subset()