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