haar.c

/*
 * This file is part of the OpenMV project.
 *
 * Copyright (c) 2013-2021 Ibrahim Abdelkader <iabdalkader@openmv.io>
 * Copyright (c) 2013-2021 Kwabena W. Agyeman <kwagyeman@openmv.io>
 *
 * This work is licensed under the MIT license, see the file LICENSE for details.
 *
 * Viola-Jones object detector implementation.
 * Based on the work of Francesco Comaschi (f.comaschi@tue.nl)
 */
#include <stdio.h>
#include "py/obj.h"
#include "py/nlr.h"

#include "xalloc.h"
#include "imlib.h"
// built-in cascades
#include "cascade.h"
#include "file_utils.h"

static int eval_weak_classifier(cascade_t *cascade, point_t pt, int t_idx, int w_idx, int r_idx) {
    int32_t sumw = 0;
    mw_image_t *sum = cascade->sum;

    /* The node threshold is multiplied by the standard deviation of the sub window */
    int32_t t = cascade->tree_thresh_array[t_idx] * cascade->std;

    for (int i = 0; i < cascade->num_rectangles_array[t_idx]; i++) {
        int x = cascade->rectangles_array[r_idx + (i << 2) + 0];
        int y = cascade->rectangles_array[r_idx + (i << 2) + 1];
        int w = cascade->rectangles_array[r_idx + (i << 2) + 2];
        int h = cascade->rectangles_array[r_idx + (i << 2) + 3];
        // Lookup the feature
        sumw += imlib_integral_mw_lookup(sum, pt.x + x, y, w, h) * (cascade->weights_array[w_idx + i] << 12);
    }

    if (sumw >= t) {
        return cascade->alpha2_array[t_idx];
    }

    return cascade->alpha1_array[t_idx];
}

static int run_cascade_classifier(cascade_t *cascade, point_t pt) {
    int win_w = cascade->window.w;
    int win_h = cascade->window.h;
    uint32_t n = (win_w * win_h);
    uint32_t i_s = imlib_integral_mw_lookup(cascade->sum, pt.x, 0, win_w, win_h);
    uint32_t i_sq = imlib_integral_mw_lookup(cascade->ssq, pt.x, 0, win_w, win_h);
    uint32_t m = i_s / n;
    uint32_t v = i_sq / n - (m * m);

    // Skip homogeneous regions.
    if (v < (50 * 50)) {
        return 0;
    }

    cascade->std = fast_sqrtf(i_sq * n - (i_s * i_s));
    for (int i = 0, w_idx = 0, r_idx = 0, t_idx = 0; i < cascade->n_stages; i++) {
        int stage_sum = 0;
        for (int j = 0; j < cascade->stages_array[i]; j++, t_idx++) {
            // Send the shifted window to a haar filter
            stage_sum += eval_weak_classifier(cascade, pt, t_idx, w_idx, r_idx);
            w_idx += cascade->num_rectangles_array[t_idx];
            r_idx += cascade->num_rectangles_array[t_idx] * 4;
        }
        // If the sum is below the stage threshold, no objects were detected
        if (stage_sum < (cascade->threshold * cascade->stages_thresh_array[i])) {
            return 0;
        }
    }
    return 1;
}

