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