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
 728           XXX maybe threshold, so if signal is hugely bigger in one, don't ban it.
 729         */
 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       else{
 739         fl->map[i].lines[line->dir] = line->index;
 740         fl->map[i].signal[line->dir] = signal;
 741       }
 742     }
 743   }
 744 }
 745
 746 static void
 747 debug_map_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 748   guint32 *data = (guint32*)fl->debug->imageData;
 749   memset(data, 0, sparrow->in.size);
 750   for (guint i = 0; i < sparrow->in.pixcount; i++){
 751     data[i] |= fl->map[i].signal[SPARROW_HORIZONTAL] << sparrow->in.gshift;
 752     data[i] |= fl->map[i].signal[SPARROW_VERTICAL] << sparrow->in.rshift;
 753   }
 754   MAYBE_DEBUG_IPL(fl->debug);
 755 }
 756
 757 /* draw the line (in sparrow->colour) */
 758 static inline void
 759 draw_line(GstSparrow * sparrow, sparrow_line_t *line, guint8 *out){
 760   guint32 *p = (guint32 *)out;
 761   guint32 colour = sparrow->out.colours[sparrow->colour];
 762   int i;
 763   if (line->dir == SPARROW_HORIZONTAL){
 764     p += line->offset * sparrow->out.width;
 765     for (i = 0; i < sparrow->out.width; i++){
 766       p[i] = colour;
 767     }
 768   }
 769   else {
 770     guint32 *p = (guint32 *)out;
 771     p += line->offset;
 772     for(i = 0; i < sparrow->out.height; i++){
 773       *p = colour;
 774       p += sparrow->out.width;
 775     }
 776   }
 777 }
 778
 779 static void
 780 jump_state(GstSparrow *sparrow, sparrow_find_lines_t *fl, edges_state_t state){
 781   if (state == EDGES_NEXT_STATE){
 782     fl->state++;
 783   }
 784   else {
 785     fl->state = state;
 786   }
 787   switch (fl->state){
 788   case EDGES_FIND_NOISE:
 789     sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
 790     break;
 791   case EDGES_FIND_LINES:
 792     sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
 793     break;
 794   case EDGES_FIND_CORNERS:
 795     sparrow->countdown = 4;
 796     break;
 797   default:
 798     GST_DEBUG("jumped to non-existent state %d\n", fl->state);
 799     break;
 800   }
 801 }
 802
 803 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
 804    until last one.
 805  */
 806 static inline void
 807 draw_lines(GstSparrow *sparrow, sparrow_find_lines_t *fl, guint8 *in, guint8 *out)
 808 {
 809   sparrow_line_t *line = fl->shuffled_lines[fl->current];
 810   sparrow->countdown--;
 811   memset(out, 0, sparrow->out.size);
 812   if (sparrow->countdown){
 813     draw_line(sparrow, line, out);
 814   }
 815   else{
 816     /*show nothing, look for result */
 817     look_for_line(sparrow, in, fl, line);
 818     if (sparrow->debug){
 819       debug_map_image(sparrow, fl);
 820     }
 821     fl->current++;
 822     if (fl->current == fl->n_lines){
 823       jump_state(sparrow, fl, EDGES_NEXT_STATE);
 824     }
 825     else{
 826       sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
 827     }
 828   }
 829 }
 830
 831 #define LINE_THRESHOLD 32
 832
 833 static inline void
 834 find_threshold(GstSparrow *sparrow, sparrow_find_lines_t *fl, guint8 *in, guint8 *out)
 835 {
 836   memset(out, 0, sparrow->out.size);
 837   /*XXX should average/median over a range of frames */
 838   if (sparrow->countdown == 0){
 839     memcpy(fl->threshold->imageData, in, sparrow->in.size);
 840     /*add a constant, and smooth */
 841     cvAddS(fl->threshold, cvScalarAll(LINE_THRESHOLD), fl->working, NULL);
 842     cvSmooth(fl->working, fl->threshold, CV_GAUSSIAN, 3, 0, 0, 0);
 843     //cvSmooth(fl->working, fl->threshold, CV_MEDIAN, 3, 0, 0, 0);
 844     jump_state(sparrow, fl, EDGES_NEXT_STATE);
 845   }
 846   sparrow->countdown--;
 847 }
 848
 849 /*match up lines and find corners */
 850 static inline int
 851 find_corners(GstSparrow *sparrow, sparrow_find_lines_t *fl)
 852 {
 853   sparrow->countdown--;
 854   switch(sparrow->countdown){
 855   case 3:
 856     make_clusters(sparrow, fl);
 857     break;
 858   case 2:
 859     make_corners(sparrow, fl);
 860     break;
 861   case 1:
 862     make_map(sparrow, fl);
 863     break;
 864   case 0:
 865 #if USE_FULL_LUT
 866     corners_to_full_lut(sparrow, fl);
 867 #else
 868     corners_to_lut(sparrow, fl);
 869 #endif
 870     break;
 871   default:
 872     GST_DEBUG("how did sparrow->countdown get to be %d?", sparrow->countdown);
 873     sparrow->countdown = 4;
 874   }
 875   return sparrow->countdown;
 876 }
 877
 878
 879 INVISIBLE sparrow_state
 880 mode_find_edges(GstSparrow *sparrow, guint8 *in, guint8 *out){
 881   sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
 882   switch (fl->state){
 883   case EDGES_FIND_NOISE:
 884     find_threshold(sparrow, fl, in, out);
 885     break;
 886   case EDGES_FIND_LINES:
 887     draw_lines(sparrow, fl, in, out);
 888     break;
 889   case EDGES_FIND_CORNERS:
 890     if (find_corners(sparrow, fl))
 891       break;
 892     return SPARROW_NEXT_STATE;
 893   case EDGES_NEXT_STATE:
 894     break; /*shush gcc */
 895   }
 896   return SPARROW_STATUS_QUO;
 897 }
 898
 899 INVISIBLE void
 900 finalise_find_edges(GstSparrow *sparrow){
 901   sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
 902   //DEBUG_FIND_LINES(fl);
 903   if (sparrow->save && *(sparrow->save)){
 904     GST_DEBUG("about to save to %s\n", sparrow->save);
 905     dump_edges_info(sparrow, fl, sparrow->save);
 906   }
 907   if (sparrow->debug){
 908     cvReleaseImage(&fl->debug);
 909   }
 910   free(fl->h_lines);
 911   free(fl->shuffled_lines);
 912   free(fl->map);
 913   free(fl->mesh);
 914   free(fl->clusters);
 915   cvReleaseImage(&fl->threshold);
 916   cvReleaseImage(&fl->working);
 917   cvReleaseImageHeader(&fl->input);
 918   free(fl);
 919   GST_DEBUG("freed everything\n");
 920   sparrow->helper_struct = NULL;
 921 }
 922
 923 /*reduce the signal a little bit more, avoiding overflow later */
 924 #define COLOUR_QUANT  1
 925
 926 static void
 927 setup_colour_shifts(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 928   switch (sparrow->colour){
 929   case SPARROW_WHITE:
 930   case SPARROW_GREEN:
 931     fl->shift1 = sparrow->in.gshift + COLOUR_QUANT;
 932     fl->shift2 = sparrow->in.gshift + COLOUR_QUANT;
 933     break;
 934   case SPARROW_MAGENTA:
 935     fl->shift1 = sparrow->in.rshift + COLOUR_QUANT;
 936     fl->shift2 = sparrow->in.bshift + COLOUR_QUANT;
 937     break;
 938   }
 939 }
 940
 941 INVISIBLE void
 942 init_find_edges(GstSparrow *sparrow){
 943   gint32 w = sparrow->out.width;
 944   gint32 h = sparrow->out.height;
 945   gint i;
 946   sparrow_find_lines_t *fl = zalloc_aligned_or_die(sizeof(sparrow_find_lines_t));
 947   sparrow->helper_struct = (void *)fl;
 948
 949   gint h_lines = (h + LINE_PERIOD - 1) / LINE_PERIOD;
 950   gint v_lines = (w + LINE_PERIOD - 1) / LINE_PERIOD;
 951   gint n_lines = (h_lines + v_lines);
 952   gint n_corners = (h_lines * v_lines);
 953   fl->n_hlines = h_lines;
 954   fl->n_vlines = v_lines;
 955   fl->n_lines = n_lines;
 956
 957   fl->h_lines = malloc_aligned_or_die(sizeof(sparrow_line_t) * n_lines);
 958   fl->shuffled_lines = malloc_aligned_or_die(sizeof(sparrow_line_t*) * n_lines);
 959   GST_DEBUG("shuffled lines, malloced %p\n", fl->shuffled_lines);
 960
 961   fl->map = zalloc_aligned_or_die(sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
 962   fl->clusters = zalloc_or_die(n_corners * sizeof(sparrow_cluster_t));
 963   fl->mesh = zalloc_aligned_or_die(n_corners * sizeof(sparrow_corner_t));
 964
 965   sparrow_line_t *line = fl->h_lines;
 966   sparrow_line_t **sline = fl->shuffled_lines;
 967   int offset;
 968
 969   for (i = 0, offset = H_LINE_OFFSET; offset < h;
 970        i++, offset += LINE_PERIOD){
 971     line->offset = offset;
 972     line->dir = SPARROW_HORIZONTAL;
 973     line->index = i;
 974     *sline = line;
 975     line++;
 976     sline++;
 977   }
 978
 979   /*now add the vertical lines */
 980   fl->v_lines = line;
 981   for (i = 0, offset = V_LINE_OFFSET; offset < w;
 982        i++, offset += LINE_PERIOD){
 983     line->offset = offset;
 984     line->dir = SPARROW_VERTICAL;
 985     line->index = i;
 986     *sline = line;
 987     line++;
 988     sline++;
 989   }
 990   //DEBUG_FIND_LINES(fl);
 991
 992   GST_DEBUG("allocated %d lines, status %d\n", n_lines, line - fl->h_lines);
 993
 994   /*now shuffle */
 995   for (i = 0; i < n_lines; i++){
 996     int j = RANDINT(sparrow, 0, n_lines);
 997     sparrow_line_t *tmp = fl->shuffled_lines[j];
 998     fl->shuffled_lines[j] = fl->shuffled_lines[i];
 999     fl->shuffled_lines[i] = tmp;
1000   }
1001
1002   setup_colour_shifts(sparrow, fl);
1003
1004   if (sparrow->reload){
1005     if (access(sparrow->reload, R_OK)){
1006       GST_DEBUG("sparrow>reload is '%s' and it is UNREADABLE\n", sparrow->reload);
1007       exit(1);
1008     }
1009     read_edges_info(sparrow, fl, sparrow->reload);
1010     memset(fl->map, 0, sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
1011     //memset(fl->clusters, 0, n_corners * sizeof(sparrow_cluster_t));
1012     memset(fl->mesh, 0, n_corners * sizeof(sparrow_corner_t));
1013     jump_state(sparrow, fl, EDGES_FIND_CORNERS);
1014   }
1015   else {
1016     jump_state(sparrow, fl, EDGES_FIND_NOISE);
1017   }
1018   /* opencv images for threshold finding */
1019   CvSize size = {sparrow->in.width, sparrow->in.height};
1020   fl->working = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
1021   fl->threshold = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
1022
1023   /*input has no data allocated -- it uses latest frame*/
1024   fl->input = init_ipl_image(&sparrow->in, PIXSIZE);
1025
1026   //DEBUG_FIND_LINES(fl);
1027   if (sparrow->debug){
1028     fl->debug = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
1029   }
1030 }
1031