array_t *imlib_detect_objects(image_t *image, cascade_t *cascade, rectangle_t *roi) {
    // Integral images
    mw_image_t sum;
    mw_image_t ssq;

    // Detected objects array
    array_t *objects;

    // Allocate the objects array
    array_alloc(&objects, xfree);

    // Set cascade image pointers
    cascade->img = image;
    cascade->sum = &sum;
    cascade->ssq = &ssq;

    // Set scanning step.
    // Viola and Jones achieved best results using a scaling factor
    // of 1.25 and a scanning factor proportional to the current scale.
    // Start with a step of 5% of the image width and reduce at each scaling step
    cascade->step = (roi->w * 50) / 1000;

    // Make sure step is less than window height + 1
    if (cascade->step > cascade->window.h) {
        cascade->step = cascade->window.h;
    }

    // Allocate integral images
    imlib_integral_mw_alloc(&sum, roi->w, cascade->window.h + 1);
    imlib_integral_mw_alloc(&ssq, roi->w, cascade->window.h + 1);

    // Iterate over the image pyramid
    for (float factor = 1.0f; ; factor *= cascade->scale_factor) {
        // Set the scaled width and height
        int szw = roi->w / factor;
        int szh = roi->h / factor;

        // Break if scaled image is smaller than feature size
        if (szw < cascade->window.w || szh < cascade->window.h) {
            break;
        }

        // Set the integral images scale
        imlib_integral_mw_scale(roi, &sum, szw, szh);
        imlib_integral_mw_scale(roi, &ssq, szw, szh);

        // Compute new scaled integral images
        imlib_integral_mw_ss(image, &sum, &ssq, roi);

        // Scale the scanning step
        cascade->step = cascade->step / factor;
        cascade->step = (cascade->step == 0) ? 1 : cascade->step;

        // Process image at the current scale
        // When filter window shifts to borders, some margin need to be kept
        int y2 = szh - cascade->window.h;
        int x2 = szw - cascade->window.w;

        // Shift the filter window over the image.
        for (int y = 0; y < y2; y += cascade->step) {
            for (int x = 0; x < x2; x += cascade->step) {
                point_t p = {x, y};
                // If an object is detected, record the coordinates of the filter window
                if (run_cascade_classifier(cascade, p) > 0) {
                    array_push_back(objects,
                                    rectangle_alloc(fast_roundf(x * factor) + roi->x, fast_roundf(y * factor) + roi->y,
                                                    fast_roundf(cascade->window.w * factor),
                                                    fast_roundf(cascade->window.h * factor)));
                }
            }

            // If not last line, shift integral images
            if ((y + cascade->step) < y2) {
                imlib_integral_mw_shift_ss(image, &sum, &ssq, roi, cascade->step);
            }
        }
    }

    imlib_integral_mw_free(&ssq);
    imlib_integral_mw_free(&sum);

    if (array_length(objects) > 1) {
        // Merge objects detected at different scales
        objects = rectangle_merge(objects);
    }

    return objects;
}

#if defined(IMLIB_ENABLE_IMAGE_FILE_IO)
int imlib_load_cascade_from_file(cascade_t *cascade, const char *path) {
    int i;
    FIL fp;
    FRESULT res = FR_OK;

    file_open(&fp, path, true, FA_READ | FA_OPEN_EXISTING);

    // Read detection window size
    file_read(&fp, &cascade->window, sizeof(cascade->window));

    // Read num stages
    file_read(&fp, &cascade->n_stages, sizeof(cascade->n_stages));

    cascade->stages_array = xalloc(sizeof(*cascade->stages_array) * cascade->n_stages);
    cascade->stages_thresh_array = xalloc(sizeof(*cascade->stages_thresh_array) * cascade->n_stages);
    if (cascade->stages_array == NULL ||
        cascade->stages_thresh_array == NULL) {
        res = 20;
        goto error;
    }

    /* read num features in each stages */
    file_read(&fp, cascade->stages_array, sizeof(uint8_t) * cascade->n_stages);

    /* sum num of features in each stages*/
    for (i = 0, cascade->n_features = 0; i < cascade->n_stages; i++) {
        cascade->n_features += cascade->stages_array[i];
    }

    /* alloc features thresh array, alpha1, alpha 2,rects weights and rects*/
    cascade->tree_thresh_array = xalloc(sizeof(*cascade->tree_thresh_array) * cascade->n_features);
    cascade->alpha1_array = xalloc(sizeof(*cascade->alpha1_array) * cascade->n_features);
    cascade->alpha2_array = xalloc(sizeof(*cascade->alpha2_array) * cascade->n_features);
    cascade->num_rectangles_array = xalloc(sizeof(*cascade->num_rectangles_array) * cascade->n_features);

    if (cascade->tree_thresh_array == NULL ||
        cascade->alpha1_array == NULL ||
        cascade->alpha2_array == NULL ||
        cascade->num_rectangles_array == NULL) {
        res = 20;
        goto error;
    }

    /* read stages thresholds */
    file_read(&fp, cascade->stages_thresh_array, sizeof(int16_t) * cascade->n_stages);

    /* read features thresholds */
    file_read(&fp, cascade->tree_thresh_array, sizeof(*cascade->tree_thresh_array) * cascade->n_features);

    /* read alpha 1 */
    file_read(&fp, cascade->alpha1_array, sizeof(*cascade->alpha1_array) * cascade->n_features);

    /* read alpha 2 */
    file_read(&fp, cascade->alpha2_array, sizeof(*cascade->alpha2_array) * cascade->n_features);

    /* read num rectangles per feature*/
    file_read(&fp, cascade->num_rectangles_array, sizeof(*cascade->num_rectangles_array) * cascade->n_features);

    /* sum num of recatngles per feature*/
    for (i = 0, cascade->n_rectangles = 0; i < cascade->n_features; i++) {
        cascade->n_rectangles += cascade->num_rectangles_array[i];
    }

    cascade->weights_array = xalloc(sizeof(*cascade->weights_array) * cascade->n_rectangles);
    cascade->rectangles_array = xalloc(sizeof(*cascade->rectangles_array) * cascade->n_rectangles * 4);

    if (cascade->weights_array == NULL ||
        cascade->rectangles_array == NULL) {
        res = 20;
        goto error;
    }

    /* read rectangles weights */
    file_read(&fp, cascade->weights_array, sizeof(*cascade->weights_array) * cascade->n_rectangles);

    /* read rectangles num rectangles * 4 points */
    file_read(&fp, cascade->rectangles_array, sizeof(*cascade->rectangles_array) * cascade->n_rectangles * 4);

error:
    file_close(&fp);
    return res;
}
#endif //(IMLIB_ENABLE_IMAGE_FILE_IO)

