Merge pull request #3 from sergionr2/c-extension

C++ extension

.gitignore

@@ -5,6 +5,7 @@ lib/
 __pycache__/
 *.x
 build/
+tmp/
 *.png
 *.jpg
 *.pyc
@@ -18,3 +19,4 @@ render/
 *.mp4
 *.pkl
 *.pth
+*.so

README.md

@@ -21,9 +21,12 @@ En français: [http://enstar.ensta-paristech.fr/blog/public/racing_car/](http://
 - [Additional Camera Piece](https://cad.onshape.com/documents/1c4a51d839f2a5989e78ef1f/w/1af5b4b508310461911ecd97/e/a35856fc588eb371f0bac58b)
 - [Raspberry Pi Holder](https://cad.onshape.com/documents/621b6943711d60790ddc2b9f/w/c29ba5f453ce625afc8128f6/e/1aa39940e0bdabd3303d76c4)
 
+Note: the Battery Holder was designed for this [External Battery](https://www.amazon.fr/gp/product/B00Y7S4JRQ/ref=oh_aui_detailpage_o00_s01?ie=UTF8&psc=1)
+
+
 ### Training Data
 
-**Outdated** (you have to use convert_old_format.py to use current code, now all the informations are in a pickle file)
+**Outdated** (you have to use convert_old_format.py to use current code, now labels of training images are in a pickle file)
 
 The training data (7600+ labeled images) can be downloaded [here](https://www.dropbox.com/s/24x9b6kob5c5847/training_data.zip?dl=0)
 
@@ -112,6 +115,13 @@ The best model (lowest error on the validation data) will be saved as *mlp_model
 python -m train.test -f path/input/folder -w mlp_model_tmp
 ```
 
+## Benchmark
+
+For profiling 5000 iterations of image processing:
+```
+python -m opencv.benchmark -i path/to/input/image.jpg -n 5000
+```
+
 ### Installation
 
 #### Recommended : Use an image with everything already installed
@@ -130,13 +140,12 @@ OS: [Ubuntu MATE 16.04](https://ubuntu-mate.org/raspberry-pi/) for raspberry pi
 
 
 Installed softwares:
- - all the dependencies for that project (OpenCV 3.2.0, PyTorch, ...)
+ - all the dependencies for that project (OpenCV >= 3.1, PyTorch, ...)
  - the current project (in the folder RacingRobot/)
  - ROS Kinetic
 Camera and ssh are enabled.
 
 
-
 2. Identify the name of your sd card using:
 ```
 fdisk -l
@@ -266,9 +275,13 @@ pip install torch-0.4.0a0+b23fa21-cp27-cp27mu-linux_armv7l.whl
 
 Or follow this tutorial:
 [PyTorch on the raspberry pi](http://book.duckietown.org/fall2017/duckiebook/pytorch_install.html)
+
 0. Make sure you have at least 3 Go of Swap. (see link above)
+
 1. (optional) Install a recent version of cmake + scikit-build + ninja
+
 2. Install PyTorch
+
 ```
 # don't forget to set the env variables:
 export NO_CUDA=1
@@ -279,6 +292,10 @@ sudo -EH python setup.py install
 sudo -H pip install torchvision
 ```
 
+### C++ Extension
+
+Please read [opencv/c_extension/README.md](opencv/c_extension/README.md) for more information.
+
 ### Contributors
 - Sergio Nicolas Rodriguez Rodriguez
 - Antonin Raffin

arduino/parameters.h

@@ -6,7 +6,7 @@
 #define DIRECTION_PIN 4
 #define SERVOMOTOR_PIN 6
 #define START_PIN 12
-#define INITIAL_THETA 105
+#define INITIAL_THETA 110
 #define THETA_MIN 60
 #define THETA_MAX 150
 #define SPEED_MAX 100

arduino/slave.cpp

@@ -84,7 +84,7 @@ int convertOrderToPWM(float speedOrder)
 
 void getMessageFromSerial()
 {
-  while(Serial.available() > 0)
+  if(Serial.available() > 0)
   {
     // The first byte received is the instruction
     Order orderReceived = readOrder();

command/python/common.py

@@ -37,14 +37,13 @@ def clear(self):
 
 
 BAUDRATE = 115200
-is_connected_lock = threading.Lock()
 is_connected = False
 # Number of messages we can send to the Arduino without receiving a RECEIVED response
 n_messages_allowed = 3
 n_received_semaphore = threading.Semaphore(n_messages_allowed)
 serial_lock = threading.Lock()
 command_queue = CustomQueue(2)  # Must be >= 2 (motor + servo order)
-rate = 1 / 1000  # 1000 Hz (limit the rate of communication with the arduino)
+rate = 1 / 2000  # 2000 Hz (limit the rate of communication with the arduino)
 
 
 def resetCommandQueue():

command/python/teleop.py

@@ -15,9 +15,9 @@
 STOP = (0, 0)
 KEY_CODE_SPACE = 32
 
-MAX_SPEED = 50
+MAX_SPEED = 30
 MAX_TURN = 45
-THETA_MIN = 60
+THETA_MIN = 70
 THETA_MAX = 150
 STEP_SPEED = 10
 STEP_TURN = 30

constants.py

@@ -13,7 +13,7 @@
 R2 = [0, 100, MAX_WIDTH, 50]
 R3 = [0, 75, MAX_WIDTH, 50]
 R4 = [0, 50, MAX_WIDTH, 50]
-REGIONS = np.array([R1, R2, R3])
+REGIONS = np.array([R0, R1, R2])
 # Training
 # 80 = CAMERA_RESOLUTION[0] // 4
 WIDTH, HEIGHT = 80, 20 # Shape of the resized input image fed to our model
@@ -25,16 +25,16 @@
 THETA_MIN = 70  # value in [0, 255] sent to the servo
 THETA_MAX = 150
 ERROR_MAX = 1.0
-MAX_SPEED_STRAIGHT_LINE = 50  # order between 0 and 100
-MAX_SPEED_SHARP_TURN = 15
-MIN_SPEED = 10
+MAX_SPEED_STRAIGHT_LINE = 40  # order between 0 and 100
+MAX_SPEED_SHARP_TURN = 30
+MIN_SPEED = 25
 
 # PID Control
 Kp_turn = 40
-Kp_line = 35
-Kd = 30
-Ki = 0.0
-ALPHA = 0.8  # alpha of the moving mean for the turn coefficient
+Kp_line = 40
+Kd = 1
+Ki = 0
+ALPHA = 1  # alpha of the moving mean for the turn coefficient
 # Main Program
 FPS = 90
 N_SECONDS = 77  # number of seconds before exiting the program

main.py

@@ -49,7 +49,6 @@ def main_control(out_queue, resolution, n_seconds=5):
     start_time = time.time()
     error, errorD, errorI = 0, 0, 0
     last_error = 0
-    initialized = False  # compute derivative error for t > 1 only
     # Neutral Angle
     theta_init = (THETA_MAX + THETA_MIN) / 2
     # Middle of the image
@@ -67,6 +66,9 @@ def ctrl_c(signum, frame):
     last_time = time.time()
     last_time_update = time.time()
     pbar = tqdm(total=n_seconds)
+    # Number of time the command queue was full
+    n_full = 0
+    n_total = 0
 
     while time.time() - start_time < n_seconds and not should_exit[0]:
         # Display progress bar
@@ -79,9 +81,8 @@ def ctrl_c(signum, frame):
 
         # Compute the error to the center of the line
         # We want the line to be in the middle of the image
-        # Here we use the farthest centroids
-        # TODO: try with the mean of the centroids to reduce noise
-        error = (x_center - centroids[-1, 0]) / max_error_px
+        # Here we use the second centroid
+        error = (x_center - centroids[1, 0]) / max_error_px
 
         # Represent line curve as a number in [0, 1]
         # h = 0 -> straight line
@@ -105,17 +106,13 @@ def ctrl_c(signum, frame):
         # Reduce speed if we have a high error
         speed_order = t * MIN_SPEED + (1 - t) * v_max
 
-        if initialized:
-            errorD = error - last_error
-        else:
-            initialized = True
+        errorD = error - last_error
         # Update derivative error
         last_error = error
 
         # PID Control
-        # TODO: add dt in the equation
         dt = time.time() - last_time
-        u_angle = Kp * error + Kd * errorD + Ki * errorI
+        u_angle = Kp * error + Kd * (errorD / dt) + Ki * (errorI * dt)
         # Update integral error
         errorI += error
         last_time = time.time()
@@ -128,13 +125,17 @@ def ctrl_c(signum, frame):
             common.command_queue.put_nowait((Order.MOTOR, int(speed_order)))
             common.command_queue.put_nowait((Order.SERVO, angle_order))
         except fullException:
-            print("Command queue is full")
+            n_full += 1
+            # print("Command queue is full")
+        n_total += 1
 
     # SEND STOP ORDER at the end
     forceStop()
     # Make sure STOP order is sent
     time.sleep(0.2)
     pbar.close()
+    print("{:.2f}% of time the command queue was full".format(100 * n_full / n_total))
+    print("Main loop: {:.2f} Hz".format((n_total - n_full) / (time.time() - start_time)))
 
 
 if __name__ == '__main__':
@@ -158,7 +159,6 @@ def ctrl_c(signum, frame):
             is_connected = True
 
     print("Connected to Arduino")
-    resolution = CAMERA_RESOLUTION
 
     # Image processing queue, output centroids
     out_queue = queue.Queue()
@@ -169,7 +169,7 @@ def ctrl_c(signum, frame):
     # It starts 2 threads:
     #  - one for retrieving images from camera
     #  - one for processing the images
-    image_thread = ImageProcessingThread(Viewer(out_queue, resolution, debug=False, fps=FPS), exit_condition)
+    image_thread = ImageProcessingThread(Viewer(out_queue, CAMERA_RESOLUTION, debug=False, fps=FPS), exit_condition)
     # Wait for camera warmup
     time.sleep(1)
 
@@ -184,7 +184,7 @@ def ctrl_c(signum, frame):
         t.start()
 
     print("Starting Control Thread")
-    main_control(out_queue, resolution=resolution, n_seconds=N_SECONDS)
+    main_control(out_queue, resolution=CAMERA_RESOLUTION, n_seconds=N_SECONDS)
 
     # End the threads
     exit_event.set()

opencv/benchmark.py

@@ -11,24 +11,29 @@
 import numpy as np
 
 from opencv.image_processing import processImage
-
-N_ITER = 5000
+from opencv.c_extension import fastProcessImage
 
 parser = argparse.ArgumentParser(description='Benchmark line detection algorithm')
-parser.add_argument('-i', '--input_image', help='Input Image', default="", type=str, required=True)
+parser.add_argument('-i', '--input_image', help='Input Image', default="", type=str, required=False)
+parser.add_argument('-n', '--num_iterations', help='Number of iteration', default=1, type=int, required=False)
+
 args = parser.parse_args()
 
+N_ITER = args.num_iterations
 
-image = cv2.imread(args.input_image)
+image = cv2.imread(args.input_image).astype(np.float32)
+# image = 155 * np.ones((240, 320, 3), dtype=np.uint8)
 time_deltas = []
 for i in range(N_ITER):
     start_time = time.time()
-    turn_percent, centroids = processImage(image, debug=False, regions=None, interactive=False)
+    # turn_percent, centroids = processImage(image, debug=False, regions=None, interactive=False)
+    turn_percent, centroids = fastProcessImage(image)
     time_deltas.append(time.time() - start_time)
     # print(centroids)
+    # print(turn_percent)
 
 time_deltas = np.array(time_deltas)
 print("Total time: {:.6f}s".format(time_deltas.sum()))
-print("Mean time: {:.6f}s".format(time_deltas.mean()))
-print("Std time: {:.6f}s".format(time_deltas.std()))
-print("Median time: {:.6f}s".format(np.median(time_deltas)))
+print("Mean time: {:.4f}ms".format(1000 * time_deltas.mean()))
+print("Std time: {:.4f}ms".format(1000 * time_deltas.std()))
+print("Median time: {:.4f}ms".format(1000 * np.median(time_deltas)))

opencv/c_extension/CMakeLists.txt

@@ -0,0 +1,62 @@
+project(fast_image_processing)
+
+#**************************************************************************************************
+# General cMake settings
+#**************************************************************************************************
+cmake_minimum_required(VERSION 3.5)
+set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake ${CMAKE_MODULE_PATH})
+
+#**************************************************************************************************
+# Find Package **************************************************************************************************
+find_package(OpenCV 3 REQUIRED)
+MESSAGE( STATUS "OpenCV_INCLUDE_DIRS : " ${OpenCV_INCLUDE_DIRS} )
+MESSAGE( STATUS "OpenCV_LIB_DIRS : " ${OpenCV_LIB_DIRS} )
+MESSAGE( STATUS "OpenCV_LIBS : " ${OpenCV_LIBS} )
+
+find_package(PythonLibs REQUIRED)
+MESSAGE( STATUS "PYTHON_INCLUDE_DIRS : " ${PYTHON_INCLUDE_DIRS} )
+MESSAGE( STATUS "PYTHON_LIBRARIES : " ${PYTHON_LIBRARIES} )
+
+find_package(Eigen3 REQUIRED)
+
+find_package(NumPy REQUIRED)
+
+set(PYBIND11_CPP_STANDARD -std=c++14)
+find_package(pybind11 REQUIRED)
+
+#**************************************************************************************************
+# Include **************************************************************************************************
+include_directories(${PYTHON_INCLUDE_DIRS})
+include_directories(${EIGEN3_INCLUDE_DIRS})
+include_directories(${OpenCV_INCLUDE_DIRS})
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+include_directories(${NUMPY_INCLUDE_DIRS})
+include_directories(${PYBIND11_INCLUDE_DIRS})
+
+# include_directories(${CMAKE_CURRENT_SOURCE_DIR}/pybind11/include)
+
+#**************************************************************************************************
+# Set variable **************************************************************************************************
+SET(SOURCES
+  ${CMAKE_CURRENT_SOURCE_DIR}/fast_image_processing.cpp
+  ${CMAKE_CURRENT_SOURCE_DIR}/ndarray_converter.cpp
+)
+
+#**************************************************************************************************
+# Set compiler **************************************************************************************************
+SET(CMAKE_CXX_FLAGS "-std=c++14 -Wall -O3 -fPIC")
+
+#**************************************************************************************************
+# Linker **************************************************************************************************
+LINK_DIRECTORIES(
+  ${OpenCV_LIB_DIR}
+)
+
+#**************************************************************************************************
+# Make configuration
+#**************************************************************************************************
+add_library(fast_image_processing SHARED ${SOURCES})
+target_link_libraries(fast_image_processing ${PYTHON_LIBRARIES} ${OpenCV_LIBS} ${Eigen3_LIBS})
+SET_TARGET_PROPERTIES(fast_image_processing PROPERTIES PREFIX "")
+
+install(TARGETS fast_image_processing DESTINATION ${CMAKE_CURRENT_SOURCE_DIR})

opencv/c_extension/Readme.md

@@ -0,0 +1,42 @@
+# C++ extension for fast image processing
+
+It is based on [pybind11_opencv_numpy](https://github.com/edmBernard/pybind11_opencv_numpy).
+that allows interpolation between cv::Mat <-> np.array.
+
+Depending on the resize strategy (nearest neighbors or bilinear), the speedup compare to pure python + numpy is between x2 and x6.
+
+## Dependencies
+
+- [PyBind11 2.2.1](https://github.com/pybind/pybind11)
+- OpenCV 3 with Eigen support
+- [Eigen 3.3](http://eigen.tuxfamily.org/index.php?title=Main_Page)
+
+### Compile with CMake
+
+#### On the rapsberry pi
+```
+./pi_cmake.sh
+```
+
+#### On the Laptop
+```bash
+mkdir build && cd build
+# configure make
+cmake ..
+# generate the fast_image_processing.so library
+make
+# move fast_image_processing.so library in current folder
+make install
+```
+
+### Compile with setup.py
+WARNING: you have to manually edit the path to OpenCV + Eigen
+```
+./compile.sh
+```
+
+### Run
+To run the test, you need to copy `mlp_model.npz` in this folder and have an image named `test_sun.jpg` (ideally taken from the raspicam).
+```bash
+python test.py
+```