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 static void
  71 debug_map_lut(GstSparrow *sparrow, sparrow_find_lines_t *fl){
  72   sparrow_map_lut_t *map_lut = sparrow->map_lut;
  73   if (sparrow->debug){
  74     debug_frame(sparrow, (guint8*)map_lut, sparrow->out.width, sparrow->out.height, PIXSIZE);
  75   }
  76 }
  77
  78
  79 static void
  80 corners_to_full_lut(GstSparrow *sparrow, sparrow_find_lines_t *fl){
  81   DEBUG_FIND_LINES(fl);
  82   sparrow_corner_t *mesh = fl->mesh;   /*maps regular points in ->out to points in ->in */
  83   sparrow_map_lut_t *map_lut = sparrow->map_lut;
  84   int mesh_w = fl->n_vlines;
  85   int mesh_h = fl->n_hlines;
  86   int mcy, mmy, mcx, mmx; /*Mesh Corner|Modulus X|Y*/
  87   int y = H_LINE_OFFSET;
  88   sparrow_corner_t *mesh_row = mesh;
  89   int max_x = sparrow->in.width - 1;
  90   int max_y = sparrow->in.height - 1;
  91
  92   for(mcy = 0; mcy < mesh_h - 1; mcy++){
  93     for (mmy = 0; mmy < LINE_PERIOD; mmy++, y++){
  94       sparrow_corner_t *mesh_square = mesh_row;
  95       int i = y * sparrow->out.width + V_LINE_OFFSET;
  96       for(mcx = 0; mcx < mesh_w - 1; mcx++){
  97         int iy = mesh_square->y + mmy * mesh_square->dyd;
  98         int ix = mesh_square->x + mmy * mesh_square->dxd;
  99         for (mmx = 0; mmx < LINE_PERIOD; mmx++, i++){
 100           int ixx = CLAMP(ix >> SPARROW_FIXED_POINT, 0, max_x);
 101           int iyy = CLAMP(iy >> SPARROW_FIXED_POINT, 0, max_y);
 102           if(sparrow->screenmask[iyy * sparrow->in.width + ixx]){
 103             map_lut[i].x = ixx;
 104             map_lut[i].y = iyy;
 105           }
 106           ix += mesh_square->dxr;
 107           iy += mesh_square->dyr;
 108         }
 109         mesh_square++;
 110       }
 111     }
 112     mesh_row += mesh_w;
 113   }
 114   sparrow->map_lut = map_lut;
 115   debug_map_lut(sparrow, fl);
 116 }
 117
 118 #define INTXY(x)((x) / (1 << SPARROW_FIXED_POINT))
 119 #define FLOATXY(x)(((double)(x)) / (1 << SPARROW_FIXED_POINT))
 120
 121 static inline int
 122 clamp_intxy(int x, const int max_plus_one){
 123   if (x < 0)
 124     return 0;
 125   x >>= SPARROW_FIXED_POINT;
 126   if (x >= max_plus_one)
 127     return max_plus_one - 1;
 128   return x;
 129 }
 130
 131 static void
 132 debug_corners_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 133   sparrow_corner_t *mesh = fl->mesh;
 134   guint32 *data = (guint32*)fl->debug->imageData;
 135   guint w = fl->debug->width;
 136   guint h = fl->debug->height;
 137   memset(data, 0, sparrow->in.size);
 138   guint32 colours[4] = {0xff0000ff, 0x00ff0000, 0x0000ff00, 0xcccccccc};
 139   for (int i = 0; i < fl->n_vlines * fl->n_hlines; i++){
 140     sparrow_corner_t *c = &mesh[i];
 141     int x = c->x;
 142     int y = c->y;
 143     /*
 144     GST_DEBUG("i %d status %d x: %f, y: %f  dxr %f dyr %f dxd %f dyd %f\n"
 145         "int x, y %d,%d (raw %d,%d) data %p\n",
 146         i, c->status, FLOATXY(x), FLOATXY(y),
 147         FLOATXY(c->dxr), FLOATXY(c->dyr), FLOATXY(c->dxd), FLOATXY(c->dyd),
 148         INTXY(x), INTXY(y), x, y, data);
 149     */
 150     int txr = x;
 151     int txd = x;
 152     int tyr = y;
 153     int tyd = y;
 154     for (int j = 1; j < LINE_PERIOD; j+= 2){
 155       txr += c->dxr * 2;
 156       txd += c->dxd * 2;
 157       tyr += c->dyr * 2;
 158       tyd += c->dyd * 2;
 159       guint hl = clamp_intxy(tyr, h) * w + clamp_intxy(txr, w);
 160       if (hl < sparrow->in.pixcount){
 161         data[hl] = 0x88000088;
 162       }
 163       else{
 164         GST_WARNING("overflow in debug_corners: hl %d, txr %d tyr %d",
 165             hl, txr, tyr);
 166       }
 167       guint vl = clamp_intxy(tyd, h) * w + clamp_intxy(txd, w);
 168       if (vl < sparrow->in.pixcount){
 169         data[vl] = 0x00663300;
 170       }
 171       else{
 172         GST_WARNING("overflow in debug_corners: vl %d, txd %d tyd %d",
 173             vl, txd, tyd);
 174       }
 175     }
 176     data[clamp_intxy(y, h) * w + clamp_intxy(x, w)] = colours[c->status];
 177   }
 178   MAYBE_DEBUG_IPL(fl->debug);
 179 }
 180
 181
 182 static void
 183 debug_clusters(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 184   guint32 *data = (guint32*)fl->debug->imageData;
 185   memset(data, 0, sparrow->in.size);
 186   int width = fl->n_vlines;
 187   int height = fl->n_hlines;
 188   sparrow_cluster_t *clusters = fl->clusters;
 189   int i, j;
 190   guint32 colour;
 191   guint32 colours[4] = {0xff0000ff, 0x0000ff00, 0x00ff0000,
 192                        0x00ff00ff};
 193   for (i = 0; i < width * height; i++){
 194     colour = colours[i % 5];
 195     sparrow_voter_t *v = clusters[i].voters;
 196     for (j = 0; j < clusters[i].n; j++){
 197       data[v[j].y * sparrow->in.width +
 198           v[j].x] = (colour * (v[j].signal / 2)) / 256;
 199     }
 200   }
 201   MAYBE_DEBUG_IPL(fl->debug);
 202 }
 203
 204
 205 #define SIGNAL_QUANT 1
 206
 207 /*maximum number of pixels in a cluster */
 208 #define CLUSTER_SIZE 8
 209
 210
 211 /*find map points with common intersection data, and collect them into clusters */
 212 static void
 213 make_clusters(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 214   sparrow_cluster_t *clusters = fl->clusters;
 215   int x, y;
 216   /*special case: spurious values collect up at 0,0 */
 217   fl->map[0].signal[SPARROW_VERTICAL] = 0;
 218   fl->map[0].signal[SPARROW_HORIZONTAL] = 0;
 219   /*each point in fl->map is in a vertical line, a horizontal line, both, or
 220     neither.  Only the "both" case matters. */
 221   for (y = 0; y < sparrow->in.height; y++){
 222     for (x = 0; x < sparrow->in.width; x++){
 223       sparrow_intersect_t *p = &fl->map[y * sparrow->in.width + x];
 224       guint vsig = p->signal[SPARROW_VERTICAL];
 225       guint hsig = p->signal[SPARROW_HORIZONTAL];
 226       /*remembering that 0 is valid as a line number, but not as a signal */
 227       if (! (vsig && hsig)){
 228         continue;
 229       }
 230       /*This one is lobbying for the position of a corner.*/
 231       int vline = p->lines[SPARROW_VERTICAL];
 232       int hline = p->lines[SPARROW_HORIZONTAL];
 233       if (vline == BAD_PIXEL || hline == BAD_PIXEL){
 234         GST_DEBUG("ignoring bad pixel %d, %d\n", x, y);
 235         continue;
 236       }
 237       sparrow_cluster_t *cluster = &clusters[hline * fl->n_vlines + vline];
 238       sparrow_voter_t *voters = cluster->voters;
 239       int n = cluster->n;
 240       guint signal = (vsig * hsig) / SIGNAL_QUANT;
 241       GST_DEBUG("signal at %p (%d, %d): %dv %dh, product %u, lines: %dv %dh\n"
 242           "cluster is %p, n is %d\n", p, x, y,
 243           vsig, hsig, signal, vline, hline, cluster, n);
 244       if (signal == 0){
 245         GST_WARNING("signal at %p (%d, %d) is %d following quantisation!\n",
 246             p, x, y, signal);
 247       }
 248
 249       if (n < CLUSTER_SIZE){
 250         voters[n].x = x;
 251         voters[n].y = y;
 252         voters[n].signal = signal;
 253         cluster->n++;
 254       }
 255       else {
 256         /*duplicate x, y, signal, so they aren't mucked up */
 257         guint ts = signal;
 258         int tx = x;
 259         int ty = y;
 260         /*replaced one ends up here */
 261         int ts2;
 262         int tx2;
 263         int ty2;
 264         for (int j = 0; j < CLUSTER_SIZE; j++){
 265           if (voters[j].signal < ts){
 266             ts2 = voters[j].signal;
 267             tx2 = voters[j].x;
 268             ty2 = voters[j].y;
 269             voters[j].signal = ts;
 270             voters[j].x = tx;
 271             voters[j].y = ty;
 272             ts = ts2;
 273             tx = tx2;
 274             ty = ty2;
 275           }
 276         }
 277         GST_DEBUG("more than %d pixels at cluster for corner %d, %d."
 278             "Dropped %u for %u\n",
 279             CLUSTER_SIZE, vline, hline, ts2, signal);
 280       }
 281     }
 282   }
 283   if (sparrow->debug){
 284     debug_clusters(sparrow, fl);
 285   }
 286 }
 287
 288
 289 static inline void
 290 drop_cluster_voter(sparrow_cluster_t *cluster, int n)
 291 {
 292   if (n < cluster->n){
 293     for (int i = n; i < cluster->n - 1; i++){
 294       cluster->voters[i] = cluster->voters[i + 1];
 295     }
 296     cluster->n--;
 297   }
 298 }
 299
 300 static inline int sort_median(int *a, guint n)
 301 {
 302   guint i, j;
 303   /*stupid sort, but n is very small*/
 304   for (i = 0; i <  n; i++){
 305     for (j = i + 1; j < n; j++){
 306       if (a[i] > a[j]){
 307         int tmp = a[j];
 308         a[j] = a[i];
 309         a[i] = tmp;
 310       }
 311     }
 312   }
 313   guint middle = n / 2;
 314   int answer = a[middle];
 315
 316   if ((n & 1) == 0){
 317     answer += a[middle - 1];
 318     answer /= 2;
 319   }
 320   return answer;
 321 }
 322
 323 #define EUCLIDEAN_D2(ax, ay, bx, by)((ax - bx) * (ax - bx) + (ay - by) * (ay - by))
 324 #define EUCLIDEAN_THRESHOLD 9
 325
 326 static inline void
 327 euclidean_discard_cluster_outliers(sparrow_cluster_t *cluster)
 328 {
 329   /* Calculate distance between each pair.  Discard points with maximum sum,
 330      then recalculate until all are within threshold.
 331  */
 332   int i, j;
 333   int dsums[CLUSTER_SIZE] = {0};
 334   for (i = 0; i < cluster->n; i++){
 335     for (j = i + 1; j < cluster->n; i++){
 336       int d = EUCLIDEAN_D2(cluster->voters[i].x, cluster->voters[i].y,
 337           cluster->voters[j].x, cluster->voters[j].y);
 338       dsums[i] += d;
 339       dsums[j] += d;
 340     }
 341   }
 342
 343   int worst_d, worst_i, threshold;
 344   do {
 345     threshold = EUCLIDEAN_THRESHOLD * cluster->n;
 346     worst_i = 0;
 347     worst_d = 0;
 348     for (i = 0; i < cluster->n; i++){
 349       if (dsums[i] > worst_d){
 350         worst_d = dsums[i];
 351         worst_i = i;
 352       }
 353     }
 354     if (worst_d > threshold){
 355       GST_DEBUG("failing point %d, distance sq %d, threshold %d\n",
 356           worst_i, worst_d, threshold);
 357       drop_cluster_voter(cluster, worst_i);
 358     }
 359   } while(worst_d > threshold && cluster->n);
 360 }
 361
 362 static inline void
 363 median_discard_cluster_outliers(sparrow_cluster_t *cluster)
 364 {
 365   int xvals[CLUSTER_SIZE];
 366   int yvals[CLUSTER_SIZE];
 367   int i;
 368   for (i = 0; i < cluster->n; i++){
 369     /*XXX could sort here*/
 370     xvals[i] = cluster->voters[i].x;
 371     yvals[i] = cluster->voters[i].y;
 372   }
 373   const int xmed = sort_median(xvals, cluster->n);
 374   const int ymed = sort_median(yvals, cluster->n);
 375
 376   for (i = 0; i < cluster->n; i++){
 377     int dx = cluster->voters[i].x - xmed;
 378     int dy = cluster->voters[i].y - ymed;
 379     if (dx * dx + dy * dy > OUTLIER_THRESHOLD){
 380       drop_cluster_voter(cluster, i);
 381     }
 382   }
 383 }
 384
 385 /*create the mesh */
 386 static inline void
 387 make_corners(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 388   //DEBUG_FIND_LINES(fl);
 389   int width = fl->n_vlines;
 390   int height = fl->n_hlines;
 391   sparrow_cluster_t *clusters = fl->clusters;
 392   sparrow_corner_t *mesh = fl->mesh;
 393   int x, y, i;
 394
 395   i = 0;
 396   for (y = 0; y < height; y++){
 397     for (x = 0; x < width; x++, i++){
 398       sparrow_cluster_t *cluster = clusters + i;
 399       if (cluster->n == 0){
 400         continue;
 401       }
 402 #if 0
 403       /*discard outliers based on sum of squared distances: good points should
 404         be in a cluster, and have lowest sum*/
 405       euclidean_discard_cluster_outliers(cluster);
 406 #else
 407       /*discard values away from median x, y values.
 408        (each dimension is calculated independently)*/
 409       median_discard_cluster_outliers(cluster);
 410 #endif
 411       /* now find a weighted average position */
 412       /*64 bit to avoid overflow -- should probably just use floating point
 413         (or reduce signal)*/
 414       guint64 xsum, ysum;
 415       guint xmean, ymean;
 416       guint64 votes;
 417       int j;
 418       xsum = 0;
 419       ysum = 0;
 420       votes = 0;
 421       for (j = 0; j < cluster->n; j++){
 422         votes += cluster->voters[j].signal;
 423         ysum += cluster->voters[j].y * cluster->voters[j].signal;
 424         xsum += cluster->voters[j].x * cluster->voters[j].signal;
 425       }
 426       if (votes){
 427         xmean = (xsum << SPARROW_FIXED_POINT) / votes;
 428         ymean = (ysum << SPARROW_FIXED_POINT) / votes;
 429       }
 430       else {
 431         GST_WARNING("corner %d, %d voters, sum %d,%d, somehow has no votes\n",
 432             i, cluster->n, xsum, ysum);
 433       }
 434
 435       GST_DEBUG("corner %d: %d voters, %d votes, sum %d,%d, mean %d,%d\n",
 436           i, cluster->n, votes, xsum, ysum, xmean, ymean);
 437
 438       mesh[i].x = xmean;
 439       mesh[i].y = ymean;
 440       mesh[i].status = CORNER_EXACT;
 441       GST_DEBUG("found corner %d at (%3f, %3f)\n",
 442           i, FLOATXY(xmean), FLOATXY(ymean));
 443     }
 444   }
 445 }
 446
 447
 448 static inline void
 449 make_map(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 450   int i;
 451   int width = fl->n_vlines;
 452   int height = fl->n_hlines;
 453   sparrow_corner_t *mesh = fl->mesh;
 454   gint x, y;
 455
 456   DEBUG_FIND_LINES(fl);
 457   /* calculate deltas toward adjacent corners */
 458   /* try to extrapolate left and up, if possible, so need to go backwards. */
 459   i = width * height - 1;
 460   for (y = height - 1; y >= 0; y--){
 461     for (x = width - 1; x >= 0; x--, i--){
 462       sparrow_corner_t *corner = &mesh[i];
 463       /* calculate the delta to next corner. If this corner is on edge, delta is
 464        0 and next is this.*/
 465       sparrow_corner_t *right = (x == width - 1) ? corner : corner + 1;
 466       sparrow_corner_t *down =  (y == height - 1) ? corner : corner + width;
 467       GST_DEBUG("i %d xy %d,%d width %d. in_xy %d,%d; down in_xy %d,%d; right in_xy %d,%d\n",
 468           i, x, y, width, corner->in_x, corner->in_y, down->in_x,
 469           down->in_y, right->in_x,  right->in_y);
 470       if (corner->status != CORNER_UNUSED){
 471         corner->dxr = (right->in_x - corner->in_x);
 472         corner->dyr = (right->in_y - corner->in_y);
 473         corner->dxd = (down->in_x -  corner->in_x);
 474         corner->dyd = (down->in_y -  corner->in_y);
 475       }
 476       else {
 477           /*copy from both right and down, if they both exist. */
 478         struct {
 479           int dxr;
 480           int dyr;
 481           int dxd;
 482           int dyd;
 483         } dividends = {0, 0, 0, 0};
 484         struct {
 485           int r;
 486           int d;
 487         } divisors = {0, 0};
 488
 489         if (right != corner){
 490           if (right->dxr || right->dyr){
 491             dividends.dxr += right->dxr;
 492             dividends.dyr += right->dyr;
 493             divisors.r++;
 494           }
 495           if (right->dxd || right->dyd){
 496             dividends.dxd += right->dxd;
 497             dividends.dyd += right->dyd;
 498             divisors.d++;
 499           }
 500         }
 501         if (down != corner){
 502           if (down->dxr || down->dyr){
 503             dividends.dxr += down->dxr;
 504             dividends.dyr += down->dyr;
 505             divisors.r++;
 506           }
 507           if (down->dxd || down->dyd){
 508             dividends.dxd += down->dxd;
 509             dividends.dyd += down->dyd;
 510             divisors.d++;
 511           }
 512         }
 513         corner->dxr = divisors.r ? dividends.dxr / divisors.r : 0;
 514         corner->dyr = divisors.r ? dividends.dyr / divisors.r : 0;
 515         corner->dxd = divisors.d ? dividends.dxd / divisors.d : 0;
 516         corner->dyd = divisors.d ? dividends.dyd / divisors.d : 0;
 517
 518         /*now extrapolate position, preferably from both left and right */
 519         if (right == corner){
 520           if (down != corner){ /*use down only */
 521             corner->in_x = down->in_x - corner->dxd;
 522             corner->in_y = down->in_y - corner->dyd;
 523           }
 524           else {/*oh no*/
 525             GST_DEBUG("can't reconstruct corner %d, %d: no useable neighbours\n", x, y);
 526             /*it would be easy enough to look further, but hopefully of no
 527               practical use */
 528           }
 529         }
 530         else if (down == corner){ /*use right only */
 531           corner->in_x = right->in_x - corner->dxr;
 532           corner->in_y = right->in_y - corner->dyr;
 533         }
 534         else { /* use both */
 535           corner->in_x = right->in_x - corner->dxr;
 536           corner->in_y = right->in_y - corner->dyr;
 537           corner->in_x += down->in_x - corner->dxd;
 538           corner->in_y += down->in_y - corner->dyd;
 539           corner->in_x >>= 1;
 540           corner->in_y >>= 1;
 541         }
 542         corner->status = CORNER_PROJECTED;
 543       }
 544     }
 545   }
 546   /*now quantise delta values.  It would be wrong to do it earlier, when they
 547     are being used to calculate whole mesh jumps, but from now they are
 548     primarily going to used for pixel (mesh / LINE_PERIOD) jumps. To do this in
 549   corners_to_lut puts a whole lot of division in a tight loop.*/
 550   for (i = 0; i < width * height; i++){
 551     sparrow_corner_t *corner = &mesh[i];
 552     corner->dxr = QUANTISE_DELTA(corner->dxr);
 553     corner->dyr = QUANTISE_DELTA(corner->dyr);
 554     corner->dxd = QUANTISE_DELTA(corner->dxd);
 555     corner->dyd = QUANTISE_DELTA(corner->dyd);
 556   }
 557   DEBUG_FIND_LINES(fl);
 558   if (sparrow->debug){
 559     debug_corners_image(sparrow, fl);
 560   }
 561 }
 562
 563
 564
 565 static void
 566 look_for_line(GstSparrow *sparrow, guint8 *in, sparrow_find_lines_t *fl,
 567     sparrow_line_t *line){
 568   guint i;
 569   guint32 colour;
 570   guint32 cmask = sparrow->out.colours[sparrow->colour];
 571   int signal;
 572
 573   /* subtract background noise */
 574   fl->input->imageData = (char *)in;
 575   cvSub(fl->input, fl->threshold, fl->working, NULL);
 576   guint32 *in32 = (guint32 *)fl->working->imageData;
 577
 578   for (i = 0; i < sparrow->in.pixcount; i++){
 579     colour = in32[i] & cmask;
 580     signal = (((colour >> fl->shift1) & COLOUR_MASK) +
 581             ((colour >> fl->shift2) & COLOUR_MASK));
 582     if (signal){
 583       if (fl->map[i].lines[line->dir]){
 584         /*assume the pixel is on for everyone and will just confuse
 585           matters. ignore it.
 586         */
 587
 588         if (fl->map[i].lines[line->dir] != BAD_PIXEL){
 589           /*
 590           GST_DEBUG("HEY, expected point %d to be in line %d (dir %d) "
 591               "and thus empty, but it is also in line %d\n"
 592               "old signal %d, new signal %d, marking as BAD\n",
 593               i, line->index, line->dir, fl->map[i].lines[line->dir],
 594               fl->map[i].signal[line->dir], signal);
 595           */
 596           fl->map[i].lines[line->dir] = BAD_PIXEL;
 597           fl->map[i].signal[line->dir] = 0;
 598         }
 599       }
 600       else{
 601         fl->map[i].lines[line->dir] = line->index;
 602         fl->map[i].signal[line->dir] = signal;
 603       }
 604     }
 605   }
 606 }
 607
 608 static void
 609 debug_map_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 610   guint32 *data = (guint32*)fl->debug->imageData;
 611   memset(data, 0, sparrow->in.size);
 612   for (guint i = 0; i < sparrow->in.pixcount; i++){
 613     data[i] |= fl->map[i].signal[SPARROW_HORIZONTAL] << sparrow->in.gshift;
 614     data[i] |= fl->map[i].signal[SPARROW_VERTICAL] << sparrow->in.rshift;
 615     data[i] |= ((fl->map[i].lines[SPARROW_VERTICAL] == BAD_PIXEL) ||
 616         (fl->map[i].lines[SPARROW_HORIZONTAL] == BAD_PIXEL)) ? 255 << sparrow->in.bshift : 0;
 617   }
 618   MAYBE_DEBUG_IPL(fl->debug);
 619 }
 620
 621 /* draw the line (in sparrow->colour) */
 622 static inline void
 623 draw_line(GstSparrow * sparrow, sparrow_line_t *line, guint8 *out){
 624   guint32 *p = (guint32 *)out;
 625   guint32 colour = sparrow->out.colours[sparrow->colour];
 626   int i;
 627   if (line->dir == SPARROW_HORIZONTAL){
 628     p += line->offset * sparrow->out.width;
 629     for (i = 0; i < sparrow->out.width; i++){
 630       p[i] = colour;
 631     }
 632   }
 633   else {
 634     guint32 *p = (guint32 *)out;
 635     p += line->offset;
 636     for(i = 0; i < sparrow->out.height; i++){
 637       *p = colour;
 638       p += sparrow->out.width;
 639     }
 640   }
 641 }
 642
 643 static void
 644 jump_state(GstSparrow *sparrow, sparrow_find_lines_t *fl, edges_state_t state){
 645   if (state == EDGES_NEXT_STATE){
 646     fl->state++;
 647   }
 648   else {
 649     fl->state = state;
 650   }
 651   switch (fl->state){
 652   case EDGES_FIND_NOISE:
 653     sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
 654     break;
 655   case EDGES_FIND_LINES:
 656     sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
 657     break;
 658   case EDGES_FIND_CORNERS:
 659     sparrow->countdown = 7;
 660     break;
 661   case EDGES_WAIT_FOR_PLAY:
 662     global_number_of_edge_finders--;
 663     sparrow->countdown = 300;
 664     break;
 665   default:
 666     GST_DEBUG("jumped to non-existent state %d\n", fl->state);
 667     break;
 668   }
 669 }
 670
 671 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
 672    until last one.
 673  */
 674 static inline void
 675 draw_lines(GstSparrow *sparrow, sparrow_find_lines_t *fl, guint8 *in, guint8 *out)
 676 {
 677   sparrow_line_t *line = fl->shuffled_lines[fl->current];
 678   sparrow->countdown--;
 679   memset(out, 0, sparrow->out.size);
 680   if (sparrow->countdown){
 681     draw_line(sparrow, line, out);
 682   }
 683   else{
 684     /*show nothing, look for result */
 685     look_for_line(sparrow, in, fl, line);
 686     if (sparrow->debug){
 687       debug_map_image(sparrow, fl);
 688     }
 689     fl->current++;
 690     if (fl->current == fl->n_lines){
 691       jump_state(sparrow, fl, EDGES_NEXT_STATE);
 692     }
 693     else{
 694       sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
 695     }
 696   }
 697 }
 698
 699 #define LINE_THRESHOLD 32
 700
 701 static inline void
 702 find_threshold(GstSparrow *sparrow, sparrow_find_lines_t *fl, guint8 *in, guint8 *out)
 703 {
 704   memset(out, 0, sparrow->out.size);
 705   /*XXX should average/median over a range of frames */
 706   if (sparrow->countdown == 0){
 707     memcpy(fl->threshold->imageData, in, sparrow->in.size);
 708     /*add a constant, and smooth */
 709     cvAddS(fl->threshold, cvScalarAll(LINE_THRESHOLD), fl->working, NULL);
 710     cvSmooth(fl->working, fl->threshold, CV_GAUSSIAN, 3, 0, 0, 0);
 711     //cvSmooth(fl->working, fl->threshold, CV_MEDIAN, 3, 0, 0, 0);
 712     jump_state(sparrow, fl, EDGES_NEXT_STATE);
 713   }
 714   sparrow->countdown--;
 715 }
 716
 717 /*match up lines and find corners */
 718 static inline int
 719 find_corners(GstSparrow *sparrow, sparrow_find_lines_t *fl)
 720 {
 721   sparrow->countdown--;
 722   switch(sparrow->countdown){
 723   case 4:
 724     make_clusters(sparrow, fl);
 725     break;
 726   case 3:
 727     make_corners(sparrow, fl);
 728     break;
 729   case 2:
 730     make_map(sparrow, fl);
 731     break;
 732   case 1:
 733     fix_map(sparrow, fl);
 734     break;
 735   case 0:
 736 #if USE_FULL_LUT
 737     corners_to_full_lut(sparrow, fl);
 738 #else
 739     corners_to_lut(sparrow, fl);
 740 #endif
 741     jump_state(sparrow, fl, EDGES_NEXT_STATE);
 742     break;
 743   default:
 744     GST_DEBUG("how did sparrow->countdown get to be %d?", sparrow->countdown);
 745     sparrow->countdown = 5;
 746   }
 747   return sparrow->countdown;
 748 }
 749
 750 /*use a dirty shared variable*/
 751 static gboolean
 752 wait_for_play(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 753   if (global_number_of_edge_finders == 0 ||
 754       sparrow->countdown == 0){
 755     return TRUE;
 756   }
 757   sparrow->countdown--;
 758   return FALSE;
 759 }
 760
 761 INVISIBLE sparrow_state
 762 mode_find_edges(GstSparrow *sparrow, guint8 *in, guint8 *out){
 763   sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
 764   switch (fl->state){
 765   case EDGES_FIND_NOISE:
 766     find_threshold(sparrow, fl, in, out);
 767     break;
 768   case EDGES_FIND_LINES:
 769     draw_lines(sparrow, fl, in, out);
 770     break;
 771   case EDGES_FIND_CORNERS:
 772     memset(out, 0, sparrow->out.size);
 773     find_corners(sparrow, fl);
 774     break;
 775   case EDGES_WAIT_FOR_PLAY:
 776     memset(out, 0, sparrow->out.size);
 777     if (wait_for_play(sparrow, fl)){
 778       return SPARROW_NEXT_STATE;
 779     }
 780     break;
 781   default:
 782     GST_WARNING("strange state in mode_find_edges: %d", fl->state);
 783     memset(out, 0, sparrow->out.size);
 784   }
 785   return SPARROW_STATUS_QUO;
 786 }
 787
 788 INVISIBLE void
 789 finalise_find_edges(GstSparrow *sparrow){
 790   sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
 791   //DEBUG_FIND_LINES(fl);
 792   if (sparrow->save && *(sparrow->save)){
 793     GST_DEBUG("about to save to %s\n", sparrow->save);
 794     dump_edges_info(sparrow, fl, sparrow->save);
 795   }
 796   if (sparrow->debug){
 797     cvReleaseImage(&fl->debug);
 798   }
 799   free(fl->h_lines);
 800   free(fl->shuffled_lines);
 801   free(fl->map);
 802   free(fl->mesh);
 803   free(fl->clusters);
 804   cvReleaseImage(&fl->threshold);
 805   cvReleaseImage(&fl->working);
 806   cvReleaseImageHeader(&fl->input);
 807   free(fl);
 808   GST_DEBUG("freed everything\n");
 809   sparrow->helper_struct = NULL;
 810 }
 811
 812 static void
 813 setup_colour_shifts(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 814   /*COLOUR_QUANT reduces the signal a little bit more, avoiding overflow
 815     later */
 816   switch (sparrow->colour){
 817   case SPARROW_WHITE:
 818   case SPARROW_GREEN:
 819     fl->shift1 = sparrow->in.gshift + COLOUR_QUANT;
 820     fl->shift2 = sparrow->in.gshift + COLOUR_QUANT;
 821     break;
 822   case SPARROW_MAGENTA:
 823     fl->shift1 = sparrow->in.rshift + COLOUR_QUANT;
 824     fl->shift2 = sparrow->in.bshift + COLOUR_QUANT;
 825     break;
 826   }
 827 }
 828
 829 INVISIBLE void
 830 init_find_edges(GstSparrow *sparrow){
 831   gint i;
 832   sparrow_find_lines_t *fl = zalloc_aligned_or_die(sizeof(sparrow_find_lines_t));
 833   sparrow->helper_struct = (void *)fl;
 834
 835   gint h_lines = (sparrow->out.height + LINE_PERIOD - 1) / LINE_PERIOD;
 836   gint v_lines = (sparrow->out.width + LINE_PERIOD - 1) / LINE_PERIOD;
 837   gint n_lines_max = (h_lines + v_lines);
 838   gint n_corners = (h_lines * v_lines);
 839   fl->n_hlines = h_lines;
 840   fl->n_vlines = v_lines;
 841
 842   fl->h_lines = malloc_aligned_or_die(sizeof(sparrow_line_t) * n_lines_max);
 843   fl->shuffled_lines = malloc_aligned_or_die(sizeof(sparrow_line_t *) * n_lines_max);
 844   GST_DEBUG("shuffled lines, malloced %p\n", fl->shuffled_lines);
 845
 846   GST_DEBUG("map is going to be %d * %d \n", sizeof(sparrow_intersect_t), sparrow->in.pixcount);
 847   fl->map = zalloc_aligned_or_die(sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
 848   fl->clusters = zalloc_or_die(n_corners * sizeof(sparrow_cluster_t));
 849   fl->mesh = zalloc_aligned_or_die(n_corners * sizeof(sparrow_corner_t));
 850
 851   sparrow_line_t *line = fl->h_lines;
 852   sparrow_line_t **sline = fl->shuffled_lines;
 853   int offset;
 854
 855   for (i = 0, offset = H_LINE_OFFSET; offset < sparrow->out.height;
 856        i++, offset += LINE_PERIOD){
 857     line->offset = offset;
 858     line->dir = SPARROW_HORIZONTAL;
 859     line->index = i;
 860     *sline = line;
 861     line++;
 862     sline++;
 863     //GST_DEBUG("line %d h has offset %d\n", i, offset);
 864   }
 865
 866   /*now add the vertical lines */
 867   fl->v_lines = line;
 868   for (i = 0, offset = V_LINE_OFFSET; offset < sparrow->out.width;
 869        i++, offset += LINE_PERIOD){
 870     line->offset = offset;
 871     line->dir = SPARROW_VERTICAL;
 872     line->index = i;
 873     *sline = line;
 874     line++;
 875     sline++;
 876     //GST_DEBUG("line %d v has offset %d\n", i, offset);
 877   }
 878   //DEBUG_FIND_LINES(fl);
 879   fl->n_lines = line - fl->h_lines;
 880   GST_DEBUG("allocated %d lines, made %d\n", n_lines_max, fl->n_lines);
 881
 882   /*now shuffle */
 883   for (i = 0; i < fl->n_lines; i++){
 884     int j = RANDINT(sparrow, 0, fl->n_lines);
 885     sparrow_line_t *tmp = fl->shuffled_lines[j];
 886     fl->shuffled_lines[j] = fl->shuffled_lines[i];
 887     fl->shuffled_lines[i] = tmp;
 888   }
 889
 890   setup_colour_shifts(sparrow, fl);
 891
 892   /* opencv images for threshold finding */
 893   CvSize size = {sparrow->in.width, sparrow->in.height};
 894   fl->working = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
 895   fl->threshold = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
 896
 897   /*input has no data allocated -- it uses latest frame*/
 898   fl->input = init_ipl_image(&sparrow->in, PIXSIZE);
 899   //DEBUG_FIND_LINES(fl);
 900   if (sparrow->debug){
 901     fl->debug = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
 902   }
 903
 904   if (sparrow->reload){
 905     if (access(sparrow->reload, R_OK)){
 906       GST_DEBUG("sparrow>reload is '%s' and it is UNREADABLE\n", sparrow->reload);
 907       exit(1);
 908     }
 909     read_edges_info(sparrow, fl, sparrow->reload);
 910     memset(fl->map, 0, sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
 911     //memset(fl->clusters, 0, n_corners * sizeof(sparrow_cluster_t));
 912     memset(fl->mesh, 0, n_corners * sizeof(sparrow_corner_t));
 913     jump_state(sparrow, fl, EDGES_FIND_CORNERS);
 914   }
 915   else {
 916     jump_state(sparrow, fl, EDGES_FIND_NOISE);
 917   }
 918
 919   global_number_of_edge_finders++;
 920 }
 921