be less harsh when setting threshold for finding lines
[sparrow.git] / edges.c
blob8e9048646ae13738ac943a28d6b537f9c4044119
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 *mesh = fl->mesh;
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];
225 GST_DEBUG("signal at %p (%d, %d): %dv %dh, lines: %dh %dv\n", p, x, y,
226 p->signal[SPARROW_HORIZONTAL], p->signal[SPARROW_VERTICAL],
227 vline, hline);
229 sparrow_cluster_t *cluster = &clusters[vline * height + hline];
230 int n = cluster->n;
231 GST_DEBUG("cluster is %p, n is %d\n", cluster, n);
232 if (n < 8){
233 cluster->voters[n].x = x << SPARROW_FIXED_POINT;
234 cluster->voters[n].y = y << SPARROW_FIXED_POINT;
235 cluster->voters[n].signal = (SIG_WEIGHT + p->signal[SPARROW_HORIZONTAL]) *
236 (SIG_WEIGHT + p->signal[SPARROW_VERTICAL]);
237 cluster->n++;
238 /*these next two could of course be computed from the offset */
240 else {
241 GST_DEBUG("more than 8 pixels at cluster for corner %d, %d\n",
242 vline, hline);
243 /*if this message ever turns up, replace the weakest signals or add
244 more slots.*/
248 //DEBUG_FIND_LINES(fl);
249 i = 0;
250 for (y = 0; y < height; y++){
251 for (x = 0; x < width; x++, i++){
252 /* how to do this?
253 1. centre of gravity (x,y, weighted average)
254 2. discard outliers? look for connectedness? but if 2 are outliers?
256 sparrow_cluster_t *cluster = clusters + i;
257 if (cluster->n == 0){
258 continue;
260 int xsum, ysum;
261 int xmean, ymean;
262 int votes;
263 while(1) {
264 int j;
265 xsum = 0;
266 ysum = 0;
267 votes = 0;
268 for (j = 0; j < cluster->n; j++){
269 votes += cluster->voters[j].signal;
270 ysum += cluster->voters[j].y * cluster->voters[j].signal;
271 xsum += cluster->voters[j].x * cluster->voters[j].signal;
273 if (votes == 0){
274 /* don't diminish signal altogether. The previous iteration's means
275 will be used. */
276 break;
278 xmean = xsum / votes;
279 ymean = ysum / votes;
280 int worst = -1;
281 int worstn;
282 int devsum = 0;
283 for (j = 0; j < cluster->n; j++){
284 int xdiff = abs(cluster->voters[j].x - xmean);
285 int ydiff = abs(cluster->voters[j].y - ymean);
286 devsum += xdiff + ydiff;
287 if (xdiff + ydiff > worst){
288 worst = xdiff + ydiff;
289 worstn = j;
292 /*a bad outlier has significantly greater than average deviation
293 (but how much is bad? median deviation would be more useful)*/
294 if (worst > 3 * devsum / cluster->n){
295 /* reduce the worst ones weight. it is a silly aberration. */
296 cluster->voters[worstn].signal /= OUTLIER_PENALTY;
297 GST_DEBUG("dropping outlier at %d,%d (mean %d,%d)\n",
298 cluster->voters[worstn].x, cluster->voters[worstn].y, xmean, ymean);
299 continue;
301 break;
303 mesh[i].out_x = x * LINE_PERIOD;
304 mesh[i].out_y = y * LINE_PERIOD;
305 mesh[i].in_x = xmean;
306 mesh[i].in_y = ymean;
307 mesh[i].used = TRUE;
308 double div = (double)(1 << SPARROW_FIXED_POINT); /*for printf only*/
309 GST_DEBUG("found corner %d (%d,%d) at (%3f, %3f)\n",
310 i, mesh[i].out_x, mesh[i].out_y,
311 xmean / div, ymean / div);
314 //DEBUG_FIND_LINES(fl);
315 /* calculate deltas toward adjacent corners */
316 /* try to extrapolate left and up, if possible, so need to go backwards. */
317 for (y = height - 2; y >= 0; y--){
318 for (x = width - 2; x >= 0; x--){
319 i = y * width + x;
320 if (mesh[i].used){
321 mesh[i].dxh = (mesh[i + 1].in_x - mesh[i].in_x) / LINE_PERIOD;
322 mesh[i].dyh = (mesh[i + 1].in_y - mesh[i].in_y) / LINE_PERIOD;
323 mesh[i].dxv = (mesh[i + width].in_x - mesh[i].in_x) / LINE_PERIOD;
324 mesh[i].dyv = (mesh[i + width].in_y - mesh[i].in_y) / LINE_PERIOD;
326 else {
327 /*prefer copy from left unless it is itself reconstructed,
328 (for no great reason)
329 A mixed copy would be possible and better */
330 if(mesh[i + 1].used &&
331 (mesh[i + 1].used < mesh[i + width].used)){
332 mesh[i].dxh = mesh[i + 1].dxh;
333 mesh[i].dyh = mesh[i + 1].dyh;
334 mesh[i].dxv = mesh[i + 1].dxv;
335 mesh[i].dyv = mesh[i + 1].dyv;
336 mesh[i].in_x = mesh[i + 1].in_x - mesh[i + 1].dxh * LINE_PERIOD;
337 mesh[i].in_y = mesh[i + 1].in_y - mesh[i + 1].dyh * LINE_PERIOD;
338 mesh[i].out_x = mesh[i + 1].out_x - 1;
339 mesh[i].out_y = mesh[i + 1].out_y;
340 mesh[i].used = mesh[i + 1].used + 1;
342 else if(mesh[i + width].used){
343 mesh[i].dxh = mesh[i + width].dxh;
344 mesh[i].dyh = mesh[i + width].dyh;
345 mesh[i].dxv = mesh[i + width].dxv;
346 mesh[i].dyv = mesh[i + width].dyv;
347 mesh[i].in_x = mesh[i + width].in_x - mesh[i + width].dxv * LINE_PERIOD;
348 mesh[i].in_y = mesh[i + width].in_y - mesh[i + width].dyv * LINE_PERIOD;
349 mesh[i].out_x = mesh[i + width].out_x;
350 mesh[i].out_y = mesh[i + width].out_y - 1;
351 mesh[i].used = mesh[i + width].used + 1;
356 if (sparrow->debug){
357 DEBUG_FIND_LINES(fl);
358 debug_corners_image(sparrow, fl);
360 //DEBUG_FIND_LINES(fl);
364 /* With no line drawn (in our colour) look at the background noise. Any real
365 signal has to be stringer than this.
367 XXX looking for simple maximum -- maybe heap or histogram might be better,
368 so as to be less susceptible to wierd outliers (e.g., bad pixels). */
369 static void
370 look_for_threshold(GstSparrow *sparrow, guint8 *in, sparrow_find_lines_t *fl){
371 //DEBUG_FIND_LINES(fl);
372 int i;
373 guint32 colour;
374 guint32 signal;
375 guint32 *in32 = (guint32 *)in;
376 guint32 highest = 0;
377 for (i = 0; i < (int)sparrow->in.pixcount; i++){
378 colour = in32[i] & sparrow->colour;
379 signal = ((colour >> fl->shift1) +
380 (colour >> fl->shift2)) & 0x1ff;
381 if (signal > highest){
382 highest = signal;
385 fl->threshold = highest + 1;
386 GST_DEBUG("found maximum noise of %d, using threshold %d\n", highest, fl->threshold);
390 static void
391 look_for_line(GstSparrow *sparrow, guint8 *in, sparrow_find_lines_t *fl,
392 sparrow_line_t *line){
393 guint i;
394 guint32 colour;
395 int signal;
396 guint32 *in32 = (guint32 *)in;
397 for (i = 0; i < sparrow->in.pixcount; i++){
398 colour = in32[i] & sparrow->colour;
399 signal = ((colour >> fl->shift1) +
400 (colour >> fl->shift2)) & 0x1ff;
401 if (signal > fl->threshold){
402 if (fl->map[i].lines[line->dir]){
403 GST_DEBUG("HEY, expected point %d to be in line %d (dir %d)"
404 "and thus empty, but it is also in line %d\n",
405 i, line->index, line->dir, fl->map[i].lines[line->dir]);
407 fl->map[i].lines[line->dir] = line->index;
408 fl->map[i].signal[line->dir] = signal;
413 static void
414 debug_map_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
415 guint32 *data = (guint32*)fl->debug->imageData;
416 memset(data, 0, sparrow->in.size);
417 for (guint i = 0; i < sparrow->in.pixcount; i++){
418 data[i] |= fl->map[i].signal[SPARROW_HORIZONTAL] << sparrow->in.gshift;
419 data[i] |= fl->map[i].signal[SPARROW_VERTICAL] << sparrow->in.rshift;
421 MAYBE_DEBUG_IPL(fl->debug);
424 /* draw the line (in sparrow->colour) */
425 static inline void
426 draw_line(GstSparrow * sparrow, sparrow_line_t *line, guint8 *out){
427 guint32 *p = (guint32 *)out;
428 int i;
429 if (line->dir == SPARROW_HORIZONTAL){
430 p += line->offset * sparrow->out.width;
431 for (i = 0; i < sparrow->out.width; i++){
432 p[i] = sparrow->colour;
435 else {
436 guint32 *p = (guint32 *)out;
437 p += line->offset;
438 for(i = 0; i < sparrow->out.height; i++){
439 *p = sparrow->colour;
440 p += sparrow->out.width;
446 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
447 until last one.
450 INVISIBLE sparrow_state
451 mode_find_edges(GstSparrow *sparrow, guint8 *in, guint8 *out){
452 sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
453 //DEBUG_FIND_LINES(fl);
454 if (fl->current == fl->n_lines){
455 goto done;
457 sparrow_line_t *line = fl->shuffled_lines[fl->current];
459 sparrow->countdown--;
460 memset(out, 0, sparrow->out.size);
461 if (sparrow->countdown){
462 /* show the line except on the first round, when we find a threshold*/
463 if (fl->threshold){
464 GST_DEBUG("current %d line %p\n", fl->current, line);
465 draw_line(sparrow, line, out);
468 else{
469 /*show nothing, look for result */
470 if (fl->threshold){
471 look_for_line(sparrow, in, fl, line);
472 fl->current++;
474 else {
475 look_for_threshold(sparrow, in, fl);
477 sparrow->countdown = sparrow->lag + 2;
479 if (sparrow->debug){
480 debug_map_image(sparrow, fl);
482 return SPARROW_STATUS_QUO;
483 done:
484 /*match up lines and find corners */
485 find_corners(sparrow, in, fl);
486 corners_to_lut(sparrow, fl);
488 return SPARROW_NEXT_STATE;
491 INVISIBLE void
492 finalise_find_edges(GstSparrow *sparrow){
493 sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
494 //DEBUG_FIND_LINES(fl);
495 if (sparrow->debug){
496 cvReleaseImage(&fl->debug);
498 free(fl->h_lines);
499 //DEBUG_FIND_LINES(fl);
500 free(fl->shuffled_lines);
501 free(fl->map);
502 //DEBUG_FIND_LINES(fl);
503 free(fl->mesh);
504 free(fl->clusters);
505 //DEBUG_FIND_LINES(fl);
506 free(fl);
507 sparrow->helper_struct = NULL;
511 static void
512 setup_colour_shifts(GstSparrow *sparrow, sparrow_find_lines_t *fl){
513 //DEBUG_FIND_LINES(fl);
515 switch (sparrow->colour){
516 case SPARROW_WHITE:
517 case SPARROW_GREEN:
518 fl->shift1 = sparrow->in.gshift;
519 fl->shift2 = sparrow->in.gshift;
520 break;
521 case SPARROW_MAGENTA:
522 fl->shift1 = sparrow->in.rshift;
523 fl->shift2 = sparrow->in.bshift;
524 break;
528 INVISIBLE void
529 init_find_edges(GstSparrow *sparrow){
530 gint32 w = sparrow->out.width;
531 gint32 h = sparrow->out.height;
532 gint i;
533 sparrow_find_lines_t *fl = zalloc_aligned_or_die(sizeof(sparrow_find_lines_t));
534 sparrow->helper_struct = (void *)fl;
536 gint h_lines = (h + LINE_PERIOD - 1) / LINE_PERIOD;
537 gint v_lines = (w + LINE_PERIOD - 1) / LINE_PERIOD;
538 gint n_lines = (h_lines + v_lines);
539 gint n_corners = (h_lines * v_lines);
540 fl->n_hlines = h_lines;
541 fl->n_vlines = v_lines;
542 fl->n_lines = n_lines;
544 fl->h_lines = malloc_aligned_or_die(sizeof(sparrow_line_t) * n_lines);
545 fl->shuffled_lines = malloc_aligned_or_die(sizeof(sparrow_line_t*) * n_lines);
546 GST_DEBUG("shuffled lines, malloced %p\n", fl->shuffled_lines);
547 fl->map = zalloc_aligned_or_die(sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
548 fl->clusters = zalloc_or_die(n_corners * sizeof(sparrow_cluster_t));
549 fl->mesh = zalloc_aligned_or_die(n_corners * sizeof(sparrow_corner_t));
551 sparrow_line_t *line = fl->h_lines;
552 sparrow_line_t **sline = fl->shuffled_lines;
553 int offset = LINE_PERIOD / 2;
555 for (i = 0, offset = LINE_PERIOD / 2; offset < h;
556 i++, offset += LINE_PERIOD){
557 line->offset = offset;
558 line->dir = SPARROW_HORIZONTAL;
559 line->index = i;
560 *sline = line;
561 line++;
562 sline++;
565 /*now add the vertical lines */
566 fl->v_lines = line;
567 for (i = 0, offset = LINE_PERIOD / 2; offset < w;
568 i++, offset += LINE_PERIOD){
569 line->offset = offset;
570 line->dir = SPARROW_VERTICAL;
571 line->index = i;
572 *sline = line;
573 line++;
574 sline++;
576 //DEBUG_FIND_LINES(fl);
578 GST_DEBUG("allocated %d lines, used %d\n", n_lines, line - fl->h_lines);
580 /*now shuffle */
581 for (i = 0; i < n_lines - 1; i++){
582 int j = RANDINT(sparrow, 0, n_lines);
583 sparrow_line_t *tmp = fl->shuffled_lines[j];
584 fl->shuffled_lines[j] = fl->shuffled_lines[i];
585 fl->shuffled_lines[i] = tmp;
588 setup_colour_shifts(sparrow, fl);
589 sparrow->countdown = sparrow->lag + 2;
590 //DEBUG_FIND_LINES(fl);
591 if (sparrow->debug){
592 CvSize size = {sparrow->in.width, sparrow->in.height};
593 fl->debug = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);