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 %d,%d (mean %d,%d)\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.pixcount; 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 %d, using threshold %d\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.pixcount; 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 static void
 380 debug_map_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 381   guint32 *data = (guint32*)fl->debug->imageData;
 382   memset(data, 0, sparrow->in.size);
 383   for (guint i = 0; i < sparrow->in.pixcount; i++){
 384     data[i] |= fl->map[i].signal[SPARROW_HORIZONTAL] << sparrow->in.gshift;
 385     data[i] |= fl->map[i].signal[SPARROW_VERTICAL] << sparrow->in.rshift;
 386   }
 387   MAYBE_DEBUG_IPL(fl->debug);
 388 }
 389
 390 /* draw the line (in sparrow->colour) */
 391 static inline void
 392 draw_line(GstSparrow * sparrow, sparrow_line_t *line, guint8 *out){
 393   guint32 *p = (guint32 *)out;
 394   int i;
 395   if (line->dir == SPARROW_HORIZONTAL){
 396     p += line->offset * sparrow->out.width;
 397     for (i = 0; i < sparrow->out.width; i++){
 398       p[i] = sparrow->colour;
 399     }
 400   }
 401   else {
 402     guint32 *p = (guint32 *)out;
 403     p += line->offset;
 404     for(i = 0; i < sparrow->out.height; i++){
 405       *p = sparrow->colour;
 406       p += sparrow->out.width;
 407     }
 408   }
 409 }
 410
 411
 412 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
 413    until last one.
 414  */
 415
 416 INVISIBLE sparrow_state
 417 mode_find_edges(GstSparrow *sparrow, guint8 *in, guint8 *out){
 418   sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
 419   sparrow_line_t *line = fl->shuffled_lines[fl->current];
 420   sparrow->countdown--;
 421   memset(out, 0, sparrow->out.size);
 422   if (sparrow->countdown){
 423     /* show the line except on the first round, when we find a threshold*/
 424     if (fl->threshold){
 425       draw_line(sparrow, line, out);
 426     }
 427   }
 428   else{
 429       /*show nothing, look for result */
 430     if (fl->threshold){
 431       if (fl->current == fl->n_lines){
 432         goto done;
 433       }
 434       look_for_line(sparrow, in, fl, line);
 435       fl->current++;
 436     }
 437     else {
 438       look_for_threshold(sparrow, in, fl);
 439     }
 440     sparrow->countdown = sparrow->lag + 2;
 441   }
 442   if (sparrow->debug){
 443     debug_map_image(sparrow, fl);
 444   }
 445   return SPARROW_STATUS_QUO;
 446  done:
 447   /*match up lines and find corners */
 448   find_corners(sparrow, in, fl);
 449   corners_to_lut(sparrow, fl);
 450
 451   /*free stuff!, including fl, and reset pointer to NULL*/
 452
 453   return SPARROW_NEXT_STATE;
 454 }
 455
 456 INVISIBLE void
 457 finalise_find_edges(GstSparrow *sparrow){
 458   sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
 459   //debug_find_lines(fl);
 460   if (sparrow->debug){
 461     cvReleaseImage(&fl->debug);
 462   }
 463   free(fl->h_lines);
 464   free(fl->shuffled_lines);
 465   free(fl->map);
 466   free(fl->mesh);
 467   free(fl->clusters);
 468   free(fl->corners);
 469   free(fl);
 470 }
 471
 472
 473 static void
 474 setup_colour_shifts(GstSparrow *sparrow, sparrow_find_lines_t *fl){
 475   switch (sparrow->colour){
 476   case SPARROW_WHITE:
 477   case SPARROW_GREEN:
 478     fl->shift1 = sparrow->in.gshift;
 479     fl->shift2 = sparrow->in.gshift;
 480     break;
 481   case SPARROW_MAGENTA:
 482     fl->shift1 = sparrow->in.rshift;
 483     fl->shift2 = sparrow->in.bshift;
 484     break;
 485   }
 486 }
 487
 488 INVISIBLE void
 489 init_find_edges(GstSparrow *sparrow){
 490   gint32 w = sparrow->out.width;
 491   gint32 h = sparrow->out.height;
 492   gint i;
 493   sparrow_find_lines_t *fl = zalloc_aligned_or_die(sizeof(sparrow_find_lines_t));
 494   sparrow->helper_struct = (void *)fl;
 495
 496   gint h_lines = (h + LINE_PERIOD - 1) / LINE_PERIOD;
 497   gint v_lines = (w + LINE_PERIOD - 1) / LINE_PERIOD;
 498   gint n_lines = (h_lines + v_lines);
 499   gint n_corners = (h_lines * v_lines);
 500   fl->n_hlines = h_lines;
 501   fl->n_vlines = v_lines;
 502   fl->n_lines = n_lines;
 503
 504   fl->h_lines = malloc_aligned_or_die(sizeof(sparrow_line_t) * n_lines);
 505   fl->shuffled_lines = malloc_aligned_or_die(sizeof(sparrow_line_t*) * n_lines);
 506   fl->map = zalloc_aligned_or_die(sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
 507   fl->clusters = malloc_or_die(n_corners * sizeof(sparrow_cluster_t));
 508   fl->corners = zalloc_aligned_or_die(n_corners * sizeof(sparrow_corner_t));
 509   fl->mesh = malloc_aligned_or_die(sizeof(sparrow_corner_t) * h_lines * v_lines);
 510
 511   sparrow_line_t *line = fl->h_lines;
 512   sparrow_line_t **sline = fl->shuffled_lines;
 513   int offset = LINE_PERIOD / 2;
 514
 515   for (i = 0, offset = LINE_PERIOD / 2; offset < h;
 516        i++, offset += LINE_PERIOD){
 517     line->offset = offset;
 518     line->dir = SPARROW_HORIZONTAL;
 519     line->index = i;
 520     *sline = line;
 521     line++;
 522     sline++;
 523   }
 524
 525   /*now add the vertical lines */
 526   fl->v_lines = line;
 527   for (i = 0, offset = LINE_PERIOD / 2; offset < w;
 528        i++, offset += LINE_PERIOD){
 529     line->offset = offset;
 530     line->dir = SPARROW_VERTICAL;
 531     line->index = i;
 532     *sline = line;
 533     line++;
 534     sline++;
 535   }
 536
 537   GST_DEBUG("allocated %d lines, used %d\n", n_lines, line - fl->h_lines);
 538
 539   /*now shuffle (triangluar, to no particular advantage) */
 540   for (i = 0; i < n_lines - 1; i++){
 541     int j = RANDINT(sparrow, i + 1, n_lines);
 542     sparrow_line_t *tmp = fl->shuffled_lines[j];
 543     fl->shuffled_lines[j] = fl->shuffled_lines[i];
 544     fl->shuffled_lines[i] = tmp;
 545   }
 546
 547   setup_colour_shifts(sparrow, fl);
 548   sparrow->countdown = sparrow->lag + 2;
 549   //debug_find_lines(fl);
 550   if (sparrow->debug){
 551     CvSize size = {sparrow->in.width, sparrow->in.height};
 552     fl->debug = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
 553   }
 554 }