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