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