Photos: Building a frame for the LIDAR, Camera and GPS antenna - 4 hours and 65$ aproximately

Driving toyota prius using joystick

This is initial test of car control using a joystick.

$8 car control for self driving car

See it at: https://kuasha.github.io/ccwriteup/carcontrol.html

Building a realistic car simulator

As part of the Udacity Self Racing Cars team I worked with the simulator to test our models without the real car. Looking at ebay listing of toyota prius steering column I have found that it is cheap to buy the real parts to build a more realistic simulator.

I am thinking of buying an ECU and other parts to build a more realistic simulator where I can steer/ brake / accel the car from these hardware setup in front of the computer.

Self Driving Car Tutorial (Part N) : Developing convolutional neural network

I have been talking to a friend about building a self driving car and told him that I can show him how to build it it few short sessions. He seemed interested. So, I decided to write up some tutorials to show how its done.I am starting from the end- because its the one on which I am currently working. This and a lot of interesting stuff is taught in Udacity Self Driving Car Engineer Nanodegree program. Check that out at Udacity.com.

This is how it looks when I used the network (with some max-pooling and dropout layers) for a fully autonomous drive on Udacity simulator:

I am taking the convolutional neural network developed at NVIDIA research (this is a tutorial - so we should take existing research work rather than creating our own) in this tutorial. The paper can be found at NVIDIA Self Driving Car.

Below is how their neural network looks.

I'll go step by step how to build the network. The network is shown in the bottom up structure in the image. At the bottom we provide a 66x200 size image that has 3 color layers (RGB). Then it is normalized. We'll start from the normalized layer. So our input size is 66x200 and depth is 3.

First lets take a camera image.

This image is of size: 160x320. We resize it to 100x200 and crop out top 34 pixels. This can be done using OpenCV like below:

image = cv2.imread("./sample.jpg")
img = cv2.resize(image, (200,100))
crp=img[34:,:]
plt.imshow(crp)

This can be done in the model so that the cropping is done on the GPU:

model.add(Cropping2D(cropping=((68,0),(0,0))))

And we get an image like this with shape 3@66x200:

Now we will use keras to build the neural network. In my setup I am using tensorflow as the keras back end. Lets create a sequential network:

input_shape = (66, 200, 3)
net = Sequential()

Now lets add the normalization layer:

model.add(Lambda(lambda x: x / 255.0 - 0.5, input_shape=(160,320,3)))

From the network image above we need a 5x5 convolutional layer - we'll use ReLU activation which is a function that basically sets all negative values to zero:

layer1 = Convolution2D(24, 5, 5, 
              input_shape=input_shape, border_mode='valid', activation='relu')
net.add(layer1)
#output size = 24@31x94

From network we see that we have 4 more convolutional layers:

net.add(Convolution2D(36, 5, 5, border_mode='valid', activation='relu'))
#output size = 36@14x47

net.add(Convolution2D(48, 5, 5, border_mode='valid', activation='relu'))
#output size = 48@5x22

net.add(Convolution2D(64, 3, 3, border_mode='valid', activation='relu'))
#output size = 64@3x20

net.add(Convolution2D(64, 3, 3, border_mode='valid', activation='relu'))
#output size = 64@1x18

Now we add the flatten layer:

net.add(Flatten())

Simple huh? Now we add a fully connected layer (Dense layer) of size 1156:

net.add(Dense(115))

We then add remaining 4 dense layer as shown in the network image:

net.add(Dense(100))
net.add(Dense(50))
net.add(Dense(10))
net.add(Dense(1))

That's it. We have build our network. Lets compile the network and see the summary of the network to make sure we have done it right:

net.compile(loss='mean_squared_error', optimizer='adam')
net.summary()

Here is the summary output:

