├── LICENSE
├── PredictSteeringAngle.cpp
├── README.md
├── RecordSteeringData.cpp
└── steering_wheel_image.jpg


/LICENSE:
--------------------------------------------------------------------------------
 1 | The MIT License (MIT)
 2 | 
 3 | Copyright (c) 2016 Sully Chen
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/PredictSteeringAngle.cpp:
--------------------------------------------------------------------------------
  1 | //This is a modified version of BVLC Caffe's "classification.cpp"
  2 | 
  3 | //  Created by Sully Chen
  4 | //  Copyright © 2015 Sully Chen. All rights reserved.
  5 | 
  6 | //This software is for video demonstration purposes only. Please DO NOT use this to pilot a car!
  7 | 
  8 | #include <iostream>
  9 | #include <caffe/caffe.hpp>
 10 | #define USE_OPENCV = 1;
 11 | #ifdef USE_OPENCV
 12 | #include "opencv2/opencv.hpp"
 13 | #include <opencv2/core/core.hpp>
 14 | #include <opencv2/highgui/highgui.hpp>
 15 | #include <opencv2/imgproc/imgproc.hpp>
 16 | #endif  // USE_OPENCV
 17 | #include <algorithm>
 18 | #include <iosfwd>
 19 | #include <memory>
 20 | #include <string>
 21 | #include <utility>
 22 | #include <vector>
 23 | #include <thread>
 24 | 
 25 | #include <stdio.h>    /* Standard input/output definitions */
 26 | #include <stdint.h>   /* Standard types */
 27 | #include <unistd.h>   /* UNIX standard function definitions */
 28 | #include <fcntl.h>    /* File control definitions */
 29 | #include <errno.h>    /* Error number definitions */
 30 | #include <termios.h>  /* POSIX terminal control definitions */
 31 | #include <sys/ioctl.h>
 32 | #include <getopt.h>
 33 | 
 34 | #include "SFML/Graphics.hpp"
 35 | 
 36 | const int width = 240; //SFML window width
 37 | const int height = 240; //SFML window height
 38 | 
 39 | #ifdef USE_OPENCV
 40 | using namespace caffe;  // NOLINT(build/namespaces)
 41 | using std::string;
 42 | 
 43 | char buf[256]; //where the serial messages received are stored
 44 | 
 45 | void usage(void);
 46 | int serialport_init(const char* serialport, int baud);
 47 | int serialport_writebyte(int fd, uint8_t b);
 48 | int serialport_write(int fd, const char* str);
 49 | int serialport_read_until(int fd, char* buf, char until);
 50 | 
 51 | void thread1() //messages are read in a separate thread to fix timing issues with OpenCV
 52 | {
 53 |   int fd = 0;
 54 |   char serialport[256];
 55 |   int baudrate = B115200;  // default
 56 |   fd = serialport_init("/dev/cu.usbmodem1421", baudrate); //open serial port of arduino
 57 |   if(fd == -1)
 58 |     std::cout << "Error opening port!" << std::endl;
 59 |   while (true)
 60 |     serialport_read_until(fd, buf, '\n');
 61 | }
 62 | 
 63 | /* Pair (label, confidence) representing a prediction. */
 64 | typedef std::pair<string, float> Prediction;
 65 | 
 66 | class Classifier {
 67 |  public:
 68 |   Classifier(const string& model_file,
 69 |              const string& trained_file,
 70 |              const string& mean_file,
 71 |              const string& label_file);
 72 | 
 73 |   std::vector<Prediction> Classify(const cv::Mat& img, int N = 5);
 74 | 
 75 |  private:
 76 |   void SetMean(const string& mean_file);
 77 | 
 78 |   std::vector<float> Predict(const cv::Mat& img);
 79 | 
 80 |   void WrapInputLayer(std::vector<cv::Mat>* input_channels);
 81 | 
 82 |   void Preprocess(const cv::Mat& img,
 83 |                   std::vector<cv::Mat>* input_channels);
 84 | 
 85 |  private:
 86 |   shared_ptr<Net<float> > net_;
 87 |   cv::Size input_geometry_;
 88 |   int num_channels_;
 89 |   cv::Mat mean_;
 90 |   std::vector<string> labels_;
 91 | };
 92 | 
 93 | Classifier::Classifier(const string& model_file,
 94 |                        const string& trained_file,
 95 |                        const string& mean_file,
 96 |                        const string& label_file) {
 97 | #ifdef CPU_ONLY
 98 |   Caffe::set_mode(Caffe::CPU);
 99 | #else
100 |   Caffe::set_mode(Caffe::GPU);
101 | #endif
102 | 
103 |   /* Load the network. */
104 |   net_.reset(new Net<float>(model_file, TEST));
105 |   net_->CopyTrainedLayersFrom(trained_file);
106 | 
107 |   CHECK_EQ(net_->num_inputs(), 1) << "Network should have exactly one input.";
108 |   CHECK_EQ(net_->num_outputs(), 1) << "Network should have exactly one output.";
109 | 
110 |   Blob<float>* input_layer = net_->input_blobs()[0];
111 |   num_channels_ = input_layer->channels();
112 |   CHECK(num_channels_ == 3 || num_channels_ == 1)
113 |     << "Input layer should have 1 or 3 channels.";
114 |   input_geometry_ = cv::Size(input_layer->width(), input_layer->height());
115 | 
116 |   /* Load the binaryproto mean file. */
117 |   SetMean(mean_file);
118 | 
119 |   /* Load labels. */
120 |   std::ifstream labels(label_file.c_str());
121 |   CHECK(labels) << "Unable to open labels file " << label_file;
122 |   string line;
123 |   while (std::getline(labels, line))
124 |     labels_.push_back(string(line));
125 | 
126 |   Blob<float>* output_layer = net_->output_blobs()[0];
127 |   CHECK_EQ(labels_.size(), output_layer->channels())
128 |     << "Number of labels is different from the output layer dimension.";
129 | }
130 | 
131 | static bool PairCompare(const std::pair<float, int>& lhs,
132 |                         const std::pair<float, int>& rhs) {
133 |   return lhs.first > rhs.first;
134 | }
135 | 
136 | /* Return the indices of the top N values of vector v. */
137 | static std::vector<int> Argmax(const std::vector<float>& v, int N) {
138 |   std::vector<std::pair<float, int> > pairs;
139 |   for (size_t i = 0; i < v.size(); ++i)
140 |     pairs.push_back(std::make_pair(v[i], i));
141 |   std::partial_sort(pairs.begin(), pairs.begin() + N, pairs.end(), PairCompare);
142 | 
143 |   std::vector<int> result;
144 |   for (int i = 0; i < N; ++i)
145 |     result.push_back(pairs[i].second);
146 |   return result;
147 | }
148 | 
149 | /* Return the top N predictions. */
150 | std::vector<Prediction> Classifier::Classify(const cv::Mat& img, int N) {
151 |   std::vector<float> output = Predict(img);
152 | 
153 |   N = std::min<int>(labels_.size(), N);
154 |   std::vector<int> maxN = Argmax(output, N);
155 |   std::vector<Prediction> predictions;
156 |   for (int i = 0; i < N; ++i) {
157 |     int idx = maxN[i];
158 |     predictions.push_back(std::make_pair(labels_[idx], output[idx]));
159 |   }
160 | 
161 |   return predictions;
162 | }
163 | 
164 | /* Load the mean file in binaryproto format. */
165 | void Classifier::SetMean(const string& mean_file) {
166 |   BlobProto blob_proto;
167 |   ReadProtoFromBinaryFileOrDie(mean_file.c_str(), &blob_proto);
168 | 
169 |   /* Convert from BlobProto to Blob<float> */
170 |   Blob<float> mean_blob;
171 |   mean_blob.FromProto(blob_proto);
172 |   CHECK_EQ(mean_blob.channels(), num_channels_)
173 |     << "Number of channels of mean file doesn't match input layer.";
174 | 
175 |   /* The format of the mean file is planar 32-bit float BGR or grayscale. */
176 |   std::vector<cv::Mat> channels;
177 |   float* data = mean_blob.mutable_cpu_data();
178 |   for (int i = 0; i < num_channels_; ++i) {
179 |     /* Extract an individual channel. */
180 |     cv::Mat channel(mean_blob.height(), mean_blob.width(), CV_32FC1, data);
181 |     channels.push_back(channel);
182 |     data += mean_blob.height() * mean_blob.width();
183 |   }
184 | 
185 |   /* Merge the separate channels into a single image. */
186 |   cv::Mat mean;
187 |   cv::merge(channels, mean);
188 | 
189 |   /* Compute the global mean pixel value and create a mean image
190 |    * filled with this value. */
191 |   cv::Scalar channel_mean = cv::mean(mean);
192 |   mean_ = cv::Mat(input_geometry_, mean.type(), channel_mean);
193 | }
194 | 
195 | std::vector<float> Classifier::Predict(const cv::Mat& img) {
196 |   Blob<float>* input_layer = net_->input_blobs()[0];
197 |   input_layer->Reshape(1, num_channels_,
198 |                        input_geometry_.height, input_geometry_.width);
199 |   /* Forward dimension change to all layers. */
200 |   net_->Reshape();
201 | 
202 |   std::vector<cv::Mat> input_channels;
203 |   WrapInputLayer(&input_channels);
204 | 
205 |   Preprocess(img, &input_channels);
206 | 
207 |   net_->Forward();
208 | 
209 |   /* Copy the output layer to a std::vector */
210 |   Blob<float>* output_layer = net_->output_blobs()[0];
211 |   const float* begin = output_layer->cpu_data();
212 |   const float* end = begin + output_layer->channels();
213 |   return std::vector<float>(begin, end);
214 | }
215 | 
216 | /* Wrap the input layer of the network in separate cv::Mat objects
217 |  * (one per channel). This way we save one memcpy operation and we
218 |  * don't need to rely on cudaMemcpy2D. The last preprocessing
219 |  * operation will write the separate channels directly to the input
220 |  * layer. */
221 | void Classifier::WrapInputLayer(std::vector<cv::Mat>* input_channels) {
222 |   Blob<float>* input_layer = net_->input_blobs()[0];
223 | 
224 |   int width = input_layer->width();
225 |   int height = input_layer->height();
226 |   float* input_data = input_layer->mutable_cpu_data();
227 |   for (int i = 0; i < input_layer->channels(); ++i) {
228 |     cv::Mat channel(height, width, CV_32FC1, input_data);
229 |     input_channels->push_back(channel);
230 |     input_data += width * height;
231 |   }
232 | }
233 | 
234 | void Classifier::Preprocess(const cv::Mat& img,
235 |                             std::vector<cv::Mat>* input_channels) {
236 |   /* Convert the input image to the input image format of the network. */
237 |   cv::Mat sample;
238 |   if (img.channels() == 3 && num_channels_ == 1)
239 |     cv::cvtColor(img, sample, cv::COLOR_BGR2GRAY);
240 |   else if (img.channels() == 4 && num_channels_ == 1)
241 |     cv::cvtColor(img, sample, cv::COLOR_BGRA2GRAY);
242 |   else if (img.channels() == 4 && num_channels_ == 3)
243 |     cv::cvtColor(img, sample, cv::COLOR_BGRA2BGR);
244 |   else if (img.channels() == 1 && num_channels_ == 3)
245 |     cv::cvtColor(img, sample, cv::COLOR_GRAY2BGR);
246 |   else
247 |     sample = img;
248 | 
249 |   cv::Mat sample_resized;
250 |   if (sample.size() != input_geometry_)
251 |     cv::resize(sample, sample_resized, input_geometry_);
252 |   else
253 |     sample_resized = sample;
254 | 
255 |   cv::Mat sample_float;
256 |   if (num_channels_ == 3)
257 |     sample_resized.convertTo(sample_float, CV_32FC3);
258 |   else
259 |     sample_resized.convertTo(sample_float, CV_32FC1);
260 | 
261 |   cv::Mat sample_normalized;
262 |   cv::subtract(sample_float, mean_, sample_normalized);
263 | 
264 |   /* This operation will write the separate BGR planes directly to the
265 |    * input layer of the network because it is wrapped by the cv::Mat
266 |    * objects in input_channels. */
267 |   cv::split(sample_normalized, *input_channels);
268 | 
269 |   CHECK(reinterpret_cast<float*>(input_channels->at(0).data)
270 |         == net_->input_blobs()[0]->cpu_data())
271 |     << "Input channels are not wrapping the input layer of the network.";
272 | }
273 | 
274 | int main(int argc, char** argv) {
275 |   if (argc != 5) {
276 |     std::cerr << "Usage: " << argv[0]
277 |               << " deploy.prototxt network.caffemodel"
278 |               << " mean.binaryproto labels.txt" << std::endl;
279 |     return 1;
280 |   }
281 | 
282 |   //SFML things to draw steering wheel
283 |   sf::String title_string = "Predicted";
284 |   sf::String title_string2 = "Actual";
285 |   sf::RenderWindow window(sf::VideoMode(width, height), title_string);
286 |   sf::RenderWindow window2(sf::VideoMode(width, height), title_string2);
287 |   window.setFramerateLimit(30);
288 |   window2.setFramerateLimit(30);
289 | 
290 |   ::google::InitGoogleLogging(argv[0]);
291 | 
292 |   string model_file   = argv[1];
293 |   string trained_file = argv[2];
294 |   string mean_file    = argv[3];
295 |   string label_file   = argv[4];
296 |   Classifier classifier(model_file, trained_file, mean_file, label_file);
297 | 
298 |   cv::namedWindow("img",1);
299 | 
300 |   //open camera
301 |   cv::VideoCapture cap(0);
302 |   if(!cap.isOpened())
303 |     return -1;
304 | 
305 |   //load the steering wheel image
306 |   sf::Texture steeringWheelTexture;
307 |   steeringWheelTexture.loadFromFile("steering_wheel_image.jpg");
308 | 
309 |   //create steering wheel sprite
310 |   sf::Sprite steeringWheelSprite;
311 |   steeringWheelSprite.setTexture(steeringWheelTexture);
312 |   steeringWheelSprite.setPosition(120, 120);
313 |   steeringWheelSprite.setRotation(30.0f);
314 |   steeringWheelSprite.setOrigin(120, 120);
315 | 
316 |   double smoothed_angle = 0.0f; //Used as the final steering output
317 | 
318 |   int actual_angle = 0;
319 | 
320 |   std::thread t(thread1);
321 | 
322 |   while (true)
323 |   {
324 |     int actual_angle = (int)::atof(buf);
325 |     if (cv::waitKey(10) == 'q') break; // break the loop if the key "q" is pressed
326 |     cv::Mat img;
327 |     cap >> img; // get a new frame from camera
328 | 
329 |     //crop the camera image
330 |     cv::Rect myROI(280, 0, 720, 720);
331 |     cv::Mat croppedImage = img(myROI);
332 | 
333 |     resize(croppedImage, img, cv::Size(), 256.0f/720.0f, 256.0f/720.0f, cv::INTER_LANCZOS4); //resize to 256x256
334 | 
335 |     cv::imshow("img", img); //show the image
336 | 
337 |     //canny edge filtering
338 |     Canny(img, img, 50, 200, 3);
339 | 
340 |     //Preprocessing to remove highly cluttered areas from the image
341 |     cv::Mat mask;
342 |     GaussianBlur(img, mask, cv::Size(35, 35), 10, 10);
343 |     threshold(mask, mask, 60, 255, cv::THRESH_BINARY_INV);
344 |     bitwise_and(img, mask, img);
345 | 
346 |     cv::imshow("filtered img", img); //show the image
347 | 
348 |     std::vector<Prediction> predictions = classifier.Classify(img); //Use BVLC Caffe to classify the image
349 | 
350 |     system("clear"); //clear the console for neatness
351 | 
352 |     std::vector<int> angle_categories; //This vector stores all of the possible angle outputs from the classifier
353 |     std::vector<double> outputs; //This vector stores all of the corresponding prediction outputs from the classifier
354 | 
355 |     //process classifier outputs
356 |     for (int i = 0; i < predictions.size(); i++)
357 |     {
358 |       Prediction p = predictions[i];
359 |       int angle_category;
360 | 
361 |       //if the category is just "0", then the angle_category is 0
362 |       if (p.first == "0")
363 |         angle_category = 0;
364 |       else //otherwise, do further processing
365 |       {
366 |         //categories are in the format "posXXX" or "negXXX", where XXX is an angle measure
367 |         angle_category = std::stoi(p.first.substr(3, p.first.length() - 3)); //convert the string to an integer, ignoring the first three characters which are likely "pos" or "neg"
368 | 
369 |         //if the first three letters are "neg", multiply the category by -1 to make the category negative
370 |         if (p.first.substr(0, 3) == "neg")
371 |           angle_category *= -1;
372 |       }
373 |       angle_categories.push_back(angle_category); //store the possible categories in the categories vector
374 |       outputs.push_back(p.second); //store the corresponding output in the outputs vector
375 |     }
376 | 
377 |     double output_angle = 0.0f; //this is used for a processing step before the final angle
378 | 
379 |     //do a weighted sum of the classifier outputs
380 |     for (int i = 0; i < angle_categories.size(); i++)
381 |       output_angle += angle_categories[i] * outputs[i];
382 | 
383 |     //make smooth angle transitions by turning the steering wheel based on the difference of the current angle
384 |     //and the predicted angle
385 |     smoothed_angle += 1.0f * pow(std::abs((output_angle - smoothed_angle)), 2.0f / 3.0f)
386 |                           * (output_angle - smoothed_angle) / std::abs(output_angle - smoothed_angle);
387 | 
388 |     //print the angle
389 |     if (smoothed_angle < 0)
390 |       std::cout << std::fixed << std::setprecision(2) << -1.0f * smoothed_angle << " degrees left" << std::endl;
391 |     else if (smoothed_angle > 0)
392 |       std::cout << std::fixed << std::setprecision(2) << smoothed_angle << " degrees right" << std::endl;
393 |     else
394 |       std::cout << "0 degrees" << std::endl;
395 | 
396 |       //update SFML things
397 |       steeringWheelSprite.setRotation(smoothed_angle);
398 | 
399 |       window.clear(sf::Color::White);
400 |       window2.clear(sf::Color::White);
401 | 
402 |       window.draw(steeringWheelSprite);
403 | 
404 |       steeringWheelSprite.setRotation(actual_angle);
405 |       window2.draw(steeringWheelSprite);
406 | 
407 |       window.display();
408 |       window2.display();
409 |   }
410 | }
411 | #else
412 | int main(int argc, char** argv) {
413 |   LOG(FATAL) << "This example requires OpenCV; compile with USE_OPENCV.";
414 | }
415 | #endif  // USE_OPENCV
416 | 
417 | int serialport_writebyte( int fd, uint8_t b)
418 | {
419 |     int n = write(fd,&b,1);
420 |     if( n!=1)
421 |         return -1;
422 |     return 0;
423 | }
424 | 
425 | int serialport_write(int fd, const char* str)
426 | {
427 |     int len = strlen(str);
428 |     int n = write(fd, str, len);
429 |     if( n!=len )
430 |         return -1;
431 |     return 0;
432 | }
433 | 
434 | int serialport_read_until(int fd, char* buf, char until)
435 | {
436 |     char b[1];
437 |     int i=0;
438 |     do {
439 |         int n = read(fd, b, 1);  // read a char at a time
440 |         if( n==-1) return -1;    // couldn't read
441 |         if( n==0 ) {
442 |             usleep( 10 * 1000 ); // wait 10 msec try again
443 |             continue;
444 |         }
445 |         buf[i] = b[0]; i++;
446 |     } while( b[0] != until );
447 | 
448 |     buf[i] = 0;  // null terminate the string
449 |     return 0;
450 | }
451 | 
452 | // takes the string name of the serial port (e.g. "/dev/tty.usbserial","COM1")
453 | // and a baud rate (bps) and connects to that port at that speed and 8N1.
454 | // opens the port in fully raw mode so you can send binary data.
455 | // returns valid fd, or -1 on error
456 | int serialport_init(const char* serialport, int baud)
457 | {
458 |     struct termios toptions;
459 |     int fd;
460 | 
461 |     //fprintf(stderr,"init_serialport: opening port %s @ %d bps\n",
462 |     //        serialport,baud);
463 | 
464 |     fd = open(serialport, O_RDWR | O_NOCTTY | O_NDELAY);
465 |     if (fd == -1)  {
466 |         perror("init_serialport: Unable to open port ");
467 |         return -1;
468 |     }
469 | 
470 |     if (tcgetattr(fd, &toptions) < 0) {
471 |         perror("init_serialport: Couldn't get term attributes");
472 |         return -1;
473 |     }
474 |     speed_t brate = baud; // let you override switch below if needed
475 |     switch(baud) {
476 |     case 4800:   brate=B4800;   break;
477 |     case 9600:   brate=B9600;   break;
478 | #ifdef B14400
479 |     case 14400:  brate=B14400;  break;
480 | #endif
481 |     case 19200:  brate=B19200;  break;
482 | #ifdef B28800
483 |     case 28800:  brate=B28800;  break;
484 | #endif
485 |     case 38400:  brate=B38400;  break;
486 |     case 57600:  brate=B57600;  break;
487 |     case 115200: brate=B115200; break;
488 |     }
489 |     cfsetispeed(&toptions, brate);
490 |     cfsetospeed(&toptions, brate);
491 | 
492 |     // 8N1
493 |     toptions.c_cflag &= ~PARENB;
494 |     toptions.c_cflag &= ~CSTOPB;
495 |     toptions.c_cflag &= ~CSIZE;
496 |     toptions.c_cflag |= CS8;
497 |     // no flow control
498 |     toptions.c_cflag &= ~CRTSCTS;
499 | 
500 |     toptions.c_cflag |= CREAD | CLOCAL;  // turn on READ & ignore ctrl lines
501 |     toptions.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl
502 | 
503 |     toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw
504 |     toptions.c_oflag &= ~OPOST; // make raw
505 | 
506 |     // see: http://unixwiz.net/techtips/termios-vmin-vtime.html
507 |     toptions.c_cc[VMIN]  = 0;
508 |     toptions.c_cc[VTIME] = 20;
509 | 
510 |     if( tcsetattr(fd, TCSANOW, &toptions) < 0) {
511 |         perror("init_serialport: Couldn't set term attributes");
512 |         return -1;
513 |     }
514 | 
515 |     return fd;
516 | }
517 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Caffe-Autopilot
 2 | 
 3 | **This software is for video demonstration purposes only. Please _DO_ _NOT_ use this to pilot a car!**
 4 | 
 5 | A car autopilot system developed using C++, [BVLC Caffe](https://github.com/BVLC/caffe), [OpenCV](http://opencv.org/), and [SFML](http://www.sfml-dev.org/)
 6 | 
 7 | **The model in this repository has _only_ been trained on roads with a visible, solid yellow line divider. More models are being trained for different road types and driving conditions. These models will be uploaded when they are ready.**
 8 | 
 9 | [Video here](https://www.youtube.com/watch?v=fSbWnQ_wzvM)
10 | 
11 | [Datasets and models](https://drive.google.com/open?id=0B-KJCaaF7ellNFVFSUpVWGlTUWM)
12 | 
13 | ## How it works
14 | 
15 | Deep convolutional neural nets are powerful.
16 | 
17 | First, a dataset is collected using a webcam and an Arduino with a CANBUS shield to read the steering wheel angles from the OBD-II port of the car.
18 | 
19 | The images are preprocessed with Canny edge filtering and a certain threshold operation as they are collected.
20 | The purpose of the preprocessing is to make less work for the convolutional neural network by extracting the important features with image processing algorithms ahead of time.
21 | 
22 | Then, [BVLC Caffe](https://github.com/BVLC/caffe) is used to train a deep convolutional neural network (specifically AlexNet).
23 | 
24 | To predict steering angles based on input images, a weighted sum of the BVLC Caffe classifier outputs is used to calculate the final angle. Some smoothing algorithms are also used to smooth the motion of the predicted steering wheel angle.
25 | 


--------------------------------------------------------------------------------
/RecordSteeringData.cpp:
--------------------------------------------------------------------------------
  1 | //This code uses arduino serial communication code by Tod E. Kurt, tod@todbot.com http://todbot.com/blog/
  2 | 
  3 | //  Created by Sully Chen
  4 | //  Copyright © 2015 Sully Chen. All rights reserved.
  5 | 
  6 | #include <stdio.h>    /* Standard input/output definitions */
  7 | #include <stdlib.h>
  8 | #include <stdint.h>   /* Standard types */
  9 | #include <string.h>   /* String function definitions */
 10 | #include <unistd.h>   /* UNIX standard function definitions */
 11 | #include <fcntl.h>    /* File control definitions */
 12 | #include <errno.h>    /* Error number definitions */
 13 | #include <termios.h>  /* POSIX terminal control definitions */
 14 | #include <sys/ioctl.h>
 15 | #include <getopt.h>
 16 | #include "opencv2/opencv.hpp"
 17 | #include <fstream>
 18 | #include <thread>
 19 | 
 20 | using namespace std;
 21 | using namespace cv;
 22 | 
 23 | void usage(void);
 24 | int serialport_init(const char* serialport, int baud);
 25 | int serialport_writebyte(int fd, uint8_t b);
 26 | int serialport_write(int fd, const char* str);
 27 | int serialport_read_until(int fd, char* buf, char until);
 28 | 
 29 | void usage(void) {
 30 |     printf("Usage: arduino-serial -p <serialport> [OPTIONS]\n"
 31 |     "\n"
 32 |     "Options:\n"
 33 |     "  -h, --help                   Print this help message\n"
 34 |     "  -p, --port=serialport        Serial port Arduino is on\n"
 35 |     "  -b, --baud=baudrate          Baudrate (bps) of Arduino\n"
 36 |     "  -s, --send=data              Send data to Arduino\n"
 37 |     "  -r, --receive                Receive data from Arduino & print it out\n"
 38 |     "  -n  --num=num                Send a number as a single byte\n"
 39 |     "  -d  --delay=millis           Delay for specified milliseconds\n"
 40 |     "\n"
 41 |     "Note: Order is important. Set '-b' before doing '-p'. \n"
 42 |     "      Used to make series of actions:  '-d 2000 -s hello -d 100 -r' \n"
 43 |     "      means 'wait 2secs, send 'hello', wait 100msec, get reply'\n"
 44 |     "\n");
 45 | }
 46 | 
 47 | char buf[256]; //where the serial messages received are stored
 48 | 
 49 | void thread1() //messages are read in a separate thread to fix timing issues with OpenCV
 50 | {
 51 |   int fd = 0;
 52 |   char serialport[256];
 53 |   int baudrate = B115200;  // default
 54 |   fd = serialport_init("/dev/cu.usbmodem1421", baudrate); //open serial port of arduino
 55 |   if(fd == -1)
 56 |     std::cout << "Error opening port!" << std::endl;
 57 |   while (true)
 58 |     serialport_read_until(fd, buf, '\n');
 59 | }
 60 | 
 61 | int main(int argc, char *argv[])
 62 | {
 63 |   //generate data directories
 64 |   for (int i = -18; i <= 18; i++)
 65 |   {
 66 |     std::string s;
 67 |     if (i == 0)
 68 |       s = "mkdir 0";
 69 |     else if (i < 0)
 70 |       s = "mkdir neg" + std::to_string(-i * 5);
 71 |     else
 72 |       s = "mkdir pos" + std::to_string(i * 5);
 73 |     std::cout << s << std::endl;
 74 |     system(s.c_str());
 75 |   }
 76 | 
 77 |   //open camera
 78 |   VideoCapture cap(0);
 79 |   if(!cap.isOpened())
 80 |       return -1;
 81 |   Mat edges;
 82 |   namedWindow("frame",1);
 83 |   int i = 0;
 84 |   std::cout << "Input starting index: ";
 85 |   cin >> i;
 86 |   cout << "\n";
 87 |   std::thread t(thread1);
 88 |   while (true)
 89 |   {
 90 |     int key_press = waitKey(10);
 91 |     if (key_press != 's')
 92 |     {
 93 |       Mat frame;
 94 |       cap >> frame; // get a new frame from camera
 95 | 
 96 |       //crop and resize
 97 |       Rect myROI(280, 0, 720, 720);
 98 |       Mat croppedImage = frame(myROI);
 99 |       resize(croppedImage, frame, Size(), 256.0f/720.0f, 256.0f/720.0f, cv::INTER_LANCZOS4);
100 | 
101 |       //canny edge filtering
102 |       Canny(frame, frame, 50, 200, 3);
103 | 
104 |       //Preprocessing to remove highly cluttered areas from the image
105 |       Mat mask;
106 |       GaussianBlur(frame, mask, Size(35, 35), 10, 10);
107 |       threshold(mask, mask, 60, 255, THRESH_BINARY_INV);
108 |       bitwise_and(frame, mask, frame);
109 | 
110 |       //get steering angle from the serial data
111 |       int angle = (int)::atof(buf);
112 | 
113 |       if (angle <= 90 && angle >= -90)
114 |       {
115 |         std::string s;
116 |         angle = (angle / 5) * 5;
117 |         if (angle == 0)
118 |           s = "0";
119 |         else if (angle < 0)
120 |           s = "neg" + std::to_string(-angle);
121 |         else
122 |           s = "pos" + std::to_string(angle);
123 |           std::cout << s << std::endl;
124 |           imwrite(s + "/" + std::to_string(i) + ".jpg", frame);
125 |         if (key_press == 'q')
126 |           break;
127 |         i++;
128 |       }
129 |       imshow("frame", frame);
130 |     } else while (waitKey() != 'c');
131 |   }
132 |   exit(EXIT_SUCCESS);
133 | }
134 | 
135 | int serialport_writebyte( int fd, uint8_t b)
136 | {
137 |     int n = write(fd,&b,1);
138 |     if( n!=1)
139 |         return -1;
140 |     return 0;
141 | }
142 | 
143 | int serialport_write(int fd, const char* str)
144 | {
145 |     int len = strlen(str);
146 |     int n = write(fd, str, len);
147 |     if( n!=len )
148 |         return -1;
149 |     return 0;
150 | }
151 | 
152 | int serialport_read_until(int fd, char* buf, char until)
153 | {
154 |     char b[1];
155 |     int i=0;
156 |     do {
157 |         int n = read(fd, b, 1);  // read a char at a time
158 |         if( n==-1) return -1;    // couldn't read
159 |         if( n==0 ) {
160 |             usleep( 10 * 1000 ); // wait 10 msec try again
161 |             continue;
162 |         }
163 |         buf[i] = b[0]; i++;
164 |     } while( b[0] != until );
165 | 
166 |     buf[i] = 0;  // null terminate the string
167 |     return 0;
168 | }
169 | 
170 | // takes the string name of the serial port (e.g. "/dev/tty.usbserial","COM1")
171 | // and a baud rate (bps) and connects to that port at that speed and 8N1.
172 | // opens the port in fully raw mode so you can send binary data.
173 | // returns valid fd, or -1 on error
174 | int serialport_init(const char* serialport, int baud)
175 | {
176 |     struct termios toptions;
177 |     int fd;
178 | 
179 |     //fprintf(stderr,"init_serialport: opening port %s @ %d bps\n",
180 |     //        serialport,baud);
181 | 
182 |     fd = open(serialport, O_RDWR | O_NOCTTY | O_NDELAY);
183 |     if (fd == -1)  {
184 |         perror("init_serialport: Unable to open port ");
185 |         return -1;
186 |     }
187 | 
188 |     if (tcgetattr(fd, &toptions) < 0) {
189 |         perror("init_serialport: Couldn't get term attributes");
190 |         return -1;
191 |     }
192 |     speed_t brate = baud; // let you override switch below if needed
193 |     switch(baud) {
194 |     case 4800:   brate=B4800;   break;
195 |     case 9600:   brate=B9600;   break;
196 | #ifdef B14400
197 |     case 14400:  brate=B14400;  break;
198 | #endif
199 |     case 19200:  brate=B19200;  break;
200 | #ifdef B28800
201 |     case 28800:  brate=B28800;  break;
202 | #endif
203 |     case 38400:  brate=B38400;  break;
204 |     case 57600:  brate=B57600;  break;
205 |     case 115200: brate=B115200; break;
206 |     }
207 |     cfsetispeed(&toptions, brate);
208 |     cfsetospeed(&toptions, brate);
209 | 
210 |     // 8N1
211 |     toptions.c_cflag &= ~PARENB;
212 |     toptions.c_cflag &= ~CSTOPB;
213 |     toptions.c_cflag &= ~CSIZE;
214 |     toptions.c_cflag |= CS8;
215 |     // no flow control
216 |     toptions.c_cflag &= ~CRTSCTS;
217 | 
218 |     toptions.c_cflag |= CREAD | CLOCAL;  // turn on READ & ignore ctrl lines
219 |     toptions.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl
220 | 
221 |     toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw
222 |     toptions.c_oflag &= ~OPOST; // make raw
223 | 
224 |     // see: http://unixwiz.net/techtips/termios-vmin-vtime.html
225 |     toptions.c_cc[VMIN]  = 0;
226 |     toptions.c_cc[VTIME] = 20;
227 | 
228 |     if( tcsetattr(fd, TCSANOW, &toptions) < 0) {
229 |         perror("init_serialport: Couldn't set term attributes");
230 |         return -1;
231 |     }
232 | 
233 |     return fd;
234 | }
235 | 


--------------------------------------------------------------------------------
/steering_wheel_image.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/SullyChen/Caffe-Autopilot/886ff759b5d7f32f86727a7190f534868098636b/steering_wheel_image.jpg


--------------------------------------------------------------------------------