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