Create edge-impulse-advanced-v2.h

examples/dot5-portenta-machine-learning/dot55-robocar/dot551-robocar-v1/edge-impulse-advanced-v2.h

@@ -0,0 +1,359 @@
+/* Edge Impulse Arduino examples
+ * Copyright (c) 2021 EdgeImpulse Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+/* Includes ---------------------------------------------------------------- */
+
+#include "camera.h"
+#include "himax.h"
+#include "edge-impulse-sdk/dsp/image/image.hpp"
+
+/* Constant defines -------------------------------------------------------- */
+#define EI_CAMERA_RAW_FRAME_BUFFER_COLS           320
+#define EI_CAMERA_RAW_FRAME_BUFFER_ROWS           320
+
+// frame buffer allocation options:
+//    - static (default if none below is chosen)
+//    - heap or
+//    - SDRAM
+#define EI_CAMERA_FRAME_BUFFER_SDRAM
+//#define EI_CAMERA_FRAME_BUFFER_HEAP
+
+#ifdef EI_CAMERA_FRAME_BUFFER_SDRAM
+#include "SDRAM.h"
+#endif
+
+#define ALIGN_PTR(p,a)   ((p & (a-1)) ?(((uintptr_t)p + a) & ~(uintptr_t)(a-1)) : p)
+
+/* Edge Impulse ------------------------------------------------------------- */
+
+typedef struct {
+    size_t width;
+    size_t height;
+} ei_device_resize_resolutions_t;
+
+/**
+ * @brief      Check if new serial data is available
+ *
+ * @return     Returns number of available bytes
+ */
+int ei_get_serial_available(void) {
+    return Serial.available();
+}
+
+/**
+ * @brief      Get next available byte
+ *
+ * @return     byte
+ */
+char ei_get_serial_byte(void) {
+    return Serial.read();
+}
+
+
+/**
+* @brief      Printf function uses vsnprintf and output using Arduino Serial
+*
+* @param[in]  format     Variable argument list
+*/
+void ei_printf(const char *format, ...) {
+   static char print_buf[1024] = { 0 };
+
+   va_list args;
+   va_start(args, format);
+   int r = vsnprintf(print_buf, sizeof(print_buf), format, args);
+   va_end(args);
+
+   if (r > 0) {
+       Serial.write(print_buf);
+   }
+}
+
+/* Private variables ------------------------------------------------------- */
+static bool debug_nn = false; // Set this to true to see e.g. features generated from the raw signal
+static bool is_initialised = false;
+static bool is_ll_initialised = false;
+HM01B0 himax;
+static Camera cam(himax);
+FrameBuffer fb;
+
+
+/*
+** @brief points to the output of the capture
+*/
+static uint8_t *ei_camera_capture_out = NULL;
+
+/*
+** @brief used to store the raw frame
+*/
+#if defined(EI_CAMERA_FRAME_BUFFER_SDRAM) || defined(EI_CAMERA_FRAME_BUFFER_HEAP)
+static uint8_t *ei_camera_frame_mem;
+static uint8_t *ei_camera_frame_buffer; // 32-byte aligned
+#else
+static uint8_t ei_camera_frame_buffer[EI_CAMERA_RAW_FRAME_BUFFER_COLS * EI_CAMERA_RAW_FRAME_BUFFER_ROWS] __attribute__((aligned(32)));
+#endif
+
+/* Function definitions ------------------------------------------------------- */
+bool ei_camera_init(void);
+void ei_camera_deinit(void);
+bool ei_camera_capture(uint32_t img_width, uint32_t img_height, uint8_t *out_buf) ;
+int calculate_resize_dimensions(uint32_t out_width, uint32_t out_height, uint32_t *resize_col_sz, uint32_t *resize_row_sz, bool *do_resize);
+
+
+
+// setup and loop from orignal code moved to main sketch
+
+
+
+
+
+
+
+
+
+/**
+ * @brief   Setup image sensor & start streaming
+ *
+ * @retval  false if initialisation failed
+ */
+bool ei_camera_init(void) {
+    if (is_initialised) return true;
+
+    if (is_ll_initialised == false) {
+        if (!cam.begin(CAMERA_R320x320, CAMERA_GRAYSCALE, 30)) {
+            ei_printf("ERR: Failed to initialise camera\r\n");
+            return false;
+        }
+
+    #ifdef EI_CAMERA_FRAME_BUFFER_SDRAM
+        ei_camera_frame_mem = (uint8_t *) SDRAM.malloc(EI_CAMERA_RAW_FRAME_BUFFER_COLS * EI_CAMERA_RAW_FRAME_BUFFER_ROWS + 32 /*alignment*/);
+        if(ei_camera_frame_mem == NULL) {
+            ei_printf("failed to create ei_camera_frame_mem\r\n");
+            return false;
+        }
+        ei_camera_frame_buffer = (uint8_t *)ALIGN_PTR((uintptr_t)ei_camera_frame_mem, 32);
+    #endif
+
+        is_ll_initialised = true;
+    }
+
+    // initialize frame buffer
+#if defined(EI_CAMERA_FRAME_BUFFER_HEAP)
+    ei_camera_frame_mem = (uint8_t *) ei_malloc(EI_CAMERA_RAW_FRAME_BUFFER_COLS * EI_CAMERA_RAW_FRAME_BUFFER_ROWS + 32 /*alignment*/);
+    if(ei_camera_frame_mem == NULL) {
+        ei_printf("failed to create ei_camera_frame_mem\r\n");
+        return false;
+    }
+    ei_camera_frame_buffer = (uint8_t *)ALIGN_PTR((uintptr_t)ei_camera_frame_mem, 32);
+#endif
+
+    fb.setBuffer(ei_camera_frame_buffer);
+    is_initialised = true;
+
+    return true;
+}
+
+/**
+ * @brief      Stop streaming of sensor data
+ */
+void ei_camera_deinit(void) {
+
+#if defined(EI_CAMERA_FRAME_BUFFER_HEAP)
+    ei_free(ei_camera_frame_mem);
+    ei_camera_frame_mem = NULL;
+    ei_camera_frame_buffer = NULL;
+#endif
+
+    is_initialised = false;
+}
+
+/**
+ * @brief      Capture, rescale and crop image
+ *
+ * @param[in]  img_width     width of output image
+ * @param[in]  img_height    height of output image
+ * @param[in]  out_buf       pointer to store output image, NULL may be used
+ *                           if ei_camera_frame_buffer is to be used for capture and resize/cropping.
+ *
+ * @retval     false if not initialised, image captured, rescaled or cropped failed
+ *
+ */
+bool ei_camera_capture(uint32_t img_width, uint32_t img_height, uint8_t *out_buf) {
+    bool do_resize = false;
+    bool do_crop = false;
+
+    if (!is_initialised) {
+        ei_printf("ERR: Camera is not initialized\r\n");
+        return false;
+    }
+
+    int snapshot_response = cam.grabFrame(fb, 3000);
+    if (snapshot_response != 0) {
+        ei_printf("ERR: Failed to get snapshot (%d)\r\n", snapshot_response);
+        return false;
+    }
+
+    uint32_t resize_col_sz;
+    uint32_t resize_row_sz;
+    // choose resize dimensions
+    int res = calculate_resize_dimensions(img_width, img_height, &resize_col_sz, &resize_row_sz, &do_resize);
+    if (res) {
+        ei_printf("ERR: Failed to calculate resize dimensions (%d)\r\n", res);
+        return false;
+    }
+
+    if ((img_width != resize_col_sz)
+        || (img_height != resize_row_sz)) {
+        do_crop = true;
+    }
+
+    // The following variables should always be assigned
+    // if this routine is to return true
+    // cutout values
+    ei_camera_capture_out = ei_camera_frame_buffer;
+
+    if (do_resize) {
+
+        // if only resizing then and out_buf provided then use itinstead.
+        if (out_buf && !do_crop) ei_camera_capture_out = out_buf;
+
+        //ei_printf("resize cols: %d, rows: %d\r\n", resize_col_sz,resize_row_sz);
+        ei::image::processing::resize_image(
+            ei_camera_frame_buffer,
+            EI_CAMERA_RAW_FRAME_BUFFER_COLS,
+            EI_CAMERA_RAW_FRAME_BUFFER_ROWS,
+            ei_camera_capture_out,
+            resize_col_sz,
+            resize_row_sz,
+            1); // bytes per pixel
+    }
+
+    if (do_crop) {
+        uint32_t crop_col_sz;
+        uint32_t crop_row_sz;
+        uint32_t crop_col_start;
+        uint32_t crop_row_start;
+        crop_row_start = (resize_row_sz - img_height) / 2;
+        crop_col_start = (resize_col_sz - img_width) / 2;
+        crop_col_sz = img_width;
+        crop_row_sz = img_height;
+
+        // if (also) cropping and out_buf provided then use it instead.
+        if (out_buf) ei_camera_capture_out = out_buf;
+
+        //ei_printf("crop cols: %d, rows: %d\r\n", crop_col_sz,crop_row_sz);
+        ei::image::processing::cropImage(
+            ei_camera_frame_buffer,
+            resize_col_sz,
+            resize_row_sz,
+            crop_col_start,
+            crop_row_start,
+            ei_camera_capture_out,
+            crop_col_sz,
+            crop_row_sz,
+            8); // bits per pixel
+    }
+
+    return true;
+}
+
+/**
+ * @brief      Convert monochrome data to rgb values
+ *
+ * @param[in]  mono_data  The mono data
+ * @param      r          red pixel value
+ * @param      g          green pixel value
+ * @param      b          blue pixel value
+ */
+static inline void mono_to_rgb(uint8_t mono_data, uint8_t *r, uint8_t *g, uint8_t *b) {
+    uint8_t v = mono_data;
+    *r = *g = *b = v;
+}
+
+
+int ei_camera_cutout_get_data(size_t offset, size_t length, float *out_ptr) {
+    size_t bytes_left = length;
+    size_t out_ptr_ix = 0;
+
+    // read byte for byte
+    while (bytes_left != 0) {
+
+        // grab the value and convert to r/g/b
+        uint8_t pixel = ei_camera_capture_out[offset];
+
+        uint8_t r, g, b;
+        mono_to_rgb(pixel, &r, &g, &b);
+
+        // then convert to out_ptr format
+        float pixel_f = (r << 16) + (g << 8) + b;
+        out_ptr[out_ptr_ix] = pixel_f;
+
+        // and go to the next pixel
+        out_ptr_ix++;
+        offset++;
+        bytes_left--;
+    }
+
+    // and done!
+    return 0;
+}
+
+/**
+ * @brief      Determine whether to resize and to which dimension
+ *
+ * @param[in]  out_width     width of output image
+ * @param[in]  out_height    height of output image
+ * @param[out] resize_col_sz       pointer to frame buffer's column/width value
+ * @param[out] resize_row_sz       pointer to frame buffer's rows/height value
+ * @param[out] do_resize     returns whether to resize (or not)
+ *
+ */
+int calculate_resize_dimensions(uint32_t out_width, uint32_t out_height, uint32_t *resize_col_sz, uint32_t *resize_row_sz, bool *do_resize)
+{
+    size_t list_size = 6;
+    const ei_device_resize_resolutions_t list[list_size] = {
+        {128, 96},
+        {160, 120},
+        {200, 150},
+        {256, 192},
+        {320, 240},
+    };
+
+    // (default) conditions
+    *resize_col_sz = EI_CAMERA_RAW_FRAME_BUFFER_COLS;
+    *resize_row_sz = EI_CAMERA_RAW_FRAME_BUFFER_ROWS;
+    *do_resize = false;
+
+    for (size_t ix = 0; ix < list_size; ix++) {
+        if ((out_width <= list[ix].width) && (out_height <= list[ix].height)) {
+            *resize_col_sz = list[ix].width;
+            *resize_row_sz = list[ix].height;
+            *do_resize = true;
+            break;
+        }
+    }
+
+    return 0;
+}
+
+#if !defined(EI_CLASSIFIER_SENSOR) || EI_CLASSIFIER_SENSOR != EI_CLASSIFIER_SENSOR_CAMERA
+#error "Invalid model for current sensor"
+#endif