____________________________________________________________________________________________________
Layer (type)                     Output Shape          Param #     Connected to                     
====================================================================================================
convolution2d_1 (Convolution2D)  (None, 62, 196, 24)   1824        convolution2d_input_1[0][0]      
____________________________________________________________________________________________________
convolution2d_2 (Convolution2D)  (None, 58, 192, 36)   21636       convolution2d_1[0][0]            
____________________________________________________________________________________________________
convolution2d_3 (Convolution2D)  (None, 54, 188, 48)   43248       convolution2d_2[0][0]            
____________________________________________________________________________________________________
convolution2d_4 (Convolution2D)  (None, 52, 186, 64)   27712       convolution2d_3[0][0]            
____________________________________________________________________________________________________
convolution2d_5 (Convolution2D)  (None, 50, 184, 64)   36928       convolution2d_4[0][0]            
____________________________________________________________________________________________________
flatten_1 (Flatten)              (None, 588800)        0           convolution2d_5[0][0]            
____________________________________________________________________________________________________
dense_1 (Dense)                  (None, 1156)          680653956   flatten_1[0][0]                  
____________________________________________________________________________________________________
dense_2 (Dense)                  (None, 100)           115700      dense_1[0][0]                    
____________________________________________________________________________________________________
dense_3 (Dense)                  (None, 50)            5050        dense_2[0][0]                    
____________________________________________________________________________________________________
dense_4 (Dense)                  (None, 10)            510         dense_3[0][0]                    
____________________________________________________________________________________________________
dense_5 (Dense)                  (None, 1)             11          dense_4[0][0]                    
====================================================================================================
Total params: 680,906,575
Trainable params: 680,906,575
Non-trainable params: 0
________________________

Looks good. Now we can generate inputs by driving our car with camera attached and a way to measure steering angle and train the network.

The output of the network is steering angle. So given a new image the network will tell what should be the cars steering angle. With right training the car should be bale to steer a car given there is mechanical / electrical components to steer the wheel.

Now sit back and relax while the car is being driven by the network.

Driving Udacity car simulator with home made steering wheel (!)

OK, its really a bar than a wheel-

I took an old servo motor and attached a wooden bar and removed all circuitry keeping the variable resistor. Then measured the position of the wheel using an arduino and sending that data to PC using serial port.
The middle pin of variable resistor is attached to A0 pin of the arduino and other two pins are attached to ground and +5v. This is creating a voltage divider and the measurement ranges from 0 to 1024. The measurement of 512 is received when the steering position is straight ahead.

The code

Here is arduino code that reads analog input from A0 and send the value to serial port. The code is adapted from Arduino IDE Analog sketch demo with minor changes.

int sensorPin = A0;    // select the input pin for the potentiometer
int sensorValue = 0;  // variable to store the value coming from the sensor
int newSensorValue = 0;
void setup() {
  Serial.begin(9600);
}

void loop() {  
  // read the value from the sensor:
  newSensorValue = analogRead(sensorPin);
  if(abs(newSensorValue - sensorValue) > 5){
    sensorValue = newSensorValue;
    Serial.println(sensorValue);
  }
  delay(10);
}

Here is python code that reads the data from arduino serial port and update angle variable. This angle is that is being sent to the simulator:

import serial
from time import sleep
import threading

port = "/dev/ttyACM1"
ser = serial.Serial(port, 9600)
angle = 0.0

def read_steering_angle():
    print("Monitoring steering angle")
    global angle
    while True:
        data = ser.readline()
        if len(data) > 0:
            #print('Got serial data:', data)

            angle = float(int(data) - 512) / 50.0

    return angle

t = threading.Thread(target=read_steering_angle)
t.daemon = True
t.start()


while True:
    print(angle)
    sleep(1)

Demo

Here is how I am driving using the steering.

Here is screen capture of driving with the steering.

Tracking lanes on real hardware

The car hardware can be controlled easily from the drive module. I am using arduino to control the drive wheel and steering. I use COM over USB  to communicate with arduino from drive module.

The localization is not working yet and I need to create a PID controller. I am excited to finish last bit of kalman filter to track lanes correctly, localization using particle filter and PID controller to drive the car.

Tracking lanes

I have taken the caltech cordova1 dataset (search of the term will find the location to download) to test the algorithm I develop to detect lanes. Here is the algorithm in short (without kalman filter and noise elimination):

First convert to bird eye view of the frame

Then convert the image to grayscale
Apply Gaussian filter to smooth out outliers
Use canny method to create binary image (detect edges)
Run probabilistic hough transform to get the lines
Remove lines with wrong angle and size
Use perspective transform to get image co-ordinates
Draw the lines on original frame

