# Content tagged self-driving-car

## Tags

## Months

## Intro

The goal of this project is to plan a path for a car to make its way through highway traffic. You have at your disposal a map consisting of a set of waypoints. The waypoints follow the center of the road, and each of them comes with the normal vector. You also know the positions and velocities of nearby vehicles. Your car needs to obey the speed limit of 50 MPH (22.35 m/s), not collide with other traffic, keep acceleration within certain limits, and minimize jerk (the time derivative of acceleration). The path that you need to compute is a set of successive Cartesian coordinates that the car will visit perfectly every 0.02 seconds.

Here's the result I got. It's not completely optimal at times, but I used large safety margins and did not spend much time tweaking it.

## Path planning

Udacity recommends using minimum jerk trajectories defined in the Frenet coordinate space to solve the path planning problem. This approach, however, struck me as suboptimal and hard to do well because of the distance distortions caused by the nonlinearity of the coordinate-space transforms between Cartesian and Frenet and back. Therefore, I decided to reuse the code I wrote for doing model-predictive control. However, instead of using actuations computed by the algorithm, I used the model states themselves as a source of coordinates that the vehicle should visit.

The controller follows a trajectory defined as a polynomial fitted to the waypoints representing the center of one of the lanes. In the first step, it computes 75 points starting from the current position of the car. In each successive step, it takes 25 points from the previously computed trajectory that the car still did not visit, it estimates the vehicle's state at the 25th point and uses this estimated state as an input to the solver. The points produced this way complement the path. This cycle repeats itself every 250 ms with the target speed given by:

\[ v = \begin{cases} v_l - 0.25 \cdot (25 - d_l) & d_l \leq 25 \\ 22.2 & d_l \gt 25 \\ \end{cases} \]Where \( v_l \) is the speed of the leader and \( d_l \) is the car's distance from it. Subtracting the proximity penalty tends to make speed "bounce" less than when using fractional terms. I was too lazy to tune a PID controller for this task.

## Lane selection

The most optimal lane is selected using a simple finite state machine with cost functions associated with state transitions. Every three seconds the algorithm evaluates these functions for all reachable states and chooses the cheapest move.

The cost function for keeping the current lane (the `KL2KL`

transition)
penalizes the difference in speed between the vehicle and the leader (if any)
and their proximity. The one for changing lanes (the `KL2CL`

transition) does a
similar calculation for the target lane. Additionally, it increases the cost if
a follower is close enough adding a further penalty proportional to its speed.

The start up state allows the car to ramp up to the target speed before the actual lane-change logic kicks in. Doing so avoids erratic behavior arising from the penalization of speed differences.

The exact logic is here.

## Conclusions

Using the MPC approach has the advantage of letting you tune more parameters of the trajectory. You can choose how much acceleration and jerk you're going to tolerate, increasing or decreasing the perceived aggressiveness of lane changes.

I also tried incorporating the predicted trajectories of nearby vehicles into the solver's cost function, but the car ended up going out of the lane, accelerating or breaking aggressively to avoid collisions. It is something that you would do while driving in the real world, but it breaks the rules of this particular game. Another problem was that I used a Naïve Bayes Classifier to predict the behavior of other vehicles and it is not accurate enough. The mispredictions made the car behave erratically trying to avoid collisions with non-existing obstacles.

You can see the full code here.

## What is it about?

Semantic segmentation is a process of dividing an image into sets of pixels sharing similar properties and assigning to each of these sets one of the pre-defined labels. Ideally, you would like to get a picture such as the one below. It's a result of blending color-coded class labels with the original image. This sample comes from the CityScapes dataset.

## How is it done?

Figuring out object boundaries in an image is hard. There's a variety of "classical" approaches taking into account colors and gradients that obtained encouraging results, see this paper by Shi and Malik for example. However, in 2015 and 2016, Long, Shelhamer, and Darrell presented a method using Fully Convolutional Networks that significantly improved the accuracy (the mean intersection over union metric) and the inference speed. My goal was to replicate their architecture and use it to segment road scenes.

A fully convolutional network differs from a regular convolutional network in the fact that it has the final fully-connected classifier stripped off. Its goal is to take an image as an input and produce an equally-sized output in which each pixel is represented by a softmax distribution describing the probability of this pixel belonging to a given class. I took this picture from one of the papers mentioned above:

For the results presented in this post, I used the pre-trained VGG16 network provided by Udacity for the beta test of their Advanced Deep Learning Capstone. I took layers 3, 4, and 7 and combined them in the manner described in the picture below, which, again, is taken from one of the papers by Long et al.

