esp-who/components/image_util/image_util.c

401 lines
12 KiB
C

#include <stdio.h>
#include <stdint.h>
#include <assert.h>
#include <stdlib.h>
#include <math.h>
#include <stdbool.h>
#include <string.h>
#include "dl_lib.h"
#include "image_util.h"
#include "freertos/FreeRTOS.h"
void image_resize_linear (uint8_t *dst_image, uint8_t *src_image, int dst_w, int dst_h, int dst_c, int src_w, int src_h)
{/*{{{*/
float scale_x = (float)src_w / dst_w;
float scale_y = (float)src_h / dst_h;
int dst_stride = dst_c * dst_w;
int src_stride = dst_c * src_w;
for (int y = 0; y < dst_h; y++)
{
float fy[2];
fy[0] = (float)((y + 0.5) * scale_y - 0.5); // y
int src_y = (int)fy[0]; // y1
fy[0] -= src_y; // y - y1
fy[1] = 1 - fy[0]; // y2 - y
src_y = DL_IMAGE_MAX(0, src_y);
src_y = DL_IMAGE_MIN(src_y, src_h - 2);
for (int x = 0; x < dst_w; x++)
{
float fx[2];
fx[0] = (float)((x + 0.5) * scale_x - 0.5); // x
int src_x = (int)fx[0]; // x1
fx[0] -= src_x; // x - x1
if (src_x < 0)
{
fx[0] = 0;
src_x = 0;
}
if (src_x > src_w - 2)
{
fx[0] = 0;
src_x = src_w - 2;
}
fx[1] = 1 - fx[0]; // x2 - x
for (int c = 0; c < dst_c; c++)
{
dst_image[y * dst_stride + x * dst_c + c] = round(src_image[src_y * src_stride + src_x * dst_c + c] * fx[1] * fy[1]
+ src_image[src_y * src_stride + (src_x + 1) * dst_c + c] * fx[0] * fy[1]
+ src_image[(src_y + 1) * src_stride + src_x * dst_c + c] * fx[1] * fy[0]
+ src_image[(src_y + 1) * src_stride + (src_x + 1) * dst_c + c] * fx[0] * fy[0]);
}
}
}
}/*}}}*/
// TODO: Crop and align the face according to the output size ,angle,ratio,and center. Using the image_resize_linear inside.
// @crop_image: here should be a 56x56x3
// @src_image: input original image.
// @rotate_angle: radius that stands for the rotation. Reverse clock is the positive direction.
// @ratio: the ratio input/output eye distance.
// @center: x,y coordinates that is the rotation center in the input image as well as the cropped image center.
void image_cropper(uint8_t *rot_data, uint8_t *src_data, int rot_w, int rot_h, int rot_c, int src_w, int src_h, float rotate_angle, float ratio, float* center)
{
float rot_w_start = 0.5f -(float)rot_w/2;
float rot_h_start = 0.5f -(float)rot_h/2;
//rotate_angle must be radius
float si = sin(rotate_angle);
float co = cos(rotate_angle);
int rot_stride = rot_w * rot_c;
int src_stride = rot_c * src_w;
for (int y = 0; y < rot_h; y++)
{
for (int x = 0; x < rot_w; x++)
{
float xs,ys,xr,yr;
xs = ratio * (rot_w_start + x);
ys = ratio * (rot_h_start + y);
xr = xs*co + ys*si;
yr = -xs*si + ys*co;
float fy[2];
fy[0] = center[1] + yr; // y
int src_y = (int)fy[0]; // y1
fy[0] -= src_y; // y - y1
fy[1] = 1 - fy[0]; // y2 - y
src_y = DL_IMAGE_MAX(0, src_y);
src_y = DL_IMAGE_MIN(src_y, src_h - 2);
float fx[2];
fx[0] = center[0] + xr; // x
int src_x = (int)fx[0]; // x1
fx[0] -= src_x; // x - x1
if (src_x < 0)
{
fx[0] = 0;
src_x = 0;
}
if (src_x > src_w - 2)
{
fx[0] = 0;
src_x = src_w - 2;
}
fx[1] = 1 - fx[0]; // x2 - x
for (int c = 0; c < rot_c; c++)
{
rot_data[y * rot_stride + x * rot_c + c] = round(src_data[src_y * src_stride + src_x * rot_c + c] * fx[1] * fy[1]
+ src_data[src_y * src_stride + (src_x + 1) * rot_c + c] * fx[0] * fy[1]
+ src_data[(src_y + 1) * src_stride + src_x * rot_c + c] * fx[1] * fy[0]
+ src_data[(src_y + 1) * src_stride + (src_x + 1) * rot_c + c] * fx[0] * fy[0]);
}
}
}
}
void image_sort_insert_by_score (image_list_t *image_sorted_list, const image_list_t *insert_list)
{/*{{{*/
if (insert_list == NULL || insert_list->head == NULL)
return;
image_box_t *box = insert_list->head;
if (NULL == image_sorted_list->head)
{
image_sorted_list->head = insert_list->head;
box = insert_list->head->next;
image_sorted_list->head->next = NULL;
}
image_box_t *head = image_sorted_list->head;
while (box)
{
// insert in head
if (box->score > head->score)
{
image_box_t *tmp = box;
box = box->next;
tmp->next = head;
head = tmp;
}
else
{
image_box_t *curr = head->next;
image_box_t *prev = head;
while (curr)
{
if (box->score > curr->score)
{
image_box_t *tmp = box;
box = box->next;
tmp->next = curr;
prev->next = tmp;
break;
}
prev = curr;
curr = curr->next;
}
// insert in tail
if (NULL == curr)
{
image_box_t *tmp = box;
box = box->next;
tmp->next = NULL;
prev->next = tmp;
}
}
}
image_sorted_list->head = head;
image_sorted_list->len += insert_list->len;
}/*}}}*/
image_list_t *image_get_valid_boxes (fptp_t *score, fptp_t *offset, int width, int height, fptp_t score_threshold, fptp_t scale)
{/*{{{*/
typedef struct
{
short valid_x;
short valid_y;
int valid_idx;
} valid_index_t;
valid_index_t *valid_indexes = (valid_index_t *)calloc(width * height, sizeof(valid_index_t));
int valid_count = 0;
int index = 0;
for (int y = 0; y < height; y++)
for (int x = 0; x < width; x++)
{
if (score[2 * index + 1] > score_threshold)
{
valid_indexes[valid_count].valid_x = x;
valid_indexes[valid_count].valid_y = y;
valid_indexes[valid_count].valid_idx = index;
valid_count++;
}
index++;
}
if (0 == valid_count)
{
free(valid_indexes);
return NULL;
}
image_box_t *valid_box = (image_box_t *)calloc(valid_count, sizeof(image_box_t));
image_list_t *valid_list = (image_list_t *)calloc(1, sizeof(image_list_t));
valid_list->head = valid_box;
valid_list->origin_head = valid_box;
valid_list->len = valid_count;
for (int i = 0; i < valid_count; i++)
{
fptp_t x1 = valid_indexes[i].valid_x * scale * 2.0;
fptp_t y1 = valid_indexes[i].valid_y * scale * 2.0;
int valid_i = valid_indexes[i].valid_idx;
valid_box[i].score = score[2 * valid_i + 1];
valid_box[i].box.box_p[0] = x1;
valid_box[i].box.box_p[1] = y1;
valid_box[i].box.box_p[2] = x1 + MIN_FACE * scale;
valid_box[i].box.box_p[3] = y1 + MIN_FACE * scale;
valid_box[i].offset.box_p[0] = offset[valid_i * 4 + 0];
valid_box[i].offset.box_p[1] = offset[valid_i * 4 + 1];
valid_box[i].offset.box_p[2] = offset[valid_i * 4 + 2];
valid_box[i].offset.box_p[3] = offset[valid_i * 4 + 3];
valid_box[i].next = &(valid_box[i + 1]);
}
valid_box[valid_count - 1].next = NULL;
free(valid_indexes);
return valid_list;
}/*}}}*/
void image_nms_process (image_list_t *image_list, fptp_t nms_threshold, int same_area)
{/*{{{*/
/**** Init ****/
int num_supressed = 0;
image_box_t *head = image_list->head;
/**** Compute Box Area ****/
fptp_t kept_box_area = 0;
fptp_t other_box_area = 0;
if (same_area)
{
image_get_area(&(head->box), &kept_box_area);
other_box_area = kept_box_area;
}
/**** Compare IOU ****/
image_box_t *kept_box = head;
while (kept_box)
{
image_box_t *other_box = kept_box->next;
image_box_t *prev = kept_box;
while (other_box)
{
// kept_box is contained in other_box
if (((kept_box->box.box_p[0] > other_box->box.box_p[0])
&& (kept_box->box.box_p[1] > other_box->box.box_p[1])
&& (kept_box->box.box_p[2] < other_box->box.box_p[2])
&& (kept_box->box.box_p[3] < other_box->box.box_p[3]))
// kept_box contains other_box
|| ((kept_box->box.box_p[0] < other_box->box.box_p[0])
&& (kept_box->box.box_p[1] < other_box->box.box_p[1])
&& (kept_box->box.box_p[2] > other_box->box.box_p[2])
&& (kept_box->box.box_p[3] > other_box->box.box_p[3])))
// supress
{
num_supressed++;
prev->next = other_box->next;
other_box = other_box->next;
continue;
}
box_t inter_box;
inter_box.box_p[0] = DL_IMAGE_MAX(kept_box->box.box_p[0], other_box->box.box_p[0]);
inter_box.box_p[1] = DL_IMAGE_MAX(kept_box->box.box_p[1], other_box->box.box_p[1]);
inter_box.box_p[2] = DL_IMAGE_MIN(kept_box->box.box_p[2], other_box->box.box_p[2]);
inter_box.box_p[3] = DL_IMAGE_MIN(kept_box->box.box_p[3], other_box->box.box_p[3]);
fptp_t inter_w, inter_h;
image_get_width_and_height(&inter_box, &inter_w, &inter_h);
if (inter_w > 0 && inter_h > 0)
{
if (!same_area)
{
image_get_area(&(kept_box->box), &kept_box_area);
image_get_area(&(other_box->box), &other_box_area);
}
fptp_t inter_area = inter_w * inter_h;
fptp_t iou = inter_area / (kept_box_area + other_box_area - inter_area);
if (iou > nms_threshold)
{
num_supressed++;
// Delete duplicated box
// Here we cannot free a single box, because these boxes are allocated by calloc, we need to free all the calloced memory together.
prev->next = other_box->next;
other_box = other_box->next;
continue;
}
}
prev = other_box;
other_box = other_box->next;
}
kept_box = kept_box->next;
}
image_list->len -= num_supressed;
}/*}}}*/
void transform_input_image (uint8_t *m, uint16_t *bmp, int count)
{/*{{{*/
uc_t dst[24];
for (int x = 0; x < count; x += 8)
{
rgb565_to_888(*bmp++, dst);
rgb565_to_888(*bmp++, dst + 3);
rgb565_to_888(*bmp++, dst + 6);
rgb565_to_888(*bmp++, dst + 9);
rgb565_to_888(*bmp++, dst + 12);
rgb565_to_888(*bmp++, dst + 15);
rgb565_to_888(*bmp++, dst + 18);
rgb565_to_888(*bmp++, dst + 21);
memcpy(m + x * 3, dst, 24 * sizeof(uint8_t));
}
}/*}}}*/
void transform_output_image (uint16_t *bmp, uint8_t *m, int count)
{/*{{{*/
for (int x = 0; x < count; x++)
{
rgb888_to_565(bmp, m[2], m[1], m[0]);
bmp++;
m += 3;
}
}/*}}}*/
void draw_rectangle(uint16_t *buf, box_array_t *boxes, int width)
{/*{{{*/
uint16_t p[14];
for (int i = 0; i < boxes->len; i++)
{
// rectangle box
for (int j = 0; j < 4; j++)
p[j] = (uint16_t)boxes->box[i].box_p[j];
// landmark
for (int j = 0; j < 10; j++)
p[j + 4] = (uint16_t)boxes->landmark[i].landmark_p[j];
if ((p[2] < p[0]) || (p[3] < p[1]))
return;
#define GREEN RGB565_MASK_GREEN
#define RED RGB565_MASK_GREEN
// rectangle box
for (int w = p[0]; w < p[2] + 1; w++)
{
int x1 = (p[1] * width + w);
int x2 = (p[3] * width + w);
buf[x1] = GREEN; // Green
buf[x2] = GREEN;
}
for (int h = p[1]; h < p[3] + 1; h++)
{
int y1 = (h * width + p[0]);
int y2 = (h * width + p[2]);
buf[y1] = GREEN; // Green
buf[y2] = GREEN;
}
// landmark
for (int j = 0; j < 10; j+=2)
{
int x = p[j + 5] * width + p[j + 4];
buf[x] = RED; // Red
buf[x + 1] = RED;
buf[x + 2] = RED;
buf[width + x] = RED; // Red
buf[width + x + 1] = RED;
buf[width + x + 2] = RED;
buf[2 * width + x] = RED; // Red
buf[2 * width + x + 1] = RED;
buf[2 * width + x + 2] = RED;
}
}
}/*}}}*/