├── README.md ├── __pycache__ └── utils.cpython-35.pyc ├── drive.py ├── environment-gpu.yml ├── environments.yml ├── images ├── center.png ├── flip.png ├── jungle_track.png ├── lake_track.png ├── left.png ├── right.png └── trans.png ├── model.h5 ├── model.py ├── self-driving-car.ipynb └── utils.py /README.md: -------------------------------------------------------------------------------- 1 | # How_to_simulate_a_self_driving_car 2 | This is the code for "How to Simulate a Self-Driving Car" by Siraj Raval on Youtube 3 | 4 | # This video will be released on Wednesday, May 17th at 10 AM PST. This code is a work in progress. 5 | 6 | ## Overview 7 | 8 | This is the code for [this](https://youtu.be/EaY5QiZwSP4) video on Youtube by Siraj Raval. We're going to use Udacity's [self driving car simulator](https://github.com/udacity/self-driving-car-sim) as a testbed for training an autonomous car. 9 | 10 | ## Dependencies 11 | 12 | You can install all dependencies by running one of the following commands 13 | 14 | You need a [anaconda](https://www.continuum.io/downloads) or [miniconda](https://conda.io/miniconda.html) to use the environment setting. 15 | 16 | ```python 17 | # Use TensorFlow without GPU 18 | conda env create -f environments.yml 19 | 20 | # Use TensorFlow with GPU 21 | conda env create -f environment-gpu.yml 22 | ``` 23 | 24 | Or you can manually install the required libraries (see the contents of the environemnt*.yml files) using pip. 25 | 26 | 27 | ## Usage 28 | 29 | 30 | ### Run the pretrained model 31 | 32 | Start up [the Udacity self-driving simulator](https://github.com/udacity/self-driving-car-sim), choose a scene and press the Autonomous Mode button. Then, run the model as follows: 33 | 34 | ```python 35 | python drive.py model.h5 36 | ``` 37 | 38 | ### To train the model 39 | 40 | You'll need the data folder which contains the training images. 41 | 42 | ```python 43 | python model.py 44 | ``` 45 | 46 | This will generate a file `model-.h5` whenever the performance in the epoch is better than the previous best. For example, the first epoch will generate a file called `model-000.h5`. 47 | 48 | ## Credits 49 | 50 | The credits for this code go to [naokishibuya](https://github.com/naokishibuya). I've merely created a wrapper to get people started. 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /__pycache__/utils.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/llSourcell/How_to_simulate_a_self_driving_car/8805a0f2ba962198d7aef482331a3eb4433d4199/__pycache__/utils.cpython-35.pyc -------------------------------------------------------------------------------- /drive.py: -------------------------------------------------------------------------------- 1 | #parsing command line arguments 2 | import argparse 3 | #decoding camera images 4 | import base64 5 | #for frametimestamp saving 6 | from datetime import datetime 7 | #reading and writing files 8 | import os 9 | #high level file operations 10 | import shutil 11 | #matrix math 12 | import numpy as np 13 | #real-time server 14 | import socketio 15 | #concurrent networking 16 | import eventlet 17 | #web server gateway interface 18 | import eventlet.wsgi 19 | #image manipulation 20 | from PIL import Image 21 | #web framework 22 | from flask import Flask 23 | #input output 24 | from io import BytesIO 25 | 26 | #load our saved model 27 | from keras.models import load_model 28 | 29 | #helper class 30 | import utils 31 | 32 | #initialize our server 33 | sio = socketio.Server() 34 | #our flask (web) app 35 | app = Flask(__name__) 36 | #init our model and image array as empty 37 | model = None 38 | prev_image_array = None 39 | 40 | #set min/max speed for our autonomous car 41 | MAX_SPEED = 25 42 | MIN_SPEED = 10 43 | 44 | #and a speed limit 45 | speed_limit = MAX_SPEED 46 | 47 | #registering event handler for the server 48 | @sio.on('telemetry') 49 | def telemetry(sid, data): 50 | if data: 51 | # The current steering angle of the car 52 | steering_angle = float(data["steering_angle"]) 53 | # The current throttle of the car, how hard to push peddle 54 | throttle = float(data["throttle"]) 55 | # The current speed of the car 56 | speed = float(data["speed"]) 57 | # The current image from the center camera of the car 58 | image = Image.open(BytesIO(base64.b64decode(data["image"]))) 59 | try: 60 | image = np.asarray(image) # from PIL image to numpy array 61 | image = utils.preprocess(image) # apply the preprocessing 62 | image = np.array([image]) # the model expects 4D array 63 | 64 | # predict the steering angle for the image 65 | steering_angle = float(model.predict(image, batch_size=1)) 66 | # lower the throttle as the speed increases 67 | # if the speed is above the current speed limit, we are on a downhill. 68 | # make sure we slow down first and then go back to the original max speed. 69 | global speed_limit 70 | if speed > speed_limit: 71 | speed_limit = MIN_SPEED # slow down 72 | else: 73 | speed_limit = MAX_SPEED 74 | throttle = 1.0 - steering_angle**2 - (speed/speed_limit)**2 75 | 76 | print('{} {} {}'.format(steering_angle, throttle, speed)) 77 | send_control(steering_angle, throttle) 78 | except Exception as e: 79 | print(e) 80 | 81 | # save frame 82 | if args.image_folder != '': 83 | timestamp = datetime.utcnow().strftime('%Y_%m_%d_%H_%M_%S_%f')[:-3] 84 | image_filename = os.path.join(args.image_folder, timestamp) 85 | image.save('{}.jpg'.format(image_filename)) 86 | else: 87 | 88 | sio.emit('manual', data={}, skip_sid=True) 89 | 90 | 91 | @sio.on('connect') 92 | def connect(sid, environ): 93 | print("connect ", sid) 94 | send_control(0, 0) 95 | 96 | 97 | def send_control(steering_angle, throttle): 98 | sio.emit( 99 | "steer", 100 | data={ 101 | 'steering_angle': steering_angle.__str__(), 102 | 'throttle': throttle.__str__() 103 | }, 104 | skip_sid=True) 105 | 106 | 107 | if __name__ == '__main__': 108 | parser = argparse.ArgumentParser(description='Remote Driving') 109 | parser.add_argument( 110 | 'model', 111 | type=str, 112 | help='Path to model h5 file. Model should be on the same path.' 113 | ) 114 | parser.add_argument( 115 | 'image_folder', 116 | type=str, 117 | nargs='?', 118 | default='', 119 | help='Path to image folder. This is where the images from the run will be saved.' 120 | ) 121 | args = parser.parse_args() 122 | 123 | #load model 124 | model = load_model(args.model) 125 | 126 | if args.image_folder != '': 127 | print("Creating image folder at {}".format(args.image_folder)) 128 | if not os.path.exists(args.image_folder): 129 | os.makedirs(args.image_folder) 130 | else: 131 | shutil.rmtree(args.image_folder) 132 | os.makedirs(args.image_folder) 133 | print("RECORDING THIS RUN ...") 134 | else: 135 | print("NOT RECORDING THIS RUN ...") 136 | 137 | # wrap Flask application with engineio's middleware 138 | app = socketio.Middleware(sio, app) 139 | 140 | # deploy as an eventlet WSGI server 141 | eventlet.wsgi.server(eventlet.listen(('', 4567)), app) 142 | -------------------------------------------------------------------------------- /environment-gpu.yml: -------------------------------------------------------------------------------- 1 | name: car-behavioral-cloning 2 | channels: 3 | - https://conda.anaconda.org/menpo 4 | - conda-forge 5 | dependencies: 6 | - python==3.5.2 7 | - numpy 8 | - matplotlib 9 | - jupyter 10 | - opencv3 11 | - pillow 12 | - scikit-learn 13 | - scikit-image 14 | - scipy 15 | - h5py 16 | - eventlet 17 | - flask-socketio 18 | - seaborn 19 | - pandas 20 | - imageio 21 | - pip: 22 | - moviepy 23 | - tensorflow-gpu==1.1 24 | - keras==1.2 25 | -------------------------------------------------------------------------------- /environments.yml: -------------------------------------------------------------------------------- 1 | name: car-behavioral-cloning 2 | channels: 3 | - https://conda.anaconda.org/menpo 4 | - conda-forge 5 | dependencies: 6 | - python==3.5.2 7 | - numpy 8 | - matplotlib 9 | - jupyter 10 | - opencv3 11 | - pillow 12 | - scikit-learn 13 | - scikit-image 14 | - scipy 15 | - h5py 16 | - eventlet 17 | - flask-socketio 18 | - seaborn 19 | - pandas 20 | - imageio 21 | - pip: 22 | - moviepy 23 | - tensorflow==1.1 24 | - keras==1.2 25 | -------------------------------------------------------------------------------- /images/center.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/llSourcell/How_to_simulate_a_self_driving_car/8805a0f2ba962198d7aef482331a3eb4433d4199/images/center.png -------------------------------------------------------------------------------- /images/flip.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/llSourcell/How_to_simulate_a_self_driving_car/8805a0f2ba962198d7aef482331a3eb4433d4199/images/flip.png -------------------------------------------------------------------------------- /images/jungle_track.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/llSourcell/How_to_simulate_a_self_driving_car/8805a0f2ba962198d7aef482331a3eb4433d4199/images/jungle_track.png -------------------------------------------------------------------------------- /images/lake_track.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/llSourcell/How_to_simulate_a_self_driving_car/8805a0f2ba962198d7aef482331a3eb4433d4199/images/lake_track.png -------------------------------------------------------------------------------- /images/left.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/llSourcell/How_to_simulate_a_self_driving_car/8805a0f2ba962198d7aef482331a3eb4433d4199/images/left.png -------------------------------------------------------------------------------- /images/right.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/llSourcell/How_to_simulate_a_self_driving_car/8805a0f2ba962198d7aef482331a3eb4433d4199/images/right.png -------------------------------------------------------------------------------- /images/trans.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/llSourcell/How_to_simulate_a_self_driving_car/8805a0f2ba962198d7aef482331a3eb4433d4199/images/trans.png -------------------------------------------------------------------------------- /model.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/llSourcell/How_to_simulate_a_self_driving_car/8805a0f2ba962198d7aef482331a3eb4433d4199/model.h5 -------------------------------------------------------------------------------- /model.py: -------------------------------------------------------------------------------- 1 | import pandas as pd # data analysis toolkit - create, read, update, delete datasets 2 | import numpy as np #matrix math 3 | from sklearn.model_selection import train_test_split #to split out training and testing data 4 | #keras is a high level wrapper on top of tensorflow (machine learning library) 5 | #The Sequential container is a linear stack of layers 6 | from keras.models import Sequential 7 | #popular optimization strategy that uses gradient descent 8 | from keras.optimizers import Adam 9 | #to save our model periodically as checkpoints for loading later 10 | from keras.callbacks import ModelCheckpoint 11 | #what types of layers do we want our model to have? 12 | from keras.layers import Lambda, Conv2D, MaxPooling2D, Dropout, Dense, Flatten 13 | #helper class to define input shape and generate training images given image paths & steering angles 14 | from utils import INPUT_SHAPE, batch_generator 15 | #for command line arguments 16 | import argparse 17 | #for reading files 18 | import os 19 | 20 | #for debugging, allows for reproducible (deterministic) results 21 | np.random.seed(0) 22 | 23 | 24 | def load_data(args): 25 | """ 26 | Load training data and split it into training and validation set 27 | """ 28 | #reads CSV file into a single dataframe variable 29 | data_df = pd.read_csv(os.path.join(os.getcwd(), args.data_dir, 'driving_log.csv'), names=['center', 'left', 'right', 'steering', 'throttle', 'reverse', 'speed']) 30 | 31 | #yay dataframes, we can select rows and columns by their names 32 | #we'll store the camera images as our input data 33 | X = data_df[['center', 'left', 'right']].values 34 | #and our steering commands as our output data 35 | y = data_df['steering'].values 36 | 37 | #now we can split the data into a training (80), testing(20), and validation set 38 | #thanks scikit learn 39 | X_train, X_valid, y_train, y_valid = train_test_split(X, y, test_size=args.test_size, random_state=0) 40 | 41 | return X_train, X_valid, y_train, y_valid 42 | 43 | 44 | def build_model(args): 45 | """ 46 | NVIDIA model used 47 | Image normalization to avoid saturation and make gradients work better. 48 | Convolution: 5x5, filter: 24, strides: 2x2, activation: ELU 49 | Convolution: 5x5, filter: 36, strides: 2x2, activation: ELU 50 | Convolution: 5x5, filter: 48, strides: 2x2, activation: ELU 51 | Convolution: 3x3, filter: 64, strides: 1x1, activation: ELU 52 | Convolution: 3x3, filter: 64, strides: 1x1, activation: ELU 53 | Drop out (0.5) 54 | Fully connected: neurons: 100, activation: ELU 55 | Fully connected: neurons: 50, activation: ELU 56 | Fully connected: neurons: 10, activation: ELU 57 | Fully connected: neurons: 1 (output) 58 | 59 | # the convolution layers are meant to handle feature engineering 60 | the fully connected layer for predicting the steering angle. 61 | dropout avoids overfitting 62 | ELU(Exponential linear unit) function takes care of the Vanishing gradient problem. 63 | """ 64 | model = Sequential() 65 | model.add(Lambda(lambda x: x/127.5-1.0, input_shape=INPUT_SHAPE)) 66 | model.add(Conv2D(24, 5, 5, activation='elu', subsample=(2, 2))) 67 | model.add(Conv2D(36, 5, 5, activation='elu', subsample=(2, 2))) 68 | model.add(Conv2D(48, 5, 5, activation='elu', subsample=(2, 2))) 69 | model.add(Conv2D(64, 3, 3, activation='elu')) 70 | model.add(Conv2D(64, 3, 3, activation='elu')) 71 | model.add(Dropout(args.keep_prob)) 72 | model.add(Flatten()) 73 | model.add(Dense(100, activation='elu')) 74 | model.add(Dense(50, activation='elu')) 75 | model.add(Dense(10, activation='elu')) 76 | model.add(Dense(1)) 77 | model.summary() 78 | 79 | return model 80 | 81 | 82 | def train_model(model, args, X_train, X_valid, y_train, y_valid): 83 | """ 84 | Train the model 85 | """ 86 | #Saves the model after every epoch. 87 | #quantity to monitor, verbosity i.e logging mode (0 or 1), 88 | #if save_best_only is true the latest best model according to the quantity monitored will not be overwritten. 89 | #mode: one of {auto, min, max}. If save_best_only=True, the decision to overwrite the current save file is 90 | # made based on either the maximization or the minimization of the monitored quantity. For val_acc, 91 | #this should be max, for val_loss this should be min, etc. In auto mode, the direction is automatically 92 | # inferred from the name of the monitored quantity. 93 | checkpoint = ModelCheckpoint('model-{epoch:03d}.h5', 94 | monitor='val_loss', 95 | verbose=0, 96 | save_best_only=args.save_best_only, 97 | mode='auto') 98 | 99 | #calculate the difference between expected steering angle and actual steering angle 100 | #square the difference 101 | #add up all those differences for as many data points as we have 102 | #divide by the number of them 103 | #that value is our mean squared error! this is what we want to minimize via 104 | #gradient descent 105 | model.compile(loss='mean_squared_error', optimizer=Adam(lr=args.learning_rate)) 106 | 107 | #Fits the model on data generated batch-by-batch by a Python generator. 108 | 109 | #The generator is run in parallel to the model, for efficiency. 110 | #For instance, this allows you to do real-time data augmentation on images on CPU in 111 | #parallel to training your model on GPU. 112 | #so we reshape our data into their appropriate batches and train our model simulatenously 113 | model.fit_generator(batch_generator(args.data_dir, X_train, y_train, args.batch_size, True), 114 | args.samples_per_epoch, 115 | args.nb_epoch, 116 | max_q_size=1, 117 | validation_data=batch_generator(args.data_dir, X_valid, y_valid, args.batch_size, False), 118 | nb_val_samples=len(X_valid), 119 | callbacks=[checkpoint], 120 | verbose=1) 121 | 122 | #for command line args 123 | def s2b(s): 124 | """ 125 | Converts a string to boolean value 126 | """ 127 | s = s.lower() 128 | return s == 'true' or s == 'yes' or s == 'y' or s == '1' 129 | 130 | 131 | def main(): 132 | """ 133 | Load train/validation data set and train the model 134 | """ 135 | parser = argparse.ArgumentParser(description='Behavioral Cloning Training Program') 136 | parser.add_argument('-d', help='data directory', dest='data_dir', type=str, default='data') 137 | parser.add_argument('-t', help='test size fraction', dest='test_size', type=float, default=0.2) 138 | parser.add_argument('-k', help='drop out probability', dest='keep_prob', type=float, default=0.5) 139 | parser.add_argument('-n', help='number of epochs', dest='nb_epoch', type=int, default=10) 140 | parser.add_argument('-s', help='samples per epoch', dest='samples_per_epoch', type=int, default=20000) 141 | parser.add_argument('-b', help='batch size', dest='batch_size', type=int, default=40) 142 | parser.add_argument('-o', help='save best models only', dest='save_best_only', type=s2b, default='true') 143 | parser.add_argument('-l', help='learning rate', dest='learning_rate', type=float, default=1.0e-4) 144 | args = parser.parse_args() 145 | 146 | #print parameters 147 | print('-' * 30) 148 | print('Parameters') 149 | print('-' * 30) 150 | for key, value in vars(args).items(): 151 | print('{:<20} := {}'.format(key, value)) 152 | print('-' * 30) 153 | 154 | #load data 155 | data = load_data(args) 156 | #build model 157 | model = build_model(args) 158 | #train model on data, it saves as model.h5 159 | train_model(model, args, *data) 160 | 161 | 162 | if __name__ == '__main__': 163 | main() 164 | 165 | -------------------------------------------------------------------------------- /self-driving-car.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# How to Make a Self-Driving Car\n", 8 | "\n", 9 | "Udacity recently open sourced their self driving car simulator\n", 10 | "originally built for SDND students\n", 11 | "\n", 12 | "![alt text](https://github.com/udacity/self-driving-car-sim/raw/master/sim_image.png \"Logo Title Text 1\")\n", 13 | "\n", 14 | "- built in Unity (free game making engine https://unity3d.com/)\n", 15 | "- add new tracks, change prebuilt scripts like gravity acceleration easily\n", 16 | "\n", 17 | "Code:\n", 18 | "https://github.com/udacity/self-driving-car-sim\n", 19 | " \n", 20 | "## Data Generation \n", 21 | "\n", 22 | "- records images from center, left, and right cameras w/ associated steering angle, speed, throttle and brake. \n", 23 | "- saves to CSV\n", 24 | "- ideally you have a joystick, but keyboard works too\n", 25 | "\n", 26 | "\n", 27 | "## Training Mode - Behavioral cloning\n", 28 | "\n", 29 | "We use a 9 layer convolutional network, based off of Nvidia's\n", 30 | "end-to-end learning for self driving car paper. 72 hours of driving data was collected in all sorts of conditions from human drivers\n", 31 | "\n", 32 | "https://www.youtube.com/watch?v=NJU9ULQUwng\n", 33 | "\n", 34 | "#### Hardware design:\n", 35 | "\n", 36 | "![alt text](https://devblogs.nvidia.com/parallelforall/wp-content/uploads/2016/08/data-collection-system.png \"Logo Title Text 1\")\n", 37 | "\n", 38 | "- 3 cameras\n", 39 | "- The steering command is obtained by tapping into the vehicle’s Controller Area Network (CAN) bus.\n", 40 | "- Nvidia's Drive PX onboard computer with GPUs\n", 41 | "\n", 42 | "In order to make the system independent of the car geometry, the steering command is 1/r, where r is the turning radius in meters. 1/r was used instead of r to prevent a singularity when driving straight (the turning radius for driving straight is infinity). 1/r smoothly transitions through zero from left turns (negative values) to right turns (positive values).\n", 43 | "\n", 44 | "\n", 45 | "#### Software Design (supervised learning!) :\n", 46 | "\n", 47 | "![alt text](https://devblogs.nvidia.com/parallelforall/wp-content/uploads/2016/08/training.png \"Logo Title Text 1\")\n", 48 | "\n", 49 | "Images are fed into a CNN that then computes a proposed steering command. The proposed command is compared to the desired command for that image, and the weights of the CNN are adjusted to bring the CNN output closer to the desired output. The weight adjustment is accomplished using back propagation\n", 50 | "\n", 51 | "Eventually, it generated steering commands using just a single camera\n", 52 | "\n", 53 | "![alt text](https://devblogs.nvidia.com/parallelforall/wp-content/uploads/2016/08/inference.png \"Logo Title Text 1\")\n", 54 | "\n", 55 | "## Testing mode\n", 56 | "\n", 57 | "We will just run autonomous mode, then run our model and the car will start driving\n", 58 | "\n", 59 | "![alt text](https://cdn-images-1.medium.com/max/1440/1*nlusa_fC5BnsgnWPFnov7Q.tiff \"Logo Title Text 1\")\n", 60 | "\n" 61 | ] 62 | }, 63 | { 64 | "cell_type": "code", 65 | "execution_count": null, 66 | "metadata": {}, 67 | "outputs": [], 68 | "source": [ 69 | "# Step 1 - Install dependencies\n", 70 | "\n", 71 | "#TensorFlow without GPU\n", 72 | "conda env create -f environments.yml \n", 73 | "\n", 74 | "#Use TensorFlow with GPU\n", 75 | "conda env create -f environments-gpu.yml\n", 76 | "\n", 77 | "\n", 78 | "#Step 2 - Generate Data\n", 79 | "\n", 80 | "#Step 3 - Write Training script\n", 81 | "\n", 82 | "#Step 4 - Train\n", 83 | "\n", 84 | "#Step 5 - Write Testing script\n", 85 | "\n" 86 | ] 87 | } 88 | ], 89 | "metadata": { 90 | "kernelspec": { 91 | "display_name": "Python 3", 92 | "language": "python", 93 | "name": "python3" 94 | }, 95 | "language_info": { 96 | "codemirror_mode": { 97 | "name": "ipython", 98 | "version": 3 99 | }, 100 | "file_extension": ".py", 101 | "mimetype": "text/x-python", 102 | "name": "python", 103 | "nbconvert_exporter": "python", 104 | "pygments_lexer": "ipython3", 105 | "version": "3.5.2" 106 | }, 107 | "widgets": { 108 | "state": {}, 109 | "version": "1.1.2" 110 | } 111 | }, 112 | "nbformat": 4, 113 | "nbformat_minor": 2 114 | } 115 | -------------------------------------------------------------------------------- /utils.py: -------------------------------------------------------------------------------- 1 | import cv2, os 2 | import numpy as np 3 | import matplotlib.image as mpimg 4 | 5 | 6 | IMAGE_HEIGHT, IMAGE_WIDTH, IMAGE_CHANNELS = 66, 200, 3 7 | INPUT_SHAPE = (IMAGE_HEIGHT, IMAGE_WIDTH, IMAGE_CHANNELS) 8 | 9 | 10 | def load_image(data_dir, image_file): 11 | """ 12 | Load RGB images from a file 13 | """ 14 | return mpimg.imread(os.path.join(data_dir, image_file.strip())) 15 | 16 | 17 | def crop(image): 18 | """ 19 | Crop the image (removing the sky at the top and the car front at the bottom) 20 | """ 21 | return image[60:-25, :, :] # remove the sky and the car front 22 | 23 | 24 | def resize(image): 25 | """ 26 | Resize the image to the input shape used by the network model 27 | """ 28 | return cv2.resize(image, (IMAGE_WIDTH, IMAGE_HEIGHT), cv2.INTER_AREA) 29 | 30 | 31 | def rgb2yuv(image): 32 | """ 33 | Convert the image from RGB to YUV (This is what the NVIDIA model does) 34 | """ 35 | return cv2.cvtColor(image, cv2.COLOR_RGB2YUV) 36 | 37 | 38 | def preprocess(image): 39 | """ 40 | Combine all preprocess functions into one 41 | """ 42 | image = crop(image) 43 | image = resize(image) 44 | image = rgb2yuv(image) 45 | return image 46 | 47 | 48 | def choose_image(data_dir, center, left, right, steering_angle): 49 | """ 50 | Randomly choose an image from the center, left or right, and adjust 51 | the steering angle. 52 | """ 53 | choice = np.random.choice(3) 54 | if choice == 0: 55 | return load_image(data_dir, left), steering_angle + 0.2 56 | elif choice == 1: 57 | return load_image(data_dir, right), steering_angle - 0.2 58 | return load_image(data_dir, center), steering_angle 59 | 60 | 61 | def random_flip(image, steering_angle): 62 | """ 63 | Randomly flipt the image left <-> right, and adjust the steering angle. 64 | """ 65 | if np.random.rand() < 0.5: 66 | image = cv2.flip(image, 1) 67 | steering_angle = -steering_angle 68 | return image, steering_angle 69 | 70 | 71 | def random_translate(image, steering_angle, range_x, range_y): 72 | """ 73 | Randomly shift the image virtially and horizontally (translation). 74 | """ 75 | trans_x = range_x * (np.random.rand() - 0.5) 76 | trans_y = range_y * (np.random.rand() - 0.5) 77 | steering_angle += trans_x * 0.002 78 | trans_m = np.float32([[1, 0, trans_x], [0, 1, trans_y]]) 79 | height, width = image.shape[:2] 80 | image = cv2.warpAffine(image, trans_m, (width, height)) 81 | return image, steering_angle 82 | 83 | 84 | def random_shadow(image): 85 | """ 86 | Generates and adds random shadow 87 | """ 88 | # (x1, y1) and (x2, y2) forms a line 89 | # xm, ym gives all the locations of the image 90 | x1, y1 = IMAGE_WIDTH * np.random.rand(), 0 91 | x2, y2 = IMAGE_WIDTH * np.random.rand(), IMAGE_HEIGHT 92 | xm, ym = np.mgrid[0:IMAGE_HEIGHT, 0:IMAGE_WIDTH] 93 | 94 | # mathematically speaking, we want to set 1 below the line and zero otherwise 95 | # Our coordinate is up side down. So, the above the line: 96 | # (ym-y1)/(xm-x1) > (y2-y1)/(x2-x1) 97 | # as x2 == x1 causes zero-division problem, we'll write it in the below form: 98 | # (ym-y1)*(x2-x1) - (y2-y1)*(xm-x1) > 0 99 | mask = np.zeros_like(image[:, :, 1]) 100 | mask[(ym - y1) * (x2 - x1) - (y2 - y1) * (xm - x1) > 0] = 1 101 | 102 | # choose which side should have shadow and adjust saturation 103 | cond = mask == np.random.randint(2) 104 | s_ratio = np.random.uniform(low=0.2, high=0.5) 105 | 106 | # adjust Saturation in HLS(Hue, Light, Saturation) 107 | hls = cv2.cvtColor(image, cv2.COLOR_RGB2HLS) 108 | hls[:, :, 1][cond] = hls[:, :, 1][cond] * s_ratio 109 | return cv2.cvtColor(hls, cv2.COLOR_HLS2RGB) 110 | 111 | 112 | def random_brightness(image): 113 | """ 114 | Randomly adjust brightness of the image. 115 | """ 116 | # HSV (Hue, Saturation, Value) is also called HSB ('B' for Brightness). 117 | hsv = cv2.cvtColor(image, cv2.COLOR_RGB2HSV) 118 | ratio = 1.0 + 0.4 * (np.random.rand() - 0.5) 119 | hsv[:,:,2] = hsv[:,:,2] * ratio 120 | return cv2.cvtColor(hsv, cv2.COLOR_HSV2RGB) 121 | 122 | 123 | def augument(data_dir, center, left, right, steering_angle, range_x=100, range_y=10): 124 | """ 125 | Generate an augumented image and adjust steering angle. 126 | (The steering angle is associated with the center image) 127 | """ 128 | image, steering_angle = choose_image(data_dir, center, left, right, steering_angle) 129 | image, steering_angle = random_flip(image, steering_angle) 130 | image, steering_angle = random_translate(image, steering_angle, range_x, range_y) 131 | image = random_shadow(image) 132 | image = random_brightness(image) 133 | return image, steering_angle 134 | 135 | 136 | def batch_generator(data_dir, image_paths, steering_angles, batch_size, is_training): 137 | """ 138 | Generate training image give image paths and associated steering angles 139 | """ 140 | images = np.empty([batch_size, IMAGE_HEIGHT, IMAGE_WIDTH, IMAGE_CHANNELS]) 141 | steers = np.empty(batch_size) 142 | while True: 143 | i = 0 144 | for index in np.random.permutation(image_paths.shape[0]): 145 | center, left, right = image_paths[index] 146 | steering_angle = steering_angles[index] 147 | # argumentation 148 | if is_training and np.random.rand() < 0.6: 149 | image, steering_angle = augument(data_dir, center, left, right, steering_angle) 150 | else: 151 | image = load_image(data_dir, center) 152 | # add the image and steering angle to the batch 153 | images[i] = preprocess(image) 154 | steers[i] = steering_angle 155 | i += 1 156 | if i == batch_size: 157 | break 158 | yield images, steers 159 | 160 | --------------------------------------------------------------------------------