[Opencv]sped up the image conversions (#1824)

First tests show that the image conversions are now 2 to 4 times faster!

sw/airborne/modules/computer_vision/opencv_example.cpp

@@ -53,7 +53,7 @@ int opencv_example(char *img, int width, int height)
 
   // Convert back to YUV422, and put it in place of the original image
 //  grayscale_opencv_to_yuv422(image, img, width, height);
-  color_opencv_to_yuv422(image, img, width, height);
+  colorrgb_opencv_to_yuv422(image, img, width, height);
 
   return 0;
 }

sw/airborne/modules/computer_vision/opencv_image_functions.cpp

@@ -34,32 +34,73 @@ using namespace std;
 #include <opencv2/imgproc/imgproc.hpp>
 using namespace cv;
 
-void color_opencv_to_yuv422(Mat image, char *img, int width, int height)
+void coloryuv_opencv_to_yuv422(Mat image, char *img, int width, int height)
+{
+  CV_Assert(image.depth() == CV_8U);
+  CV_Assert(image.channels() == 3);
+
+  int nRows = image.rows;
+  int nCols = image.cols;
+
+  // If the image is one block in memory we can iterate over it all at once!
+  if (image.isContinuous()) {
+    nCols *= nRows;
+    nRows = 1;
+  }
+
+  // Iterate over the image, setting only the Y value
+  // and setting U and V to 127
+  int i, j;
+  uchar *p;
+  int index_img = 0;
+  for (i = 0; i < nRows; ++i) {
+    p = image.ptr<uchar>(i);
+    for (j = 0; j < nCols; j += 6) {
+      img[index_img++] = p[j + 1]; //U
+      img[index_img++] = p[j];//Y
+      img[index_img++] = p[j + 2]; //V
+      img[index_img++] = p[j + 3]; //Y
+
+
+    }
+  }
+}
+
+void colorrgb_opencv_to_yuv422(Mat image, char *img, int width, int height)
 {
   // Convert to YUV color space
   cvtColor(image, image, COLOR_BGR2YUV);
-
-  for (int row = 0; row < height; row++) {
-    for (int col = 0; col < width; col++) {
-      // Extract pixel color from image
-      cv::Vec3b &c = image.at<cv::Vec3b>(row, col);
-
-      // Set image buffer values
-      int i = row * width + col;
-      img[2 * i + 1] = c[0]; // y;
-      img[2 * i] = col % 2 ? c[1] : c[2]; // u or v
-    }
-  }
+  // then call the to color function
+  coloryuv_opencv_to_yuv422(image, img, width, height);
 }
 
 
 void grayscale_opencv_to_yuv422(Mat image, char *img, int width, int height)
 {
-  for (int row = 0; row < height; row++) {
-    for (int col = 0; col < width; col++) {
+  CV_Assert(image.depth() == CV_8U);
+  CV_Assert(image.channels() == 1);
+
+  int n_rows = image.rows;
+  int n_cols = image.cols;
+
+  // If the image is one block in memory we can iterate over it all at once!
+  if (image.isContinuous()) {
+    n_cols *= n_rows;
+    n_rows = 1;
+  }
+
+  // Iterate over the image, setting only the Y value
+  // and setting U and V to 127
+  int i, j;
+  uchar *p;
+  int index_img = 0;
+  for (i = 0; i < n_rows; ++i) {
+    p = image.ptr<uchar>(i);
+    for (j = 0; j < n_cols; j++) {
+      img[index_img++] = 127;
+      img[index_img++] = p[j];
+
 
-      img[(row * width + col) * 2 + 1] =   image.at<uint8_t>(row, col);
-      img[(row * width + col) * 2 ] = 127;
     }
   }
 }

sw/airborne/modules/computer_vision/opencv_image_functions.h

@@ -31,6 +31,21 @@
 #include <opencv2/core/core.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 
-void color_opencv_to_yuv422(cv::Mat image, char *img, int width, int height);
+/**
+ * Converts cv::Mat with three channels to a YUV422 image.
+ * Note that the rgb function first converts to YUV, and then to YUV422 making
+ * this function slower than coloryuv_opencv_to_yuv422.
+ */
+void colorrgb_opencv_to_yuv422(cv::Mat image, char *img, int width, int height);
+
+/**
+ * Converts cv::Mat with three channels YUV to a YUV422 image.
+ */
+void coloryuv_opencv_to_yuv422(cv::Mat image, char *img, int width, int height);
+
+/**
+ * Converts cv::Mat with one to a YUV422 image.
+ * The U and V channels are set to 127.
+ */
 void grayscale_opencv_to_yuv422(cv::Mat image, char *img, int width, int height);
 #endif