Restructure Image Library

* Split image library into multiple source files
* Add new img/ to src
* Add median, kmeans, median, blob, SURF.
This commit is contained in:
iabdalkader 2014-03-01 15:27:37 +02:00
parent 41936ff96a
commit f5b91439b2
14 changed files with 9238 additions and 624 deletions

View File

@ -16,14 +16,14 @@ BUILD_DIR=build
ifeq ($(DEBUG), 1)
CFLAGS = -O0 -g -DPENDSV_DEBUG
else
CFLAGS = -O2 -g
CFLAGS = -Os -g
endif
# Compiler Flags
CFLAGS += -Wall -mlittle-endian -mthumb -nostartfiles -mcpu=cortex-m4 -mabi=aapcs-linux
CFLAGS += -fsingle-precision-constant -Wdouble-promotion -mfpu=fpv4-sp-d16 -mfloat-abi=hard -fshort-enums
CFLAGS += -I. -I../include/CMSIS -I../include/StdPeriph -I../include/USB_OTG -I../include/FatFS -I../include/MicroPython\
-I../include/MicroPython/py -I./py -DSTM32F40_41xxx -DUSE_USB_OTG_FS -DARM_MATH_CM4 -D__FPU_PRESENT -std=gnu99 -DOSC=12
-I../include/MicroPython/py -I./py -I./img -DSTM32F40_41xxx -DUSE_USB_OTG_FS -DARM_MATH_CM4 -D__FPU_PRESENT -std=gnu99 -DOSC=12
# Linker Flags
LDFLAGS = -mcpu=cortex-m4 -mabi=aapcs-linux -mthumb -mlittle-endian -mfloat-abi=hard -mfpu=fpv4-sp-d16
@ -32,7 +32,7 @@ LDFLAGS += -Tstm32f4xx.ld -L. -L../lib
# Sources
SRCS = $(wildcard *.c) $(wildcard py/*.c)
SRCS = $(wildcard *.c) $(wildcard py/*.c) $(wildcard img/*.c)
OBJS = $(addprefix $(BUILD_DIR)/, $(SRCS:.c=.o))
# Libraries
@ -44,6 +44,7 @@ all:: $(BUILD_DIR) $(BUILD_DIR) $(BUILD_DIR)/$(BIN).bin
$(BUILD_DIR):
mkdir $@
mkdir $@/py
mkdir $@/img
$(BUILD_DIR)/$(BIN).bin: $(BUILD_DIR)/$(BIN).elf
$(OBJCOPY) -Obinary $^ $@

71
src/img/blob.c Normal file
View File

@ -0,0 +1,71 @@
#include <libmp.h>
#include "xalloc.h"
#include "imlib.h"
#include <math.h>
#include <arm_math.h>
array_t *imlib_count_blobs(struct image *image)
{
array_t *blobs;
array_t *points;
array_alloc(&blobs, xfree);
array_alloc_init(&points, xfree, 300);
uint16_t *pixels = (uint16_t*) image->pixels;
for (int y=1; y<image->h-1; y++) {
for (int x=1; x<image->w-1; x++) {
int i=y*image->w+x;
if (pixels[i]) {
/* new blob */
rectangle_t *blob = rectangle_alloc(image->w, image->h, 0, 0);
array_push_back(blobs, blob);
/* flood fill */
array_push_back(points, point_alloc(x, y));
while(array_length(points)) {
int i = array_length(points)-1;
point_t *p = array_at(points, i);
int px = p->x;
int py = p->y;
array_erase(points, i);
/* add point to blob */
if (px < blob->x) {
blob->x = px;
}
if (py < blob->y) {
blob->y = y;
}
if (px > blob->w) {
blob->w = px;
}
if (py > blob->h) {
blob->h = py;
}
if (pixels[py*image->w+px]) {
pixels[py*image->w+px]=0x0000;
array_push_back(points, point_alloc(px-1, py));
array_push_back(points, point_alloc(px+1, py));
array_push_back(points, point_alloc(px, py-1));
array_push_back(points, point_alloc(px, py+1));
}
}
}
}
}
for (int i=0; i<array_length(blobs); i++) {
rectangle_t *blob = array_at(blobs, i);
if (blob->w < 10) { /* blob too small */
array_erase(blobs, i);
i--;
} else {
blob->w = blob->w - blob->x;
blob->h = blob->h - blob->y;
}
}
return blobs;
}

323
src/img/haar.c Normal file
View File

@ -0,0 +1,323 @@
#include <libmp.h>
#include "xalloc.h"
#include "imlib.h"
#include <math.h>
#include <arm_math.h>
/* Viola-Jones face detector implementation
* Original Author: Francesco Comaschi (f.comaschi@tue.nl)
*/
static int evalWeakClassifier(struct cascade *cascade, int std, int p_offset, int tree_index, int w_index, int r_index )
{
int i, sumw=0;
struct rectangle tr;
struct integral_image *sum = &cascade->sum;
/* the node threshold is multiplied by the standard deviation of the image */
int t = cascade->tree_thresh_array[tree_index] * std;
for (i=0; i<cascade->num_rectangles_array[tree_index]; i++) {
tr.x = cascade->rectangles_array[r_index + i*4 + 0];
tr.y = cascade->rectangles_array[r_index + i*4 + 1];
tr.w = cascade->rectangles_array[r_index + i*4 + 2];
tr.h = cascade->rectangles_array[r_index + i*4 + 3];
sumw += (
*((sum->data + sum->w*(tr.y ) + (tr.x )) + p_offset)
- *((sum->data + sum->w*(tr.y ) + (tr.x + tr.w)) + p_offset)
- *((sum->data + sum->w*(tr.y + tr.h) + (tr.x )) + p_offset)
+ *((sum->data + sum->w*(tr.y + tr.h) + (tr.x + tr.w)) + p_offset))
* cascade->weights_array[w_index + i]*4096;
}
if (sumw >= t) {
return cascade->alpha2_array[tree_index];
}
return cascade->alpha1_array[tree_index];
}
static int runCascadeClassifier(struct cascade* cascade, struct point pt, int start_stage)
{
int i, j;
int p_offset;
int32_t mean;
int32_t std;
int w_index = 0;
int r_index = 0;
int stage_sum;
int tree_index = 0;
int x,y,offset;
uint32_t sumsq=0;
vec_t v0, v1;
for (y=pt.y; y<cascade->window.w; y++) {
for (x=pt.x; x<cascade->window.w; x+=2) {
offset = y*cascade->img->w+x;
v0.s0 = cascade->img->pixels[offset+0];
v0.s1 = cascade->img->pixels[offset+1];
v1.s0 = cascade->img->pixels[offset+0];
v1.s1 = cascade->img->pixels[offset+1];
sumsq = __SMLAD(v0.i, v1.i, sumsq);
}
}
/* Image normalization */
int win_w = cascade->window.w - 1;
int win_h = cascade->window.h - 1;
p_offset = pt.y * (cascade->sum.w) + pt.x;
mean = cascade->sum.data[p_offset]
- cascade->sum.data[win_w + p_offset]
- cascade->sum.data[cascade->sum.w * win_h + p_offset]
+ cascade->sum.data[cascade->sum.w * win_h + win_w + p_offset];
std = sqrtf(sumsq * cascade->window.w * cascade->window.h - mean * mean);
for (i=start_stage; i<cascade->n_stages; i++) {
stage_sum = 0;
for (j=0; j<cascade->stages_array[i]; j++, tree_index++) {
/* send the shifted window to a haar filter */
stage_sum += evalWeakClassifier(cascade, std, p_offset, tree_index, w_index, r_index);
w_index+=cascade->num_rectangles_array[tree_index];
r_index+=4 * cascade->num_rectangles_array[tree_index];
}
/* If the sum is below the stage threshold, no faces are detected */
if (stage_sum < 0.4*cascade->stages_thresh_array[i]) {
return -i;
}
}
return 1;
}
static void ScaleImageInvoker(struct cascade *cascade, float factor, int sum_row, int sum_col, struct array *vec)
{
int result;
int x, y, x2, y2;
struct point p;
struct size win_size;
win_size.w = roundf(cascade->window.w*factor);
win_size.h = roundf(cascade->window.h*factor);
/* When filter window shifts to image boarder, some margin need to be kept */
y2 = sum_row - win_size.h;
x2 = sum_col - win_size.w;
/* Shift the filter window over the image. */
for (x=0; x<=x2; x+=cascade->step) {
for (y=0; y<=y2; y+=cascade->step) {
p.x = x;
p.y = y;
result = runCascadeClassifier(cascade, p, 0);
/* If a face is detected, record the coordinates of the filter window */
if (result > 0) {
struct rectangle *r = xalloc(sizeof(struct rectangle));
r->x = roundf(x*factor);
r->y = roundf(y*factor);
r->w = win_size.w;
r->h = win_size.h;
array_push_back(vec, r);
}
}
}
}
struct array *imlib_detect_objects(struct image *image, struct cascade *cascade)
{
/* scaling factor */
float factor;
struct array *objects;
struct image img;
struct integral_image sum;
/* allocate buffer for scaled image */
img.w = image->w;
img.h = image->h;
img.bpp = image->bpp;
/* use the second half of the framebuffer */
img.pixels = image->pixels+(image->w * image->h);
/* allocate buffer for integral image */
sum.w = image->w;
sum.h = image->h;
//sum.data = xalloc(image->w *image->h*sizeof(*sum.data));
sum.data = (uint32_t*) (image->pixels+(image->w * image->h * 2));
/* allocate the detections array */
array_alloc(&objects, xfree);
/* set cascade image pointer */
cascade->img = &img;
/* iterate over the image pyramid */
for(factor=1.0f; ; factor*=cascade->scale_factor) {
/* size of the scaled image */
struct size sz = {
(image->w/factor),
(image->h/factor)
};
/* if scaled image is smaller than the original detection window, break */
if ((sz.w - cascade->window.w) <= 0 ||
(sz.h - cascade->window.h) <= 0) {
break;
}
/* Set the width and height of the images */
img.w = sz.w;
img.h = sz.h;
sum.w = sz.w;
sum.h = sz.h;
/* downsample using nearest neighbor */
imlib_scale_image(image, &img);
/* compute a new integral image */
imlib_integral_image(&img, &sum);
/* sets images for haar classifier cascade */
cascade->sum = sum;
/* process the current scale with the cascaded fitler. */
ScaleImageInvoker(cascade, factor, sum.h, sum.w, objects);
}
//xfree(sum.data);
objects = rectangle_merge(objects);
return objects;
}
int imlib_load_cascade(struct cascade *cascade, const char *path)
{
int i;
UINT n_out;
FIL fp;
FRESULT res=FR_OK;
res = f_open(&fp, path, FA_READ|FA_OPEN_EXISTING);
if (res != FR_OK) {
return res;
}
/* read detection window size */
res = f_read(&fp, &cascade->window, sizeof(cascade->window), &n_out);
if (res != FR_OK || n_out != sizeof(cascade->window)) {
goto error;
}
/* read num stages */
res = f_read(&fp, &cascade->n_stages, sizeof(cascade->n_stages), &n_out);
if (res != FR_OK || n_out != sizeof(cascade->n_stages)) {
goto error;
}
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 */
res = f_read(&fp, cascade->stages_array, sizeof(uint8_t) * cascade->n_stages, &n_out);
if (res != FR_OK || n_out != sizeof(uint8_t) * cascade->n_stages) {
goto error;
}
/* 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 */
res = f_read(&fp, cascade->stages_thresh_array, sizeof(int16_t)*cascade->n_stages, &n_out);
if (res != FR_OK || n_out != sizeof(int16_t)*cascade->n_stages) {
goto error;
}
/* read features thresholds */
res = f_read(&fp, cascade->tree_thresh_array, sizeof(*cascade->tree_thresh_array)*cascade->n_features, &n_out);
if (res != FR_OK || n_out != sizeof(*cascade->tree_thresh_array)*cascade->n_features) {
goto error;
}
/* read alpha 1 */
res = f_read(&fp, cascade->alpha1_array, sizeof(*cascade->alpha1_array)*cascade->n_features, &n_out);
if (res != FR_OK || n_out != sizeof(*cascade->alpha1_array)*cascade->n_features) {
goto error;
}
/* read alpha 2 */
res = f_read(&fp, cascade->alpha2_array, sizeof(*cascade->alpha2_array)*cascade->n_features, &n_out);
if (res != FR_OK || n_out != sizeof(*cascade->alpha2_array)*cascade->n_features) {
goto error;
}
/* read num rectangles per feature*/
res = f_read(&fp, cascade->num_rectangles_array, sizeof(*cascade->num_rectangles_array)*cascade->n_features, &n_out);
if (res != FR_OK || n_out != sizeof(*cascade->num_rectangles_array)*cascade->n_features) {
goto error;
}
/* 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 */
res =f_read(&fp, cascade->weights_array, sizeof(*cascade->weights_array)*cascade->n_rectangles, &n_out);
if (res != FR_OK || n_out != sizeof(*cascade->weights_array)*cascade->n_rectangles) {
goto error;
}
/* read rectangles num rectangles * 4 points */
res = f_read(&fp, cascade->rectangles_array, sizeof(*cascade->rectangles_array)*cascade->n_rectangles *4, &n_out);
if (res != FR_OK || n_out != sizeof(*cascade->rectangles_array)*cascade->n_rectangles *4) {
goto error;
}
error:
f_close(&fp);
return res;
}

View File

@ -18,15 +18,155 @@
__typeof__ (b) _b = (b); \
_a > _b ? _a : _b; })
#define PIXEL_AT(src, x, y) \
({ __typeof__ (x) _x = (x); \
__typeof__ (y) _y = (y); \
src->data[_y*src->w+_x]; })
#define MAX_GRAY_LEVEL (255)
float imlib_distance(struct color *c0, struct color *c1)
/* RGB565->LAB lookup */
extern const int8_t lab_table[65536];
const uint8_t xyz_table[256]= {
0.083381, 0.098368, 0.114819, 0.132772, 0.152264, 0.173331, 0.196007, 0.220325,
0.246318, 0.274017, 0.303452, 0.334654, 0.367651, 0.402472, 0.439144, 0.477695,
0.518152, 0.560539, 0.604883, 0.651209, 0.699541, 0.749903, 0.802319, 0.856813,
0.913406, 0.972122, 1.032982, 1.096009, 1.161225, 1.228649, 1.298303, 1.370208,
1.444384, 1.520851, 1.599629, 1.680738, 1.764195, 1.850022, 1.938236, 2.028856,
2.121901, 2.217388, 2.315337, 2.415763, 2.518686, 2.624122, 2.732089, 2.842604,
2.955683, 3.071344, 3.189603, 3.310477, 3.433981, 3.560131, 3.688945, 3.820437,
3.954624, 4.091520, 4.231141, 4.373503, 4.518620, 4.666509, 4.817182, 4.970657,
5.126946, 5.286065, 5.448028, 5.612849, 5.780543, 5.951124, 6.124605, 6.301002,
6.480327, 6.662594, 6.847817, 7.036010, 7.227185, 7.421357, 7.618538, 7.818742,
8.021982, 8.228271, 8.437621, 8.650046, 8.865559, 9.084171, 9.305896, 9.530747,
9.758735, 9.989873, 10.224173, 10.461648, 10.702310, 10.946171, 11.193243, 11.443537,
11.697067, 11.953843, 12.213877, 12.477182, 12.743768, 13.013648, 13.286832, 13.563333,
13.843162, 14.126329, 14.412847, 14.702727, 14.995979, 15.292615, 15.592646, 15.896084,
16.202938, 16.513219, 16.826940, 17.144110, 17.464740, 17.788842, 18.116424, 18.447499,
18.782077, 19.120168, 19.461783, 19.806932, 20.155625, 20.507874, 20.863687, 21.223076,
21.586050, 21.952620, 22.322796, 22.696587, 23.074005, 23.455058, 23.839757, 24.228112,
24.620133, 25.015828, 25.415209, 25.818285, 26.225066, 26.635560, 27.049779, 27.467731,
27.889426, 28.314874, 28.744084, 29.177065, 29.613827, 30.054379, 30.498731, 30.946892,
31.398871, 31.854678, 32.314321, 32.777810, 33.245154, 33.716362, 34.191442, 34.670406,
35.153260, 35.640014, 36.130678, 36.625260, 37.123768, 37.626212, 38.132601, 38.642943,
39.157248, 39.675523, 40.197778, 40.724021, 41.254261, 41.788507, 42.326767, 42.869050,
43.415364, 43.965717, 44.520119, 45.078578, 45.641102, 46.207700, 46.778380, 47.353150,
47.932018, 48.514994, 49.102085, 49.693300, 50.288646, 50.888132, 51.491767, 52.099557,
52.711513, 53.327640, 53.947949, 54.572446, 55.201140, 55.834039, 56.471151, 57.112483,
57.758044, 58.407842, 59.061884, 59.720179, 60.382734, 61.049557, 61.720656, 62.396039,
63.075714, 63.759687, 64.447968, 65.140564, 65.837482, 66.538730, 67.244316, 67.954247,
68.668531, 69.387176, 70.110189, 70.837578, 71.569350, 72.305513, 73.046074, 73.791041,
74.540421, 75.294222, 76.052450, 76.815115, 77.582222, 78.353779, 79.129794, 79.910274,
80.695226, 81.484657, 82.278575, 83.076988, 83.879901, 84.687323, 85.499261, 86.315721,
87.136712, 87.962240, 88.792312, 89.626935, 90.466117, 91.309865, 92.158186, 93.011086,
93.868573, 94.730654, 95.597335, 96.468625, 97.344529, 98.225055, 99.110210, 100.000000,
};
uint16_t f_sqrt_q16(uint16_t a)
{
float sum=0.0f;
uint16_t op = a;
uint16_t res = 0;
uint16_t one = 1uL << 14;
while (one > op) {
one >>= 2;
}
while (one != 0) {
if (op >= res + one) {
op = op - (res + one);
res = res + 2 * one;
}
res >>= 1;
one >>= 2;
}
return res;
}
uint32_t f_sqrt_q32(uint32_t a)
{
uint32_t op = a;
uint32_t res = 0;
uint32_t one = 1uL << 30;
while (one > op) {
one >>= 2;
}
while (one != 0) {
if (op >= res + one) {
op = op - (res + one);
res = res + 2 * one;
}
res >>= 1;
one >>= 2;
}
return res;
}
uint16_t imlib_lab_distance(struct color *c0, struct color *c1)
{
uint16_t sum=0;
sum += (c0->L - c1->L) * (c0->L - c1->L);
sum += (c0->A - c1->A) * (c0->A - c1->A);
sum += (c0->B - c1->B) * (c0->B - c1->B);
return f_sqrt_q16(sum);
}
uint16_t imlib_rgb_distance(struct color *c0, struct color *c1)
{
uint16_t sum=0;
sum += (c0->r - c1->r) * (c0->r - c1->r);
sum += (c0->g - c1->g) * (c0->g - c1->g);
sum += (c0->b - c1->b) * (c0->b - c1->b);
return sqrtf(sum);
return f_sqrt_q16(sum);
}
uint16_t imlib_hsv_distance(struct color *c0, struct color *c1)
{
uint16_t sum=0;
sum += (c0->h - c1->h) * (c0->h - c1->h);
sum += (c0->s - c1->s) * (c0->s - c1->s);
sum += (c0->v - c1->v) * (c0->v - c1->v);
return f_sqrt_q16(sum);
}
void imlib_rgb_to_lab(struct color *rgb, struct color *lab)
{
float t;
float v[3];
float xyz[3];
const float c1 = 16.0f/ 116.0f;
for (int i=0; i<3; i++) {
t = rgb->vec[i]/255.0f;
if (t > 0.04045f) {
t = xyz_table[rgb->vec[i]];
} else {
t/= 1292.0f;
}
v[i]=t;
}
xyz[0] = (v[0] * 0.4124f + v[1] * 0.3576f + v[2] * 0.1805f) / 95.047f ;
xyz[1] = (v[0] * 0.2126f + v[1] * 0.7152f + v[2] * 0.0722f) / 100.0f ;
xyz[2] = (v[0] * 0.0193f + v[1] * 0.1192f + v[2] * 0.9505f) / 108.883f ;
for (int i=0; i<3; i++) {
t = xyz[i];
if (t > 0.008856f) {
t = cbrtf(t);
} else {
t = (7.787f * t) + c1;
}
xyz[i]=t;
}
lab->L = (int8_t) (116.0f * xyz[1]-16.0f);
lab->A = (int8_t) (500.0f * (xyz[0]-xyz[1]));
lab->B = (int8_t) (200.0f * (xyz[1]-xyz[2]));
}
void imlib_rgb_to_hsv(struct color *rgb, struct color *hsv)
@ -83,70 +223,13 @@ void imlib_grayscale_to_rgb565(struct image *image)
#endif
}
void imlib_detect_color(struct image *image, struct color *color, struct rectangle *rectangle, int threshold)
{
int x,y;
uint8_t p0,p1;
struct color rgb;
struct color hsv;
int pixels = 1;
rectangle->w = 0;
rectangle->h = 0;
rectangle->x = image->w;
rectangle->y = image->h;
//to avoid sqrt we use squared values
threshold *= threshold;
for (y=0; y<image->h; y++) {
for (x=0; x<image->w; x++) {
int i=y*image->w*image->bpp+x*image->bpp;
p0 = image->pixels[i];
p1 = image->pixels[i+1];
/* map RGB565 to RGB888 */
rgb.r = (uint8_t) (p0>>3) * 255/31;
rgb.g = (uint8_t) (((p0&0x07)<<3) | (p1>>5)) * 255/63;
rgb.b = (uint8_t) (p1&0x1F) * 255/31;
/* convert RGB to HSV */
imlib_rgb_to_hsv(&rgb, &hsv);
/* difference between target Hue and pixel Hue squared */
hsv.h = (hsv.h - color->h) * (hsv.h - color->h);
/* add pixel if within threshold */
if (hsv.h < threshold && hsv.s > color->s && hsv.v > color->v) { //s==pale
pixels++;
if (x < rectangle->x) {
rectangle->x = x;
}
if (y < rectangle->y) {
rectangle->y = y;
}
if (x > rectangle->w) {
rectangle->w = x;
}
if (y > rectangle->h) {
rectangle->h = y;
}
}
}
}
rectangle->w = rectangle->w-rectangle->x;
rectangle->h = rectangle->h-rectangle->y;
}
void imlib_erosion_filter(struct image *src, uint8_t *kernel, int k_size)
{
int x, y, j, k;
int w = src->w;
int h = src->h;
/* TODO */
uint8_t *dst = xcalloc(w*h, 1);
uint8_t *dst = xalloc0(w*h);
for (y=0; y<h-k_size; y++) {
for (x=0; x<w-k_size; x++) {
@ -167,6 +250,30 @@ void imlib_erosion_filter(struct image *src, uint8_t *kernel, int k_size)
xfree(dst);
}
void imlib_threshold(image_t *image, struct color *color, int threshold)
{
color_t lab1,lab2;
uint16_t *pixels = (uint16_t*) image->pixels;
/* Convert reference RGB to LAB */
imlib_rgb_to_lab(color, &lab1);
for (int y=0; y<image->h; y++) {
for (int x=0; x<image->w; x++) {
int i=y*image->w+x;
lab2.L = lab_table[pixels[i]*3];
lab2.A = lab_table[pixels[i]*3+1];
lab2.B = lab_table[pixels[i]*3+2];
/* add pixel if within threshold */
if (imlib_lab_distance(&lab1, &lab2)<threshold) {
pixels[i] = 0xFFFF;
} else {
pixels[i] = 0x0000;
}
}
}
}
int imlib_image_mean(struct image *src)
{
int s=0;
@ -243,10 +350,10 @@ void imlib_draw_rectangle(struct image *image, struct rectangle *r)
{
int i;
uint8_t c=0xFF;
int x = MIN(MAX(r->x, 0), image->w);
int y = MIN(MAX(r->y, 0), image->h);
int w = (x+r->w) > image->w ? (image->w-x):r->w;
int h = (y+r->h) > image->h ? (image->h-y):r->h;
int x = MIN(MAX(r->x, 1), image->w)-1;
int y = MIN(MAX(r->y, 1), image->h)-1;
int w = (x+r->w) >= image->w ? (image->w-x):r->w;
int h = (y+r->h) >= image->h ? (image->h-y):r->h;
x *= image->bpp;
w *= image->bpp;
@ -288,426 +395,3 @@ void imlib_histeq(struct image *src)
src->pixels[i] = (uint8_t) ((MAX_GRAY_LEVEL/(float)a) * hist[src->pixels[i]]);
}
}
int imlib_image_sumsq(struct image *image, int u, int v, int w, int h)
{
vec_t v0;
vec_t v1;
int x,y;
int sumsq=0;
int offset=0;
for (y=v; y<h+v; y++) {
for (x=u; x<w+u; x+=2) {
offset = y*image->w+x;
v0.s0 = image->data[offset+0];
v0.s1 = image->data[offset+1];
v1.s0 = image->data[offset+0];
v1.s1 = image->data[offset+1];
sumsq = __SMLAD(v0.i, v1.i, sumsq);
}
}
return sumsq;
}
/* Viola-Jones face detector implementation
* Original Author: Francesco Comaschi (f.comaschi@tue.nl)
*/
static int evalWeakClassifier(struct cascade *cascade, int std, int p_offset, int tree_index, int w_index, int r_index )
{
int i, sumw=0;
struct rectangle tr;
struct integral_image *sum = &cascade->sum;
/* the node threshold is multiplied by the standard deviation of the image */
int t = cascade->tree_thresh_array[tree_index] * std;
for (i=0; i<cascade->num_rectangles_array[tree_index]; i++) {
tr.x = cascade->rectangles_array[r_index + i*4 + 0];
tr.y = cascade->rectangles_array[r_index + i*4 + 1];
tr.w = cascade->rectangles_array[r_index + i*4 + 2];
tr.h = cascade->rectangles_array[r_index + i*4 + 3];
sumw += (
*((sum->data + sum->w*(tr.y ) + (tr.x )) + p_offset)
- *((sum->data + sum->w*(tr.y ) + (tr.x + tr.w)) + p_offset)
- *((sum->data + sum->w*(tr.y + tr.h) + (tr.x )) + p_offset)
+ *((sum->data + sum->w*(tr.y + tr.h) + (tr.x + tr.w)) + p_offset))
* cascade->weights_array[w_index + i]*4096;
}
if (sumw >= t) {
return cascade->alpha2_array[tree_index];
}
return cascade->alpha1_array[tree_index];
}
static int runCascadeClassifier(struct cascade* cascade, struct point pt, int start_stage)
{
int i, j;
int p_offset;
int32_t mean;
int32_t std;
int w_index = 0;
int r_index = 0;
int stage_sum;
int tree_index = 0;
int x,y,offset;
uint32_t sumsq=0;
vec_t v0, v1;
for (y=pt.y; y<cascade->window.w; y++) {
for (x=pt.x; x<cascade->window.w; x+=2) {
offset = y*cascade->img->w+x;
v0.s0 = cascade->img->pixels[offset+0];
v0.s1 = cascade->img->pixels[offset+1];
v1.s0 = cascade->img->pixels[offset+0];
v1.s1 = cascade->img->pixels[offset+1];
sumsq = __SMLAD(v0.i, v1.i, sumsq);
}
}
/* Image normalization */
int win_w = cascade->window.w - 1;
int win_h = cascade->window.h - 1;
p_offset = pt.y * (cascade->sum.w) + pt.x;
mean = cascade->sum.data[p_offset]
- cascade->sum.data[win_w + p_offset]
- cascade->sum.data[cascade->sum.w * win_h + p_offset]
+ cascade->sum.data[cascade->sum.w * win_h + win_w + p_offset];
std = sqrtf(sumsq * cascade->window.w * cascade->window.h - mean * mean);
for (i=start_stage; i<cascade->n_stages; i++) {
stage_sum = 0;
for (j=0; j<cascade->stages_array[i]; j++, tree_index++) {
/* send the shifted window to a haar filter */
stage_sum += evalWeakClassifier(cascade, std, p_offset, tree_index, w_index, r_index);
w_index+=cascade->num_rectangles_array[tree_index];
r_index+=4 * cascade->num_rectangles_array[tree_index];
}
/* If the sum is below the stage threshold, no faces are detected */
if (stage_sum < 0.4*cascade->stages_thresh_array[i]) {
return -i;
}
}
return 1;
}
static void ScaleImageInvoker(struct cascade *cascade, float factor, int sum_row, int sum_col, struct array *vec)
{
int result;
int x, y, x2, y2;
struct point p;
struct size win_size;
win_size.w = roundf(cascade->window.w*factor);
win_size.h = roundf(cascade->window.h*factor);
/* When filter window shifts to image boarder, some margin need to be kept */
y2 = sum_row - win_size.h;
x2 = sum_col - win_size.w;
/* Shift the filter window over the image. */
for (x=0; x<=x2; x+=cascade->step) {
for (y=0; y<=y2; y+=cascade->step) {
p.x = x;
p.y = y;
result = runCascadeClassifier(cascade, p, 0);
/* If a face is detected, record the coordinates of the filter window */
if (result > 0) {
struct rectangle *r = xalloc(sizeof(struct rectangle));
r->x = roundf(x*factor);
r->y = roundf(y*factor);
r->w = win_size.w;
r->h = win_size.h;
array_push_back(vec, r);
}
}
}
}
struct rectangle *rectangle_clone(struct rectangle *rect)
{
struct rectangle *rectangle;
rectangle = xalloc(sizeof(struct rectangle));
memcpy(rectangle, rect, sizeof(struct rectangle));
return rectangle;
}
void rectangle_add(struct rectangle *rect0, struct rectangle *rect1)
{
rect0->x += rect1->x;
rect0->y += rect1->y;
rect0->w += rect1->w;
rect0->h += rect1->h;
}
void rectangle_div(struct rectangle *rect0, int c)
{
rect0->x /= c;
rect0->y /= c;
rect0->w /= c;
rect0->h /= c;
}
void rectangle_merge(struct rectangle *rect0, struct rectangle *rect1)
{
rect0->x = (rect0->x < rect1->x)? rect0->x:rect1->x;
rect0->y = (rect0->y < rect1->y)? rect0->y:rect1->y;
rect0->w = (rect0->w > rect1->w)? rect0->w:rect1->w;
rect0->h = (rect0->h > rect1->h)? rect0->h:rect1->h;
}
int rectangle_intersects(struct rectangle *rect0, struct rectangle *rect1)
{
return ((rect0->x < (rect1->x+rect1->w)) &&
(rect0->y < (rect1->y+rect1->h)) &&
((rect0->x+rect0->w) > rect1->x) &&
((rect0->y+rect0->h) > rect1->y));
}
struct array *imlib_merge_detections(struct array *rectangles)
{
int j;
struct array *objects;
struct array *overlap;
struct rectangle *rect1, *rect2;
array_alloc(&objects, xfree);
array_alloc(&overlap, xfree);
/* merge overlaping detections */
while (array_length(rectangles)) {
/* check for overlaping detections */
rect1 = (struct rectangle *) array_at(rectangles, 0);
for (j=1; j<array_length(rectangles); j++) {
rect2 = (struct rectangle *) array_at(rectangles, j);
if (rectangle_intersects(rect1, rect2)) {
array_push_back(overlap, rectangle_clone(rect2));
array_erase(rectangles, j--);
}
}
/* add the overlaping detections */
int count = array_length(overlap)+1;
while (array_length(overlap)) {
rect2 = (struct rectangle *) array_at(overlap, 0);
rectangle_add(rect1, rect2);
array_erase(overlap, 0);
}
/* average the overlaping detections */
rectangle_div(rect1, count);
array_push_back(objects, rectangle_clone(rect1));
array_erase(rectangles, 0);
}
array_free(overlap);
array_free(rectangles);
return objects;
}
struct array *imlib_detect_objects(struct image *image, struct cascade *cascade)
{
/* scaling factor */
float factor;
struct array *objects;
struct image img;
struct integral_image sum;
/* allocate buffer for scaled image */
img.w = image->w;
img.h = image->h;
img.bpp = image->bpp;
/* use the second half of the framebuffer */
img.pixels = image->pixels+(image->w * image->h);
/* allocate buffer for integral image */
sum.w = image->w;
sum.h = image->h;
//sum.data = xalloc(image->w *image->h*sizeof(*sum.data));
sum.data = (uint32_t*) (image->pixels+(image->w * image->h * 2));
/* allocate the detections array */
array_alloc(&objects, xfree);
/* set cascade image pointer */
cascade->img = &img;
/* iterate over the image pyramid */
for(factor=1.0f; ; factor*=cascade->scale_factor) {
/* size of the scaled image */
struct size sz = {
(image->w/factor),
(image->h/factor)
};
/* if scaled image is smaller than the original detection window, break */
if ((sz.w - cascade->window.w) <= 0 ||
(sz.h - cascade->window.h) <= 0) {
break;
}
/* Set the width and height of the images */
img.w = sz.w;
img.h = sz.h;
sum.w = sz.w;
sum.h = sz.h;
/* downsample using nearest neighbor */
imlib_scale_image(image, &img);
/* compute a new integral image */
imlib_integral_image(&img, &sum);
/* sets images for haar classifier cascade */
cascade->sum = sum;
/* process the current scale with the cascaded fitler. */
ScaleImageInvoker(cascade, factor, sum.h, sum.w, objects);
}
//xfree(sum.data);
objects = imlib_merge_detections(objects);
return objects;
}
int imlib_load_cascade(struct cascade *cascade, const char *path)
{
int i;
UINT n_out;
FIL fp;
FRESULT res=FR_OK;
res = f_open(&fp, path, FA_READ|FA_OPEN_EXISTING);
if (res != FR_OK) {
return res;
}
/* read detection window size */
res = f_read(&fp, &cascade->window, sizeof(cascade->window), &n_out);
if (res != FR_OK || n_out != sizeof(cascade->window)) {
goto error;
}
/* read num stages */
res = f_read(&fp, &cascade->n_stages, sizeof(cascade->n_stages), &n_out);
if (res != FR_OK || n_out != sizeof(cascade->n_stages)) {
goto error;
}
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 */
res = f_read(&fp, cascade->stages_array, sizeof(uint8_t) * cascade->n_stages, &n_out);
if (res != FR_OK || n_out != sizeof(uint8_t) * cascade->n_stages) {
goto error;
}
/* 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 */
res = f_read(&fp, cascade->stages_thresh_array, sizeof(int16_t)*cascade->n_stages, &n_out);
if (res != FR_OK || n_out != sizeof(int16_t)*cascade->n_stages) {
goto error;
}
/* read features thresholds */
res = f_read(&fp, cascade->tree_thresh_array, sizeof(*cascade->tree_thresh_array)*cascade->n_features, &n_out);
if (res != FR_OK || n_out != sizeof(*cascade->tree_thresh_array)*cascade->n_features) {
goto error;
}
/* read alpha 1 */
res = f_read(&fp, cascade->alpha1_array, sizeof(*cascade->alpha1_array)*cascade->n_features, &n_out);
if (res != FR_OK || n_out != sizeof(*cascade->alpha1_array)*cascade->n_features) {
goto error;
}
/* read alpha 2 */
res = f_read(&fp, cascade->alpha2_array, sizeof(*cascade->alpha2_array)*cascade->n_features, &n_out);
if (res != FR_OK || n_out != sizeof(*cascade->alpha2_array)*cascade->n_features) {
goto error;
}
/* read num rectangles per feature*/
res = f_read(&fp, cascade->num_rectangles_array, sizeof(*cascade->num_rectangles_array)*cascade->n_features, &n_out);
if (res != FR_OK || n_out != sizeof(*cascade->num_rectangles_array)*cascade->n_features) {
goto error;
}
/* 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 */
res =f_read(&fp, cascade->weights_array, sizeof(*cascade->weights_array)*cascade->n_rectangles, &n_out);
if (res != FR_OK || n_out != sizeof(*cascade->weights_array)*cascade->n_rectangles) {
goto error;
}
/* read rectangles num rectangles * 4 points */
res = f_read(&fp, cascade->rectangles_array, sizeof(*cascade->rectangles_array)*cascade->n_rectangles *4, &n_out);
if (res != FR_OK || n_out != sizeof(*cascade->rectangles_array)*cascade->n_rectangles *4) {
goto error;
}
error:
f_close(&fp);
return res;
}

206
src/img/imlib.h Normal file
View File

@ -0,0 +1,206 @@
#ifndef __IMLIB_H__
#define __IMLIB_H__
#include <stdint.h>
#include <stdbool.h>
#include "array.h"
typedef struct point {
uint16_t x;
uint16_t y;
} point_t;
typedef struct size {
int w;
int h;
} wsize_t;
typedef struct rectangle {
int x;
int y;
int w;
int h;
} rectangle_t;
typedef struct color {
union {
uint8_t vec[3];
struct {
uint8_t r;
uint8_t g;
uint8_t b;
};
struct {
int h;
int s;
int v;
};
struct {
int8_t L;
int8_t A;
int8_t B;
};
struct {
float x;
float y;
float z;
};
};
} color_t;
typedef struct image {
int w;
int h;
int bpp;
union {
uint8_t *pixels;
uint8_t *data;
};
} image_t;
typedef struct integral_image {
int w;
int h;
uint32_t *data;
} i_image_t;
typedef struct {
union {
struct {
uint8_t c0;
uint8_t c1;
uint8_t c2;
uint8_t c3;
};
struct {
uint16_t s0;
uint16_t s1;
};
struct {
uint32_t i;
};
};
}vec_t;
typedef struct cluster {
array_t *points;
point_t centroid;
} cluster_t;
#define SURF_DESC_SIZE (32)
typedef struct ipoint {
/* Coordinates of the detected interest point */
float x, y;
/* Detected scale */
float scale;
/* Orientation measured anti-clockwise from +ve x-axis */
float orientation;
/* Sign of laplacian for fast matching purposes */
int laplacian;
/* Vector of descriptor components */
float descriptor[SURF_DESC_SIZE];
/* Placeholds for point motion */
float dx, dy;
/* Used to store cluster index */
int clusterIndex;
} i_point_t;
typedef struct response_layer {
int width;
int height;
int step;
int filter;
} response_layer_t;
typedef struct surf {
image_t *img; /* Image to find Ipoints in */
i_image_t *i_img; /* Integral image */
array_t *ipts; /* Reference to vector of Ipoints */
array_t *rmap; /* Response map */
bool upright; /* Run in rotation invariant mode? */
int octaves; /* Number of octaves to calculate */
int intervals; /* Number of intervals per octave */
int init_sample; /* Initial sampling step */
float thresh; /* Blob response threshold */
} surf_t;
/* Haar cascade struct */
typedef struct cascade {
/* Step size of filter window shifting */
int step;
/* scaling step size */
float scale_factor;
/* number of stages in the cascade */
int n_stages;
/* number of features in the cascade */
int n_features;
/* number of rectangles in the cascade */
int n_rectangles;
/* size of the window used in the training set */
struct size window;
/* pointer to current scaled image in the pyramid */
struct image *img;
/* pointer to current integral image */
struct integral_image sum;
/* haar cascade arrays */
uint8_t *stages_array;
int16_t *stages_thresh_array;
int16_t *tree_thresh_array;
int16_t *alpha1_array;
int16_t *alpha2_array;
int8_t *num_rectangles_array;
int8_t *weights_array;
int8_t *rectangles_array;
} cascade_t;
/* Point functions */
point_t *point_alloc(int x, int y);
int point_equal(point_t *p1, point_t *p2);
float point_distance(point_t *p1, point_t *p2);
/* Rectangle functions */
rectangle_t *rectangle_alloc();
rectangle_t *rectangle_clone(rectangle_t *r);
void rectangle_add(rectangle_t *r0, rectangle_t *r1);
void rectangle_div(rectangle_t *r0, int c);
int rectangle_intersects(rectangle_t *r0, rectangle_t *r1);
struct array *rectangle_merge(struct array *r);
/* Clustering functions */
array_t *cluster_kmeans(array_t *points, int k);
/* Dela E on RGB/HSV/LAB */
uint16_t imlib_lab_distance(struct color *c0, struct color *c1);
uint16_t imlib_rgb_distance(struct color *c0, struct color *c1);
uint16_t imlib_rgb_distance(struct color *c0, struct color *c1);
/* Color space conversion */
void imlib_rgb_to_lab(struct color *rgb, struct color *lab);
void imlib_rgb_to_hsv(struct color *rgb, struct color *hsv);
/* Image filtering functions */
void imlib_histeq(struct image *src);
void imlib_median_filter(image_t *src, int r);
void imlib_erosion_filter(struct image *src, uint8_t *kernel, int k_size);
void imlib_threshold(struct image *image, struct color *color, int threshold);
array_t *imlib_count_blobs(struct image *image);
/* Integral image functions */
void imlib_integral_image(struct image *src, struct integral_image *sum);
void imlib_integral_image_sq(struct image *src, struct integral_image *sum);
uint32_t imlib_integral_lookup(struct integral_image *src, int x, int y, int w, int h);
/* Template matching */
int imlib_save_template(struct image *image, const char *path);
int imlib_load_template(struct image *image, const char *path);
float imlib_template_match(struct image *image, struct image *template, struct rectangle *r);
/* Haar/VJ */
int imlib_load_cascade(struct cascade* cascade, const char *path);
struct array *imlib_detect_objects(struct image *image, struct cascade* cascade);
void imlib_scale_image(struct image *src, struct image *dst);
void imlib_draw_rectangle(struct image *image, struct rectangle *r);
int imlib_image_mean(struct image *src);
void imlib_subimage(struct image *src_img, struct image *dst_img, int x_off, int y_off);
void imlib_blit(struct image *dst_img, struct image *src_img, int x_off, int y_off);
#endif //__IMLIB_H__

125
src/img/kmeans.c Normal file
View File

@ -0,0 +1,125 @@
#include <float.h>
#include <limits.h>
#include <math.h>
#include <arm_math.h>
#include "imlib.h"
#include "array.h"
#include "xalloc.h"
#include "rng.h"
static cluster_t *cluster_alloc(int cx, int cy)
{
cluster_t *c=NULL;
c = xalloc(sizeof(*c));
if (c != NULL) {
/* initial centroid */
c->centroid.x = cx;
c->centroid.y = cy;
array_alloc(&c->points, NULL);
}
return c;
}
static void cluster_free(void *c)
{
cluster_t *cl = c;
array_free(cl->points);
xfree(cl);
}
static void cluster_reset(array_t *clusters)
{
int k = array_length(clusters);
/* reset clusters */
for (int j=0; j<k; j++) {
cluster_t *cl = array_at(clusters, j);
// array_resize(cl->points, 0);
array_free(cl->points);
array_alloc(&cl->points, NULL);
}
}
static int cluster_update(array_t *clusters)
{
int k = array_length(clusters);
/* update clusters */
for (int j=0; j<k; j++) {
point_t sum={0,0};
cluster_t *cl = array_at(clusters, j);
point_t old_c = cl->centroid;
int cl_size = array_length(cl->points);
/* sum all points in this cluster */
for (int i=0; i<cl_size; i++) {
point_t *p = array_at(cl->points, i);
sum.x += p->x;
sum.y += p->y;
}
cl->centroid.x = sum.x/cl_size;
cl->centroid.y = sum.y/cl_size;
if (point_equal(&cl->centroid, &old_c)) {
/* cluster centroid didn't move */
return 0;
}
}
return 1;
}
static void cluster_points(array_t *clusters, array_t *points)
{
int n = array_length(points);
int k = array_length(clusters);
for (int i=0; i<n; i++) {
float distance = FLT_MAX;
cluster_t *cl_nearest = NULL;
point_t *p = array_at(points, i);
for (int j=0; j<k; j++) {
cluster_t *cl = array_at(clusters, j);
float d = point_distance(p, &cl->centroid);
if (d < distance) {
distance = d;
cl_nearest = cl;
}
}
if (cl_nearest == NULL) {
__asm__ volatile ("BKPT");
}
/* copy point and add to cluster */
array_push_back(cl_nearest->points, p);
}
}
array_t *cluster_kmeans(array_t *points, int k)
{
array_t *clusters=NULL;
/* alloc clusters array */
array_alloc(&clusters, cluster_free);
/* select K clusters randomly */
for (int i=0; i<k; i++) {
int pidx = rng_randint(0, array_length(points)-1);
point_t *p = array_at(points, pidx);
array_push_back(clusters, cluster_alloc(p->x, p->y));
}
int cl_changed = 1;
do {
/* reset clusters */
cluster_reset(clusters);
/* add points to clusters */
cluster_points(clusters, points);
/* update centroids */
cl_changed = cluster_update(clusters);
} while (cl_changed);
return clusters;
}

7284
src/img/lab.c Normal file

File diff suppressed because it is too large Load Diff

100
src/img/median.c Normal file
View File

@ -0,0 +1,100 @@
#include <libmp.h>
#include "xalloc.h"
#include "imlib.h"
#include <math.h>
#include <arm_math.h>
#define R8(p) \
(uint8_t)((p>>11) * 255/31)
#define G8(p) \
(uint8_t)(((p>>5)&0x3F)* 255/63)
#define B8(p) \
(uint8_t)((p&0x1F) * 255/31)
#define R(p) \
(uint8_t)((p>>11)&0x1F)
#define G(p) \
(uint8_t)((p>>5) &0x3F)
#define B(p) \
(uint8_t)(p&0x1F)
#define SWAP(x)\
({ uint16_t _x = (x); \
(((_x & 0xff)<<8 |(_x & 0xff00) >> 8));})
typedef struct {
int n;
int r[32];
int g[64];
int b[32];
} color_histo_t;
void del_pixels(image_t * im, int row, int col, int size, color_histo_t *h)
{
int i;
uint16_t c;
if (col < 0 || col >= im->w) return;
for (i = row - size; i <= row + size && i < im->h; i++) {
if (i < 0) continue;
c = SWAP(((uint16_t*)im->pixels)[i*im->w+col]);
h->r[R(c)]--;
h->g[G(c)]--;
h->b[B(c)]--;
h->n--;
}
}
void add_pixels(image_t * im, int row, int col, int size, color_histo_t *h)
{
int i;
uint16_t c;
if (col < 0 || col >= im->w) return;
for (i = row - size; i <= row + size && i < im->h; i++) {
if (i < 0) continue;
c = SWAP(((uint16_t*)im->pixels)[i*im->w+col]);
h->r[R(c)]++;
h->g[G(c)]++;
h->b[B(c)]++;
h->n++;
}
}
void init_histo(image_t *im, int row, int size, color_histo_t *h)
{
memset(h, 0, sizeof(color_histo_t));
for (int j = 0; j < size && j < im->w; j++) {
add_pixels(im, row, j, size, h);
}
}
uint16_t median(const int *x, int n)
{
uint16_t i;
for (n /= 2, i = 0;(n -= x[i]) > 0; i++);
return i;
}
void imlib_median_filter(image_t *in, int size)
{
uint16_t r,g,b;
color_histo_t *h = xalloc(sizeof(*h));
uint16_t *data = (uint16_t*) (in->data+(in->w * in->h*2));
for (int row = 0; row<in->h; row ++) {
for (int col = 0; col<in->w; col++) {
if (!col) {
init_histo(in, row, size, h);
} else {
del_pixels(in, row, col - size, size, h);
add_pixels(in, row, col + size, size, h);
}
r = median(h->r, h->n);
g = median(h->g, h->n);
b = median(h->b, h->n);
data[row*in->w+col] = SWAP(((r << 11) | (g << 5) | b));
}
}
memcpy(in->data, data, (in->w*in->h*2));
}

30
src/img/point.c Normal file
View File

@ -0,0 +1,30 @@
#include <float.h>
#include <limits.h>
#include <math.h>
#include <arm_math.h>
#include "imlib.h"
#include "array.h"
#include "xalloc.h"
point_t *point_alloc(int x, int y)
{
point_t *p = xalloc(sizeof(*p));
if (p != NULL) {
p->x = x;
p->y = y;
}
return p;
}
int point_equal(point_t *p1, point_t *p2)
{
return ((p1->x==p2->x)&&(p1->y==p2->y));
}
float point_distance(point_t *p1, point_t *p2)
{
float sum=0.0f;
sum += (p1->x - p2->x) * (p1->x - p2->x);
sum += (p1->y - p2->y) * (p1->y - p2->y);
return sqrtf(sum);
}

93
src/img/rectangle.c Normal file
View File

@ -0,0 +1,93 @@
#include <float.h>
#include <limits.h>
#include <math.h>
#include <arm_math.h>
#include "imlib.h"
#include "array.h"
#include "xalloc.h"
rectangle_t *rectangle_alloc(int x, int y, int w, int h)
{
rectangle_t *rectangle;
rectangle = xalloc(sizeof(*rectangle));
rectangle->x = x;
rectangle->y = y;
rectangle->w = w;
rectangle->h = h;
return rectangle;
}
rectangle_t *rectangle_clone(rectangle_t *rect)
{
rectangle_t *rectangle;
rectangle = xalloc(sizeof(rectangle_t));
memcpy(rectangle, rect, sizeof(rectangle_t));
return rectangle;
}
void rectangle_add(rectangle_t *rect0, rectangle_t *rect1)
{
rect0->x += rect1->x;
rect0->y += rect1->y;
rect0->w += rect1->w;
rect0->h += rect1->h;
}
void rectangle_div(rectangle_t *rect0, int c)
{
rect0->x /= c;
rect0->y /= c;
rect0->w /= c;
rect0->h /= c;
}
int rectangle_intersects(rectangle_t *rect0, rectangle_t *rect1)
{
return ((rect0->x < (rect1->x+rect1->w)) &&
(rect0->y < (rect1->y+rect1->h)) &&
((rect0->x+rect0->w) > rect1->x) &&
((rect0->y+rect0->h) > rect1->y));
}
array_t *rectangle_merge(array_t *rectangles)
{
int j;
array_t *objects;
array_t *overlap;
rectangle_t *rect1, *rect2;
array_alloc(&objects, xfree);
array_alloc(&overlap, xfree);
/* merge overlaping detections */
while (array_length(rectangles)) {
/* check for overlaping detections */
rect1 = (rectangle_t *) array_at(rectangles, 0);
for (j=1; j<array_length(rectangles); j++) {
rect2 = (rectangle_t *) array_at(rectangles, j);
if (rectangle_intersects(rect1, rect2)) {
array_push_back(overlap, rectangle_clone(rect2));
array_erase(rectangles, j--);
}
}
/* add the overlaping detections */
int count = array_length(overlap)+1;
while (array_length(overlap)) {
rect2 = (rectangle_t *) array_at(overlap, 0);
rectangle_add(rect1, rect2);
array_erase(overlap, 0);
}
/* average the overlaping detections */
rectangle_div(rect1, count);
array_push_back(objects, rectangle_clone(rect1));
array_erase(rectangles, 0);
}
array_free(overlap);
array_free(rectangles);
return objects;
}

View File

@ -63,7 +63,7 @@ int imlib_load_template(struct image *image, const char *path)
goto error;
}
printf("temp:%d %d \n", image->w, image->h);
printf("loading template:%dx%d \n", image->w, image->h);
image->data = xalloc(sizeof(*image->data)*image->w*image->h);
if (image->data == NULL) {
goto error;

View File

@ -1,118 +0,0 @@
#ifndef __IMLIB_H__
#define __IMLIB_H__
#include <stdint.h>
struct point {
int x;
int y;
};
struct size {
int w;
int h;
};
struct rectangle {
int x;
int y;
int w;
int h;
};
struct color {
union {
struct {
uint8_t r;
uint8_t g;
uint8_t b;
};
struct {
int h;
int s;
int v;
};
};
};
struct image {
int w;
int h;
int bpp;
union {
uint8_t *pixels;
uint8_t *data;
};
};
struct integral_image {
int w;
int h;
uint32_t *data;
};
typedef struct {
union {
struct {
uint8_t c0;
uint8_t c1;
uint8_t c2;
uint8_t c3;
};
struct {
uint16_t s0;
uint16_t s1;
};
struct {
uint32_t i;
};
};
}vec_t;
struct cascade {
/* Step size of filter window shifting */
int step;
/* scaling step size */
float scale_factor;
/* number of stages in the cascade */
int n_stages;
/* number of features in the cascade */
int n_features;
/* number of rectangles in the cascade */
int n_rectangles;
/* size of the window used in the training set */
struct size window;
/* pointer to current scaled image in the pyramid */
struct image *img;
/* pointer to current integral image */
struct integral_image sum;
/* haar cascade arrays */
uint8_t *stages_array;
int16_t *stages_thresh_array;
int16_t *tree_thresh_array;
int16_t *alpha1_array;
int16_t *alpha2_array;
int8_t *num_rectangles_array;
int8_t *weights_array;
int8_t *rectangles_array;
};
float imlib_distance(struct color *c0, struct color *c1);
void imlib_rgb_to_hsv(struct color *rgb, struct color *hsv);
void imlib_grayscale_to_rgb565(struct image *image);
void imlib_detect_color(struct image *image, struct color *color, struct rectangle *rectangle, int threshold);
void imlib_erosion_filter(struct image *src, uint8_t *kernel, int k_size);
void imlib_scale_image(struct image *src, struct image *dst);
void imlib_draw_rectangle(struct image *image, struct rectangle *r);
void imlib_histeq(struct image *src);
struct array *imlib_detect_objects(struct image *image, struct cascade* cascade);
int imlib_load_cascade(struct cascade* cascade, const char *path);
float imlib_template_match(struct image *image, struct image *template, struct rectangle *r);
int imlib_save_template(struct image *image, const char *path);
int imlib_load_template(struct image *image, const char *path);
int imlib_image_mean(struct image *src);
void imlib_subimage(struct image *src_img, struct image *dst_img, int x_off, int y_off);
void imlib_blit(struct image *dst_img, struct image *src_img, int x_off, int y_off);
void imlib_integral_image(struct image *src, struct integral_image *sum);
void imlib_integral_image_sq(struct image *src, struct integral_image *sum);
uint32_t imlib_integral_lookup(struct integral_image *src, int x, int y, int w, int h);
#endif //__IMLIB_H__

View File

@ -53,6 +53,19 @@ mp_obj_t py_imlib_histeq(mp_obj_t image_obj)
return mp_const_none;
}
mp_obj_t py_imlib_median(mp_obj_t image_obj, mp_obj_t ksize)
{
struct image *image;
/* get image pointer */
image = (struct image*) py_image_cobj(image_obj);
/* sanity checks */
//PY_ASSERT_TRUE(sensor.pixformat == PIXFORMAT_GRAYSCALE);
imlib_median_filter(image, mp_obj_get_int(ksize));
return mp_const_none;
}
mp_obj_t py_imlib_draw_rectangle(mp_obj_t image_obj, mp_obj_t rectangle_obj)
{
struct rectangle r;
@ -72,11 +85,10 @@ mp_obj_t py_imlib_draw_rectangle(mp_obj_t image_obj, mp_obj_t rectangle_obj)
return mp_const_none;
}
mp_obj_t py_imlib_detect_color(mp_obj_t image_obj, mp_obj_t color_obj, mp_obj_t threshold)
mp_obj_t py_imlib_threshold(mp_obj_t image_obj, mp_obj_t color_obj, mp_obj_t threshold)
{
/* C stuff */
struct color color;
struct rectangle rectangle;
struct image *image;
/* sanity checks */
@ -84,21 +96,53 @@ mp_obj_t py_imlib_detect_color(mp_obj_t image_obj, mp_obj_t color_obj, mp_obj_t
mp_obj_t *col_obj;
col_obj = mp_obj_get_array_fixed_n(color_obj, 3);
color.h = mp_obj_get_int(col_obj[0]);
color.s = mp_obj_get_int(col_obj[1]);
color.v = mp_obj_get_int(col_obj[2]);
color.r = mp_obj_get_int(col_obj[0]);
color.g = mp_obj_get_int(col_obj[1]);
color.b = mp_obj_get_int(col_obj[2]);
/* get image pointer */
image = py_image_cobj(image_obj);
imlib_detect_color(image, &color, &rectangle, mp_obj_get_int(threshold));
/* Threshold image using reference color */
imlib_threshold(image, &color, mp_obj_get_int(threshold));
mp_obj_t rec_obj[4];
rec_obj[0] = mp_obj_new_int(rectangle.x);
rec_obj[1] = mp_obj_new_int(rectangle.y);
rec_obj[2] = mp_obj_new_int(rectangle.w);
rec_obj[3] = mp_obj_new_int(rectangle.h);
return rt_build_tuple(4, rec_obj);
return mp_const_none;
}
mp_obj_t py_imlib_count_blobs(mp_obj_t image_obj)
{
/* C stuff */
array_t *blobs;
struct image *image;
/* MP List */
mp_obj_t objects_list = mp_const_none;
/* sanity checks */
PY_ASSERT_TRUE(sensor.pixformat == PIXFORMAT_RGB565);
/* get image pointer */
image = py_image_cobj(image_obj);
/* run color dector */
blobs = imlib_count_blobs(image);
/* Create empty Python list */
objects_list = rt_build_list(0, NULL);
if (array_length(blobs)) {
for (int j=0; j<array_length(blobs); j++) {
rectangle_t *b = array_at(blobs, j);
mp_obj_t r[4];
r[0] = mp_obj_new_int(b->x);
r[1] = mp_obj_new_int(b->y);
r[2] = mp_obj_new_int(b->w);
r[3] = mp_obj_new_int(b->h);
rt_list_append(objects_list, rt_build_tuple(4, r));
}
}
array_free(blobs);
return objects_list;
}
mp_obj_t py_imlib_detect_objects(mp_obj_t image_obj, mp_obj_t cascade_obj)
@ -202,7 +246,7 @@ mp_obj_t py_imlib_save_template(mp_obj_t image_obj, mp_obj_t rectangle_obj, mp_o
image = py_image_cobj(image_obj);
t.w = r.w;
t.h = r.h;
t.data = xalloc(sizeof(*t.data)*t.w*t.h);
t.data = xalloc(sizeof(*t.data)*t.w*t.h); /* TODO this is not really needed */
imlib_subimage(image, &t, r.x, r.y);
@ -263,6 +307,19 @@ mp_obj_t py_imlib_blit(mp_obj_t image_obj, mp_obj_t template_obj)
return mp_const_true;
}
mp_obj_t py_imlib_surf(mp_obj_t image_obj)
{
struct image *image;
/* get image pointer */
image = (struct image*) py_image_cobj(image_obj);
/* sanity checks */
PY_ASSERT_TRUE(sensor.pixformat == PIXFORMAT_GRAYSCALE);
test_surf(image);
return mp_const_none;
}
void py_imlib_print(void (*print)(void *env, const char *fmt, ...), void *env, mp_obj_t self_in, mp_print_kind_t kind)
{
//print(env, "<image width:%d height:%d bpp:%d>", self->width, self->height, self->bpp);
@ -280,9 +337,12 @@ mp_obj_t py_imlib_init()
rt_store_attr(m, qstr_from_str("save_template"), rt_make_function_n(3, py_imlib_save_template));
rt_store_attr(m, qstr_from_str("template_match"), rt_make_function_n(3, py_imlib_template_match));
rt_store_attr(m, qstr_from_str("histeq"), rt_make_function_n(1, py_imlib_histeq));
rt_store_attr(m, qstr_from_str("median"), rt_make_function_n(2, py_imlib_median));
rt_store_attr(m, qstr_from_str("draw_rectangle"), rt_make_function_n(2, py_imlib_draw_rectangle));
rt_store_attr(m, qstr_from_str("detect_color"), rt_make_function_n(3, py_imlib_detect_color));
rt_store_attr(m, qstr_from_str("threshold"), rt_make_function_n(3, py_imlib_threshold));
rt_store_attr(m, qstr_from_str("count_blobs"), rt_make_function_n(1, py_imlib_count_blobs));
rt_store_attr(m, qstr_from_str("detect_objects"), rt_make_function_n(2, py_imlib_detect_objects));
rt_store_attr(m, qstr_from_str("surf"), rt_make_function_n(1, py_imlib_surf));
return m;
}

755
src/surf.c Normal file
View File

@ -0,0 +1,755 @@
/***********************************************************
* --- OpenSURF --- *
* This library is distributed under the GNU GPL. Please *
* use the contact form at http://www.chrisevansdev.com *
* for more information. *
* *
* C. Evans, Research Into Robust Visual Features, *
* MSc University of Bristol, 2008. *
* *
************************************************************/
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <float.h>
#include "array.h"
#include "xalloc.h"
#include "imlib.h"
#include "arm_math.h"
#define OCTAVES 5
#define INTERVALS 4
#define MIN(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a < _b ? _a : _b; })
#define MAX(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a > _b ? _a : _b; })
static const float pi = 3.14159f;
//! lookup table for 2d gaussian (sigma = 2.5) where (0,0) is top left and (6,6) is bottom right
static const float gauss25 [7][7] = {
{0.02546481f, 0.02350698f, 0.01849125f, 0.01239505f, 0.00708017, 0.00344629f, 0.00142946f},
{0.02350698f, 0.02169968f, 0.01706957f, 0.01144208f, 0.00653582, 0.00318132f, 0.00131956f},
{0.01849125f, 0.01706957f, 0.01342740f, 0.00900066f, 0.00514126, 0.00250252f, 0.00103800f},
{0.01239505f, 0.01144208f, 0.00900066f, 0.00603332f, 0.00344629, 0.00167749f, 0.00069579f},
{0.00708017f, 0.00653582f, 0.00514126f, 0.00344629f, 0.00196855, 0.00095820f, 0.00039744f},
{0.00344629f, 0.00318132f, 0.00250252f, 0.00167749f, 0.00095820, 0.00046640f, 0.00019346f},
{0.00142946f, 0.00131956f, 0.00103800f, 0.00069579f, 0.00039744, 0.00019346f, 0.00008024f},
};
void arm_mat_set(const arm_matrix_instance_f32 * pSrc, int r, int c, float32_t v)
{
pSrc->pData[r*pSrc->numCols+c] = v;
}
arm_matrix_instance_f32 *arm_mat_new(int r, int c)
{
arm_matrix_instance_f32 *m=(arm_matrix_instance_f32 *)xalloc(sizeof(*m));
arm_mat_init_f32(m, r, c, (float32_t*)xalloc(r*c*sizeof(float32_t)));
return m;
}
void arm_mat_free(arm_matrix_instance_f32 *m)
{
xfree(m->pData);
xfree(m);
}
//! Round float to nearest integer
static inline int fRound(float flt)
{
return (int) floor(flt+0.5f);
}
/* Computes the sum of pixels within the rectangle
specified by the top-left start co-ordinate and size. */
static float box_integral(i_image_t *img, int row, int col, int rows, int cols)
{
int step = img->w;
uint32_t *data = img->data;
// The subtraction by one for row/col is because row/col is inclusive.
int r1 = MIN(row, img->h) - 1;
int c1 = MIN(col, img->w) - 1;
int r2 = MIN(row + rows, img->h) - 1;
int c2 = MIN(col + cols, img->w) - 1;
float A=0.0f;
float B=0.0f;
float C=0.0f;
float D=0.0f;
if (r1 >= 0 && c1 >= 0) A = data[r1 * step + c1]/255.0f;
if (r1 >= 0 && c2 >= 0) B = data[r1 * step + c2]/255.0f;
if (r2 >= 0 && c1 >= 0) C = data[r2 * step + c1]/255.0f;
if (r2 >= 0 && c2 >= 0) D = data[r2 * step + c2]/255.0f;
return MAX(0, (A - B - C + D));
}
//! Calculate the value of the 2d gaussian at x,y
static inline float gaussian(int x, int y, float sig)
{
return (1.0f/(2.0f*pi*sig*sig)) * expf( -(x*x+y*y)/(2.0f*sig*sig));
}
//! Calculate the value of the 2d gaussian at x,y
static inline float gaussianf(float x, float y, float sig)
{
return 1.0f/(2.0f*pi*sig*sig) * expf(-(x*x+y*y)/(2.0f*sig*sig));
}
//! Calculate Haar wavelet responses in x direction
static inline float haar_x(surf_t *surf, int row, int column, int s)
{
return box_integral(surf->i_img, row-s/2, column, s, s/2)
-1 * box_integral(surf->i_img, row-s/2, column-s/2, s, s/2);
}
//! Calculate Haar wavelet responses in y direction
static inline float haar_y(surf_t *surf, int row, int column, int s)
{
return box_integral(surf->i_img, row, column-s/2, s/2, s)
-1 * box_integral(surf->i_img, row-s/2, column-s/2, s/2, s);
}
//! Get the angle from the +ve x-axis of the vector given by (X Y)
static float get_angle(float x, float y)
{
if(x > 0 && y >= 0)
return atan(y/x);
if(x < 0 && y >= 0)
return pi - atanf(-y/x);
if(x < 0 && y < 0)
return pi + atanf(y/x);
if(x > 0 && y < 0)
return 2*pi - atanf(-y/x);
return 0;
}
//! Assign the supplied Ipoint an orientation
static void get_orientation(surf_t *surf, i_point_t *ipt)
{
float gauss = 0.f, scale = ipt->scale;
const int s = fRound(scale);
const int r = fRound(ipt->y);
const int c = fRound(ipt->x);
float resX[109];
float resY[109];
float Ang[109];
const int id[] = {6,5,4,3,2,1,0,1,2,3,4,5,6};
int idx = 0;
int rad = 6;
// calculate haar responses for points within radius of 6*scale
for(int i = -rad; i <= rad; ++i) {
for(int j = -rad; j <= rad; ++j) {
if(i*i + j*j < rad*rad) {
gauss = (float)(gauss25[id[i+rad]][id[j+rad]]); // could use abs() rather than id lookup, but this way is faster
resX[idx] = gauss * haar_x(surf, r+j*s, c+i*s, 4*s);
resY[idx] = gauss * haar_y(surf, r+j*s, c+i*s, 4*s);
Ang[idx] = get_angle(resX[idx], resY[idx]);
++idx;
}
}
}
// calculate the dominant direction
float sumX=0.f, sumY=0.f;
float max=0.f, orientation = 0.f;
float ang1=0.f, ang2=0.f;
// loop slides pi/3 window around feature point
for(ang1 = 0; ang1 < 2*pi; ang1+=0.15f) {
ang2 = ( ang1+pi/3.0f > 2*pi ? ang1-5.0f*pi/3.0f : ang1+pi/3.0f);
sumX = sumY = 0.f;
for(unsigned int k = 0; k < idx; ++k) {
// get angle from the x-axis of the sample point
float ang = Ang[k];
// determine whether the point is within the window
if (ang1 < ang2 && ang1 < ang && ang < ang2) {
sumX+=resX[k];
sumY+=resY[k];
} else if (ang2 < ang1 && ((ang > 0 && ang < ang2) || (ang > ang1 && ang < 2*pi) )) {
sumX+=resX[k];
sumY+=resY[k];
}
}
// if the vector produced from this window is longer than all
// previous vectors then this forms the new dominant direction
if (sumX*sumX + sumY*sumY > max) {
// store largest orientation
max = sumX*sumX + sumY*sumY;
orientation = get_angle(sumX, sumY);
}
}
// assign orientation of the dominant response vector
ipt->orientation = orientation;
}
//-------------------------------------------------------
//! Get the modified descriptor. See Agrawal ECCV 08
//! Modified descriptor contributed by Pablo Fernandez
static void get_descriptor(surf_t *surf, i_point_t *ipt, bool bUpright)
{
int y, x, sample_x, sample_y, count=0;
int i = 0, ix = 0, j = 0, jx = 0, xs = 0, ys = 0;
float scale, *desc, dx, dy, mdx, mdy, co, si;
float gauss_s1 = 0.f, gauss_s2 = 0.f;
float rx = 0.f, ry = 0.f, rrx = 0.f, rry = 0.f, len = 0.f;
float cx = -0.5f, cy = 0.f; //Subregion centers for the 4x4 gaussian weighting
scale = ipt->scale;
x = fRound(ipt->x);
y = fRound(ipt->y);
desc = ipt->descriptor;
if (bUpright) {
co = 1.0f;
si = 0.0f;
} else {
co = arm_cos_f32(ipt->orientation);
si = arm_sin_f32(ipt->orientation);
}
i = -8;
//Calculate descriptor for this interest point
while (i < 12) {
j = -8;
i = i-4;
cx += 1.f;
cy = -0.5f;
while (j < 12) {
dx=dy=mdx=mdy=0.f;
cy += 1.f;
j = j - 4;
ix = i + 5;
jx = j + 5;
xs = fRound(x + ( -jx*scale*si + ix*scale*co));
ys = fRound(y + ( jx*scale*co + ix*scale*si));
for (int k = i; k < i + 9; ++k) {
for (int l = j; l < j + 9; ++l) {
//Get coords of sample point on the rotated axis
sample_x = fRound(x + (-l*scale*si + k*scale*co));
sample_y = fRound(y + ( l*scale*co + k*scale*si));
//Get the gaussian weighted x and y responses
gauss_s1 = gaussian(xs-sample_x,ys-sample_y,2.5f*scale);
rx = haar_x(surf, sample_y, sample_x, 2*fRound(scale));
ry = haar_y(surf, sample_y, sample_x, 2*fRound(scale));
//Get the gaussian weighted x and y responses on rotated axis
rrx = gauss_s1*(-rx*si + ry*co);
rry = gauss_s1*(rx*co + ry*si);
dx += rrx;
dy += rry;
mdx += fabsf(rrx);
mdy += fabsf(rry);
}
}
//Add the values to the descriptor vector
gauss_s2 = gaussian(cx-2.0f,cy-2.0f,1.5f);
desc[count++] = dx*gauss_s2;
desc[count++] = dy*gauss_s2;
desc[count++] = mdx*gauss_s2;
desc[count++] = mdy*gauss_s2;
len += (dx*dx + dy*dy + mdx*mdx + mdy*mdy) * gauss_s2*gauss_s2;
j += 9;
if (count == SURF_DESC_SIZE) {
goto done;
}
}
i += 9;
}
done:
//Convert to Unit Vector
len = sqrtf(len);
for(int i = 0; i <SURF_DESC_SIZE; ++i)
desc[i] /= len;
}
static response_layer_t *response_layer_new(int width, int height, int step, int filter)
{
response_layer_t *layer = NULL;
layer = xalloc(sizeof(*layer));
layer->width = width;
layer->height = height;
layer->step = step;
layer->filter = filter;
return layer;
}
float surf_get_laplacian(surf_t *surf, response_layer_t *rl, unsigned int r, unsigned int c)
{
int step = rl->step; // step size for this filter
int b = (rl->filter - 1) / 2; // border for this filter
int l = rl->filter / 3; // lobe for this filter (filter size / 3)
int w = rl->filter; // filter size
float inverse_area = 1.f/(w*w); // normalisation factor
float Dxx, Dyy, Dxy;
r *= step;
c *= step;
// Compute response components
Dxx = box_integral(surf->i_img, r - l + 1, c - b, 2*l - 1, w)
- box_integral(surf->i_img, r - l + 1, c - l / 2, 2*l - 1, l)*3;
Dyy = box_integral(surf->i_img, r - b, c - l + 1, w, 2*l - 1)
- box_integral(surf->i_img, r - l / 2, c - l + 1, l, 2*l - 1)*3;
Dxy = + box_integral(surf->i_img, r - l, c + 1, l, l)
+ box_integral(surf->i_img, r + 1, c - l, l, l)
- box_integral(surf->i_img, r - l, c - l, l, l)
- box_integral(surf->i_img, r + 1, c + 1, l, l);
// Normalise the filter responses with respect to their size
Dxx *= inverse_area;
Dyy *= inverse_area;
Dxy *= inverse_area;
// Get the determinant of hessian response & laplacian sign
return (Dxx + Dyy >= 0 ? 1 : 0);
}
//! Calculate DoH responses for supplied layer
float surf_get_response(surf_t *surf, response_layer_t *rl, response_layer_t *src, unsigned int r, unsigned int c)
{
int step = rl->step; // step size for this filter
int b = (rl->filter - 1) / 2; // border for this filter
int l = rl->filter / 3; // lobe for this filter (filter size / 3)
int w = rl->filter; // filter size
float inverse_area = 1.f/(w*w); // normalisation factor
float Dxx, Dyy, Dxy;
int scale = rl->width / src->width;
r *= step*scale;
c *= step*scale;
// Compute response components
Dxx = box_integral(surf->i_img, r - l + 1, c - b, 2*l - 1, w)
- box_integral(surf->i_img, r - l + 1, c - l / 2, 2*l - 1, l)*3;
Dyy = box_integral(surf->i_img, r - b, c - l + 1, w, 2*l - 1)
- box_integral(surf->i_img, r - l / 2, c - l + 1, l, 2*l - 1)*3;
Dxy = + box_integral(surf->i_img, r - l, c + 1, l, l)
+ box_integral(surf->i_img, r + 1, c - l, l, l)
- box_integral(surf->i_img, r - l, c - l, l, l)
- box_integral(surf->i_img, r + 1, c + 1, l, l);
// Normalise the filter responses with respect to their size
Dxx *= inverse_area;
Dyy *= inverse_area;
Dxy *= inverse_area;
// Get the determinant of hessian response & laplacian sign
return (Dxx * Dyy - 0.81f * Dxy * Dxy);
}
//! Computes the partial derivatives in x, y, and scale of a pixel.
static void surf_deriv3D(surf_t *surf, arm_matrix_instance_f32 *m1, int r, int c, response_layer_t *t, response_layer_t *m, response_layer_t *b)
{
float32_t dx, dy, ds;
dx = (surf_get_response(surf, m, t, r, c + 1) - surf_get_response(surf, m, t, r, c - 1)) / 2.0;
dy = (surf_get_response(surf, m, t, r + 1, c) - surf_get_response(surf, m, t, r - 1, c)) / 2.0;
ds = (surf_get_response(surf, t, t, r, c) - surf_get_response(surf, b, t, r, c)) / 2.0;
arm_mat_set(m1, 0, 0, dx );
arm_mat_set(m1, 1, 0, dy );
arm_mat_set(m1, 2, 0, ds );
}
//! Computes the 3D Hessian matrix for a pixel.
void surf_hessian3D(surf_t *surf, arm_matrix_instance_f32 *m1, int r, int c, response_layer_t *t, response_layer_t *m, response_layer_t *b)
{
float32_t v, dxx, dyy, dss, dxy, dxs, dys;
v = surf_get_response(surf, m,t, r, c);
dxx = surf_get_response(surf, m,t, r, c + 1) + surf_get_response(surf, m, t,r, c - 1) - 2 * v;
dyy = surf_get_response(surf, m,t,r + 1, c) + surf_get_response(surf, m, t, r - 1, c) - 2 * v;
dss = surf_get_response(surf, t, t, r, c) + surf_get_response(surf, b, t, r, c) - 2 * v;
dxy = ( surf_get_response(surf, m, t, r + 1, c + 1) - surf_get_response(surf, m, t, r + 1, c - 1) -
surf_get_response(surf, m, t, r - 1, c + 1) + surf_get_response(surf, m, t, r - 1, c - 1) ) / 4.0;
dxs = ( surf_get_response(surf, t, t, r, c + 1) - surf_get_response(surf, t, t, r, c - 1) -
surf_get_response(surf, b, t, r, c + 1) + surf_get_response(surf, b, t, r, c - 1) ) / 4.0;
dys = ( surf_get_response(surf, t, t, r + 1, c) - surf_get_response(surf, t, t, r - 1, c) -
surf_get_response(surf, b, t, r + 1, c) + surf_get_response(surf, b, t, r - 1, c) ) / 4.0;
arm_mat_set(m1, 0, 0, dxx);
arm_mat_set(m1, 0, 1, dxy);
arm_mat_set(m1, 0, 2, dxs);
arm_mat_set(m1, 1, 0, dxy);
arm_mat_set(m1, 1, 1, dyy);
arm_mat_set(m1, 1, 2, dys);
arm_mat_set(m1, 2, 0, dxs);
arm_mat_set(m1, 2, 1, dys);
arm_mat_set(m1, 2, 2, dss);
}
//! Performs one step of extremum interpolation.
static void surf_interpolate_step(surf_t *surf, int r, int c, response_layer_t *t, response_layer_t *m, response_layer_t *b, float32_t* xi, float32_t* xr, float32_t* xc )
{
arm_matrix_instance_f32 *H=arm_mat_new(3,3);
arm_matrix_instance_f32 *H_t=arm_mat_new(3,3);
arm_matrix_instance_f32 *H_inv=arm_mat_new(3,3);
arm_matrix_instance_f32 *dD=arm_mat_new(3, 1);
arm_matrix_instance_f32 *X=arm_mat_new(3, 1);
surf_hessian3D(surf, H, r, c, t, m, b);
surf_deriv3D(surf, dD, r, c, t, m, b);
arm_mat_inverse_f32(H, H_inv);
arm_mat_trans_f32(H_inv, H_t);
arm_mat_scale_f32(H_t, -1, H);
arm_mat_mult_f32(H, dD, X);
*xi = X->pData[2];
*xr = X->pData[1];
*xc = X->pData[0];
arm_mat_free(H);
arm_mat_free(H_t);
arm_mat_free(H_inv);
arm_mat_free(dD);
arm_mat_free(X);
}
//! Interpolate scale-space extrema to subpixel accuracy to form an image feature.
static void interpolate_extremum(surf_t *surf, int r, int c, response_layer_t *t, response_layer_t *m, response_layer_t *b)
{
// get the step distance between filters
// check the middle filter is mid way between top and bottom
int filterStep = (m->filter - b->filter);
//assert(filterStep > 0 && t->filter - m->filter == m->filter - b->filter);
// Get the offsets to the actual location of the extremum
float32_t xi = 0, xr = 0, xc = 0;
surf_interpolate_step(surf, r, c, t, m, b, &xi, &xr, &xc );
// If point is sufficiently close to the actual extremum
if(fabsf( xi ) < 0.5f && fabsf( xr ) < 0.5f && fabsf( xc ) < 0.5f ) {
i_point_t *ipt = xalloc(sizeof(*ipt));
ipt->x = (float)((c + xc) * t->step);
ipt->y = (float)((r + xr) * t->step);
ipt->scale = (float)((0.1333f) * (m->filter + xi * filterStep));
ipt->laplacian = (int)(surf_get_laplacian(surf, m, r, c));
array_push_back(surf->ipts, ipt);
}
}
//! Non Maximal Suppression function
static int is_extremum(surf_t *surf, int r, int c, response_layer_t *t, response_layer_t *m, response_layer_t *b)
{
// bounds check
int layerBorder = (t->filter + 1) / (2 * t->step);
if (r <= layerBorder || r >= t->height - layerBorder || c <= layerBorder || c >= t->width - layerBorder)
return 0;
// check the candidate point in the middle layer is above thresh
float candidate = surf_get_response(surf, m, t, r, c);
if (candidate < surf->thresh)
return 0;
for (int rr = -1; rr <=1; ++rr) {
for (int cc = -1; cc <=1; ++cc) {
// if any response in 3x3x3 is greater candidate not maximum
if (surf_get_response(surf, t, t, r+rr, c+cc) >= candidate ||
((rr != 0 || cc != 0) && surf_get_response(surf, m, t, r+rr, c+cc) >= candidate) ||
surf_get_response(surf, b, t, r+rr, c+cc) >= candidate)
return 0;
}
}
return 1;
}
//! Build map of DoH responses
static void build_response_map(surf_t *surf)
{
// Calculate responses for the first 4 octaves:
// Oct1: 9, 15, 21, 27
// Oct2: 15, 27, 39, 51
// Oct3: 27, 51, 75, 99
// Oct4: 51, 99, 147,195
// Oct5: 99, 195,291,387
// Get image attributes
int w = (surf->i_img->w / surf->init_sample);
int h = (surf->i_img->h / surf->init_sample);
int s = (surf->init_sample);
// Calculate approximated determinant of hessian values
if (surf->octaves >= 1) {
array_push_back(surf->rmap, response_layer_new(w, h, s, 9));
array_push_back(surf->rmap, response_layer_new(w, h, s, 15));
array_push_back(surf->rmap, response_layer_new(w, h, s, 21));
array_push_back(surf->rmap, response_layer_new(w, h, s, 27));
}
if (surf->octaves >= 2) {
array_push_back(surf->rmap, response_layer_new(w/2, h/2, s*2, 39));
array_push_back(surf->rmap, response_layer_new(w/2, h/2, s*2, 51));
}
if (surf->octaves >= 3) {
array_push_back(surf->rmap, response_layer_new(w/4, h/4, s*4, 75));
array_push_back(surf->rmap, response_layer_new(w/4, h/4, s*4, 99));
}
if (surf->octaves >= 4) {
array_push_back(surf->rmap, response_layer_new(w/8, h/8, s*8, 147));
array_push_back(surf->rmap, response_layer_new(w/8, h/8, s*8, 195));
}
if (surf->octaves >= 5) {
array_push_back(surf->rmap,response_layer_new(w/16, h/16, s*16, 291));
array_push_back(surf->rmap,response_layer_new(w/16, h/16, s*16, 387));
}
}
//! Find the image features and write into vector of features
static void get_ipoints(surf_t *surf)
{
// filter index map
static const int filter_map[OCTAVES][INTERVALS] = {
{0,1,2,3},
{1,3,4,5},
{3,5,6,7},
{5,7,8,9},
{7,9,10,11}
};
// Build the response map
build_response_map(surf);
// Get the response layers
response_layer_t *b, *m, *t;
for (int o=0; o<surf->octaves; ++o) {
for (int i=0; i<=1; ++i) {
b = array_at(surf->rmap, filter_map[o][i]);
m = array_at(surf->rmap, filter_map[o][i+1]);
t = array_at(surf->rmap, filter_map[o][i+2]);
// loop over middle response layer at density of the most
// sparse layer (always top), to find maxima across scale and space
for (int r=0; r<t->height; ++r) {
for (int c=0; c<t->width; ++c) {
if (is_extremum(surf, r, c, t, m, b)) {
interpolate_extremum(surf, r, c, t, m, b);
}
}
}
}
}
}
float i_point_sub(i_point_t *lhs, i_point_t *rhs) {
float sum=0.0f;
for (int i=0; i<SURF_DESC_SIZE; ++i) {
sum += (lhs->descriptor[i] - rhs->descriptor[i])*(lhs->descriptor[i] - rhs->descriptor[i]);
}
return sqrtf(sum);
};
static array_t *get_matches(array_t *ipts1, array_t *ipts2)
{
float d1;
float d2;
float dist;
i_point_t *match;
array_t *matches;
/* Allocate interest points array */
array_alloc(&matches, NULL); /* elements won't be free'd */
for (int i=0; i< array_length(ipts1); i++) {
d1 = d2 = FLT_MAX;
i_point_t *pt1 = (i_point_t *) array_at(ipts1, i);
for (int j=0; j<array_length(ipts2); j++) {
i_point_t *pt2 = (i_point_t *) array_at(ipts2, j);
dist = i_point_sub(pt1, pt2);
if(dist<d1) { /* if this feature matches better than current best */
d2 = d1;
d1 = dist;
match = pt2;
} else if(dist<d2) { /* this feature matches better than second best */
d2 = dist;
}
}
// If match has a d1:d2 ratio < 0.65 ipoints are a match
if(d1/d2 < 0.65) {
// Store the change in position
pt1->dx = match->x - pt1->x;
pt1->dy = match->y - pt1->y;
array_push_back(matches, match);
}
}
return matches;
}
void surf_detector(surf_t *surf)
{
// Extract interest points and store in vector ipts
get_ipoints(surf);
// Get the size of the vector for fixed loop bounds
int ipts_size = array_length(surf->ipts);
// printf("points %d\n", ipts_size);
// Extract the descriptors for the ipts
if (surf->upright) {
// U-SURF loop just gets descriptors
for (int i=0; i<ipts_size; ++i) {
// Extract upright (i.e. not rotation invariant) descriptors
get_descriptor(surf, array_at(surf->ipts, i), true);
}
} else {
// Main SURF-64 loop assigns orientations and gets descriptors
for (int i = 0; i < ipts_size; ++i) {
// Assign Orientations and extract rotation invariant descriptors
get_orientation(surf, array_at(surf->ipts, i));
get_descriptor(surf, array_at(surf->ipts, i), false);
}
}
}
void test_surf(image_t *img)
{
surf_t surf = {
.upright=true,
.octaves=5,
.intervals=4,
.init_sample=2,
// .thresh=0.0004f,
.thresh=0.004f,
};
/* Allocate interest points array */
array_alloc(&surf.ipts, xfree);
/* Allocate response map array */
array_alloc(&surf.rmap, xfree);
// Create integral-image representation of the image
i_image_t *i_img = surf.i_img = xalloc(sizeof(*surf.i_img));
i_img->w = img->w;
i_img->h = img->h;
i_img->data= (uint32_t*) (img->data+(img->w * img->h)*2);
imlib_integral_image(img, surf.i_img);
surf_detector(&surf);
for (int i=0; i<array_length(surf.ipts); i++) {
i_point_t *pt = array_at(surf.ipts, i);
int w = 4*(int)pt->scale;
rectangle_t r ={pt->x-w/2, pt->y-w/2, w, w};
imlib_draw_rectangle(img, &r);
}
array_free(surf.ipts);
xfree(surf.i_img);
}
#if 0
void test_surf_match(image_t *t, image_t *f)
{
i_image_t *i_img;
surf_t surf = {
.upright=true, /* run in rotation invariant mode */
.octaves=1, /* number of octaves */
.intervals=4,
.init_sample=2,
.thresh=0.0004f,
};
/* Allocate interest points array */
array_alloc(&surf.ipts, xfree);
/* Allocate response map array */
array_alloc(&surf.rmap, xfree);
/* compute integral from template */
i_img = surf.i_img = xalloc(sizeof(*surf.i_img));
i_img->w = t->w;
i_img->h = t->h;
i_img->data=xalloc(sizeof(*i_img->data)*i_img->w*i_img->h);
imlib_integral_image(t, surf.i_img);
/* run SURF detector */
surf_detector(&surf);
/* free some stuff */
free(surf.i_img);
array_free(surf.rmap);
// array_free(surf.ipts);
/* keep ipts */
array_t *ipts1=surf.ipts;
/* Allocate second interest points array */
array_alloc(&surf.ipts, xfree);
/* Allocate second response map array */
array_alloc(&surf.rmap, xfree);
/* compute integral from image */
i_img = surf.i_img = xalloc(sizeof(*surf.i_img));
i_img->w = f->w;
i_img->h = f->h;
i_img->data=xalloc(sizeof(*i_img->data)*i_img->w*i_img->h);
imlib_integral_image(f, surf.i_img);
/* run SURF detector */
surf_detector(&surf);
/* get second ipts array */
array_t *ipts2=surf.ipts;
/* match ipts */
array_t *match = get_matches(ipts1, ipts2);
printf ("t ipts: %d\n", array_length(ipts1));
printf ("f ipts: %d\n", array_length(ipts2));
printf ("matches: %d\n", array_length(match));
for (int i=0; i<array_length(match); i++) {
i_point_t *pt = array_at(match, i);
int w = 6*(int)pt->scale;
imlib_draw_rectangle(pt->x-w/2, pt->y-w/2, w , f->w, 0x00, f->data);
}
imlib_write_image(f->data, f->w, f->h, "test.tga");
}
#endif