First, I used a 1x1 convolutions on top of each extracted layer to act as a local classifier. After doing that, these partial results are still 32, 16, and 8 times smaller than the input image, so I needed to upsample them (see below). Finally, I used a weighted addition to obtain the result. The authors of the original paper report that without weighting the learning process diverges.

## Learnable Upsampling

Upsampling is done by applying a process called transposed convolution. I will not describe it here because this post over at cv-tricks.com does a great job of doing that. I will just say that transposed convolutions (just like the regular ones) use learnable weights to produce output. The trick here is the initialization of those weights. You don't use the truncated normal distribution, but you initialize the weights in such a way that the convolution operation performs a bilinear interpolation. It's easy and interesting to test whether the implementation works correctly. When fed an image, it should produce the same image but n times larger.

```
1 img = cv2.imread(sys.argv[1])
2 print('Original size:', img.shape)
3
4 imgs = np.zeros([1, *img.shape], dtype=np.float32)
5 imgs[0,:,:,:] = img
6
7 img_input = tf.placeholder(tf.float32, [None, *img.shape])
8 upscale = upsample(img_input, 3, 8, 'upscaled')
9
10 with tf.Session() as sess:
11 sess.run(tf.global_variables_initializer())
12 upscaled = sess.run(upscale, feed_dict={img_input: imgs})
13
14 print('Upscaled:', upscaled.shape[1:])
15 cv2.imwrite(sys.argv[2], upscaled[0,:, :, :])
```

Where `upsample`

is defined here.

## Datasets

I was mainly interested in road scenes, so I played with the KITTI Road and CityScapes datasets. The first one has 289 training images with two labels (road/not road) and 290 testing samples. The second one has 2975 training, 500 validation, and 1525 testing pictures taken while driving around large German cities. It has fine-grained annotations for 29 classes (including "unlabeled" and "dynamic"). The annotations are color-based and look like the picture below.

Even though I concentrated on those two datasets, both the training and the
inference software is generic and can handle any pixel-labeled dataset. All you
need to do is to create a new `source_xxxxxx.py`

file defining your custom
samples. The definition is a class that contains seven attributes:

`image_size`

- self-evident, both horizontal and vertical dimensions need to be divisible by 32`num_classes`

- number of classes that the model is supposed to handle`label_colors`

- a dictionary mapping a class number to a color; used for blending of the classification results with input image`num_training`

- number of training samples`num_validation`

- number of validation samples`train_generator`

- a generator producing training batches`valid_generator`

- a generator producing validation batches

See `source_kitti.py`

or `source_cityscapes.py`

for a concrete
example. The training script picks the source based on the value of the
`--data-source`

parameter.

## Normalization

Typically, you would normalize the input dataset such that its mean is at zero
and its standard deviation is at one. It significantly improves convergence of
the gradient optimization. In the case of the VGG model, the authors just zeroed
the mean without scaling the variance (see section 2.1 of the paper).
Assuming that the model was trained on the ImageNet dataset, the mean values for
each channel are `muR = 123.68`

, `muG = 116.779`

, `muB = 103.939`

. The
pre-trained model provided by Udacity already has a pre-processing layer
handling these constants. Judging from the way it does it, it expects plain BGR
scaled between 0 and 255 as input.

## Label Validation

Since the network outputs softmaxed logits for each pixel, the training labels need to be one-hot encoded. According to the TensorFlow documentation, each row of labels needs to be a proper probability distribution. Otherwise, the gradient calculation will be incorrect and the whole model will diverge. So, you need to make sure that you're never in a situation where you have all zeros or multiple ones in your label vector. I have made this mistake so many time that I decided to write a checker script for my data source modules. It produces examples of training images blended with their pixel labels to check if the color maps have been defined correctly. It also checks every pixel in every sample to see if the label rows are indeed valid. See here for the source.

## Initialization of variables

Initialization of variables is a bit of a pain in TensorFlow. You can use the global initializer if you create and train your model from scratch. However, in the case when you want to do transfer learning - load a pre-trained model and extend it - there seems to be no convenient way to initialize only the variables that you created. I ended up doing acrobatics like this:

```
1 uninit_vars = []
2 uninit_tensors = []
3 for var in tf.global_variables():
4 uninit_vars.append(var)
5 uninit_tensors.append(tf.is_variable_initialized(var))
6 uninit_bools = sess.run(uninit_tensors)
7 uninit = zip(uninit_bools, uninit_vars)
8 uninit = [var for init, var in uninit if not init]
9 sess.run(tf.variables_initializer(uninit))
```

## Training

For training purposes, I reshaped both labels and logits in such a way that I
ended up with 2D tensors for both. I then used
`tf.nn.softmax_cross_entropy_with_logits`

