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 5
  33
  34 /*3 pixels manhatten distance makes you an outlier */
  35 #define OUTLIER_THRESHOLD 3 << (SPARROW_FIXED_POINT)
  36 #define OUTLIER_PENALTY 8
  37
  38 /*
  39 */
  40
  41
  42 #define SPARROW_MAP_LUT_SHIFT 1
  43 #define SPARROW_FP_2_LUT (SPARROW_FIXED_POINT - SPARROW_MAP_LUT_SHIFT)
  44
  45 #define OFFSET(x, y, w)((((y) * (w)) >> SPARROW_FIXED_POINT) + ((x) >> SPARROW_FIXED_POINT))
  46
  47 static void corners_to_lut(GstSparrow *sparrow, sparrow_find_lines_t *fl){
  48   //debug_find_lines(fl);
  49   sparrow_map_t *map = &sparrow->map; /*rows in sparrow->out */
  50   guint8 *mask = sparrow->screenmask; /*mask in sparrow->in */
  51   sparrow_corner_t *mesh = fl->mesh;   /*maps regular points in ->out to points in ->in */
  52
  53   int mesh_w = fl->n_vlines;
  54   int mesh_h = fl->n_hlines;
  55   int in_w = sparrow->in.width;
  56   int mcy, mmy, mcx; /*Mesh Corner|Modulus X|Y*/
  57
  58   int x;
  59   sparrow_map_row_t *row = map->rows;
  60   sparrow_map_point_t *p = map->point_mem;
  61   sparrow_corner_t *mesh_row = mesh;
  62   for(mcy = 0; mcy < mesh_h; mcy++){
  63     for (mmy = 0; mmy < LINE_PERIOD; mmy++){
  64       sparrow_corner_t *mesh_square = mesh_row;
  65       row->points = p;
  66       row->start = 0;
  67       row->end = 0;
  68       for(mcx = 0; mcx < mesh_w; mcx++){
  69         if (mesh_square->used){
  70           int iy = mesh_square->in_y + mmy * mesh_square->dyv;
  71           int ix = mesh_square->in_x + mmy * mesh_square->dxv;
  72           int ii = OFFSET(ix, iy, in_w);
  73           int ii_end = OFFSET(ix + (LINE_PERIOD - 1) * mesh_square->dxh,
  74               iy + (LINE_PERIOD - 1) * mesh_square->dyh, in_w);
  75           int start_on = mask[ii];
  76           int end_on = mask[ii_end];
  77           if(start_on && end_on){
  78             /*add the point, maybe switch on */
  79             if (row->start == row->end){/* if both are 0 */
  80               row->start = mcx * LINE_PERIOD;
  81             }
  82             p->x = ix;
  83             p->y = iy;
  84             p->dx = mesh_square->dxh;
  85             p->dy = mesh_square->dyh;
  86             p++;
  87           }
  88           else if (start_on){
  89             /*add the point, switch off somewhere in the middle*/
  90             for (x = 1; x < LINE_PERIOD; x++){
  91               iy += mesh_square->dyh;
  92               ix += mesh_square->dxh;
  93               ii = OFFSET(ix, iy, in_w);
  94               if (mask[ii]){
  95                 /*point is not in the same column with the others,
  96                   but sparrow knows this because the row->start says so */
  97                 row->start = mcx + x;
  98                 p->x = ix;
  99                 p->y = iy;
 100                 p->dx = mesh_square->dxh;
 101                 p->dy = mesh_square->dyh;
 102                 p++;
 103                 break;
 104               }
 105             }
 106           }
 107           else if (end_on){
 108             /* add some, switch off */
 109             for (x = 1; x < LINE_PERIOD; x++){
 110               iy += mesh_square->dyh;
 111               ix += mesh_square->dxh;
 112               ii = OFFSET(ix, iy, in_w);
 113               if (! mask[ii]){
 114                 row->end = mcx + x;
 115                 break;
 116               }
 117             }
 118           }
 119           else {
 120             /*3 cases:
 121               start > end: this is first off pixel.
 122               start == end: row hasn't started (both 0)
 123               start < end: both are set -- row is done
 124             */
 125             if (row->start > row->end){
 126               row->end = mcx * LINE_PERIOD;
 127             }
 128             else if (row->start < row->end){
 129               break;
 130             }
 131           }
 132         }
 133         mesh_square++;
 134       }
 135       row++;
 136     }
 137     mesh_row += mesh_w;
 138   }
 139 }
 140
 141 UNUSED static void
 142 corners_to_full_lut(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 143   //debug_find_lines(fl);
 144   sparrow_corner_t *mesh = fl->mesh;   /*maps regular points in ->out to points in ->in */
 145   sparrow_map_lut_t *map_lut = sparrow->map_lut;
 146   int mesh_w = fl->n_vlines;
 147   int mesh_h = fl->n_hlines;
 148   int mcy, mmy, mcx, mmx; /*Mesh Corner|Modulus X|Y*/
 149   int i = 0;
 150   sparrow_corner_t *mesh_row = mesh;
 151   for(mcy = 0; mcy < mesh_h; mcy++){
 152     for (mmy = 0; mmy < LINE_PERIOD; mmy++){
 153       sparrow_corner_t *mesh_square = mesh_row;
 154       for(mcx = 0; mcx < mesh_w; mcx++){
 155         int iy = mesh_square->in_y + mmy * mesh_square->dyv;
 156         int ix = mesh_square->in_x + mmy * mesh_square->dxv;
 157         for (mmx = 0; mmx < LINE_PERIOD; mmx++, i++){
 158           map_lut[i].x = ix >> SPARROW_FP_2_LUT;
 159           map_lut[i].y = iy >> SPARROW_FP_2_LUT;
 160           ix += mesh_square->dxh;
 161           iy += mesh_square->dyh;
 162         }
 163         mesh_square++;
 164       }
 165     }
 166     mesh_row += mesh_w;
 167   }
 168   sparrow->map_lut = map_lut;
 169 }
 170
 171
 172
 173 /*create the mesh */
 174
 175 static void
 176 find_corners(GstSparrow *sparrow, guint8 *in, sparrow_find_lines_t *fl){
 177   //debug_find_lines(fl);
 178   int i;
 179   int width = fl->n_vlines;
 180   int height = fl->n_hlines;
 181   sparrow_cluster_t *clusters = fl->clusters;
 182   sparrow_corner_t *corners = fl->corners;
 183   gint x, y;
 184   /*each point in fl->map is in a vertical line, a horizontal line, both, or
 185     neither.  Only the "both" case matters. */
 186   for (y = 0; y < sparrow->in.height; y++){
 187     for (x = 0; x < sparrow->in.width; x++){
 188       sparrow_intersect_t *p = &fl->map[y * sparrow->in.width + x];
 189       /*remembering that 0 is valid as a line no, but not as a signal */
 190       if (! p->signal[SPARROW_HORIZONTAL] ||
 191           ! p->signal[SPARROW_VERTICAL]){
 192         continue;
 193       }
 194       /*This one is lobbying for the position of a corner.*/
 195
 196       /*XXX what to do in the case that there is no intersection?  often cases
 197         this will happen in the dark bits and be OK. But if it happens in the
 198         light?*/
 199       /*linearise the xy coordinates*/
 200       int vline = p->lines[SPARROW_VERTICAL];
 201       int hline = p->lines[SPARROW_HORIZONTAL];
 202       sparrow_cluster_t *cluster = &clusters[vline * height + hline];
 203
 204       int n = cluster->n;
 205       if (n < 8){
 206         cluster->voters[n].x = x << SPARROW_FIXED_POINT;
 207         cluster->voters[n].y = y << SPARROW_FIXED_POINT;
 208         cluster->voters[n].signal = (SIG_WEIGHT + p->signal[SPARROW_HORIZONTAL]) *
 209           (SIG_WEIGHT + p->signal[SPARROW_VERTICAL]);
 210         cluster->n++;
 211         /*these next two could of course be computed from the offset */
 212       }
 213       else {
 214         GST_DEBUG("more than 8 pixels at cluster for corner %d, %d\n",
 215             vline, hline);
 216         /*if this message ever turns up, replace the weakest signals or add
 217           more slots.*/
 218       }
 219     }
 220   }
 221
 222   i = 0;
 223   for (y = 0; y < height; y++){
 224     for (x = 0; x < width; x++, i++){
 225       /* how to do this?
 226          1. centre of gravity (x,y, weighted average)
 227          2. discard outliers? look for connectedness? but if 2 are outliers?
 228       */
 229       sparrow_cluster_t *cluster = clusters + i;
 230       if (cluster->n == 0){
 231         continue;
 232       }
 233       int xsum, ysum;
 234       int xmean, ymean;
 235       int votes;
 236       while(1) {
 237         int j;
 238         xsum = 0;
 239         ysum = 0;
 240         votes = 0;
 241         for (j = 0; j < cluster->n; j++){
 242           votes += cluster->voters[j].signal;
 243           ysum += cluster->voters[j].y * cluster->voters[j].signal;
 244           xsum += cluster->voters[j].x * cluster->voters[j].signal;
 245         }
 246         if (votes == 0){
 247           /* don't diminish signal altogether. The previous iteration's means
 248              will be used. */
 249           break;
 250         }
 251         xmean = xsum / votes;
 252         ymean = ysum / votes;
 253         int worst = -1;
 254         int worstn;
 255         int devsum = 0;
 256         for (j = 0; j < cluster->n; j++){
 257           int xdiff = abs(cluster->voters[j].x - xmean);
 258           int ydiff = abs(cluster->voters[j].y - ymean);
 259           devsum += xdiff + ydiff;
 260           if (xdiff + ydiff > worst){
 261             worst = xdiff + ydiff;
 262             worstn = j;
 263           }
 264         }
 265         /*a bad outlier has significantly greater than average deviation
 266           (but how much is bad? median deviation would be more useful)*/
 267         if (worst > 3 * devsum / cluster->n){
 268           /* reduce the worst ones weight. it is a silly aberration. */
 269           cluster->voters[worstn].signal /= OUTLIER_PENALTY;
 270           GST_DEBUG("dropping outlier at %d,%d (mean %d,%d)\n",
 271               cluster->voters[worstn].x, cluster->voters[worstn].y, xmean, ymean);
 272           continue;
 273         }
 274         break;
 275       }
 276       corners[i].out_x = x * LINE_PERIOD;
 277       corners[i].out_y = y * LINE_PERIOD;
 278       corners[i].in_x = xmean;
 279       corners[i].in_y = ymean;
 280       corners[i].used = TRUE;
 281       double div = (double)(1 << SPARROW_FIXED_POINT); /*for printf only*/
 282       GST_DEBUG("found corner %d (%d,%d) at (%3f, %3f)\n",
 283           i, corners[i].out_x, corners[i].out_y,
 284           xmean / div, ymean / div);
 285     }
 286   }
 287   free(clusters);
 288
 289   /* calculate deltas toward adjacent corners */
 290   /* try to extrapolate left and up, if possible, so need to go backwards. */
 291   for (y = height - 2; y >= 0; y--){
 292     for (x = width - 2; x >= 0; x--){
 293       i = y * width + x;
 294       if (corners[i].used){
 295         corners[i].dxh = (corners[i + 1].in_x - corners[i].in_x) / LINE_PERIOD;
 296         corners[i].dyh = (corners[i + 1].in_y - corners[i].in_y) / LINE_PERIOD;
 297         corners[i].dxv = (corners[i + width].in_x - corners[i].in_x) / LINE_PERIOD;
 298         corners[i].dyv = (corners[i + width].in_y - corners[i].in_y) / LINE_PERIOD;
 299       }
 300       else {
 301           /*prefer copy from left unless it is itself reconstructed,
 302             (for no great reason)
 303             A mixed copy would be possible and better */
 304         if(corners[i + 1].used &&
 305             (corners[i + 1].used < corners[i + width].used)){
 306           corners[i].dxh = corners[i + 1].dxh;
 307           corners[i].dyh = corners[i + 1].dyh;
 308           corners[i].dxv = corners[i + 1].dxv;
 309           corners[i].dyv = corners[i + 1].dyv;
 310           corners[i].in_x = corners[i + 1].in_x - corners[i + 1].dxh * LINE_PERIOD;
 311           corners[i].in_y = corners[i + 1].in_y - corners[i + 1].dyh * LINE_PERIOD;
 312           corners[i].out_x = corners[i + 1].out_x - 1;
 313           corners[i].out_y = corners[i + 1].out_y;
 314           corners[i].used = corners[i + 1].used + 1;
 315         }
 316         else if(corners[i + width].used){
 317           corners[i].dxh = corners[i + width].dxh;
 318           corners[i].dyh = corners[i + width].dyh;
 319           corners[i].dxv = corners[i + width].dxv;
 320           corners[i].dyv = corners[i + width].dyv;
 321           corners[i].in_x = corners[i + width].in_x - corners[i + width].dxv * LINE_PERIOD;
 322           corners[i].in_y = corners[i + width].in_y - corners[i + width].dyv * LINE_PERIOD;
 323           corners[i].out_x = corners[i + width].out_x;
 324           corners[i].out_y = corners[i + width].out_y - 1;
 325           corners[i].used = corners[i + width].used + 1;
 326         }
 327       }
 328     }
 329   }
 330
 331   fl->mesh = corners;
 332   //debug_find_lines(fl);
 333
 334 }
 335
 336
 337 /* With no line drawn (in our colour) look at the background noise.  Any real
 338    signal has to be stringer than this.
 339
 340    XXX looking for simple maximum -- maybe heap or histogram might be better,
 341    so as to be less susceptible to wierd outliers (e.g., bad pixels).  */
 342 static void
 343 look_for_threshold(GstSparrow *sparrow, guint8 *in, sparrow_find_lines_t *fl){
 344   //debug_find_lines(fl);
 345   int i;
 346   guint32 colour;
 347   guint32 signal;
 348   guint32 *in32 = (guint32 *)in;
 349   guint32 highest = 0;
 350   for (i = 0;  i < (int)sparrow->in.pixcount; i++){
 351     colour = in32[i] & sparrow->colour;
 352     signal = ((colour >> fl->shift1) +
 353         (colour >> fl->shift2)) & 0x1ff;
 354     if (signal > highest){
 355       highest = signal;
 356     }
 357   }
 358   fl->threshold = highest + 10;
 359   GST_DEBUG("found maximum noise of %d, using threshold %d\n", highest, fl->threshold);
 360 }
 361
 362
 363 static void
 364 look_for_line(GstSparrow *sparrow, guint8 *in, sparrow_find_lines_t *fl,
 365     sparrow_line_t *line){
 366   guint i;
 367   guint32 colour;
 368   int signal;
 369   guint32 *in32 = (guint32 *)in;
 370   for (i = 0; i < sparrow->in.pixcount; i++){
 371     colour = in32[i] & sparrow->colour;
 372     signal = ((colour >> fl->shift1) +
 373         (colour >> fl->shift2)) & 0x1ff;
 374     if (signal > fl->threshold){
 375       if (fl->map[i].lines[line->dir]){
 376         GST_DEBUG("HEY, expected point %d to be in line %d (dir %d)"
 377             "and thus empty, but it is also in line %d\n",
 378             i, line->index, line->dir, fl->map[i].lines[line->dir]);
 379       }
 380       fl->map[i].lines[line->dir] = line->index;
 381       fl->map[i].signal[line->dir] = signal;
 382     }
 383   }
 384 }
 385
 386 static void
 387 debug_map_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 388   guint32 *data = (guint32*)fl->debug->imageData;
 389   memset(data, 0, sparrow->in.size);
 390   for (guint i = 0; i < sparrow->in.pixcount; i++){
 391     data[i] |= fl->map[i].signal[SPARROW_HORIZONTAL] << sparrow->in.gshift;
 392     data[i] |= fl->map[i].signal[SPARROW_VERTICAL] << sparrow->in.rshift;
 393   }
 394   MAYBE_DEBUG_IPL(fl->debug);
 395 }
 396
 397 /* draw the line (in sparrow->colour) */
 398 static inline void
 399 draw_line(GstSparrow * sparrow, sparrow_line_t *line, guint8 *out){
 400   guint32 *p = (guint32 *)out;
 401   int i;
 402   if (line->dir == SPARROW_HORIZONTAL){
 403     p += line->offset * sparrow->out.width;
 404     for (i = 0; i < sparrow->out.width; i++){
 405       p[i] = sparrow->colour;
 406     }
 407   }
 408   else {
 409     guint32 *p = (guint32 *)out;
 410     p += line->offset;
 411     for(i = 0; i < sparrow->out.height; i++){
 412       *p = sparrow->colour;
 413       p += sparrow->out.width;
 414     }
 415   }
 416 }
 417
 418
 419 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
 420    until last one.
 421  */
 422
 423 INVISIBLE sparrow_state
 424 mode_find_edges(GstSparrow *sparrow, guint8 *in, guint8 *out){
 425   sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
 426   debug_find_lines(fl);
 427   if (fl->current == fl->n_lines){
 428     goto done;
 429   }
 430   sparrow_line_t *line = fl->shuffled_lines[fl->current];
 431
 432   sparrow->countdown--;
 433   memset(out, 0, sparrow->out.size);
 434   if (sparrow->countdown){
 435     /* show the line except on the first round, when we find a threshold*/
 436     if (fl->threshold){
 437       GST_DEBUG("current %d line %p\n", fl->current, line);
 438       draw_line(sparrow, line, out);
 439     }
 440   }
 441   else{
 442       /*show nothing, look for result */
 443     if (fl->threshold){
 444       look_for_line(sparrow, in, fl, line);
 445       fl->current++;
 446     }
 447     else {
 448       look_for_threshold(sparrow, in, fl);
 449     }
 450     sparrow->countdown = sparrow->lag + 2;
 451   }
 452   if (sparrow->debug){
 453     debug_map_image(sparrow, fl);
 454   }
 455   return SPARROW_STATUS_QUO;
 456  done:
 457   /*match up lines and find corners */
 458   find_corners(sparrow, in, fl);
 459   corners_to_lut(sparrow, fl);
 460
 461   /*free stuff!, including fl, and reset pointer to NULL*/
 462
 463   return SPARROW_NEXT_STATE;
 464 }
 465
 466 INVISIBLE void
 467 finalise_find_edges(GstSparrow *sparrow){
 468   sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
 469   //debug_find_lines(fl);
 470   if (sparrow->debug){
 471     cvReleaseImage(&fl->debug);
 472   }
 473   free(fl->h_lines);
 474   free(fl->shuffled_lines);
 475   free(fl->map);
 476   free(fl->mesh);
 477   free(fl->clusters);
 478   free(fl->corners);
 479   free(fl);
 480 }
 481
 482
 483 static void
 484 setup_colour_shifts(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 485   //debug_find_lines(fl);
 486
 487   switch (sparrow->colour){
 488   case SPARROW_WHITE:
 489   case SPARROW_GREEN:
 490     fl->shift1 = sparrow->in.gshift;
 491     fl->shift2 = sparrow->in.gshift;
 492     break;
 493   case SPARROW_MAGENTA:
 494     fl->shift1 = sparrow->in.rshift;
 495     fl->shift2 = sparrow->in.bshift;
 496     break;
 497   }
 498 }
 499
 500 INVISIBLE void
 501 init_find_edges(GstSparrow *sparrow){
 502   gint32 w = sparrow->out.width;
 503   gint32 h = sparrow->out.height;
 504   gint i;
 505   sparrow_find_lines_t *fl = zalloc_aligned_or_die(sizeof(sparrow_find_lines_t));
 506   sparrow->helper_struct = (void *)fl;
 507
 508   gint h_lines = (h + LINE_PERIOD - 1) / LINE_PERIOD;
 509   gint v_lines = (w + LINE_PERIOD - 1) / LINE_PERIOD;
 510   gint n_lines = (h_lines + v_lines);
 511   gint n_corners = (h_lines * v_lines);
 512   fl->n_hlines = h_lines;
 513   fl->n_vlines = v_lines;
 514   fl->n_lines = n_lines;
 515
 516   fl->h_lines = malloc_aligned_or_die(sizeof(sparrow_line_t) * n_lines);
 517   fl->shuffled_lines = malloc_aligned_or_die(sizeof(sparrow_line_t*) * n_lines);
 518   GST_DEBUG("shuffled lines, malloced %p\n", fl->shuffled_lines);
 519   fl->map = zalloc_aligned_or_die(sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
 520   fl->clusters = malloc_or_die(n_corners * sizeof(sparrow_cluster_t));
 521   fl->corners = zalloc_aligned_or_die(n_corners * sizeof(sparrow_corner_t));
 522   fl->mesh = malloc_aligned_or_die(sizeof(sparrow_corner_t) * h_lines * v_lines);
 523
 524   sparrow_line_t *line = fl->h_lines;
 525   sparrow_line_t **sline = fl->shuffled_lines;
 526   int offset = LINE_PERIOD / 2;
 527
 528   for (i = 0, offset = LINE_PERIOD / 2; offset < h;
 529        i++, offset += LINE_PERIOD){
 530     line->offset = offset;
 531     line->dir = SPARROW_HORIZONTAL;
 532     line->index = i;
 533     *sline = line;
 534     line++;
 535     sline++;
 536   }
 537
 538   /*now add the vertical lines */
 539   fl->v_lines = line;
 540   for (i = 0, offset = LINE_PERIOD / 2; offset < w;
 541        i++, offset += LINE_PERIOD){
 542     line->offset = offset;
 543     line->dir = SPARROW_VERTICAL;
 544     line->index = i;
 545     *sline = line;
 546     line++;
 547     sline++;
 548   }
 549   //debug_find_lines(fl);
 550
 551   GST_DEBUG("allocated %d lines, used %d\n", n_lines, line - fl->h_lines);
 552
 553   /*now shuffle */
 554   for (i = 0; i < n_lines - 1; i++){
 555     int j = RANDINT(sparrow, 0, n_lines);
 556     sparrow_line_t *tmp = fl->shuffled_lines[j];
 557     fl->shuffled_lines[j] = fl->shuffled_lines[i];
 558     fl->shuffled_lines[i] = tmp;
 559   }
 560
 561   setup_colour_shifts(sparrow, fl);
 562   sparrow->countdown = sparrow->lag + 2;
 563   //debug_find_lines(fl);
 564   if (sparrow->debug){
 565     CvSize size = {sparrow->in.width, sparrow->in.height};
 566     fl->debug = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
 567   }
 568 }