edges.c

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