as a measure of loss and used
`AdamOptimizer`

with a learning rate of 0.0001 to minimize it. The model trained
on the KITTI dataset for 500 epochs - 14 seconds per epoch on my GeForce GTX
1080 Ti. The CityScapes dataset took 150 epochs to train - 9.5 minutes per epoch
on my GeForce vs. 25 minutes per epoch on an AWS P2 instance. The model
exhibited some overfitting. However, the visual results seemed tighter the more
it trained. In the picture below the top row contains the ground truth, the
bottom one contains the inference results (TensorBoard rocks! :).

## Results

The inference (including image processing) takes 80 milliseconds per image on average for CityScapes and 27 milliseconds for KITTI. Here are some examples from both datasets. The model seems to be able to distinguish a pedestrian from a bike rider with some degree of accuracy, which is pretty impressive!

Go here for the full code.

## The algorithm

I took courses on probability, statistics, and the Monte Carlo methods while I was at school, but the ubiquity of the algorithms based on randomness still amazes me. Imagine that you are a robot moving in a 2D space. You have a map of the area, and you know your rough initial position in it. Every step you take, you get data from the controls (speed and angular speed) that is quite noisy. You can also sense obstacles within a certain range and with a certain precision with respect to your current position and heading. How do you figure out where you are?

One good strategy to solving this problem is using a particle filter. You start by creating N "particles," or guesses as to where you are, by drawing the x- and y- position as well as the heading from the Gaussian distribution around your initial estimate. Then, every step you take, for every particle, you:

- move according to the data you got from controls taking the noise into account;
- match the sensor data to the landmarks on the map using the perspective of the particle;
- assign weight to the particle based on how well the observation matches the map.

Finally, you draw with replacement N particles from the initial set with the probability proportional to the weights. The particles that match the observation well will likely be drawn multiple times and those that don't are unlikely to be drawn at all. The repetitions are not a problem because the movement is noisy, so they will diverge after the next step you take. The particle with the highest weight is your best estimate of your actual position and heading.

## The result

I did an experiment on the Udacity data, and the approach using a 1000 particles turned out to work well comparing to using just one. The average deviation from the ground truth was around 10cm. Using one particle effectively ignores the observation data and relies only on the controls. You can follow the blue diamond in the video below to see how fast the effects of the noise accumulate. Both cases use the same noise values.

## The project

I try to avoid publishing my code solving homework assignments, but this Udacity SDC project is generic enough to be useful in a wider context. So here you have it. The task was to fuse together radar and lidar measurements using two kinds of Kalman Filters to estimate the trajectory of a moving bicycle. The unscented filter uses the CTRV model tracking the position, speed, yaw, and yaw rate, whereas the extended filter uses the constant velocity model.

Both algorithms performed well, with the CTRV model predicting the velocity significantly better. The values below are RMSEs of the prediction against the ground truth. The first two values represent the position, the last two - the velocity.

Extended filter:

```
]==> ./ExtendedKF ../data/obj_pose-laser-radar-synthetic-input.txt ../src/ekf.txt
Accuracy - RMSE:
0.0973826
0.0853209
0.441738
0.453757
```

Unscented filter:

```
]==> ./UnscentedKF ../data/obj_pose-laser-radar-synthetic-input.txt ../src/ukf.txt
Accuracy - RMSE:
0.0659867
0.0811041
0.277747
0.166186
```

## The code

I wrote a handy library that does most of the math and provides various concrete implementations of Kalman predictors and updaters:

```
1 class KalmanPredictor {
2 public:
3 virtual ~KalmanPredictor() {}
4 virtual void Predict(KalmanState &state, uint64_t dt) = 0;
5 };
6
7 class KalmanUpdater {
8 public:
9 virtual ~KalmanUpdater() {}
10 virtual void Update(KalmanState &state,
11 const Eigen::VectorXd &z) = 0;
12 };
```

The code you need to implement yourself depends on the sensor, the model, and the type of the filter you use. Ie., for the CTRV model and a Lidar measurement you only need to specify the projection matrix and the sensor noise covariance:

```
1 class LidarUpdater: public LinearKalmanUpdater {
2 public:
3 LidarUpdater() {
4 H_ = MatrixXd(2, 5);
5 H_ << 1, 0, 0, 0, 0,
6 0, 1, 0, 0, 0;
7
8 R_ = MatrixXd(2, 2);
9 R_ << 0.0225, 0,
10 0, 0.0225;
11 }
12 };
```

See here and here for more examples. The state travels around in an object of the KalmanState class:

