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
 284 static inline int
 285 clamp_intxy(int x, const int max){
 286   if (x < 0)
 287     return 0;
 288   if (x >= max << SPARROW_FIXED_POINT)
 289     return max;
 290   return x / (1 << SPARROW_FIXED_POINT);
 291 }
 292
 293 static void
 294 debug_corners_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 295   sparrow_corner_t *mesh = fl->mesh;
 296   guint32 *data = (guint32*)fl->debug->imageData;
 297   guint w = fl->debug->width;
 298   guint h = fl->debug->height;
 299   memset(data, 0, sparrow->in.size);
 300   guint32 colours[4] = {0xff0000ff, 0x00ff0000, 0x0000ff00, 0xcccccccc};
 301   for (int i = 0; i < fl->n_vlines * fl->n_hlines; i++){
 302     sparrow_corner_t *c = &mesh[i];
 303     int x = c->in_x;
 304     int y = c->in_y;
 305     GST_DEBUG("i %d status %d x: %f, y: %f  dxr %f dyr %f dxd %f dyd %f\n"
 306         "int x, y %d,%d (raw %d,%d) data %p\n",
 307         i, c->status, FLOATXY(x), FLOATXY(y),
 308         FLOATXY(c->dxr), FLOATXY(c->dyr), FLOATXY(c->dxd), FLOATXY(c->dyd),
 309         INTXY(x), INTXY(y), x, y, data);
 310     int txr = x;
 311     int txd = x;
 312     int tyr = y;
 313     int tyd = y;
 314     for (int j = 1; j < LINE_PERIOD; j+= 2){
 315       txr += c->dxr * 2;
 316       txd += c->dxd * 2;
 317       tyr += c->dyr * 2;
 318       tyd += c->dyd * 2;
 319       data[clamp_intxy(tyr, h) * w + clamp_intxy(txr, w)] = 0x000088;
 320       data[clamp_intxy(tyd, h) * w + clamp_intxy(txd, w)] = 0x663300;
 321     }
 322     data[clamp_intxy(y, h) * w + clamp_intxy(x, w)] = colours[c->status];
 323   }
 324   MAYBE_DEBUG_IPL(fl->debug);
 325 }
 326
 327
 328 static void
 329 debug_clusters(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 330   guint32 *data = (guint32*)fl->debug->imageData;
 331   memset(data, 0, sparrow->in.size);
 332   int width = fl->n_vlines;
 333   int height = fl->n_hlines;
 334   sparrow_cluster_t *clusters = fl->clusters;
 335   int i, j;
 336   guint32 colour;
 337   guint32 colours[4] = {0xff0000ff, 0x0000ff00, 0x00ff0000,
 338                        0x00ff00ff};
 339   for (i = 0; i < width * height; i++){
 340     colour = colours[i % 5];
 341     sparrow_voter_t *v = clusters[i].voters;
 342     for (j = 0; j < clusters[i].n; j++){
 343       data[v[j].y * sparrow->in.width +
 344           v[j].x] = (colour * (v[j].signal / 2)) / 256;
 345     }
 346   }
 347   MAYBE_DEBUG_IPL(fl->debug);
 348 }
 349
 350
 351 #define SIGNAL_QUANT 1
 352
 353 /*maximum number of pixels in a cluster */
 354 #define CLUSTER_SIZE 8
 355
 356
 357 /*create the mesh */
 358 static void
 359 make_clusters(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 360   sparrow_cluster_t *clusters = fl->clusters;
 361   int x, y;
 362   /*special case: spurious values collect up at 0,0 */
 363   fl->map[0].signal[SPARROW_VERTICAL] = 0;
 364   fl->map[0].signal[SPARROW_HORIZONTAL] = 0;
 365   /*each point in fl->map is in a vertical line, a horizontal line, both, or
 366     neither.  Only the "both" case matters. */
 367   for (y = 0; y < sparrow->in.height; y++){
 368     for (x = 0; x < sparrow->in.width; x++){
 369       sparrow_intersect_t *p = &fl->map[y * sparrow->in.width + x];
 370       guint vsig = p->signal[SPARROW_VERTICAL];
 371       guint hsig = p->signal[SPARROW_HORIZONTAL];
 372       /*remembering that 0 is valid as a line number, but not as a signal */
 373       if (! (vsig && hsig)){
 374         continue;
 375       }
 376       /*This one is lobbying for the position of a corner.*/
 377       int vline = p->lines[SPARROW_VERTICAL];
 378       int hline = p->lines[SPARROW_HORIZONTAL];
 379       if (vline == BAD_PIXEL || hline == BAD_PIXEL){
 380         GST_DEBUG("ignoring bad pixel %d, %d\n", x, y);
 381         continue;
 382       }
 383       sparrow_cluster_t *cluster = &clusters[hline * fl->n_vlines + vline];
 384       sparrow_voter_t *voters = cluster->voters;
 385       int n = cluster->n;
 386       guint signal = (vsig * hsig) / SIGNAL_QUANT;
 387       GST_DEBUG("signal at %p (%d, %d): %dv %dh, product %u, lines: %dv %dh\n"
 388           "cluster is %p, n is %d\n", p, x, y,
 389           vsig, hsig, signal, vline, hline, cluster, n);
 390       if (signal == 0){
 391         GST_WARNING("signal at %p (%d, %d) is %d following quantisation!\n",
 392             p, x, y, signal);
 393       }
 394
 395       if (n < CLUSTER_SIZE){
 396         voters[n].x = x;
 397         voters[n].y = y;
 398         voters[n].signal = signal;
 399         cluster->n++;
 400       }
 401       else {
 402         /*duplicate x, y, signal, so they aren't mucked up */
 403         guint ts = signal;
 404         int tx = x;
 405         int ty = y;
 406         /*replaced one ends up here */
 407         int ts2;
 408         int tx2;
 409         int ty2;
 410         for (int j = 0; j < CLUSTER_SIZE; j++){
 411           if (voters[j].signal < ts){
 412             ts2 = voters[j].signal;
 413             tx2 = voters[j].x;
 414             ty2 = voters[j].y;
 415             voters[j].signal = ts;
 416             voters[j].x = tx;
 417             voters[j].y = ty;
 418             ts = ts2;
 419             tx = tx2;
 420             ty = ty2;
 421           }
 422         }
 423         GST_DEBUG("more than %d pixels at cluster for corner %d, %d."
 424             "Dropped %u for %u\n",
 425             CLUSTER_SIZE, vline, hline, ts2, signal);
 426       }
 427     }
 428   }
 429   if (sparrow->debug){
 430     debug_clusters(sparrow, fl);
 431   }
 432 }
 433
 434
 435 static inline void
 436 drop_cluster_voter(sparrow_cluster_t *cluster, int n)
 437 {
 438   if (n < cluster->n){
 439     for (int i = n; i < cluster->n - 1; i++){
 440       cluster->voters[i] = cluster->voters[i + 1];
 441     }
 442     cluster->n--;
 443   }
 444 }
 445
 446 static inline int sort_median(int *a, guint n)
 447 {
 448   guint i, j;
 449   /*stupid sort, but n is very small*/
 450   for (i = 0; i <  n; i++){
 451     for (j = i + 1; j < n; j++){
 452       if (a[i] > a[j]){
 453         int tmp = a[j];
 454         a[j] = a[i];
 455         a[i] = tmp;
 456       }
 457     }
 458   }
 459   guint middle = n / 2;
 460   int answer = a[middle];
 461
 462   if ((n & 1) == 0){
 463     answer += a[middle - 1];
 464     answer /= 2;
 465   }
 466   return answer;
 467 }
 468
 469 #define EUCLIDEAN_D2(ax, ay, bx, by)((ax - bx) * (ax - bx) + (ay - by) * (ay - by))
 470 #define NEIGHBOURLY_THRESHOLD (LINE_PERIOD * 4)
 471
 472 static inline void
 473 neighbourly_discard_cluster_outliers(GstSparrow *sparrow, sparrow_cluster_t *cluster,
 474     sparrow_corner_t *neighbour)
 475 {
 476   /* assuming the output mesh entirely fits in the input window (which is
 477      required for sparrow to work) the neighbours should be at most
 478      LINE_PERIOD * input resolution / output resolution apart. But set the
 479      threshold higher, just in case. */
 480   const int threshold = NEIGHBOURLY_THRESHOLD * sparrow->in.height / sparrow->out.height;
 481   int i;
 482   int neighbour_d[CLUSTER_SIZE];
 483   int close = 0;
 484   for (i = 0; i < cluster->n; i++){
 485     int d = EUCLIDEAN_D2(neighbour->in_x, neighbour->in_y,
 486         cluster->voters[i].x, cluster->voters[i].y);
 487     int pass = d > threshold;
 488     neighbour_d[i] = pass;
 489     close += pass;
 490     GST_DEBUG("failing point %d, distance sq %d, threshold %d\n", i, d, threshold);
 491   }
 492   if (close > 1){
 493     for (i = 0; i < cluster->n; i++){
 494       if (! neighbour_d[i]){
 495         drop_cluster_voter(cluster, i);
 496       }
 497     }
 498   }
 499 }
 500
 501 static inline void
 502 median_discard_cluster_outliers(sparrow_cluster_t *cluster)
 503 {
 504   int xvals[CLUSTER_SIZE];
 505   int yvals[CLUSTER_SIZE];
 506   int i;
 507   for (i = 0; i < cluster->n; i++){
 508     /*XXX could sort here*/
 509     xvals[i] = cluster->voters[i].x;
 510     yvals[i] = cluster->voters[i].y;
 511   }
 512   const int xmed = sort_median(xvals, cluster->n);
 513   const int ymed = sort_median(yvals, cluster->n);
 514
 515   for (i = 0; i < cluster->n; i++){
 516     int dx = cluster->voters[i].x - xmed;
 517     int dy = cluster->voters[i].y - ymed;
 518     if (dx * dx + dy * dy > OUTLIER_THRESHOLD){
 519       drop_cluster_voter(cluster, i);
 520     }
 521   }
 522 }
 523
 524 /*create the mesh */
 525 static inline void
 526 make_corners(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 527   //DEBUG_FIND_LINES(fl);
 528   int width = fl->n_vlines;
 529   int height = fl->n_hlines;
 530   sparrow_cluster_t *clusters = fl->clusters;
 531   sparrow_corner_t *mesh = fl->mesh;
 532   int x, y, i;
 533
 534   i = 0;
 535   for (y = 0; y < height; y++){
 536     for (x = 0; x < width; x++, i++){
 537       sparrow_cluster_t *cluster = clusters + i;
 538       if (cluster->n == 0){
 539         continue;
 540       }
 541
 542       /*the good points should all be adjacent; distant ones are spurious, so
 543         are discarded.
 544
 545         This fails if all the cluster are way off. Obviously it would be good
 546         to include information about the grid in the decision, but that is not
 547         there yet. (needs iteration, really).
 548
 549       Here's a slight attempt:*/
 550 #if 0
 551       sparrow_corner_t *neighbour;
 552       if (x){
 553         neighbour = &mesh[i - 1];
 554         neighbourly_discard_cluster_outliers(sparrow, cluster, neighbour);
 555       }
 556       else if (y){
 557         neighbour = &mesh[i - width];
 558         neighbourly_discard_cluster_outliers(sparrow, cluster, neighbour);
 559       }
 560 #endif
 561       median_discard_cluster_outliers(cluster);
 562
 563       /* now find a weighted average position */
 564       /*64 bit to avoid overflow -- should probably just use floating point
 565         (or reduce signal)*/
 566       guint64 xsum, ysum;
 567       guint xmean, ymean;
 568       guint64 votes;
 569       int j;
 570       xsum = 0;
 571       ysum = 0;
 572       votes = 0;
 573       for (j = 0; j < cluster->n; j++){
 574         votes += cluster->voters[j].signal;
 575         ysum += cluster->voters[j].y * cluster->voters[j].signal;
 576         xsum += cluster->voters[j].x * cluster->voters[j].signal;
 577       }
 578       if (votes){
 579         xmean = (xsum << SPARROW_FIXED_POINT) / votes;
 580         ymean = (ysum << SPARROW_FIXED_POINT) / votes;
 581       }
 582       else {
 583         GST_WARNING("corner %d, %d voters, sum %d,%d, somehow has no votes\n",
 584             i, cluster->n, xsum, ysum);
 585       }
 586
 587       GST_DEBUG("corner %d: %d voters, %d votes, sum %d,%d, mean %d,%d\n",
 588           i, cluster->n, votes, xsum, ysum, xmean, ymean);
 589
 590       mesh[i].in_x = xmean;
 591       mesh[i].in_y = ymean;
 592       mesh[i].status = CORNER_EXACT;
 593       GST_DEBUG("found corner %d at (%3f, %3f)\n",
 594           i, FLOATXY(xmean), FLOATXY(ymean));
 595     }
 596   }
 597 }
 598
 599
 600 static inline void
 601 make_map(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 602   int i;
 603   int width = fl->n_vlines;
 604   int height = fl->n_hlines;
 605   sparrow_corner_t *mesh = fl->mesh;
 606   gint x, y;
 607
 608   DEBUG_FIND_LINES(fl);
 609   /* calculate deltas toward adjacent corners */
 610   /* try to extrapolate left and up, if possible, so need to go backwards. */
 611   i = width * height - 1;
 612   for (y = height - 1; y >= 0; y--){
 613     for (x = width - 1; x >= 0; x--, i--){
 614       sparrow_corner_t *corner = &mesh[i];
 615       /* calculate the delta to next corner. If this corner is on edge, delta is
 616        0 and next is this.*/
 617       sparrow_corner_t *right = (x == width - 1) ? corner : corner + 1;
 618       sparrow_corner_t *down =  (y == height - 1) ? corner : corner + width;
 619       GST_DEBUG("i %d xy %d,%d width %d. in_xy %d,%d; down in_xy %d,%d; right in_xy %d,%d\n",
 620           i, x, y, width, corner->in_x, corner->in_y, down->in_x,
 621           down->in_y, right->in_x,  right->in_y);
 622       if (corner->status != CORNER_UNUSED){
 623         corner->dxr = (right->in_x - corner->in_x);
 624         corner->dyr = (right->in_y - corner->in_y);
 625         corner->dxd = (down->in_x -  corner->in_x);
 626         corner->dyd = (down->in_y -  corner->in_y);
 627       }
 628       else {
 629           /*copy from both right and down, if they both exist. */
 630         struct {
 631           int dxr;
 632           int dyr;
 633           int dxd;
 634           int dyd;
 635         } dividends = {0, 0, 0, 0};
 636         struct {
 637           int r;
 638           int d;
 639         } divisors = {0, 0};
 640
 641         if (right != corner){
 642           if (right->dxr || right->dyr){
 643             dividends.dxr += right->dxr;
 644             dividends.dyr += right->dyr;
 645             divisors.r++;
 646           }
 647           if (right->dxd || right->dyd){
 648             dividends.dxd += right->dxd;
 649             dividends.dyd += right->dyd;
 650             divisors.d++;
 651           }
 652         }
 653         if (down != corner){
 654           if (down->dxr || down->dyr){
 655             dividends.dxr += down->dxr;
 656             dividends.dyr += down->dyr;
 657             divisors.r++;
 658           }
 659           if (down->dxd || down->dyd){
 660             dividends.dxd += down->dxd;
 661             dividends.dyd += down->dyd;
 662             divisors.d++;
 663           }
 664         }
 665         corner->dxr = divisors.r ? dividends.dxr / divisors.r : 0;
 666         corner->dyr = divisors.r ? dividends.dyr / divisors.r : 0;
 667         corner->dxd = divisors.d ? dividends.dxd / divisors.d : 0;
 668         corner->dyd = divisors.d ? dividends.dyd / divisors.d : 0;
 669
 670         /*now extrapolate position, preferably from both left and right */
 671         if (right == corner){
 672           if (down != corner){ /*use down only */
 673             corner->in_x = down->in_x - corner->dxd;
 674             corner->in_y = down->in_y - corner->dyd;
 675           }
 676           else {/*oh no*/
 677             GST_DEBUG("can't reconstruct corner %d, %d: no useable neighbours\n", x, y);
 678             /*it would be easy enough to look further, but hopefully of no
 679               practical use */
 680           }
 681         }
 682         else if (down == corner){ /*use right only */
 683           corner->in_x = right->in_x - corner->dxr;
 684           corner->in_y = right->in_y - corner->dyr;
 685         }
 686         else { /* use both */
 687           corner->in_x = right->in_x - corner->dxr;
 688           corner->in_y = right->in_y - corner->dyr;
 689           corner->in_x += down->in_x - corner->dxd;
 690           corner->in_y += down->in_y - corner->dyd;
 691           corner->in_x >>= 1;
 692           corner->in_y >>= 1;
 693         }
 694         corner->status = CORNER_PROJECTED;
 695       }
 696     }
 697   }
 698   /*now quantise delta values.  It would be wrong to do it earlier, when they
 699     are being used to calculate whole mesh jumps, but from now they are
 700     primarily going to used for pixel (mesh / LINE_PERIOD) jumps. To do this in
 701   corners_to_lut puts a whole lot of division in a tight loop.*/
 702   for (i = 0; i < width * height; i++){
 703     sparrow_corner_t *corner = &mesh[i];
 704     corner->dxr = QUANTISE_DELTA(corner->dxr);
 705     corner->dyr = QUANTISE_DELTA(corner->dyr);
 706     corner->dxd = QUANTISE_DELTA(corner->dxd);
 707     corner->dyd = QUANTISE_DELTA(corner->dyd);
 708   }
 709   DEBUG_FIND_LINES(fl);
 710   if (sparrow->debug){
 711     debug_corners_image(sparrow, fl);
 712   }
 713 }
 714
 715
 716
 717 static void
 718 look_for_line(GstSparrow *sparrow, guint8 *in, sparrow_find_lines_t *fl,
 719     sparrow_line_t *line){
 720   guint i;
 721   guint32 colour;
 722   guint32 cmask = sparrow->out.colours[sparrow->colour];
 723   int signal;
 724
 725   /* subtract background noise */
 726   fl->input->imageData = (char *)in;
 727   cvSub(fl->input, fl->threshold, fl->working, NULL);
 728   guint32 *in32 = (guint32 *)fl->working->imageData;
 729
 730   for (i = 0; i < sparrow->in.pixcount; i++){
 731     colour = in32[i] & cmask;
 732     signal = (((colour >> fl->shift1) +
 733             (colour >> fl->shift2))) & 0xff;
 734     if (signal){
 735       if (fl->map[i].lines[line->dir]){
 736         /*assume the pixel is on for everyone and will just confuse
 737           matters. ignore it.
 738           XXX maybe threshold, so if signal is hugely bigger in one, don't ban it.
 739         */
 740         if (fl->map[i].lines[line->dir] != BAD_PIXEL){
 741           GST_DEBUG("HEY, expected point %d to be in line %d (dir %d) "
 742               "and thus empty, but it is also in line %d\n"
 743               "old signal %d, new signal %d, marking as BAD\n",
 744               i, line->index, line->dir, fl->map[i].lines[line->dir],
 745               fl->map[i].signal[line->dir], signal);
 746           fl->map[i].lines[line->dir] = BAD_PIXEL;
 747           fl->map[i].signal[line->dir] = 0;
 748         }
 749       }
 750       else{
 751         fl->map[i].lines[line->dir] = line->index;
 752         fl->map[i].signal[line->dir] = signal;
 753       }
 754     }
 755   }
 756 }
 757
 758 static void
 759 debug_map_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 760   guint32 *data = (guint32*)fl->debug->imageData;
 761   memset(data, 0, sparrow->in.size);
 762   for (guint i = 0; i < sparrow->in.pixcount; i++){
 763     data[i] |= fl->map[i].signal[SPARROW_HORIZONTAL] << sparrow->in.gshift;
 764     data[i] |= fl->map[i].signal[SPARROW_VERTICAL] << sparrow->in.rshift;
 765     data[i] |= ((fl->map[i].lines[SPARROW_VERTICAL] == BAD_PIXEL) ||
 766         (fl->map[i].lines[SPARROW_HORIZONTAL] == BAD_PIXEL)) ? 255 << sparrow->in.bshift : 0;
 767   }
 768   MAYBE_DEBUG_IPL(fl->debug);
 769 }
 770
 771 /* draw the line (in sparrow->colour) */
 772 static inline void
 773 draw_line(GstSparrow * sparrow, sparrow_line_t *line, guint8 *out){
 774   guint32 *p = (guint32 *)out;
 775   guint32 colour = sparrow->out.colours[sparrow->colour];
 776   int i;
 777   if (line->dir == SPARROW_HORIZONTAL){
 778     p += line->offset * sparrow->out.width;
 779     for (i = 0; i < sparrow->out.width; i++){
 780       p[i] = colour;
 781     }
 782   }
 783   else {
 784     guint32 *p = (guint32 *)out;
 785     p += line->offset;
 786     for(i = 0; i < sparrow->out.height; i++){
 787       *p = colour;
 788       p += sparrow->out.width;
 789     }
 790   }
 791 }
 792
 793 static void
 794 jump_state(GstSparrow *sparrow, sparrow_find_lines_t *fl, edges_state_t state){
 795   if (state == EDGES_NEXT_STATE){
 796     fl->state++;
 797   }
 798   else {
 799     fl->state = state;
 800   }
 801   switch (fl->state){
 802   case EDGES_FIND_NOISE:
 803     sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
 804     break;
 805   case EDGES_FIND_LINES:
 806     sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
 807     break;
 808   case EDGES_FIND_CORNERS:
 809     sparrow->countdown = 4;
 810     break;
 811   default:
 812     GST_DEBUG("jumped to non-existent state %d\n", fl->state);
 813     break;
 814   }
 815 }
 816
 817 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
 818    until last one.
 819  */
 820 static inline void
 821 draw_lines(GstSparrow *sparrow, sparrow_find_lines_t *fl, guint8 *in, guint8 *out)
 822 {
 823   sparrow_line_t *line = fl->shuffled_lines[fl->current];
 824   sparrow->countdown--;
 825   memset(out, 0, sparrow->out.size);
 826   if (sparrow->countdown){
 827     draw_line(sparrow, line, out);
 828   }
 829   else{
 830     /*show nothing, look for result */
 831     look_for_line(sparrow, in, fl, line);
 832     if (sparrow->debug){
 833       debug_map_image(sparrow, fl);
 834     }
 835     fl->current++;
 836     if (fl->current == fl->n_lines){
 837       jump_state(sparrow, fl, EDGES_NEXT_STATE);
 838     }
 839     else{
 840       sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
 841     }
 842   }
 843 }
 844
 845 #define LINE_THRESHOLD 32
 846
 847 static inline void
 848 find_threshold(GstSparrow *sparrow, sparrow_find_lines_t *fl, guint8 *in, guint8 *out)
 849 {
 850   memset(out, 0, sparrow->out.size);
 851   /*XXX should average/median over a range of frames */
 852   if (sparrow->countdown == 0){
 853     memcpy(fl->threshold->imageData, in, sparrow->in.size);
 854     /*add a constant, and smooth */
 855     cvAddS(fl->threshold, cvScalarAll(LINE_THRESHOLD), fl->working, NULL);
 856     cvSmooth(fl->working, fl->threshold, CV_GAUSSIAN, 3, 0, 0, 0);
 857     //cvSmooth(fl->working, fl->threshold, CV_MEDIAN, 3, 0, 0, 0);
 858     jump_state(sparrow, fl, EDGES_NEXT_STATE);
 859   }
 860   sparrow->countdown--;
 861 }
 862
 863 /*match up lines and find corners */
 864 static inline int
 865 find_corners(GstSparrow *sparrow, sparrow_find_lines_t *fl)
 866 {
 867   sparrow->countdown--;
 868   switch(sparrow->countdown){
 869   case 3:
 870     make_clusters(sparrow, fl);
 871     break;
 872   case 2:
 873     make_corners(sparrow, fl);
 874     break;
 875   case 1:
 876     make_map(sparrow, fl);
 877     break;
 878   case 0:
 879 #if USE_FULL_LUT
 880     corners_to_full_lut(sparrow, fl);
 881 #else
 882     corners_to_lut(sparrow, fl);
 883 #endif
 884     break;
 885   default:
 886     GST_DEBUG("how did sparrow->countdown get to be %d?", sparrow->countdown);
 887     sparrow->countdown = 4;
 888   }
 889   return sparrow->countdown;
 890 }
 891
 892
 893 INVISIBLE sparrow_state
 894 mode_find_edges(GstSparrow *sparrow, guint8 *in, guint8 *out){
 895   sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
 896   switch (fl->state){
 897   case EDGES_FIND_NOISE:
 898     find_threshold(sparrow, fl, in, out);
 899     break;
 900   case EDGES_FIND_LINES:
 901     draw_lines(sparrow, fl, in, out);
 902     break;
 903   case EDGES_FIND_CORNERS:
 904     if (find_corners(sparrow, fl))
 905       break;
 906     return SPARROW_NEXT_STATE;
 907   case EDGES_NEXT_STATE:
 908     break; /*shush gcc */
 909   }
 910   return SPARROW_STATUS_QUO;
 911 }
 912
 913 INVISIBLE void
 914 finalise_find_edges(GstSparrow *sparrow){
 915   sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
 916   //DEBUG_FIND_LINES(fl);
 917   if (sparrow->save && *(sparrow->save)){
 918     GST_DEBUG("about to save to %s\n", sparrow->save);
 919     dump_edges_info(sparrow, fl, sparrow->save);
 920   }
 921   if (sparrow->debug){
 922     cvReleaseImage(&fl->debug);
 923   }
 924   free(fl->h_lines);
 925   free(fl->shuffled_lines);
 926   free(fl->map);
 927   free(fl->mesh);
 928   free(fl->clusters);
 929   cvReleaseImage(&fl->threshold);
 930   cvReleaseImage(&fl->working);
 931   cvReleaseImageHeader(&fl->input);
 932   free(fl);
 933   GST_DEBUG("freed everything\n");
 934   sparrow->helper_struct = NULL;
 935 }
 936
 937 /*reduce the signal a little bit more, avoiding overflow later */
 938 #define COLOUR_QUANT  1
 939
 940 static void
 941 setup_colour_shifts(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 942   switch (sparrow->colour){
 943   case SPARROW_WHITE:
 944   case SPARROW_GREEN:
 945     fl->shift1 = sparrow->in.gshift + COLOUR_QUANT;
 946     fl->shift2 = sparrow->in.gshift + COLOUR_QUANT;
 947     break;
 948   case SPARROW_MAGENTA:
 949     fl->shift1 = sparrow->in.rshift + COLOUR_QUANT;
 950     fl->shift2 = sparrow->in.bshift + COLOUR_QUANT;
 951     break;
 952   }
 953 }
 954
 955 INVISIBLE void
 956 init_find_edges(GstSparrow *sparrow){
 957   gint i;
 958   sparrow_find_lines_t *fl = zalloc_aligned_or_die(sizeof(sparrow_find_lines_t));
 959   sparrow->helper_struct = (void *)fl;
 960
 961   gint h_lines = (sparrow->out.height + LINE_PERIOD - 1) / LINE_PERIOD;
 962   gint v_lines = (sparrow->out.width + LINE_PERIOD - 1) / LINE_PERIOD;
 963   gint n_lines_max = (h_lines + v_lines);
 964   gint n_corners = (h_lines * v_lines);
 965   fl->n_hlines = h_lines;
 966   fl->n_vlines = v_lines;
 967
 968
 969   fl->h_lines = malloc_aligned_or_die(sizeof(sparrow_line_t) * n_lines_max);
 970   fl->shuffled_lines = malloc_aligned_or_die(sizeof(sparrow_line_t *) * n_lines_max);
 971   GST_DEBUG("shuffled lines, malloced %p\n", fl->shuffled_lines);
 972
 973   GST_DEBUG("map is going to be %d * %d \n", sizeof(sparrow_intersect_t), sparrow->in.pixcount);
 974   fl->map = zalloc_aligned_or_die(sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
 975   fl->clusters = zalloc_or_die(n_corners * sizeof(sparrow_cluster_t));
 976   fl->mesh = zalloc_aligned_or_die(n_corners * sizeof(sparrow_corner_t));
 977
 978   sparrow_line_t *line = fl->h_lines;
 979   sparrow_line_t **sline = fl->shuffled_lines;
 980   int offset;
 981
 982   for (i = 0, offset = H_LINE_OFFSET; offset < sparrow->out.height;
 983        i++, offset += LINE_PERIOD){
 984     line->offset = offset;
 985     line->dir = SPARROW_HORIZONTAL;
 986     line->index = i;
 987     *sline = line;
 988     line++;
 989     sline++;
 990     //GST_DEBUG("line %d h has offset %d\n", i, offset);
 991   }
 992
 993   /*now add the vertical lines */
 994   fl->v_lines = line;
 995   for (i = 0, offset = V_LINE_OFFSET; offset < sparrow->out.width;
 996        i++, offset += LINE_PERIOD){
 997     line->offset = offset;
 998     line->dir = SPARROW_VERTICAL;
 999     line->index = i;
1000     *sline = line;
1001     line++;
1002     sline++;
1003     //GST_DEBUG("line %d v has offset %d\n", i, offset);
1004   }
1005   //DEBUG_FIND_LINES(fl);
1006   fl->n_lines = line - fl->h_lines;
1007   GST_DEBUG("allocated %d lines, made %d\n", n_lines_max, fl->n_lines);
1008
1009   /*now shuffle */
1010   for (i = 0; i < fl->n_lines; i++){
1011     int j = RANDINT(sparrow, 0, fl->n_lines);
1012     sparrow_line_t *tmp = fl->shuffled_lines[j];
1013     fl->shuffled_lines[j] = fl->shuffled_lines[i];
1014     fl->shuffled_lines[i] = tmp;
1015   }
1016
1017   setup_colour_shifts(sparrow, fl);
1018
1019   if (sparrow->reload){
1020     if (access(sparrow->reload, R_OK)){
1021       GST_DEBUG("sparrow>reload is '%s' and it is UNREADABLE\n", sparrow->reload);
1022       exit(1);
1023     }
1024     read_edges_info(sparrow, fl, sparrow->reload);
1025     memset(fl->map, 0, sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
1026     //memset(fl->clusters, 0, n_corners * sizeof(sparrow_cluster_t));
1027     memset(fl->mesh, 0, n_corners * sizeof(sparrow_corner_t));
1028     jump_state(sparrow, fl, EDGES_FIND_CORNERS);
1029   }
1030   else {
1031     jump_state(sparrow, fl, EDGES_FIND_NOISE);
1032   }
1033   /* opencv images for threshold finding */
1034   CvSize size = {sparrow->in.width, sparrow->in.height};
1035   fl->working = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
1036   fl->threshold = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
1037
1038   /*input has no data allocated -- it uses latest frame*/
1039   fl->input = init_ipl_image(&sparrow->in, PIXSIZE);
1040
1041   //DEBUG_FIND_LINES(fl);
1042   if (sparrow->debug){
1043     fl->debug = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
1044   }
1045 }
1046