edges.c

   1 /* Copyright (C) <2010> Douglas Bagnall <douglas@halo.gen.nz>
   2  *
   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.
   7  *
   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.
  12  *
  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.
  17  */
  18
  19
  20 #include "sparrow.h"
  21 #include "gstsparrow.h"
  22 #include "edges.h"
  23
  24 #include <string.h>
  25 #include <math.h>
  26 #include <unistd.h>
  27
  28 #include "cv.h"
  29
  30
  31 static void dump_edges_info(GstSparrow *sparrow, sparrow_find_lines_t *fl, const char *filename){
  32   GST_DEBUG("about to save to %s\n", filename);
  33   FILE *f = fopen(filename, "w");
  34   sparrow_fl_condensed_t condensed;
  35   condensed.n_vlines = fl->n_vlines;
  36   condensed.n_hlines = fl->n_hlines;
  37
  38   /* simply write fl, map, clusters and mesh in sequence */
  39   GST_DEBUG("fl is %p, file is %p\n", fl, f);
  40   GST_DEBUG("fl: %d x %d\n", sizeof(sparrow_find_lines_t), 1);
  41   fwrite(&condensed, sizeof(sparrow_fl_condensed_t), 1, f);
  42   GST_DEBUG("fl->map %d x %d\n", sizeof(sparrow_intersect_t), sparrow->in.pixcount);
  43   fwrite(fl->map, sizeof(sparrow_intersect_t), sparrow->in.pixcount, f);
  44   GST_DEBUG("fl->clusters  %d x %d\n", sizeof(sparrow_cluster_t), fl->n_hlines * fl->n_vlines);
  45   fwrite(fl->clusters, sizeof(sparrow_cluster_t), fl->n_hlines * fl->n_vlines, f);
  46   GST_DEBUG("fl->mesh  %d x %d\n", sizeof(sparrow_corner_t), fl->n_hlines * fl->n_vlines);
  47   fwrite(fl->mesh, sizeof(sparrow_corner_t), fl->n_hlines * fl->n_vlines, f);
  48   /*and write the mask too */
  49   GST_DEBUG("sparrow->screenmask\n");
  50   fwrite(sparrow->screenmask, 1, sparrow->out.pixcount, f);
  51   fclose(f);
  52 }
  53
  54 static void read_edges_info(GstSparrow *sparrow, sparrow_find_lines_t *fl, const char *filename){
  55   FILE *f = fopen(filename, "r");
  56   sparrow_fl_condensed_t condensed;
  57   size_t read = fread(&condensed, sizeof(sparrow_fl_condensed_t), 1, f);
  58   assert(condensed.n_hlines == fl->n_hlines);
  59   assert(condensed.n_vlines == fl->n_vlines);
  60
  61   guint n_corners = fl->n_hlines * fl->n_vlines;
  62   read += fread(fl->map, sizeof(sparrow_intersect_t), sparrow->in.pixcount, f);
  63   read += fread(fl->clusters, sizeof(sparrow_cluster_t), n_corners, f);
  64   read += fread(fl->mesh, sizeof(sparrow_corner_t), n_corners, f);
  65   read += fread(sparrow->screenmask, 1, sparrow->out.pixcount, f);
  66   fclose(f);
  67 }
  68
  69
  70
  71 static void
  72 debug_lut(GstSparrow *sparrow, sparrow_find_lines_t *fl){
  73 }
  74
  75 #define OFFSET(x, y, w)((((y) * (w)) >> SPARROW_FIXED_POINT) + ((x) >> SPARROW_FIXED_POINT))
  76
  77 #define QUANTISE_DELTA(d)(((d) + LINE_PERIOD / 2) / LINE_PERIOD)
  78
  79 /*tolerate up to 1/8 of a pixel drift */
  80 #define MAX_DRIFT (1 << (SPARROW_FIXED_POINT - 3))
  81
  82
  83 static inline sparrow_map_path_t*
  84 possibly_new_point(sparrow_map_path_t *p, int dx, int dy){
  85   if (dx != p->dx && dy != p->dy){
  86     p++;
  87     p->dx = dx;
  88     p->dy = dy;
  89     p->n = 0;
  90   }
  91   return p;
  92 }
  93
  94 static void UNUSED
  95 corners_to_lut(GstSparrow *sparrow, sparrow_find_lines_t *fl){
  96   //DEBUG_FIND_LINES(fl);
  97   sparrow_map_t *map = &sparrow->map; /*rows in sparrow->out */
  98   guint8 *mask = sparrow->screenmask; /*mask in sparrow->in */
  99   sparrow_corner_t *mesh = fl->mesh;   /*maps regular points in ->out to points in ->in */
 100   int mesh_w = fl->n_vlines;
 101   int mesh_h = fl->n_hlines;
 102   int in_w = sparrow->in.width;
 103   int mcy, mmy, mcx; /*Mesh Corner|Modulus X|Y*/
 104
 105   int y;
 106   sparrow_corner_t *mesh_row = mesh;
 107
 108   for (y = 0; y < H_LINE_OFFSET; y++){
 109     map->rows[y].start = 0;
 110     map->rows[y].end = 0;
 111     map->rows[y].points = NULL;
 112   }
 113
 114   sparrow_map_row_t *row = map->rows + H_LINE_OFFSET;
 115   row->points = map->point_mem;
 116   sparrow_map_path_t *p = row->points;
 117
 118   for(mcy = 0; mcy < mesh_h - 1; mcy++){ /* for each mesh row */
 119     for (mmy = 0; mmy < LINE_PERIOD; mmy++){ /* for each output line */
 120       int ix, iy; /* input x, y at mesh points, interpolated vertically  */
 121       int rx, ry; /* running x, y; approximates ix, iy */
 122       int dx, dy;
 123       int on = 0;
 124       sparrow_corner_t *mesh_square = mesh_row;
 125       GST_DEBUG("row %p, y %d, row offset %d\n", row, y, row - map->rows);
 126       y++;
 127       row->points = NULL;
 128       row->start = 0;
 129       row->end = 0;
 130       for(mcx = 0; mcx < mesh_w - 1; mcx++){
 131         /*for each mesh block except the last, which has no dx,dy.
 132          Thus the mesh blocks are referenced in LINE_PERIOD passes.*/
 133         if (mesh_square->status == CORNER_UNUSED){
 134           if (! on){
 135             mesh_square++;
 136             continue;
 137           }
 138           /*lordy! continue with previous deltas*/
 139           ix = rx;
 140           iy = ry;
 141         }
 142         else {
 143           /* starting point for this row in this block. */
 144           iy = mesh_square->in_y + mmy * (mesh_square->dyd / 1);
 145           ix = mesh_square->in_x + mmy * (mesh_square->dxd / 1);
 146           /*incremental delta going left to right in this block */
 147           dy = (mesh_square->dyr / 1);
 148           dx = (mesh_square->dxr / 1);
 149         }
 150
 151         /*index of the last point in this block
 152          NB: calculating from ix, iy, which may differ slightly from rx, ry*/
 153         int lasti = OFFSET(
 154           ix + (LINE_PERIOD - 1) * dx,
 155           iy + (LINE_PERIOD - 1) * dy,
 156           in_w);
 157         //GST_DEBUG("lasti is %d, ix %d, iy %d\n", lasti, ix, iy);
 158         if (! on){
 159           if (! mask[lasti]){
 160             /*it doesn't turn on within this block (or it is of ignorably
 161               short length). */
 162             mesh_square++;
 163             continue;
 164           }
 165           /*it does turn on. so step through and find it. This happens once
 166             per line.*/
 167           rx = ix;
 168           ry = iy;
 169           int j;
 170           for (j = 0; j < LINE_PERIOD; j++){
 171             if (mask[OFFSET(rx, ry, in_w)]){
 172               break;
 173             }
 174             rx += dx;
 175             ry += dy;
 176           }
 177           row->start = mcx * LINE_PERIOD + j;
 178           row->in_x = rx;
 179           row->in_y = ry;
 180           p = possibly_new_point(p, dx, dy);
 181           row->points = p;
 182           p->n = LINE_PERIOD - j;
 183           on = 1;
 184           mesh_square++;
 185           continue;
 186         }
 187         /*it is on. */
 188         /*maybe rx, ry are drifting badly, in which case, we need to recalculate dx, dy*/
 189         if (abs(rx - ix) > MAX_DRIFT ||
 190             abs(ry - iy) > MAX_DRIFT){
 191           int y = mcy * LINE_PERIOD + mmy;
 192           int x = mcx * LINE_PERIOD;
 193           GST_DEBUG("output point %d %d, rx, ry %d, %d have got %d, %d away from target %d, %d."
 194               " dx, dy is %d, %d\n",
 195               x, y, rx, ry, rx - ix, ry - iy, ix, iy, dx, dy);
 196           sparrow_corner_t *next = mesh_square + 1;
 197           if(next->status != CORNER_UNUSED){
 198             int niy = next->in_y + mmy * (next->dyd / 1);
 199             int nix = next->in_x + mmy * (next->dxd / 1);
 200             dx = QUANTISE_DELTA(nix - ix);
 201             dy = QUANTISE_DELTA(niy - iy);
 202             GST_DEBUG("new dx, dy is %d, %d\n", dx, dy);
 203           }
 204           else{
 205             GST_DEBUG("next corner is UNUSED. dx, dy unchanged\n");
 206           }
 207         }
 208
 209         /*Probably dx/dy are different, so we need a new point */
 210         p = possibly_new_point(p, dx, dy);
 211
 212         /*does it end it this one? */
 213         if (! mask[lasti]){
 214           int j;
 215           for (j = 0; j < LINE_PERIOD; j++){
 216             if (! mask[OFFSET(rx, ry, in_w)]){
 217               break;
 218             }
 219             rx += dx;
 220             ry += dy;
 221           }
 222           p->n += j;
 223           row->end = mcx * LINE_PERIOD + j;
 224           /*this row is done! */
 225           break;
 226         }
 227         p->n += LINE_PERIOD;
 228         rx += LINE_PERIOD * dx;
 229         ry += LINE_PERIOD * dy;
 230         mesh_square++;
 231       }
 232       row++;
 233     }
 234     mesh_row += mesh_w;
 235   }
 236
 237   /*blank lines for the last few */
 238   for (y = sparrow->out.height - H_LINE_OFFSET; y < sparrow->out.height; y++){
 239     map->rows[y].start = 0;
 240     map->rows[y].end = 0;
 241     map->rows[y].points = NULL;
 242   }
 243
 244   debug_lut(sparrow, fl);
 245 }
 246
 247 UNUSED static void
 248 corners_to_full_lut(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 249   DEBUG_FIND_LINES(fl);
 250   sparrow_corner_t *mesh = fl->mesh;   /*maps regular points in ->out to points in ->in */
 251   sparrow_map_lut_t *map_lut = sparrow->map_lut;
 252   int mesh_w = fl->n_vlines;
 253   int mesh_h = fl->n_hlines;
 254   int mcy, mmy, mcx, mmx; /*Mesh Corner|Modulus X|Y*/
 255   int y = H_LINE_OFFSET;
 256   sparrow_corner_t *mesh_row = mesh;
 257   for(mcy = 0; mcy < mesh_h - 1; mcy++){
 258     for (mmy = 0; mmy < LINE_PERIOD; mmy++, y++){
 259       sparrow_corner_t *mesh_square = mesh_row;
 260       int i = y * sparrow->out.width + V_LINE_OFFSET;
 261       for(mcx = 0; mcx < mesh_w - 1; mcx++){
 262         int iy = mesh_square->in_y + mmy * mesh_square->dyd;
 263         int ix = mesh_square->in_x + mmy * mesh_square->dxd;
 264         for (mmx = 0; mmx < LINE_PERIOD; mmx++, i++){
 265           map_lut[i].x = MAX(ix >> SPARROW_FP_2_LUT, 0);
 266           map_lut[i].y = MAX(iy >> SPARROW_FP_2_LUT, 0);
 267           //GST_DEBUG("ix, iy %d, %d x, y %d, %d i: %d, y%d\n", ix, iy,
 268           //    map_lut[i].x, map_lut[i].y, i, y);
 269
 270           ix += mesh_square->dxr;
 271           iy += mesh_square->dyr;
 272         }
 273         mesh_square++;
 274       }
 275     }
 276     mesh_row += mesh_w;
 277   }
 278   sparrow->map_lut = map_lut;
 279 }
 280
 281 #define INTXY(x)((x) / (1 << SPARROW_FIXED_POINT))
 282 #define FLOATXY(x)(((double)(x)) / (1 << SPARROW_FIXED_POINT))
 283 static void
 284 debug_corners_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 285   sparrow_corner_t *mesh = fl->mesh;
 286   guint32 *data = (guint32*)fl->debug->imageData;
 287   guint w = fl->debug->width;
 288   memset(data, 0, sparrow->in.size);
 289   guint32 colours[4] = {0xff0000ff, 0x00ff0000, 0x0000ff00, 0xcccccccc};
 290   for (int i = 0; i < fl->n_vlines * fl->n_hlines; i++){
 291     sparrow_corner_t *c = &mesh[i];
 292     int x = c->in_x;
 293     int y = c->in_y;
 294     GST_DEBUG("i %d status %d x: %f, y: %f  dxr %f dyr %f dxd %f dyd %f\n"
 295         "int x, y %d,%d (raw %d,%d) data %p\n",
 296         i, c->status, FLOATXY(x), FLOATXY(y),
 297         FLOATXY(c->dxr), FLOATXY(c->dyr), FLOATXY(c->dxd), FLOATXY(c->dyd),
 298         INTXY(x), INTXY(y), x, y, data);
 299     int txr = x;
 300     int txd = x;
 301     int tyr = y;
 302     int tyd = y;
 303     for (int j = 1; j < LINE_PERIOD; j+= 2){
 304       txr += c->dxr * 2;
 305       txd += c->dxd * 2;
 306       tyr += c->dyr * 2;
 307       tyd += c->dyd * 2;
 308       data[INTXY(tyr) * w + INTXY(txr)] = 0x000088;
 309       data[INTXY(tyd) * w + INTXY(txd)] = 0x663300;
 310     }
 311     data[INTXY(y) * w + INTXY(x)] = colours[c->status];
 312   }
 313   MAYBE_DEBUG_IPL(fl->debug);
 314 }
 315
 316
 317 static void
 318 debug_clusters(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 319   guint32 *data = (guint32*)fl->debug->imageData;
 320   memset(data, 0, sparrow->in.size);
 321   int width = fl->n_vlines;
 322   int height = fl->n_hlines;
 323   sparrow_cluster_t *clusters = fl->clusters;
 324   int i, j;
 325   guint32 colour;
 326   guint32 colours[4] = {0xff0000ff, 0x0000ff00, 0x00ff0000,
 327                        0x00ff00ff};
 328   for (i = 0; i < width * height; i++){
 329     colour = colours[i % 5];
 330     sparrow_voter_t *v = clusters[i].voters;
 331     for (j = 0; j < clusters[i].n; j++){
 332       data[v[j].y * sparrow->in.width +
 333           v[j].x] = (colour * (v[j].signal / 2)) / 256;
 334     }
 335   }
 336   MAYBE_DEBUG_IPL(fl->debug);
 337 }
 338
 339
 340 #define SIGNAL_QUANT 1
 341
 342 /*maximum number of pixels in a cluster */
 343 #define CLUSTER_SIZE 8
 344
 345
 346 /*create the mesh */
 347 static void
 348 make_clusters(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 349   sparrow_cluster_t *clusters = fl->clusters;
 350   int x, y;
 351   /*special case: spurious values collect up at 0,0 */
 352   fl->map[0].signal[SPARROW_VERTICAL] = 0;
 353   fl->map[0].signal[SPARROW_HORIZONTAL] = 0;
 354   /*each point in fl->map is in a vertical line, a horizontal line, both, or
 355     neither.  Only the "both" case matters. */
 356   for (y = 0; y < sparrow->in.height; y++){
 357     for (x = 0; x < sparrow->in.width; x++){
 358       sparrow_intersect_t *p = &fl->map[y * sparrow->in.width + x];
 359       guint vsig = p->signal[SPARROW_VERTICAL];
 360       guint hsig = p->signal[SPARROW_HORIZONTAL];
 361       /*remembering that 0 is valid as a line number, but not as a signal */
 362       if (! (vsig && hsig)){
 363         continue;
 364       }
 365       /*This one is lobbying for the position of a corner.*/
 366       int vline = p->lines[SPARROW_VERTICAL];
 367       int hline = p->lines[SPARROW_HORIZONTAL];
 368       if (vline == BAD_PIXEL || hline == BAD_PIXEL){
 369         GST_DEBUG("ignoring bad pixel %d, %d\n", x, y);
 370         continue;
 371       }
 372       sparrow_cluster_t *cluster = &clusters[hline * fl->n_vlines + vline];
 373       sparrow_voter_t *voters = cluster->voters;
 374       int n = cluster->n;
 375       guint signal = (vsig * hsig) / SIGNAL_QUANT;
 376       GST_DEBUG("signal at %p (%d, %d): %dv %dh, product %u, lines: %dv %dh\n"
 377           "cluster is %p, n is %d\n", p, x, y,
 378           vsig, hsig, signal, vline, hline, cluster, n);
 379       if (signal == 0){
 380         GST_WARNING("signal at %p (%d, %d) is %d following quantisation!\n",
 381             p, x, y, signal);
 382       }
 383
 384       if (n < CLUSTER_SIZE){
 385         voters[n].x = x;
 386         voters[n].y = y;
 387         voters[n].signal = signal;
 388         cluster->n++;
 389       }
 390       else {
 391         /*duplicate x, y, signal, so they aren't mucked up */
 392         guint ts = signal;
 393         int tx = x;
 394         int ty = y;
 395         /*replaced one ends up here */
 396         int ts2;
 397         int tx2;
 398         int ty2;
 399         for (int j = 0; j < CLUSTER_SIZE; j++){
 400           if (voters[j].signal < ts){
 401             ts2 = voters[j].signal;
 402             tx2 = voters[j].x;
 403             ty2 = voters[j].y;
 404             voters[j].signal = ts;
 405             voters[j].x = tx;
 406             voters[j].y = ty;
 407             ts = ts2;
 408             tx = tx2;
 409             ty = ty2;
 410           }
 411         }
 412         GST_DEBUG("more than %d pixels at cluster for corner %d, %d."
 413             "Dropped %u for %u\n",
 414             CLUSTER_SIZE, vline, hline, ts2, signal);
 415       }
 416     }
 417   }
 418   if (sparrow->debug){
 419     debug_clusters(sparrow, fl);
 420   }
 421 }
 422
 423
 424 static inline void
 425 drop_cluster_voter(sparrow_cluster_t *cluster, int n)
 426 {
 427   if (n < cluster->n){
 428     for (int i = n; i < cluster->n - 1; i++){
 429       cluster->voters[i] = cluster->voters[i + 1];
 430     }
 431     cluster->n--;
 432   }
 433 }
 434
 435 static inline int sort_median(int *a, guint n)
 436 {
 437   guint i, j;
 438   /*stupid sort, but n is very small*/
 439   for (i = 0; i <  n; i++){
 440     for (j = i + 1; j < n; j++){
 441       if (a[i] > a[j]){
 442         int tmp = a[j];
 443         a[j] = a[i];
 444         a[i] = tmp;
 445       }
 446     }
 447   }
 448   guint middle = n / 2;
 449   int answer = a[middle];
 450
 451   if ((n & 1) == 0){
 452     answer += a[middle - 1];
 453     answer /= 2;
 454   }
 455   return answer;
 456 }
 457
 458 #define EUCLIDEAN_D2(ax, ay, bx, by)((ax - bx) * (ax - bx) + (ay - by) * (ay - by))
 459 #define NEIGHBOURLY_THRESHOLD (LINE_PERIOD * 4)
 460
 461 static inline void
 462 neighbourly_discard_cluster_outliers(GstSparrow *sparrow, sparrow_cluster_t *cluster,
 463     sparrow_corner_t *neighbour)
 464 {
 465   /* assuming the output mesh entirely fits in the input window (which is
 466      required for sparrow to work) the neighbours should be at most
 467      LINE_PERIOD * input resolution / output resolution apart. But set the
 468      threshold higher, just in case. */
 469   const int threshold = NEIGHBOURLY_THRESHOLD * sparrow->in.height / sparrow->out.height;
 470   int i;
 471   int neighbour_d[CLUSTER_SIZE];
 472   int close = 0;
 473   for (i = 0; i < cluster->n; i++){
 474     int d = EUCLIDEAN_D2(neighbour->in_x, neighbour->in_y,
 475         cluster->voters[i].x, cluster->voters[i].y);
 476     int pass = d > threshold;
 477     neighbour_d[i] = pass;
 478     close += pass;
 479     GST_DEBUG("failing point %d, distance sq %d, threshold %d\n", i, d, threshold);
 480   }
 481   if (close > 1){
 482     for (i = 0; i < cluster->n; i++){
 483       if (! neighbour_d[i]){
 484         drop_cluster_voter(cluster, i);
 485       }
 486     }
 487   }
 488 }
 489
 490 static inline void
 491 median_discard_cluster_outliers(sparrow_cluster_t *cluster)
 492 {
 493   int xvals[CLUSTER_SIZE];
 494   int yvals[CLUSTER_SIZE];
 495   int i;
 496   for (i = 0; i < cluster->n; i++){
 497     /*XXX could sort here*/
 498     xvals[i] = cluster->voters[i].x;
 499     yvals[i] = cluster->voters[i].y;
 500   }
 501   const int xmed = sort_median(xvals, cluster->n);
 502   const int ymed = sort_median(yvals, cluster->n);
 503
 504   for (i = 0; i < cluster->n; i++){
 505     int dx = cluster->voters[i].x - xmed;
 506     int dy = cluster->voters[i].y - ymed;
 507     if (dx * dx + dy * dy > OUTLIER_THRESHOLD){
 508       drop_cluster_voter(cluster, i);
 509     }
 510   }
 511 }
 512
 513 /*create the mesh */
 514 static inline void
 515 make_corners(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 516   //DEBUG_FIND_LINES(fl);
 517   int width = fl->n_vlines;
 518   int height = fl->n_hlines;
 519   sparrow_cluster_t *clusters = fl->clusters;
 520   sparrow_corner_t *mesh = fl->mesh;
 521   int x, y, i;
 522
 523   i = 0;
 524   for (y = 0; y < height; y++){
 525     for (x = 0; x < width; x++, i++){
 526       sparrow_cluster_t *cluster = clusters + i;
 527       if (cluster->n == 0){
 528         continue;
 529       }
 530
 531       /*the good points should all be adjacent; distant ones are spurious, so
 532         are discarded.
 533
 534         This fails if all the cluster are way off. Obviously it would be good
 535         to include information about the grid in the decision, but that is not
 536         there yet. (needs iteration, really).
 537
 538       Here's a slight attempt:*/
 539 #if 0
 540       sparrow_corner_t *neighbour;
 541       if (x){
 542         neighbour = &mesh[i - 1];
 543         neighbourly_discard_cluster_outliers(sparrow, cluster, neighbour);
 544       }
 545       else if (y){
 546         neighbour = &mesh[i - width];
 547         neighbourly_discard_cluster_outliers(sparrow, cluster, neighbour);
 548       }
 549 #endif
 550       median_discard_cluster_outliers(cluster);
 551
 552       /* now find a weighted average position */
 553       /*64 bit to avoid overflow -- should probably just use floating point
 554         (or reduce signal)*/
 555       guint64 xsum, ysum;
 556       guint xmean, ymean;
 557       guint64 votes;
 558       int j;
 559       xsum = 0;
 560       ysum = 0;
 561       votes = 0;
 562       for (j = 0; j < cluster->n; j++){
 563         votes += cluster->voters[j].signal;
 564         ysum += cluster->voters[j].y * cluster->voters[j].signal;
 565         xsum += cluster->voters[j].x * cluster->voters[j].signal;
 566       }
 567       if (votes){
 568         xmean = (xsum << SPARROW_FIXED_POINT) / votes;
 569         ymean = (ysum << SPARROW_FIXED_POINT) / votes;
 570       }
 571       else {
 572         GST_WARNING("corner %d, %d voters, sum %d,%d, somehow has no votes\n",
 573             i, cluster->n, xsum, ysum);
 574       }
 575
 576       GST_DEBUG("corner %d: %d voters, %d votes, sum %d,%d, mean %d,%d\n",
 577           i, cluster->n, votes, xsum, ysum, xmean, ymean);
 578
 579       mesh[i].in_x = xmean;
 580       mesh[i].in_y = ymean;
 581       mesh[i].status = CORNER_EXACT;
 582       GST_DEBUG("found corner %d at (%3f, %3f)\n",
 583           i, FLOATXY(xmean), FLOATXY(ymean));
 584     }
 585   }
 586 }
 587
 588
 589 static inline void
 590 make_map(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 591   int i;
 592   int width = fl->n_vlines;
 593   int height = fl->n_hlines;
 594   sparrow_corner_t *mesh = fl->mesh;
 595   gint x, y;
 596
 597   DEBUG_FIND_LINES(fl);
 598   /* calculate deltas toward adjacent corners */
 599   /* try to extrapolate left and up, if possible, so need to go backwards. */
 600   i = width * height - 1;
 601   for (y = height - 1; y >= 0; y--){
 602     for (x = width - 1; x >= 0; x--, i--){
 603       sparrow_corner_t *corner = &mesh[i];
 604       /* calculate the delta to next corner. If this corner is on edge, delta is
 605        0 and next is this.*/
 606       sparrow_corner_t *right = (x == width - 1) ? corner : corner + 1;
 607       sparrow_corner_t *down =  (y == height - 1) ? corner : corner + width;
 608       GST_DEBUG("i %d xy %d,%d width %d. in_xy %d,%d; down in_xy %d,%d; right in_xy %d,%d\n",
 609           i, x, y, width, corner->in_x, corner->in_y, down->in_x,
 610           down->in_y, right->in_x,  right->in_y);
 611       if (corner->status != CORNER_UNUSED){
 612         corner->dxr = (right->in_x - corner->in_x);
 613         corner->dyr = (right->in_y - corner->in_y);
 614         corner->dxd = (down->in_x -  corner->in_x);
 615         corner->dyd = (down->in_y -  corner->in_y);
 616       }
 617       else {
 618           /*copy from both right and down, if they both exist. */
 619         struct {
 620           int dxr;
 621           int dyr;
 622           int dxd;
 623           int dyd;
 624         } dividends = {0, 0, 0, 0};
 625         struct {
 626           int r;
 627           int d;
 628         } divisors = {0, 0};
 629
 630         if (right != corner){
 631           if (right->dxr || right->dyr){
 632             dividends.dxr += right->dxr;
 633             dividends.dyr += right->dyr;
 634             divisors.r++;
 635           }
 636           if (right->dxd || right->dyd){
 637             dividends.dxd += right->dxd;
 638             dividends.dyd += right->dyd;
 639             divisors.d++;
 640           }
 641         }
 642         if (down != corner){
 643           if (down->dxr || down->dyr){
 644             dividends.dxr += down->dxr;
 645             dividends.dyr += down->dyr;
 646             divisors.r++;
 647           }
 648           if (down->dxd || down->dyd){
 649             dividends.dxd += down->dxd;
 650             dividends.dyd += down->dyd;
 651             divisors.d++;
 652           }
 653         }
 654         corner->dxr = divisors.r ? dividends.dxr / divisors.r : 0;
 655         corner->dyr = divisors.r ? dividends.dyr / divisors.r : 0;
 656         corner->dxd = divisors.d ? dividends.dxd / divisors.d : 0;
 657         corner->dyd = divisors.d ? dividends.dyd / divisors.d : 0;
 658
 659         /*now extrapolate position, preferably from both left and right */
 660         if (right == corner){
 661           if (down != corner){ /*use down only */
 662             corner->in_x = down->in_x - corner->dxd;
 663             corner->in_y = down->in_y - corner->dyd;
 664           }
 665           else {/*oh no*/
 666             GST_DEBUG("can't reconstruct corner %d, %d: no useable neighbours\n", x, y);
 667             /*it would be easy enough to look further, but hopefully of no
 668               practical use */
 669           }
 670         }
 671         else if (down == corner){ /*use right only */
 672           corner->in_x = right->in_x - corner->dxr;
 673           corner->in_y = right->in_y - corner->dyr;
 674         }
 675         else { /* use both */
 676           corner->in_x = right->in_x - corner->dxr;
 677           corner->in_y = right->in_y - corner->dyr;
 678           corner->in_x += down->in_x - corner->dxd;
 679           corner->in_y += down->in_y - corner->dyd;
 680           corner->in_x >>= 1;
 681           corner->in_y >>= 1;
 682         }
 683         corner->status = CORNER_PROJECTED;
 684       }
 685     }
 686   }
 687   /*now quantise delta values.  It would be wrong to do it earlier, when they
 688     are being used to calculate whole mesh jumps, but from now they are
 689     primarily going to used for pixel (mesh / LINE_PERIOD) jumps. To do this in
 690   corners_to_lut puts a whole lot of division in a tight loop.*/
 691   for (i = 0; i < width * height; i++){
 692     sparrow_corner_t *corner = &mesh[i];
 693     corner->dxr = QUANTISE_DELTA(corner->dxr);
 694     corner->dyr = QUANTISE_DELTA(corner->dyr);
 695     corner->dxd = QUANTISE_DELTA(corner->dxd);
 696     corner->dyd = QUANTISE_DELTA(corner->dyd);
 697   }
 698   DEBUG_FIND_LINES(fl);
 699   if (sparrow->debug){
 700     debug_corners_image(sparrow, fl);
 701   }
 702 }
 703
 704
 705
 706 static void
 707 look_for_line(GstSparrow *sparrow, guint8 *in, sparrow_find_lines_t *fl,
 708     sparrow_line_t *line){
 709   guint i;
 710   guint32 colour;
 711   guint32 cmask = sparrow->out.colours[sparrow->colour];
 712   int signal;
 713
 714   /* subtract background noise */
 715   fl->input->imageData = (char *)in;
 716   cvSub(fl->input, fl->threshold, fl->working, NULL);
 717   guint32 *in32 = (guint32 *)fl->working->imageData;
 718
 719   for (i = 0; i < sparrow->in.pixcount; i++){
 720     colour = in32[i] & cmask;
 721     signal = (((colour >> fl->shift1) +
 722             (colour >> fl->shift2))) & 0xff;
 723     if (signal){
 724       if (fl->map[i].lines[line->dir]){
 725         /*assume the pixel is on for everyone and will just confuse
 726           matters. ignore it.
 727           XXX maybe threshold, so if signal is hugely bigger in one, don't ban it.
 728         */
 729         if (fl->map[i].lines[line->dir] != BAD_PIXEL){
 730           GST_DEBUG("HEY, expected point %d to be in line %d (dir %d) "
 731               "and thus empty, but it is also in line %d\n"
 732               "old signal %d, new signal %d, marking as BAD\n",
 733               i, line->index, line->dir, fl->map[i].lines[line->dir],
 734               fl->map[i].signal[line->dir], signal);
 735           fl->map[i].lines[line->dir] = BAD_PIXEL;
 736           fl->map[i].signal[line->dir] = 0;
 737         }
 738       }
 739       else{
 740         fl->map[i].lines[line->dir] = line->index;
 741         fl->map[i].signal[line->dir] = signal;
 742       }
 743     }
 744   }
 745 }
 746
 747 static void
 748 debug_map_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 749   guint32 *data = (guint32*)fl->debug->imageData;
 750   memset(data, 0, sparrow->in.size);
 751   for (guint i = 0; i < sparrow->in.pixcount; i++){
 752     data[i] |= fl->map[i].signal[SPARROW_HORIZONTAL] << sparrow->in.gshift;
 753     data[i] |= fl->map[i].signal[SPARROW_VERTICAL] << sparrow->in.rshift;
 754     data[i] |= ((fl->map[i].lines[SPARROW_VERTICAL] == BAD_PIXEL) ||
 755         (fl->map[i].lines[SPARROW_HORIZONTAL] == BAD_PIXEL)) ? 255 << sparrow->in.bshift : 0;
 756   }
 757   MAYBE_DEBUG_IPL(fl->debug);
 758 }
 759
 760 /* draw the line (in sparrow->colour) */
 761 static inline void
 762 draw_line(GstSparrow * sparrow, sparrow_line_t *line, guint8 *out){
 763   guint32 *p = (guint32 *)out;
 764   guint32 colour = sparrow->out.colours[sparrow->colour];
 765   int i;
 766   if (line->dir == SPARROW_HORIZONTAL){
 767     p += line->offset * sparrow->out.width;
 768     for (i = 0; i < sparrow->out.width; i++){
 769       p[i] = colour;
 770     }
 771   }
 772   else {
 773     guint32 *p = (guint32 *)out;
 774     p += line->offset;
 775     for(i = 0; i < sparrow->out.height; i++){
 776       *p = colour;
 777       p += sparrow->out.width;
 778     }
 779   }
 780 }
 781
 782 static void
 783 jump_state(GstSparrow *sparrow, sparrow_find_lines_t *fl, edges_state_t state){
 784   if (state == EDGES_NEXT_STATE){
 785     fl->state++;
 786   }
 787   else {
 788     fl->state = state;
 789   }
 790   switch (fl->state){
 791   case EDGES_FIND_NOISE:
 792     sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
 793     break;
 794   case EDGES_FIND_LINES:
 795     sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
 796     break;
 797   case EDGES_FIND_CORNERS:
 798     sparrow->countdown = 4;
 799     break;
 800   default:
 801     GST_DEBUG("jumped to non-existent state %d\n", fl->state);
 802     break;
 803   }
 804 }
 805
 806 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
 807    until last one.
 808  */
 809 static inline void
 810 draw_lines(GstSparrow *sparrow, sparrow_find_lines_t *fl, guint8 *in, guint8 *out)
 811 {
 812   sparrow_line_t *line = fl->shuffled_lines[fl->current];
 813   sparrow->countdown--;
 814   memset(out, 0, sparrow->out.size);
 815   if (sparrow->countdown){
 816     draw_line(sparrow, line, out);
 817   }
 818   else{
 819     /*show nothing, look for result */
 820     look_for_line(sparrow, in, fl, line);
 821     if (sparrow->debug){
 822       debug_map_image(sparrow, fl);
 823     }
 824     fl->current++;
 825     if (fl->current == fl->n_lines){
 826       jump_state(sparrow, fl, EDGES_NEXT_STATE);
 827     }
 828     else{
 829       sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
 830     }
 831   }
 832 }
 833
 834 #define LINE_THRESHOLD 32
 835
 836 static inline void
 837 find_threshold(GstSparrow *sparrow, sparrow_find_lines_t *fl, guint8 *in, guint8 *out)
 838 {
 839   memset(out, 0, sparrow->out.size);
 840   /*XXX should average/median over a range of frames */
 841   if (sparrow->countdown == 0){
 842     memcpy(fl->threshold->imageData, in, sparrow->in.size);
 843     /*add a constant, and smooth */
 844     cvAddS(fl->threshold, cvScalarAll(LINE_THRESHOLD), fl->working, NULL);
 845     cvSmooth(fl->working, fl->threshold, CV_GAUSSIAN, 3, 0, 0, 0);
 846     //cvSmooth(fl->working, fl->threshold, CV_MEDIAN, 3, 0, 0, 0);
 847     jump_state(sparrow, fl, EDGES_NEXT_STATE);
 848   }
 849   sparrow->countdown--;
 850 }
 851
 852 /*match up lines and find corners */
 853 static inline int
 854 find_corners(GstSparrow *sparrow, sparrow_find_lines_t *fl)
 855 {
 856   sparrow->countdown--;
 857   switch(sparrow->countdown){
 858   case 3:
 859     make_clusters(sparrow, fl);
 860     break;
 861   case 2:
 862     make_corners(sparrow, fl);
 863     break;
 864   case 1:
 865     make_map(sparrow, fl);
 866     break;
 867   case 0:
 868 #if USE_FULL_LUT
 869     corners_to_full_lut(sparrow, fl);
 870 #else
 871     corners_to_lut(sparrow, fl);
 872 #endif
 873     break;
 874   default:
 875     GST_DEBUG("how did sparrow->countdown get to be %d?", sparrow->countdown);
 876     sparrow->countdown = 4;
 877   }
 878   return sparrow->countdown;
 879 }
 880
 881
 882 INVISIBLE sparrow_state
 883 mode_find_edges(GstSparrow *sparrow, guint8 *in, guint8 *out){
 884   sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
 885   switch (fl->state){
 886   case EDGES_FIND_NOISE:
 887     find_threshold(sparrow, fl, in, out);
 888     break;
 889   case EDGES_FIND_LINES:
 890     draw_lines(sparrow, fl, in, out);
 891     break;
 892   case EDGES_FIND_CORNERS:
 893     if (find_corners(sparrow, fl))
 894       break;
 895     return SPARROW_NEXT_STATE;
 896   case EDGES_NEXT_STATE:
 897     break; /*shush gcc */
 898   }
 899   return SPARROW_STATUS_QUO;
 900 }
 901
 902 INVISIBLE void
 903 finalise_find_edges(GstSparrow *sparrow){
 904   sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
 905   //DEBUG_FIND_LINES(fl);
 906   if (sparrow->save && *(sparrow->save)){
 907     GST_DEBUG("about to save to %s\n", sparrow->save);
 908     dump_edges_info(sparrow, fl, sparrow->save);
 909   }
 910   if (sparrow->debug){
 911     cvReleaseImage(&fl->debug);
 912   }
 913   free(fl->h_lines);
 914   free(fl->shuffled_lines);
 915   free(fl->map);
 916   free(fl->mesh);
 917   free(fl->clusters);
 918   cvReleaseImage(&fl->threshold);
 919   cvReleaseImage(&fl->working);
 920   cvReleaseImageHeader(&fl->input);
 921   free(fl);
 922   GST_DEBUG("freed everything\n");
 923   sparrow->helper_struct = NULL;
 924 }
 925
 926 /*reduce the signal a little bit more, avoiding overflow later */
 927 #define COLOUR_QUANT  1
 928
 929 static void
 930 setup_colour_shifts(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 931   switch (sparrow->colour){
 932   case SPARROW_WHITE:
 933   case SPARROW_GREEN:
 934     fl->shift1 = sparrow->in.gshift + COLOUR_QUANT;
 935     fl->shift2 = sparrow->in.gshift + COLOUR_QUANT;
 936     break;
 937   case SPARROW_MAGENTA:
 938     fl->shift1 = sparrow->in.rshift + COLOUR_QUANT;
 939     fl->shift2 = sparrow->in.bshift + COLOUR_QUANT;
 940     break;
 941   }
 942 }
 943
 944 INVISIBLE void
 945 init_find_edges(GstSparrow *sparrow){
 946   gint i;
 947   sparrow_find_lines_t *fl = zalloc_aligned_or_die(sizeof(sparrow_find_lines_t));
 948   sparrow->helper_struct = (void *)fl;
 949
 950   gint h_lines = (sparrow->out.height + LINE_PERIOD - 1) / LINE_PERIOD;
 951   gint v_lines = (sparrow->out.width + LINE_PERIOD - 1) / LINE_PERIOD;
 952   gint n_lines_max = (h_lines + v_lines);
 953   gint n_corners = (h_lines * v_lines);
 954   fl->n_hlines = h_lines;
 955   fl->n_vlines = v_lines;
 956
 957
 958   fl->h_lines = malloc_aligned_or_die(sizeof(sparrow_line_t) * n_lines_max);
 959   fl->shuffled_lines = malloc_aligned_or_die(sizeof(sparrow_line_t *) * n_lines_max);
 960   GST_DEBUG("shuffled lines, malloced %p\n", fl->shuffled_lines);
 961
 962   GST_DEBUG("map is going to be %d * %d \n", sizeof(sparrow_intersect_t), sparrow->in.pixcount);
 963   fl->map = zalloc_aligned_or_die(sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
 964   fl->clusters = zalloc_or_die(n_corners * sizeof(sparrow_cluster_t));
 965   fl->mesh = zalloc_aligned_or_die(n_corners * sizeof(sparrow_corner_t));
 966
 967   sparrow_line_t *line = fl->h_lines;
 968   sparrow_line_t **sline = fl->shuffled_lines;
 969   int offset;
 970
 971   for (i = 0, offset = H_LINE_OFFSET; offset < sparrow->out.height;
 972        i++, offset += LINE_PERIOD){
 973     line->offset = offset;
 974     line->dir = SPARROW_HORIZONTAL;
 975     line->index = i;
 976     *sline = line;
 977     line++;
 978     sline++;
 979     //GST_DEBUG("line %d h has offset %d\n", i, offset);
 980   }
 981
 982   /*now add the vertical lines */
 983   fl->v_lines = line;
 984   for (i = 0, offset = V_LINE_OFFSET; offset < sparrow->out.width;
 985        i++, offset += LINE_PERIOD){
 986     line->offset = offset;
 987     line->dir = SPARROW_VERTICAL;
 988     line->index = i;
 989     *sline = line;
 990     line++;
 991     sline++;
 992     //GST_DEBUG("line %d v has offset %d\n", i, offset);
 993   }
 994   //DEBUG_FIND_LINES(fl);
 995   fl->n_lines = line - fl->h_lines;
 996   GST_DEBUG("allocated %d lines, made %d\n", n_lines_max, fl->n_lines);
 997
 998   /*now shuffle */
 999   for (i = 0; i < fl->n_lines; i++){
1000     int j = RANDINT(sparrow, 0, fl->n_lines);
1001     sparrow_line_t *tmp = fl->shuffled_lines[j];
1002     fl->shuffled_lines[j] = fl->shuffled_lines[i];
1003     fl->shuffled_lines[i] = tmp;
1004   }
1005
1006   setup_colour_shifts(sparrow, fl);
1007
1008   if (sparrow->reload){
1009     if (access(sparrow->reload, R_OK)){
1010       GST_DEBUG("sparrow>reload is '%s' and it is UNREADABLE\n", sparrow->reload);
1011       exit(1);
1012     }
1013     read_edges_info(sparrow, fl, sparrow->reload);
1014     memset(fl->map, 0, sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
1015     //memset(fl->clusters, 0, n_corners * sizeof(sparrow_cluster_t));
1016     memset(fl->mesh, 0, n_corners * sizeof(sparrow_corner_t));
1017     jump_state(sparrow, fl, EDGES_FIND_CORNERS);
1018   }
1019   else {
1020     jump_state(sparrow, fl, EDGES_FIND_NOISE);
1021   }
1022   /* opencv images for threshold finding */
1023   CvSize size = {sparrow->in.width, sparrow->in.height};
1024   fl->working = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
1025   fl->threshold = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
1026
1027   /*input has no data allocated -- it uses latest frame*/
1028   fl->input = init_ipl_image(&sparrow->in, PIXSIZE);
1029
1030   //DEBUG_FIND_LINES(fl);
1031   if (sparrow->debug){
1032     fl->debug = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
1033   }
1034 }
1035