```
1 struct KalmanState {
2 KalmanState(int n) {
3 x = Eigen::VectorXd(n);
4 P = Eigen::MatrixXd(n, n);
5 x.fill(0.0);
6 P.fill(0.0);
7 }
8 Eigen::VectorXd x; // mean
9 Eigen::MatrixXd P; // covariance
10 Eigen::MatrixXd sigma_points; // sigma points
11 double nis = 0; // Normalized Innovation Squared
12 };
```

All this ends up with the measurement update code boiling down to this:

```
1 double dt = measurement.timestamp - previous_timestamp_;
2 previous_timestamp_ = measurement.timestamp;
3 predictor_->Predict(state_, dt);
4 updaters_[measurement.sensor_type]->Update(state_, measurement.data);
5 return state_;
```

See the full code on GitHub.

## The challenge

Here's another cool project I have done as a part of the Udacity's self-driving car program. There were two problems solve. The first one was to find the lane lines and compute some of their properties. The second one was to detect and draw bounding boxes around nearby vehicles. Here's the result I got:

## Detecting lanes

The first thing I do after correcting for camera lens distortion is applying a combination of Sobel operators and color thresholding to get an image of edges. This operation makes lines more pronounced and therefore much easier to detect.

I then get a birds-eye view of the scene by applying a perspective transform and produce a histogram of all the white pixels located in the bottom half of the image. The peaks in this histogram indicate the presence of mostly vertical lines, which is what we're looking for. I detect all these lines by using a sliding window search. I start at the bottom of the image and move towards the top adjusting the horizontal position of each successive window to the average of the x coordinate of all the pixels contained in the previous one. Finally, I fit a parabola to all these pixels. Out of all the candidates detected this way, I select a pair that is the closest to being parallel and is roughly in the place where a lane line would be expected.

The orange area in the picture below visualizes the histogram, and the red boxes
with blue numbers in them indicate the positions of the peaks found by the
`find_peaks_cwt`

function from scipy.

Once I have found the lanes in one video frame, locating them in the next one is much simpler - their position did not change by very much. I just take all the pixels from a close vicinity of the previous detection and fit a new polynomial to them. The green area in the image below denotes the search range, and the blue lines are the newly fitted polynomials.

I then use the equations of the parabolas to calculate the curvature. The program that produced the video above uses cross-frame averaging to make the lines smoother and to vet new detections in successive video frames.

## Vehicle detection

I detect cars by dividing the image into a bunch of overlapping tiles of varying
sizes and running each tile through a classifier to check if it contains a car
or a fraction of a car. In this particular solution, I used a
linear support vector machine (`LinearSVC`

from `sklearn`

). I also wrapped
it in a `CalibratedClassifierCV`

to get a measure of confidence. I rejected
predictions of cars that were less than 85% certain. The classifier trained on
data harvested from the GTI, KITTI, and Udacity datasets from
which I collected around 25 times more background samples than cars to limit the
occurrences of false-positive detections.

As far as image features are concerned, I use only
Histograms of Oriented Gradients with parameters that are essentially the
same as the ones presented in this paper dealing with detection of humans.
I used OpenCV's `HOGDescriptor`

to extract the HOGs. The reason for this is that
it can compute the gradients taking into account all of the color channels. See
here. It is the capability that other libraries typically lack limiting
you to a form of grayscale. The training set consists of roughly 2M images of
64 by 64 pixels.

Since the samples the classifier trains on contain pictures of fractions of cars, the same car is usually detected multiple times in overlapping tiles. Also, the types of background differ quite a bit, and it's hard to find images of all the possible things that are not cars. Therefore false-positives are quite frequent. To combat these problems, I use heat maps that are averaged across five video frames. Every pixel that has less than three detections on average per frame is rejected as a false positive.

I then use OpenCV's `connectedComponentsWithStats`

to find connected components
and get centroids and bounding boxes for the detections. The centroids are used
to track the objects across frames and smooth the bounding boxes by averaging
them with 12 previous frames. To further reject false-positives, an object needs
to be classified as a car in at least 6 out of 12 consecutive frames.

## Conclusions

The topic is pretty fascinating and the results I got could be significantly improved by:

- employing smarter sliding window algorithms (i.e., having momentum) to better detect dashed lines that are substantially curved
- finding better ways to do perspective transforms
- using a better classifier for cars (a deep neural network perhaps)
- using techniques like YOLO
- using something smarter than strongly connected components to distinguish overlapping detections of different vehicles - mean shift clustering comes to mind
- making performance improvements here and there (use C++, parallelize video processing and so on)

I learned a lot of computer vision techniques and had plenty of fun doing this project. I also spent a lot of time reading the code of OpenCV. It has a lot of great tutorials, but its API documentation is lacking.