int imlib_load_cascade(cascade_t *cascade, const char *path) {
    // built-in cascade
    if (strcmp(path, "frontalface") == 0) {
        cascade->window.w = frontalface_window_w;
        cascade->window.h = frontalface_window_h;
        cascade->n_stages = frontalface_n_stages;
        cascade->stages_array = (uint8_t *) frontalface_stages_array;
        cascade->stages_thresh_array = (int16_t *) frontalface_stages_thresh_array;
        cascade->tree_thresh_array = (int16_t *) frontalface_tree_thresh_array;
        cascade->alpha1_array = (int16_t *) frontalface_alpha1_array;
        cascade->alpha2_array = (int16_t *) frontalface_alpha2_array;
        cascade->num_rectangles_array = (int8_t *) frontalface_num_rectangles_array;
        cascade->weights_array = (int8_t *) frontalface_weights_array;
        cascade->rectangles_array = (int8_t *) frontalface_rectangles_array;
    } else if (strcmp(path, "eye") == 0) {
        cascade->window.w = eye_window_w;
        cascade->window.h = eye_window_h;
        cascade->n_stages = eye_n_stages;
        cascade->stages_array = (uint8_t *) eye_stages_array;
        cascade->stages_thresh_array = (int16_t *) eye_stages_thresh_array;
        cascade->tree_thresh_array = (int16_t *) eye_tree_thresh_array;
        cascade->alpha1_array = (int16_t *) eye_alpha1_array;
        cascade->alpha2_array = (int16_t *) eye_alpha2_array;
        cascade->num_rectangles_array = (int8_t *) eye_num_rectangles_array;
        cascade->weights_array = (int8_t *) eye_weights_array;
        cascade->rectangles_array = (int8_t *) eye_rectangles_array;
    } else {
        #if defined(IMLIB_ENABLE_IMAGE_FILE_IO)
        // xml cascade
        return imlib_load_cascade_from_file(cascade, path);
        #else
        return -1;
        #endif
    }

    int i;
    // sum the number of features in all stages
    for (i = 0, cascade->n_features = 0; i < cascade->n_stages; i++) {
        cascade->n_features += cascade->stages_array[i];
    }

    // sum the number of recatngles in all features
    for (i = 0, cascade->n_rectangles = 0; i < cascade->n_features; i++) {
        cascade->n_rectangles += cascade->num_rectangles_array[i];
    }
    return FR_OK;
}