radar_error_test.py 16 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422
  1. # usage: python driving_models.py 1 0 - train the dave-orig model
  2. from __future__ import print_function
  3. import csv
  4. import shutil
  5. import sys
  6. import os
  7. sys.path.append("/home/vangogh/software/FuzzScene/code/")
  8. from data_utils import *
  9. import pandas as pd
  10. from tensorflow.keras.layers import Convolution2D, Input, Dense, Flatten, Lambda, MaxPooling2D, Dropout, Activation
  11. from tensorflow.keras.callbacks import EarlyStopping, ModelCheckpoint
  12. from tensorflow.keras import models, optimizers, backend
  13. import matplotlib.pyplot as plt
  14. import tensorflow as tf
  15. import numpy as np
  16. import time
  17. import Constants
  18. dataset_path = '/home/software/FuzzScene/code/train_carla/'
  19. def Dave_orig(input_tensor=None, load_weights=False): # original dave
  20. if input_tensor is None:
  21. input_tensor = Input(shape=(100, 100, 3))
  22. x = Convolution2D(24, (5, 5), padding='valid', activation='relu', strides=(2, 2), name='block1_conv1')(input_tensor)
  23. x = Convolution2D(36, (5, 5), padding='valid', activation='relu', strides=(2, 2), name='block1_conv2')(x)
  24. x = Convolution2D(48, (5, 5), padding='valid', activation='relu', strides=(2, 2), name='block1_conv3')(x)
  25. x = Convolution2D(64, (3, 3), padding='valid', activation='relu', strides=(1, 1), name='block1_conv4')(x)
  26. x = Convolution2D(64, (3, 3), padding='valid', activation='relu', strides=(1, 1), name='block1_conv5')(x)
  27. x = Flatten(name='flatten')(x)
  28. x = Dense(1164, activation='relu', name='fc1')(x)
  29. x = Dense(100, activation='relu', name='fc2')(x)
  30. x = Dense(50, activation='relu', name='fc3')(x)
  31. x = Dense(10, activation='relu', name='fc4')(x)
  32. x = Dense(1, name='before_prediction')(x)
  33. x = Lambda(atan_layer, output_shape=atan_layer_shape, name='prediction')(x)
  34. m = Model(input_tensor, x)
  35. if load_weights:
  36. m.load_weights(Constants.MODEL_DIR_PATH + 'Model1.h5')
  37. # compiling
  38. m.compile(loss='mse', optimizer='Adam')
  39. # m.compile(loss=[rmse], optimizer='adadelta')
  40. return m
  41. def Dave_norminit(input_tensor=None, load_weights=False): # original dave with normal initialization
  42. if input_tensor is None:
  43. input_tensor = Input(shape=(100, 100, 3))
  44. x = Convolution2D(24, (5, 5), padding='valid', activation='relu', strides=(2, 2),
  45. name='block1_conv1')(input_tensor)
  46. x = Convolution2D(36, (5, 5), padding='valid', activation='relu', strides=(2, 2),
  47. name='block1_conv2')(x)
  48. x = Convolution2D(48, (5, 5), padding='valid', activation='relu', strides=(2, 2),
  49. name='block1_conv3')(x)
  50. x = Convolution2D(64, (3, 3), padding='valid', activation='relu', strides=(1, 1),
  51. name='block1_conv4')(x)
  52. x = Convolution2D(64, (3, 3), padding='valid', activation='relu', strides=(1, 1),
  53. name='block1_conv5')(x)
  54. x = Flatten(name='flatten')(x)
  55. x = Dense(1164, kernel_initializer=normal_init, activation='relu', name='fc1')(x)
  56. x = Dense(100, kernel_initializer=normal_init, activation='relu', name='fc2')(x)
  57. x = Dense(50, kernel_initializer=normal_init, activation='relu', name='fc3')(x)
  58. x = Dense(10, kernel_initializer=normal_init, activation='relu', name='fc4')(x)
  59. x = Dense(1, name='before_prediction')(x)
  60. x = Lambda(atan_layer, output_shape=atan_layer_shape, name='prediction')(x)
  61. m = Model(input_tensor, x)
  62. if load_weights:
  63. m.load_weights(Constants.MODEL_DIR_PATH + 'Model2.h5')
  64. # compiling
  65. m.compile(loss='mse', optimizer='Adam')
  66. # m.compile(loss=[rmse], optimizer='adadelta')
  67. return m
  68. def Dave_dropout(input_tensor=None, load_weights=False): # simplified dave
  69. if input_tensor is None:
  70. input_tensor = Input(shape=(100, 100, 3))
  71. x = Convolution2D(16, (3, 3), padding='valid', activation='relu', name='block1_conv1')(input_tensor)
  72. x = MaxPooling2D(pool_size=(2, 2), name='block1_pool1')(x)
  73. x = Convolution2D(32, (3, 3), padding='valid', activation='relu', name='block1_conv2')(x)
  74. x = MaxPooling2D(pool_size=(2, 2), name='block1_pool2')(x)
  75. x = Convolution2D(64, (3, 3), padding='valid', activation='relu', name='block1_conv3')(x)
  76. x = MaxPooling2D(pool_size=(2, 2), name='block1_pool3')(x)
  77. x = Flatten(name='flatten')(x)
  78. x = Dense(500, activation='relu', name='fc1')(x)
  79. x = Dropout(.5)(x)
  80. x = Dense(100, activation='relu', name='fc2')(x)
  81. x = Dropout(.25)(x)
  82. x = Dense(20, activation='relu', name='fc3')(x)
  83. x = Dense(1, name='before_prediction')(x)
  84. x = Lambda(atan_layer, output_shape=atan_layer_shape, name="prediction")(x)
  85. m = Model(input_tensor, x)
  86. if load_weights:
  87. m.load_weights(Constants.MODEL_DIR_PATH + 'Model3.h5')
  88. # compiling
  89. m.compile(loss='mse', optimizer=optimizers.Adam(lr=1e-04))
  90. # m.compile(loss=[rmse], optimizer='adadelta')
  91. return m
  92. def Epoch_model(input_tensor=None, load_weights=False):
  93. if input_tensor is None:
  94. input_tensor = Input(shape=(128, 128, 3))
  95. x = Convolution2D(32, (3, 3), activation='relu', padding='same')(input_tensor)
  96. x = MaxPooling2D((2, 2), strides=(2, 2))(x)
  97. x = Dropout(0.25)(x)
  98. x = Convolution2D(64, (3, 3), activation='relu', padding='same')(x)
  99. x = MaxPooling2D((2, 2), strides=(2, 2))(x)
  100. x = Dropout(0.25)(x)
  101. x = Convolution2D(128, (3, 3), activation='relu', padding='same')(x)
  102. x = MaxPooling2D((2, 2), strides=(2, 2))(x)
  103. x = Dropout(0.5)(x)
  104. y = Flatten()(x)
  105. y = Dense(1024, activation='relu')(y)
  106. y = Dropout(.5)(y)
  107. y = Dense(1)(y)
  108. m = Model(input_tensor, y)
  109. if load_weights:
  110. m.load_weights(Constants.MODEL_DIR_PATH + 'Model4.h5')
  111. m.compile(loss='mse', optimizer=optimizers.Adam(lr=1e-04))
  112. return m
  113. def rmse(y_true, y_pred): # used for loss metric, output tensor
  114. '''Calculates RMSE
  115. '''
  116. return K.sqrt(K.mean(K.square(y_pred - y_true)))
  117. def calc_rmse(yhat, label): # used for loss cal, output float
  118. mse = 0.
  119. count = 0
  120. if len(yhat) != len(label):
  121. print("yhat and label have different lengths")
  122. return -1
  123. for i in range(len(yhat)):
  124. count += 1
  125. predicted_steering = yhat[i]
  126. steering = label[i]
  127. # print(predicted_steering)
  128. # print(steering)
  129. mse += (float(steering) - float(predicted_steering)) ** 2.
  130. return (mse / count) ** 0.5
  131. def calc_mse(yhat, label): # used for loss cal, output float
  132. mse = 0.
  133. count = 0
  134. if len(yhat) != len(label):
  135. print("yhat and label have different lengths")
  136. return -1
  137. for i in range(len(yhat)):
  138. count += 1
  139. predicted_steering = yhat[i]
  140. steering = label[i]
  141. # print(predicted_steering)
  142. # print(steering)
  143. mse += (float(steering) - float(predicted_steering)) ** 2.
  144. return (mse / count)
  145. def error_test(seq_label_name, model_name):
  146. batch_size = 64
  147. image_shape = (100, 100)
  148. model_existed = 1
  149. test_dataset_path = Constants.CARLA_RADAR_DATA_PATH
  150. label_path = Constants.CARLA_RADAR_LABEL_OUTPUT_PATH + seq_label_name
  151. with open(label_path, 'r') as f:
  152. rows = len(f.readlines()) - 1
  153. if rows == 0:
  154. return 0
  155. # --------------------------------------Build Model---------------------------------------- #
  156. # Dave_v1
  157. if model_name == '1':
  158. if model_existed == '0':
  159. model = Dave_orig()
  160. else:
  161. model = Dave_orig(None, True)
  162. # Dave_v2
  163. elif model_name == '2':
  164. # K.set_learning_phase(1)
  165. if model_existed == '0':
  166. model = Dave_norminit()
  167. else:
  168. model = Dave_norminit(None, True)
  169. # Dave_v3
  170. elif model_name == '3':
  171. # K.set_learning_phase(1)
  172. if model_existed == '0':
  173. model = Dave_dropout()
  174. else:
  175. model = Dave_dropout(None, True)
  176. # nb_epoch = 30
  177. # Udacity Epoch Model
  178. elif model_name == '4':
  179. if model_existed == '0':
  180. model = Epoch_model()
  181. else:
  182. model = Epoch_model(None, True)
  183. image_shape = (128, 128)
  184. batch_size = 32
  185. else:
  186. print(bcolors.FAIL + 'invalid model name, must in [1, 2, 3, 4]' + bcolors.ENDC)
  187. print(bcolors.OKGREEN + 'model %s built' % model_name + bcolors.ENDC)
  188. # --------------------------------------Training---------------------------------------- #
  189. # Dave serial model
  190. if model_name == '4':
  191. test_steering_log = label_path
  192. test_data = carla_load_steering_data(test_steering_log)
  193. test_frame_id = carla_load_frame_id(test_data)
  194. print('testset frame_id len: ', len(test_frame_id))
  195. # dataset divide
  196. # time_list_train = []
  197. time_list_test = []
  198. # for j in range(0, len(frame_id)):
  199. # time_list_train.append(frame_id[j])
  200. for j in range(0, len(test_frame_id)):
  201. time_list_test.append(test_frame_id[j])
  202. # print('time_list_train len: ', len(time_list_train))
  203. print('time_list_test len: ', len(time_list_test))
  204. print(bcolors.OKGREEN + 'Model %s trained' % model_name + bcolors.ENDC)
  205. # --------------------------------------Evaluation---------------------------------------- #
  206. # Different evaluation methods for different model
  207. if model_name != '4':
  208. K.set_learning_phase(0)
  209. test_generator, samples_per_epoch = load_carla_test_data(path=label_path, batch_size=batch_size,
  210. shape=image_shape)
  211. print('test samples: ', samples_per_epoch)
  212. loss = model.evaluate(test_generator, steps=math.ceil(samples_per_epoch * 1. / batch_size), verbose=1)
  213. print("model %s evaluate_generator loss: %.8f" % (model_name, loss))
  214. # --------------------------------------Predict Dave---------------------------------------- #
  215. filelist = []
  216. true_angle_list = []
  217. with open(label_path, 'r') as f:
  218. rows = len(f.readlines()) - 1
  219. f.seek(0)
  220. for i, line in enumerate(f):
  221. if i == 0:
  222. continue
  223. file_name = line.split(',')[1]
  224. filelist.append(test_dataset_path + 'center/' + file_name)
  225. true_angle_list.append(float(line.split(',')[3]))
  226. print("--------IMG READ-------")
  227. predict_angle_list = []
  228. imgs = []
  229. raw_imgs = []
  230. count = 0
  231. ori_image_size = (720, 1280)
  232. for f in filelist:
  233. count += 1
  234. if count % 100 == 0:
  235. print(str(count) + ' images read')
  236. orig_name = f
  237. gen_img = preprocess_image(orig_name, image_shape)
  238. raw_img = preprocess_image(orig_name, ori_image_size)
  239. imgs.append(gen_img)
  240. raw_imgs.append(raw_img)
  241. print("--------IMG READ COMPLETE-------")
  242. print("--------DAVE PREDICT-------")
  243. count = 0
  244. imgs = np.array(imgs)
  245. for i in range(len(imgs)):
  246. predict_angle_list.append(model.predict(imgs[i])[0])
  247. print("--------DAVE PREDICT COMPLETE-------")
  248. yhat = predict_angle_list
  249. test_y = true_angle_list
  250. else:
  251. test_generator = carla_data_generator(frame_id=test_frame_id,
  252. steering_log=test_steering_log,
  253. image_folder=Constants.CARLA_RADAR_PNG_OUTPUT_PATH,
  254. unique_list=time_list_test,
  255. gen_type='test',
  256. batch_size=len(time_list_test),
  257. image_size=image_shape,
  258. shuffle=False,
  259. preprocess_input=normalize_input,
  260. preprocess_output=exact_output)
  261. # --------------------------------------Predict Epoch---------------------------------------- #
  262. print("--------EPOCH PREDICT-------")
  263. test_x, test_y = next(test_generator)
  264. yhat = model.predict(test_x, verbose=1)
  265. print("--------EPOCH PREDICT COMPLETE-------")
  266. # print(yhat)
  267. loss = calc_mse(yhat, test_y)
  268. print("[" + os.path.basename(__file__) + ", Line " + str(sys._getframe().f_lineno) + ", " + sys._getframe().f_code.co_name + "] ", 'len(yhat): ', len(yhat))
  269. # # --------------------------------------FIND ERROR---------------------------------------- #
  270. # filelist_list = []
  271. # list_row = []
  272. # with open(test_dataset_path + 'label_test.csv', 'r') as f:
  273. # rows = len(f.readlines()) - 1
  274. # f.seek(0)
  275. # for i, line in enumerate(f):
  276. # if i == 0:
  277. # continue
  278. # file_name = line.split(',')[1]
  279. # filelist_list.append(file_name)
  280. # lamb = 1
  281. # error_list = []
  282. # lenm = len(filelist_list)
  283. # with open(test_dataset_path + 'model' + model_name + '_oriMSE.csv', 'r') as f:
  284. # rows = len(f.readlines()) - 1
  285. # f.seek(0)
  286. # m = 0
  287. # for i, line in enumerate(f): # [seed_name, img_id, ori_angle, 4*ori_angle]
  288. # if i == 0:
  289. # continue
  290. # predict_steering_angle = line.split(',')[1]
  291. # oriMSE = line.split(',')[2]
  292. # true_angle_gt = line.split(',')[3]
  293. # if ((float(yhat[m]) - float(predict_steering_angle)) ** 2) > (lamb * float(oriMSE)):
  294. # countcc = countcc + 1
  295. # list_row.append(
  296. # [filelist_list[m], predict_steering_angle, float(yhat[m]), true_angle_gt, model_name, m])
  297. # print(predict_steering_angle, float(yhat[m]), oriMSE)
  298. # error_list.append(m)
  299. #
  300. # if (m + 1) < lenm:
  301. # m = m + 1
  302. # else:
  303. # break
  304. ori_data_list = []
  305. with open(test_dataset_path + 'model' + model_name + '_oriMSE.csv', 'r') as f:
  306. f.seek(0)
  307. for i, line in enumerate(f):
  308. if i == 0:
  309. continue
  310. predict_steering_angle = line.split(',')[1]
  311. oriMSE = line.split(',')[2]
  312. true_angle_gt = line.split(',')[3]
  313. ori_data_list.append([predict_steering_angle, oriMSE, true_angle_gt])
  314. data_list_wb = [] # write back data # [seed_name, operator, cur_predict_angle, ori_predict_angle, is_err, ori_carla_angle, cur_carla_angle]
  315. with open(label_path, 'r') as f:
  316. f.seek(0)
  317. frame = 0
  318. last_seed_name = ''
  319. for i, line in enumerate(f):
  320. dt = line.split(',') # line in label_test.csv
  321. seed_name = dt[0] # full seed name
  322. if i == 0:
  323. continue
  324. frame += 1
  325. if seed_name != last_seed_name:
  326. frame = 1
  327. last_seed_name = seed_name
  328. operator = seed_name.split('_')[1]
  329. scene_name = int(seed_name.split('_')[3][0])
  330. cur_carla_angle = float(dt[3]) # 4*cur_carla_angle
  331. cur_predict_angle = yhat[i - 1] # cur predict angle
  332. cur_frame = (scene_name - 1) * 125 + frame - 1
  333. ori_predict_angle = ori_data_list[cur_frame][0] # data from oriMSE.csv
  334. oriMSE = ori_data_list[cur_frame][1]
  335. ori_carla_angle = ori_data_list[cur_frame][2]
  336. if ((cur_predict_angle - float(ori_predict_angle)) ** 2) > (1 * float(oriMSE)):
  337. is_err = 1
  338. else:
  339. is_err = 0
  340. data_list_wb.append(
  341. [seed_name, operator, cur_predict_angle[0], ori_predict_angle, is_err, float(ori_carla_angle),
  342. cur_carla_angle])
  343. radar_path = Constants.CARLA_RADAR_DATA_PATH + "radar" + str(model_name) + ".csv"
  344. if not os.path.exists(radar_path):
  345. shutil.copy(Constants.CARLA_RADAR_DATA_PATH + "radar_null.csv", radar_path)
  346. with open(radar_path, 'a+', encoding='utf-8') as f:
  347. csv_writer = csv.writer(f)
  348. for line in range(len(data_list_wb)):
  349. csv_writer.writerow(data_list_wb[line])
  350. print("[" + os.path.basename(__file__) + ", Line " + str(sys._getframe().f_lineno) + ", " + sys._getframe().f_code.co_name + "] ", "Append sub_csv" + label_path + " to radar" + str(model_name) + ".csv done!")
  351. if __name__ == '__main__':
  352. model_name = sys.argv[1]
  353. seq = sys.argv[2]
  354. path = 'label_test_' + str(seq) + '.csv'
  355. error_test(path, model_name)
  356. # error_list = error_test()
  357. # error_count = './error_count.csv'
  358. # with open(error_count, 'a+', encoding='utf-8') as f:
  359. # csv_writer = csv.writer(f)
  360. # csv_writer.writerow([sys.argv[3], error_list])