Draw a green line to show the vehicle path and yellow circle to show the vanishing point.

Next step would be to kalman filter to deal with noises present in the detection using vehicle speed/ acceleration (and may be other sensors) to establish prior belief. Then will use the current detected lanes and update the belief. I'll be using estimated vehicle speed and use dataset 1 for training and then test using other datasets to measure its performance.  Once it works I'll be using RANSAC to avoid going through all the points - its kind of slow otherwise (if Jetson TX1 performs around 20 fps I probably wont bother).

Calculating Cross Track Error (CTE) and steering the robot

I am one step ahead to build a full self driving toy car which is a big step toward creating a full size car.

I am now able to calculate cross track error and steer the robots front wheel to correct it. The steering driver is very crude at this moment- it has three positions Left, Middle and Right. I'll have to send more positions using a second parameter that tells exact angle. The servo motor can position itself between 0 to 180 degree angle with 1 degree resolution where 90 degree is straight forward. Wanted to make it work first then make it precise. Here is the video- please pay attention to the robots front wheel and where the lane is:

I first detect two lane marking lines and group them together (if one line is in 100 pixel, configurable, from another line they form one group). Then calculate the mid point for each group that gives me the list of lane marking. The lanes can be curved (I have to calculate a 3rd degree  bezier spline instead of straight lines that I use right now) - but still all lines should be within 100 pixel of each other for one lane marking- its like the snake algorithm to track the lanes while not exact. 

After I get all the lane markings I take the middle point of the lane from each neighboring pair of lane marking. Then from the image position I get the current car/ camera position and calculate the cross track error (CTE) by getting the difference from the lane mid point. 

I use kalman filter to eliminate single frame errors and over multiple frames the values stables (in less than 20 frames it becomes almost correct). Jetson TX1 GPU array can process 20 frames per second without any optimization effort. Here is how it looks for a single frame (this is bird eye view- transformed using wrap transform of original frame):

The image shows detected lane marking using thicker straight line, lane mid point (green) and current car position (pink) and on top it shows frame #, car position, CTE value and which way it should steer the the car.

Detecting lines using proabilistic hough transform

We use Canny algorithm to create a binary image and then use the HoughLinesP function detects line in binary image.

First we take the bird eye view image of straight labe for processing.

When the HoughLinesP is applied we get following resulting lines:

Then we take a curved lane image

When the HoughLinesP is applied we get following resulting lines:

Here is the code.

int hough(const char *input_path, const char *output_path)
{
    Mat src_o, src, dst;
    src_o = imread(input_path);

    cvtColor(src_o, src, CV_BGR2GRAY );

    Canny(src, dst, 200, 255, 3 );

    vector lines;
    HoughLinesP(dst, lines, 1, CV_PI/180, 20, 10, 20 );

    std::map  candidate;

    for( size_t i = 0; i < lines.size(); i++ )
    {
        line( src_o, Point(lines[i][0], lines[i][1]),
              Point(lines[i][2], lines[i][3]), Scalar(0,0,255), 3, 8 );
    }

    imwrite(output_path, src_o);

    return 0;
}

Next step is to take the lines and calculate the lane midpoint.

The self driving car platform (GTAR)

We are using the Traxxas Slash 4x4 and Jetson TX1 (which provides 1 TeraFLOPs peak performance) to build the self driving car platform.

First we remove the body and the radio controller from the car and added 4 narrow bolts to hold to plastic platforms.

We are going to use two piece of plastic sheets like below. Both has a few holes.

Here is how it looks after adding the lower platform.

Now we detach the Jetson TX1 bottom plastic base and remove the plastic legs.

Attach the Jetson TX1 plastic base to the lower platform. The onboard camera is facing front.

Add the Jetson TX1 board and the power supply to the platform

.

And add the upper platform and the sonar sensor in the front.

I have not received the camera yet. The camera will be placed on the upper platform and the GPS, gyroscope and compass will also be placed on the top platform.

The lower platform needs some cut to free the front and rear wheels. Other than that its complete and we are ready to add the remaining electronics and start coding.

Finally the sensor detecting obstacle in front of it: