add image dump function for grid
[sparrow.git] / edges.c
blobdba168477fda944f66db7726288aac711f74b30b
1 /* Copyright (C) <2010> Douglas Bagnall <douglas@halo.gen.nz>
3 * This library is free software; you can redistribute it and/or
4 * modify it under the terms of the GNU Library General Public
5 * License as published by the Free Software Foundation; either
6 * version 2 of the License, or (at your option) any later version.
8 * This library is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 * Library General Public License for more details.
13 * You should have received a copy of the GNU Library General Public
14 * License along with this library; if not, write to the
15 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
16 * Boston, MA 02111-1307, USA.
20 #include "sparrow.h"
21 #include "gstsparrow.h"
22 #include "edges.h"
24 #include <string.h>
25 #include <math.h>
27 #include "cv.h"
32 #define SIG_WEIGHT 5
34 /*3 pixels manhatten distance makes you an outlier */
35 #define OUTLIER_THRESHOLD 3 << (SPARROW_FIXED_POINT)
36 #define OUTLIER_PENALTY 8
42 #define SPARROW_MAP_LUT_SHIFT 1
43 #define SPARROW_FP_2_LUT (SPARROW_FIXED_POINT - SPARROW_MAP_LUT_SHIFT)
45 #define OFFSET(x, y, w)((((y) * (w)) >> SPARROW_FIXED_POINT) + ((x) >> SPARROW_FIXED_POINT))
47 static void corners_to_lut(GstSparrow *sparrow, sparrow_find_lines_t *fl){
48 //DEBUG_FIND_LINES(fl);
49 sparrow_map_t *map = &sparrow->map; /*rows in sparrow->out */
50 guint8 *mask = sparrow->screenmask; /*mask in sparrow->in */
51 sparrow_corner_t *mesh = fl->mesh; /*maps regular points in ->out to points in ->in */
53 int mesh_w = fl->n_vlines;
54 int mesh_h = fl->n_hlines;
55 int in_w = sparrow->in.width;
56 int mcy, mmy, mcx; /*Mesh Corner|Modulus X|Y*/
58 int x;
59 sparrow_map_row_t *row = map->rows;
60 sparrow_map_point_t *p = map->point_mem;
61 sparrow_corner_t *mesh_row = mesh;
62 for(mcy = 0; mcy < mesh_h; mcy++){
63 for (mmy = 0; mmy < LINE_PERIOD; mmy++){
64 sparrow_corner_t *mesh_square = mesh_row;
65 row->points = p;
66 row->start = 0;
67 row->end = 0;
68 for(mcx = 0; mcx < mesh_w; mcx++){
69 if (mesh_square->used){
70 int iy = mesh_square->in_y + mmy * mesh_square->dyv;
71 int ix = mesh_square->in_x + mmy * mesh_square->dxv;
72 int ii = OFFSET(ix, iy, in_w);
73 int ii_end = OFFSET(ix + (LINE_PERIOD - 1) * mesh_square->dxh,
74 iy + (LINE_PERIOD - 1) * mesh_square->dyh, in_w);
75 int start_on = mask[ii];
76 int end_on = mask[ii_end];
77 if(start_on && end_on){
78 /*add the point, maybe switch on */
79 if (row->start == row->end){/* if both are 0 */
80 row->start = mcx * LINE_PERIOD;
82 p->x = ix;
83 p->y = iy;
84 p->dx = mesh_square->dxh;
85 p->dy = mesh_square->dyh;
86 p++;
88 else if (start_on){
89 /*add the point, switch off somewhere in the middle*/
90 for (x = 1; x < LINE_PERIOD; x++){
91 iy += mesh_square->dyh;
92 ix += mesh_square->dxh;
93 ii = OFFSET(ix, iy, in_w);
94 if (mask[ii]){
95 /*point is not in the same column with the others,
96 but sparrow knows this because the row->start says so */
97 row->start = mcx + x;
98 p->x = ix;
99 p->y = iy;
100 p->dx = mesh_square->dxh;
101 p->dy = mesh_square->dyh;
102 p++;
103 break;
107 else if (end_on){
108 /* add some, switch off */
109 for (x = 1; x < LINE_PERIOD; x++){
110 iy += mesh_square->dyh;
111 ix += mesh_square->dxh;
112 ii = OFFSET(ix, iy, in_w);
113 if (! mask[ii]){
114 row->end = mcx + x;
115 break;
119 else {
120 /*3 cases:
121 start > end: this is first off pixel.
122 start == end: row hasn't started (both 0)
123 start < end: both are set -- row is done
125 if (row->start > row->end){
126 row->end = mcx * LINE_PERIOD;
128 else if (row->start < row->end){
129 break;
133 mesh_square++;
135 row++;
137 mesh_row += mesh_w;
141 UNUSED static void
142 corners_to_full_lut(GstSparrow *sparrow, sparrow_find_lines_t *fl){
143 //DEBUG_FIND_LINES(fl);
144 sparrow_corner_t *mesh = fl->mesh; /*maps regular points in ->out to points in ->in */
145 sparrow_map_lut_t *map_lut = sparrow->map_lut;
146 int mesh_w = fl->n_vlines;
147 int mesh_h = fl->n_hlines;
148 int mcy, mmy, mcx, mmx; /*Mesh Corner|Modulus X|Y*/
149 int i = 0;
150 sparrow_corner_t *mesh_row = mesh;
151 for(mcy = 0; mcy < mesh_h; mcy++){
152 for (mmy = 0; mmy < LINE_PERIOD; mmy++){
153 sparrow_corner_t *mesh_square = mesh_row;
154 for(mcx = 0; mcx < mesh_w; mcx++){
155 int iy = mesh_square->in_y + mmy * mesh_square->dyv;
156 int ix = mesh_square->in_x + mmy * mesh_square->dxv;
157 for (mmx = 0; mmx < LINE_PERIOD; mmx++, i++){
158 map_lut[i].x = ix >> SPARROW_FP_2_LUT;
159 map_lut[i].y = iy >> SPARROW_FP_2_LUT;
160 ix += mesh_square->dxh;
161 iy += mesh_square->dyh;
163 mesh_square++;
166 mesh_row += mesh_w;
168 sparrow->map_lut = map_lut;
171 #define INTXY(x)((x)>> SPARROW_FIXED_POINT)
172 #define FLOATXY(x)(((double)(x)) / (1 << SPARROW_FIXED_POINT))
173 static void
174 debug_corners_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
175 guint32 *data = (guint32*)fl->debug->imageData;
176 guint w = fl->debug->width;
177 memset(data, 0, sparrow->in.size);
178 guint32 colours[4] = {0xaaaaaaaa, 0x0000ff00, 0x00ff0000, 0xff0000ff};
179 for (int i = 0; i < fl->n_vlines * fl->n_hlines; i++){
180 sparrow_corner_t *c = &fl->mesh[i];
181 int x = c->in_x;
182 int y = c->in_y;
183 GST_DEBUG("i %d used %d x: %f, y: %f dxh %f dyh %f dxv %f dyv %f\n",
184 i, c->used, FLOATXY(x), FLOATXY(y),
185 FLOATXY(c->dxh), FLOATXY(c->dyh), FLOATXY(c->dxv), FLOATXY(c->dxh));
186 data[INTXY(y + c->dyh) * w + INTXY(x + c->dxh)] = 0x0000aaaa;
187 data[INTXY(y + c->dyh + c->dyh) * w + INTXY(x + c->dxh + c->dxh)] = 0x0000aaaa;
188 data[INTXY(y + c->dyv) * w + INTXY(x + c->dxv)] = 0x00aaaa00;
189 data[INTXY(y + c->dyv + c->dyv) * w + INTXY(x + c->dxv + c->dxv)] = 0x00aaaa00;
190 data[INTXY(y) * w + INTXY(x)] = colours[c->used];
192 MAYBE_DEBUG_IPL(fl->debug);
195 /*create the mesh */
197 static void
198 find_corners(GstSparrow *sparrow, guint8 *in, sparrow_find_lines_t *fl){
199 //DEBUG_FIND_LINES(fl);
200 int i;
201 int width = fl->n_vlines;
202 int height = fl->n_hlines;
203 sparrow_cluster_t *clusters = fl->clusters;
204 sparrow_corner_t *corners = fl->corners;
205 gint x, y;
206 /*each point in fl->map is in a vertical line, a horizontal line, both, or
207 neither. Only the "both" case matters. */
208 for (y = 0; y < sparrow->in.height; y++){
209 for (x = 0; x < sparrow->in.width; x++){
210 sparrow_intersect_t *p = &fl->map[y * sparrow->in.width + x];
211 /*remembering that 0 is valid as a line no, but not as a signal */
212 if (! p->signal[SPARROW_HORIZONTAL] ||
213 ! p->signal[SPARROW_VERTICAL]){
214 continue;
216 /*This one is lobbying for the position of a corner.*/
218 /*XXX what to do in the case that there is no intersection? often cases
219 this will happen in the dark bits and be OK. But if it happens in the
220 light?*/
221 /*linearise the xy coordinates*/
222 int vline = p->lines[SPARROW_VERTICAL];
223 int hline = p->lines[SPARROW_HORIZONTAL];
224 sparrow_cluster_t *cluster = &clusters[vline * height + hline];
226 int n = cluster->n;
227 if (n < 8){
228 cluster->voters[n].x = x << SPARROW_FIXED_POINT;
229 cluster->voters[n].y = y << SPARROW_FIXED_POINT;
230 cluster->voters[n].signal = (SIG_WEIGHT + p->signal[SPARROW_HORIZONTAL]) *
231 (SIG_WEIGHT + p->signal[SPARROW_VERTICAL]);
232 cluster->n++;
233 /*these next two could of course be computed from the offset */
235 else {
236 GST_DEBUG("more than 8 pixels at cluster for corner %d, %d\n",
237 vline, hline);
238 /*if this message ever turns up, replace the weakest signals or add
239 more slots.*/
244 i = 0;
245 for (y = 0; y < height; y++){
246 for (x = 0; x < width; x++, i++){
247 /* how to do this?
248 1. centre of gravity (x,y, weighted average)
249 2. discard outliers? look for connectedness? but if 2 are outliers?
251 sparrow_cluster_t *cluster = clusters + i;
252 if (cluster->n == 0){
253 continue;
255 int xsum, ysum;
256 int xmean, ymean;
257 int votes;
258 while(1) {
259 int j;
260 xsum = 0;
261 ysum = 0;
262 votes = 0;
263 for (j = 0; j < cluster->n; j++){
264 votes += cluster->voters[j].signal;
265 ysum += cluster->voters[j].y * cluster->voters[j].signal;
266 xsum += cluster->voters[j].x * cluster->voters[j].signal;
268 if (votes == 0){
269 /* don't diminish signal altogether. The previous iteration's means
270 will be used. */
271 break;
273 xmean = xsum / votes;
274 ymean = ysum / votes;
275 int worst = -1;
276 int worstn;
277 int devsum = 0;
278 for (j = 0; j < cluster->n; j++){
279 int xdiff = abs(cluster->voters[j].x - xmean);
280 int ydiff = abs(cluster->voters[j].y - ymean);
281 devsum += xdiff + ydiff;
282 if (xdiff + ydiff > worst){
283 worst = xdiff + ydiff;
284 worstn = j;
287 /*a bad outlier has significantly greater than average deviation
288 (but how much is bad? median deviation would be more useful)*/
289 if (worst > 3 * devsum / cluster->n){
290 /* reduce the worst ones weight. it is a silly aberration. */
291 cluster->voters[worstn].signal /= OUTLIER_PENALTY;
292 GST_DEBUG("dropping outlier at %d,%d (mean %d,%d)\n",
293 cluster->voters[worstn].x, cluster->voters[worstn].y, xmean, ymean);
294 continue;
296 break;
298 corners[i].out_x = x * LINE_PERIOD;
299 corners[i].out_y = y * LINE_PERIOD;
300 corners[i].in_x = xmean;
301 corners[i].in_y = ymean;
302 corners[i].used = TRUE;
303 double div = (double)(1 << SPARROW_FIXED_POINT); /*for printf only*/
304 GST_DEBUG("found corner %d (%d,%d) at (%3f, %3f)\n",
305 i, corners[i].out_x, corners[i].out_y,
306 xmean / div, ymean / div);
309 DEBUG_FIND_LINES(fl);
310 /* calculate deltas toward adjacent corners */
311 /* try to extrapolate left and up, if possible, so need to go backwards. */
312 for (y = height - 2; y >= 0; y--){
313 for (x = width - 2; x >= 0; x--){
314 i = y * width + x;
315 if (corners[i].used){
316 corners[i].dxh = (corners[i + 1].in_x - corners[i].in_x) / LINE_PERIOD;
317 corners[i].dyh = (corners[i + 1].in_y - corners[i].in_y) / LINE_PERIOD;
318 corners[i].dxv = (corners[i + width].in_x - corners[i].in_x) / LINE_PERIOD;
319 corners[i].dyv = (corners[i + width].in_y - corners[i].in_y) / LINE_PERIOD;
321 else {
322 /*prefer copy from left unless it is itself reconstructed,
323 (for no great reason)
324 A mixed copy would be possible and better */
325 if(corners[i + 1].used &&
326 (corners[i + 1].used < corners[i + width].used)){
327 corners[i].dxh = corners[i + 1].dxh;
328 corners[i].dyh = corners[i + 1].dyh;
329 corners[i].dxv = corners[i + 1].dxv;
330 corners[i].dyv = corners[i + 1].dyv;
331 corners[i].in_x = corners[i + 1].in_x - corners[i + 1].dxh * LINE_PERIOD;
332 corners[i].in_y = corners[i + 1].in_y - corners[i + 1].dyh * LINE_PERIOD;
333 corners[i].out_x = corners[i + 1].out_x - 1;
334 corners[i].out_y = corners[i + 1].out_y;
335 corners[i].used = corners[i + 1].used + 1;
337 else if(corners[i + width].used){
338 corners[i].dxh = corners[i + width].dxh;
339 corners[i].dyh = corners[i + width].dyh;
340 corners[i].dxv = corners[i + width].dxv;
341 corners[i].dyv = corners[i + width].dyv;
342 corners[i].in_x = corners[i + width].in_x - corners[i + width].dxv * LINE_PERIOD;
343 corners[i].in_y = corners[i + width].in_y - corners[i + width].dyv * LINE_PERIOD;
344 corners[i].out_x = corners[i + width].out_x;
345 corners[i].out_y = corners[i + width].out_y - 1;
346 corners[i].used = corners[i + width].used + 1;
351 if (sparrow->debug){
352 DEBUG_FIND_LINES(fl);
353 debug_corners_image(sparrow, fl);
355 //DEBUG_FIND_LINES(fl);
359 /* With no line drawn (in our colour) look at the background noise. Any real
360 signal has to be stringer than this.
362 XXX looking for simple maximum -- maybe heap or histogram might be better,
363 so as to be less susceptible to wierd outliers (e.g., bad pixels). */
364 static void
365 look_for_threshold(GstSparrow *sparrow, guint8 *in, sparrow_find_lines_t *fl){
366 //DEBUG_FIND_LINES(fl);
367 int i;
368 guint32 colour;
369 guint32 signal;
370 guint32 *in32 = (guint32 *)in;
371 guint32 highest = 0;
372 for (i = 0; i < (int)sparrow->in.pixcount; i++){
373 colour = in32[i] & sparrow->colour;
374 signal = ((colour >> fl->shift1) +
375 (colour >> fl->shift2)) & 0x1ff;
376 if (signal > highest){
377 highest = signal;
380 fl->threshold = highest + 10;
381 GST_DEBUG("found maximum noise of %d, using threshold %d\n", highest, fl->threshold);
385 static void
386 look_for_line(GstSparrow *sparrow, guint8 *in, sparrow_find_lines_t *fl,
387 sparrow_line_t *line){
388 guint i;
389 guint32 colour;
390 int signal;
391 guint32 *in32 = (guint32 *)in;
392 for (i = 0; i < sparrow->in.pixcount; i++){
393 colour = in32[i] & sparrow->colour;
394 signal = ((colour >> fl->shift1) +
395 (colour >> fl->shift2)) & 0x1ff;
396 if (signal > fl->threshold){
397 if (fl->map[i].lines[line->dir]){
398 GST_DEBUG("HEY, expected point %d to be in line %d (dir %d)"
399 "and thus empty, but it is also in line %d\n",
400 i, line->index, line->dir, fl->map[i].lines[line->dir]);
402 fl->map[i].lines[line->dir] = line->index;
403 fl->map[i].signal[line->dir] = signal;
408 static void
409 debug_map_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
410 guint32 *data = (guint32*)fl->debug->imageData;
411 memset(data, 0, sparrow->in.size);
412 for (guint i = 0; i < sparrow->in.pixcount; i++){
413 data[i] |= fl->map[i].signal[SPARROW_HORIZONTAL] << sparrow->in.gshift;
414 data[i] |= fl->map[i].signal[SPARROW_VERTICAL] << sparrow->in.rshift;
416 MAYBE_DEBUG_IPL(fl->debug);
419 /* draw the line (in sparrow->colour) */
420 static inline void
421 draw_line(GstSparrow * sparrow, sparrow_line_t *line, guint8 *out){
422 guint32 *p = (guint32 *)out;
423 int i;
424 if (line->dir == SPARROW_HORIZONTAL){
425 p += line->offset * sparrow->out.width;
426 for (i = 0; i < sparrow->out.width; i++){
427 p[i] = sparrow->colour;
430 else {
431 guint32 *p = (guint32 *)out;
432 p += line->offset;
433 for(i = 0; i < sparrow->out.height; i++){
434 *p = sparrow->colour;
435 p += sparrow->out.width;
441 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
442 until last one.
445 INVISIBLE sparrow_state
446 mode_find_edges(GstSparrow *sparrow, guint8 *in, guint8 *out){
447 sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
448 DEBUG_FIND_LINES(fl);
449 if (fl->current == fl->n_lines){
450 goto done;
452 sparrow_line_t *line = fl->shuffled_lines[fl->current];
454 sparrow->countdown--;
455 memset(out, 0, sparrow->out.size);
456 if (sparrow->countdown){
457 /* show the line except on the first round, when we find a threshold*/
458 if (fl->threshold){
459 GST_DEBUG("current %d line %p\n", fl->current, line);
460 draw_line(sparrow, line, out);
463 else{
464 /*show nothing, look for result */
465 if (fl->threshold){
466 look_for_line(sparrow, in, fl, line);
467 fl->current++;
469 else {
470 look_for_threshold(sparrow, in, fl);
472 sparrow->countdown = sparrow->lag + 2;
474 if (sparrow->debug){
475 debug_map_image(sparrow, fl);
477 return SPARROW_STATUS_QUO;
478 done:
479 /*match up lines and find corners */
480 find_corners(sparrow, in, fl);
481 corners_to_lut(sparrow, fl);
483 return SPARROW_NEXT_STATE;
486 INVISIBLE void
487 finalise_find_edges(GstSparrow *sparrow){
488 sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
489 //DEBUG_FIND_LINES(fl);
490 if (sparrow->debug){
491 cvReleaseImage(&fl->debug);
493 free(fl->h_lines);
494 free(fl->shuffled_lines);
495 free(fl->map);
496 free(fl->mesh);
497 free(fl->clusters);
498 free(fl->corners);
499 free(fl);
503 static void
504 setup_colour_shifts(GstSparrow *sparrow, sparrow_find_lines_t *fl){
505 //DEBUG_FIND_LINES(fl);
507 switch (sparrow->colour){
508 case SPARROW_WHITE:
509 case SPARROW_GREEN:
510 fl->shift1 = sparrow->in.gshift;
511 fl->shift2 = sparrow->in.gshift;
512 break;
513 case SPARROW_MAGENTA:
514 fl->shift1 = sparrow->in.rshift;
515 fl->shift2 = sparrow->in.bshift;
516 break;
520 INVISIBLE void
521 init_find_edges(GstSparrow *sparrow){
522 gint32 w = sparrow->out.width;
523 gint32 h = sparrow->out.height;
524 gint i;
525 sparrow_find_lines_t *fl = zalloc_aligned_or_die(sizeof(sparrow_find_lines_t));
526 sparrow->helper_struct = (void *)fl;
528 gint h_lines = (h + LINE_PERIOD - 1) / LINE_PERIOD;
529 gint v_lines = (w + LINE_PERIOD - 1) / LINE_PERIOD;
530 gint n_lines = (h_lines + v_lines);
531 gint n_corners = (h_lines * v_lines);
532 fl->n_hlines = h_lines;
533 fl->n_vlines = v_lines;
534 fl->n_lines = n_lines;
536 fl->h_lines = malloc_aligned_or_die(sizeof(sparrow_line_t) * n_lines);
537 fl->shuffled_lines = malloc_aligned_or_die(sizeof(sparrow_line_t*) * n_lines);
538 GST_DEBUG("shuffled lines, malloced %p\n", fl->shuffled_lines);
539 fl->map = zalloc_aligned_or_die(sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
540 fl->clusters = zalloc_or_die(n_corners * sizeof(sparrow_cluster_t));
541 fl->corners = zalloc_aligned_or_die(n_corners * sizeof(sparrow_corner_t));
542 fl->mesh = malloc_aligned_or_die(sizeof(sparrow_corner_t) * h_lines * v_lines);
544 sparrow_line_t *line = fl->h_lines;
545 sparrow_line_t **sline = fl->shuffled_lines;
546 int offset = LINE_PERIOD / 2;
548 for (i = 0, offset = LINE_PERIOD / 2; offset < h;
549 i++, offset += LINE_PERIOD){
550 line->offset = offset;
551 line->dir = SPARROW_HORIZONTAL;
552 line->index = i;
553 *sline = line;
554 line++;
555 sline++;
558 /*now add the vertical lines */
559 fl->v_lines = line;
560 for (i = 0, offset = LINE_PERIOD / 2; offset < w;
561 i++, offset += LINE_PERIOD){
562 line->offset = offset;
563 line->dir = SPARROW_VERTICAL;
564 line->index = i;
565 *sline = line;
566 line++;
567 sline++;
569 //DEBUG_FIND_LINES(fl);
571 GST_DEBUG("allocated %d lines, used %d\n", n_lines, line - fl->h_lines);
573 /*now shuffle */
574 for (i = 0; i < n_lines - 1; i++){
575 int j = RANDINT(sparrow, 0, n_lines);
576 sparrow_line_t *tmp = fl->shuffled_lines[j];
577 fl->shuffled_lines[j] = fl->shuffled_lines[i];
578 fl->shuffled_lines[i] = tmp;
581 setup_colour_shifts(sparrow, fl);
582 sparrow->countdown = sparrow->lag + 2;
583 //DEBUG_FIND_LINES(fl);
584 if (sparrow->debug){
585 CvSize size = {sparrow->in.width, sparrow->in.height};
586 fl->debug = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);