edges.c

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