## The project

A neural network learned how to drive a car by observing how I do it! :) I must say that it's one of the coolest projects that I have ever done. Udacity provided a simulator program where you had to drive a car for a while on two tracks to collect training data. Each sample consisted of a steering angle and images from three front-facing cameras.

Then, in the autonomous driving mode, you are given an image from the central camera and must send back an appropriate steering angle, such that the car does not go off-track.

An elegant solution to this problem was described in a paper by nVidia from April 2016. I managed to replicate it in the simulator. Not without issues, though. The key takeaways for me were:

- The importance of making sure that the training data sample is balanced. That is, making sure that some categories of steering angles are not over-represented.
- The importance of randomly jittering the input images. To quote another paper: "ConvNets architectures have built-in invariance to small translations, scaling and rotations. When a dataset does not naturally contain those deformations, adding them synthetically will yield more robust learning to potential deformations in the test set."
- Not over-using dropout.

The model needed to train for 35 epochs. Each epoch consisted of 24 batches of
2048 images with on-the-fly jittering. It took 104 seconds to process one epoch
on Amazon's *p2.xlarge* instance and 826 seconds to do the same thing on my
laptop. What took an hour on a *Tesla K80* GPU would have taken my laptop over
8 hours.

## Results

Below are some sample results. The driving is not very smooth, but I blame that on myself not being a good driving model ;) The second track is especially interesting, because it differs from the one that the network was trained on. Interestingly enough, a MacBook Air did no have enough juice to run both the simulator and the model, even though the model is fairly small. I ended up having to create an ssh tunnel to my Linux laptop.

## The classifier

I have built a road sign classifier recently as an assignment for one of the online courses I participate in. The particulars of the implementation are unimportant. It suffices to say that it's a variation on the solution found in this paper by Sermanet and LeCun and operates on the same data set of German road signs. The solution has around 4.3M trainable parameters, and there are around 300k training (after augmentation), 40k validation (after augmentation), and 12k testing samples. The classifier reached the testing accuracy of 98.67%, which is just about human performance. That's not bad.

The thing that I want to share the most is not all mentioned above, but the training benchmarks. I tested it on three different machines in 5 configurations in total:

**x1-cpu**: My laptop, four i7-6600U CPU cores at 2.60GHz each and 4MB cache, 16GB RAM**g2.8-cpu**: Amazon's*g2.8xlarge*instance, 32 Xeon E5-2670 CPU cores at 2.60GHz each with 20MB cache, 60GB RAM**g2.2-cpu**: Amazon's*g2.2xlarge*instance, 8 Xeon E5-2670 CPU cores at 2.60GHz each with 20MB cache, 15GB RAM**g2.8-gpu**: The same as**g2.8-cpu**but used the 4 GRID K520 GPUs**g2.2-gpu**: The same as**g2.2-cpu**but used the 1 GRID K520 GPU**p2-gpu**: Amazon's*p2.xlarge*instance, 4 Xeon E5-2686 CPU cores 2.30GHz each with 46MB cache, 60GB RAM, 1 Tesla K80 GPU

Here are the times it took to train one epoch as well as how long it would have taken to train for 540 epochs (it took 540 epochs to get the final solution):

**x1-cpu**: 40 minutes/epoch, 15 days to train**g2.8-cpu**: 6:24/epoch, 2 days 9 hours 36 minutes to train**g2.2-cpu**: 16:15/epoch, 6 days, 2 hours 15 minutes to train**g2.8-gpu**: 1:37/epoch, 14 hours, 33 minutes to train**g2.2-gpu**: 1:37/epoch, 14 hours, 33 minutes to train**p2-gpu**: 56 seconds/epoch, 8 hours, 24 minutes to train

I was quite surprised by these results. I knew that GPUs are better suited for
this purpose, but I did not know that they are this much better. The slowness of
the laptop might have been due to swapping. I run the test with the usual
(unused) laptop workload and Chrome taking a lot of RAM. I was not doing
anything during the test, though. When testing with *g2.8-cpu*, it looked like
only 24 out of the 32 CPU cores were busy. Three additional GPUs on *g2.8-gpu*
did not seem to have made any difference. *TensorFlow* allows you to pin
operations to devices, but I did not do any of that. The test just runs the same
exact graph as *g2.2-gpu*. There's likely a lot to gain by doing manual tuning.

## The results

I tested it on pictures of a bunch of French and Swiss road signs taken around where I live. These are in some cases different from their German counterparts. When the network had enough training examples, it generalized well, otherwise, not so much. In the images below, you'll find sample sign pictures and the top three logits returned by the classifier after applying softmax.