calibrate.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 #include "sparrow.h"
  20 #include "gstsparrow.h"
  21
  22 #include <string.h>
  23 #include <math.h>
  24
  25
  26 /*drawing*/
  27
  28 static inline void
  29 horizontal_line(GstSparrow *sparrow, guint8 *out, guint32 y){
  30   guint stride = sparrow->out.width * PIXSIZE;
  31   guint8 *line = out + y * stride;
  32   memset(line, 255, stride);
  33 }
  34
  35 static inline void
  36 vertical_line(GstSparrow *sparrow, guint8 *out, guint32 x){
  37   guint y;
  38   guint32 *p = (guint32 *)out;
  39   p += x;
  40   for(y = 0; y < (guint)(sparrow->out.height); y++){
  41     *p = -1;
  42     p += sparrow->out.width;
  43   }
  44 }
  45
  46 static inline void
  47 rectangle(GstSparrow *sparrow, guint8 *out, sparrow_shape_t *shape){
  48   int y, x;
  49   guint stride = sparrow->out.width;
  50   guint32 *line = ((guint32 *)out) + shape->y * stride + shape->x;
  51   for (x = 0; x < shape->w; x++){
  52     line[x] = sparrow->calibrate.outcolour;
  53   }
  54   guint32 *line2 = line + stride;
  55   for(y = 1; y < shape->h; y++){
  56     memcpy(line2, line, shape->w * PIXSIZE);
  57     line2 += stride;
  58   }
  59 }
  60
  61 static void draw_shapes(GstSparrow *sparrow, guint8 *out){
  62   int i;
  63   sparrow_shape_t *shape;
  64   for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
  65     shape = sparrow->shapes + i;
  66     switch (shape->shape){
  67     case NO_SHAPE:
  68       goto done; /* an empty one ends the list */
  69     case VERTICAL_LINE:
  70       vertical_line(sparrow, out, shape->x);
  71       break;
  72     case HORIZONTAL_LINE:
  73       horizontal_line(sparrow, out, shape->x);
  74       break;
  75     case RECTANGLE:
  76       rectangle(sparrow, out, shape);
  77       break;
  78     case FULLSCREEN:
  79       memset(out, 255, sparrow->out.size);
  80       break;
  81     }
  82   }
  83  done:
  84   return;
  85 }
  86
  87 static inline void
  88 init_one_square(GstSparrow *sparrow, sparrow_shape_t* shape){
  89     shape->shape = RECTANGLE;
  90     shape->w = CALIBRATE_SELF_SIZE;
  91     shape->h = CALIBRATE_SELF_SIZE;
  92     shape->x  = RANDINT(sparrow, sparrow->out.width / 8,
  93         sparrow->out.width * 7 / 8 - shape->w);
  94     shape->y  = RANDINT(sparrow, sparrow->out.height / 8,
  95         sparrow->out.height * 7 / 8 - shape->h);
  96 }
  97
  98
  99 /*fake other projection */
 100 static void add_random_signal(GstSparrow *sparrow, guint8 *out){
 101   int i;
 102   static sparrow_shape_t shapes[MAX_CALIBRATE_SHAPES];
 103   static int been_here = 0;
 104   static int countdown = 0;
 105   static int on = 0;
 106   if (! been_here){
 107     for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
 108       init_one_square(sparrow, &shapes[i]);
 109     }
 110     been_here = 1;
 111   }
 112   if (! countdown){
 113     on = ! on;
 114     countdown = on ? RANDINT(sparrow, CALIBRATE_ON_MIN_T, CALIBRATE_ON_MAX_T) :
 115       RANDINT(sparrow, CALIBRATE_ON_MIN_T, CALIBRATE_ON_MAX_T);
 116   }
 117   if (on){
 118     for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
 119       rectangle(sparrow, out, &shapes[i]);
 120     }
 121   }
 122   countdown--;
 123 }
 124
 125 static gboolean cycle_pattern(GstSparrow *sparrow){
 126   gboolean on = sparrow->calibrate.on;
 127   if (sparrow->calibrate.wait == 0){
 128     on = !on;
 129     if (on){
 130       sparrow->calibrate.wait = RANDINT(sparrow, CALIBRATE_ON_MIN_T, CALIBRATE_ON_MAX_T);
 131     }
 132     else{
 133       sparrow->calibrate.wait = RANDINT(sparrow, CALIBRATE_OFF_MIN_T, CALIBRATE_OFF_MAX_T);
 134     }
 135     sparrow->calibrate.on = on;
 136     sparrow->calibrate.transitions++;
 137   }
 138   sparrow->calibrate.wait--;
 139   sparrow->lag_record = (sparrow->lag_record << 1) | on;
 140   //GST_DEBUG("lag record %llx, on %i\n", sparrow->lag_record, on);
 141   return on;
 142 }
 143
 144
 145 UNUSED
 146 static void
 147 init_lines(GstSparrow *sparrow){
 148   int i;
 149   sparrow_shape_t* shape = sparrow->shapes;
 150   for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
 151     shape[i].w = 1;
 152     shape[i].h = 1;
 153     shape[i].x = 0;
 154     shape[i].y = 0;
 155     shape[i].shape = NO_SHAPE;
 156   }
 157   /* shape[0] will be set to vertical or horizontal in due course */
 158 }
 159
 160 static inline void
 161 colour_coded_pixel(guint32* pixel, guint32 lag, guint32 shift){
 162 #define CCP_SCALE 2
 163   if (shift < 9 * CCP_SCALE){
 164     shift /= CCP_SCALE;
 165     if (shift == 0){
 166       *pixel = (guint32)-1;
 167     }
 168     else{
 169       shift--;
 170       guint32 c = lag_false_colour[lag];
 171       guint32 mask = (1 << (8 - shift)) - 1;
 172       mask |= (mask << 8);
 173       mask |= (mask << 16); //XXX LUT would be quicker
 174       c >>= shift;
 175       c &= mask;
 176       *pixel = c;
 177     }
 178   }
 179 }
 180
 181
 182 static inline char *
 183 int64_to_binary_string(char *s, guint64 n){
 184   /* s should be a *65* byte array */
 185   int i;
 186   for (i = 0; i < 64; i++){
 187     s[i] = (n & (1ULL << (63 - i))) ? '*' : '.';
 188   }
 189   s[64] = 0;
 190   return s;
 191 }
 192
 193
 194 /*return 1 if a reasonably likely lag has been found */
 195
 196 static inline int
 197 find_lag(GstSparrow *sparrow){
 198   int res = 0;
 199   guint i, j;
 200   guint32 *frame = (guint32 *)sparrow->debug_frame;
 201   if (sparrow->debug){
 202     memset(frame, 0, sparrow->in.size);
 203   }
 204   guint64 target_pattern = sparrow->lag_record;
 205   guint32 overall_best = (guint32)-1;
 206   guint32 overall_lag = 0;
 207   char pattern_debug[65];
 208   int votes[MAX_CALIBRATION_LAG] = {0};
 209
 210   GST_DEBUG("pattern: %s %llx\n", int64_to_binary_string(pattern_debug, target_pattern),
 211       target_pattern);
 212
 213   for (i = 0; i < sparrow->in.pixcount; i++){
 214     guint64 record = sparrow->lag_table[i].record;
 215     if (record == 0 || ~record == 0){
 216       /*ignore this one! it'll never usefully match. */
 217       //frame[i] = 0xffffffff;
 218       continue;
 219     }
 220
 221     guint64 mask = ((guint64)-1) >> MAX_CALIBRATION_LAG;
 222     guint32 best = hamming_distance64(record, target_pattern, mask);
 223     guint32 lag = 0;
 224
 225     for (j = 1; j < MAX_CALIBRATION_LAG; j++){
 226       /*latest frame is least significant bit
 227         >> pushes into future,
 228         << pushes into past
 229         record is presumed to be a few frames past
 230         relative to main record, so we push it back.
 231       */
 232       record <<= 1;
 233       mask <<= 1;
 234       guint32 d = hamming_distance64(record, target_pattern, mask);
 235       if (d < best){
 236         best = d;
 237         lag = j;
 238       }
 239     }
 240     if (sparrow->debug){
 241       colour_coded_pixel(&frame[i], lag, best);
 242     }
 243
 244     if (best <= CALIBRATE_MAX_VOTE_ERROR){
 245       votes[lag] += 1 >> (CALIBRATE_MAX_VOTE_ERROR - best);
 246     }
 247
 248     if (best < overall_best){
 249       overall_best = best;
 250       overall_lag = lag;
 251       char pattern_debug2[65];
 252       guint64 r = sparrow->lag_table[i].record;
 253       GST_DEBUG("Best now: lag  %u! error %u pixel %u\n"
 254           "record:  %s %llx\n"
 255           "pattern: %s %llx\n",
 256           overall_lag, overall_best, i,
 257           int64_to_binary_string(pattern_debug, r), r,
 258           int64_to_binary_string(pattern_debug2, target_pattern), target_pattern
 259       );
 260     }
 261   }
 262
 263   if (sparrow->debug){
 264     debug_frame(sparrow, sparrow->debug_frame, sparrow->in.width, sparrow->in.height);
 265   }
 266
 267   /*calculate votes winner, as a check for winner-takes-all */
 268   guint popular_lag;
 269   int popular_votes = -1;
 270   for (i = 0; i < MAX_CALIBRATION_LAG; i++){
 271     if(votes[i] > popular_votes){
 272       popular_votes = votes[i];
 273       popular_lag = i;
 274     }
 275     if (votes[i]){
 276       GST_DEBUG("%d votes for %d\n", votes[i], i);
 277     }
 278   }
 279   /*votes and best have to agree, and best has to be low */
 280   if (overall_best <= CALIBRATE_MAX_BEST_ERROR &&
 281       overall_lag == popular_lag){
 282     sparrow->lag = overall_lag;
 283     res = 1;
 284   }
 285   return res;
 286 }
 287
 288 static inline void
 289 record_calibration(GstSparrow *sparrow, gint32 offset, int signal){
 290   //signal = (signal != 0);
 291   sparrow->lag_table[offset].record <<= 1;
 292   sparrow->lag_table[offset].record |= signal;
 293 }
 294
 295
 296 INVISIBLE sparrow_state
 297 mode_find_self(GstSparrow *sparrow, guint8 *in, guint8 *out){
 298   int ret = SPARROW_STATUS_QUO;
 299   guint32 i;
 300   guint32 *frame = (guint32 *)in;
 301   /* record the current signal */
 302   for (i = 0; i < sparrow->in.pixcount; i++){
 303     int signal = (((frame[i] >> sparrow->in.gshift) & 255) > CALIBRATE_SIGNAL_THRESHOLD);
 304     record_calibration(sparrow, i, signal);
 305   }
 306   if (sparrow->countdown == 0){
 307     /* analyse the signal */
 308     int r = find_lag(sparrow);
 309     if (r){
 310       GST_DEBUG("lag is set at %u! after %u cycles\n", sparrow->lag, sparrow->frame_count);
 311       ret = SPARROW_NEXT_STATE;
 312     }
 313     else {
 314       init_find_self(sparrow);
 315     }
 316   }
 317
 318   memset(out, 0, sparrow->out.size);
 319   gboolean on = cycle_pattern(sparrow);
 320   if (on){
 321     draw_shapes(sparrow, out);
 322   }
 323 #if FAKE_OTHER_PROJECTION
 324   add_random_signal(sparrow, out);
 325 #endif
 326   sparrow->countdown--;
 327   return ret;
 328 }
 329
 330
 331 static inline void
 332 abs_diff(GstSparrow *sparrow, guint8 *a, guint8 *b, guint8 *target){
 333   sparrow->in_ipl[0]->imageData = (char*) a;
 334   sparrow->in_ipl[1]->imageData = (char*) b;
 335   sparrow->in_ipl[2]->imageData = (char*) target;
 336   cvAbsDiff(sparrow->in_ipl[0], sparrow->in_ipl[1], sparrow->in_ipl[2]);
 337 }
 338
 339
 340
 341
 342 static void
 343 see_grid(GstSparrow *sparrow, guint8 *in){
 344 }
 345
 346 static void
 347 find_grid(GstSparrow *sparrow, guint8 *in, guint8 *out){
 348   see_grid(sparrow, in);
 349   int on = cycle_pattern(sparrow);
 350   memset(out, 0, sparrow->out.size);
 351   if (on){
 352     draw_shapes(sparrow, out);
 353   }
 354 }
 355
 356
 357 INVISIBLE sparrow_state
 358 mode_find_edges(GstSparrow *sparrow, guint8 *in, guint8 *out){
 359   return SPARROW_STATUS_QUO;
 360 }
 361
 362
 363
 364 /* a minature state progression within this one, in case the processing is too
 365    much for one frame.*/
 366 INVISIBLE sparrow_state
 367 mode_find_screen(GstSparrow *sparrow, guint8 *in, guint8 *out){
 368   sparrow->countdown--;
 369   sparrow_find_screen_t *finder = &(sparrow->findscreen);
 370   IplImage *im = sparrow->in_ipl[0];
 371   IplImage *green = finder->green;
 372   IplImage *working = finder->working;
 373   IplImage *mask = finder->mask;
 374   CvPoint middle, corner;
 375   switch (sparrow->countdown){
 376   case 2:
 377     /* time to look and see if the screen is there.
 378        Look at the histogram of a single channel. */
 379     im->imageData = (char*)in;
 380     guint32 gshift = sparrow->in.gshift;
 381     cvSplit(im,
 382         (gshift == 24) ? green : NULL,
 383         (gshift == 16) ? green : NULL,
 384         (gshift ==  8) ? green : NULL,
 385         (gshift ==  0) ? green : NULL);
 386     int best_t = find_edges_threshold(green);
 387     /*XXX if best_t is wrong, add to sparrow->countdown: probably the light is
 388       not really on.  But what counts as wrong? */
 389     cvCmpS(green, best_t, mask, CV_CMP_GT);
 390     goto black;
 391   case 1:
 392     /* floodfill where the screen is, removing outlying bright spots*/
 393     middle = (CvPoint){sparrow->in.width / 2, sparrow->in.height / 2};
 394     memset(working->imageData, 255, sparrow->in.size);
 395     floodfill_mono_superfast(mask, working, middle);
 396     goto black;
 397   case 0:
 398     /* floodfill the border, removing onscreen dirt.*/
 399     corner = (CvPoint){0, 0};
 400     memset(mask->imageData, 255, sparrow->in.size);
 401     floodfill_mono_superfast(working, mask, corner);
 402     sparrow->screenmask = (guint8*)mask->imageData;
 403     sparrow->screenmask_ipl = mask;
 404     cvReleaseImage(&(finder->green));
 405     cvReleaseImage(&(finder->working));
 406     goto finish;
 407   default:
 408     /*send white and wait for the picture to arrive back. */
 409     memset(out, 255, sparrow->out.size);
 410     return SPARROW_STATUS_QUO;
 411   }
 412  black:
 413   memset(out, 0, sparrow->out.size);
 414   return SPARROW_STATUS_QUO;
 415  finish:
 416   memset(out, 0, sparrow->out.size);
 417   return SPARROW_NEXT_STATE;
 418 }
 419
 420 /* wait for the other projector to stop changing, for as many frames as are
 421    set in sparrow->countdown.  When sparrow->countdown reaches 0, return 1.
 422
 423    If sparrow->countdown is already 0, do nothing.
 424
 425    If something happens, reset sparrow->countdown to <blanktime>.
 426 */
 427 static int
 428 wait_for_blank(GstSparrow *sparrow, guint8 *in, guint8 *out, int blanktime){
 429   if (sparrow->countdown){
 430     guint32 i;
 431     abs_diff(sparrow, in, sparrow->prev_frame, sparrow->work_frame);
 432     //Use the green channel
 433     for (i = (sparrow->in.gshift >> 8);
 434          i < sparrow->in.pixcount * PIXSIZE;
 435          i += 4){
 436       guint8 signal = sparrow->work_frame[i];
 437       if (signal > CALIBRATE_WAIT_SIGNAL_THRESHOLD){
 438         sparrow->countdown = blanktime;
 439         break;
 440       }
 441     }
 442     memset(out, 0, sparrow->out.size);
 443     sparrow->countdown--;
 444   }
 445   return (sparrow->countdown == 0);
 446 }
 447
 448
 449 static inline void
 450 new_calibration_colour(GstSparrow *sparrow){
 451   int c = RANDINT(sparrow, 1, 3);
 452   sparrow->calibrate.incolour = sparrow->in.colours[c];
 453   sparrow->calibrate.outcolour = sparrow->out.colours[c];
 454   //sparrow->calibrate.wait == 0;
 455 }
 456
 457 /* Choose between green or magenta, randomly and iteratively, until the
 458    other one chooses something else.  But first wait for blank? */
 459
 460 INVISIBLE sparrow_state
 461 mode_pick_colour(GstSparrow *sparrow, guint8 *in, guint8 *out){
 462   if(wait_for_blank(sparrow, in, out, WAIT_COUNTDOWN)){
 463     new_calibration_colour(sparrow);
 464     sparrow->calibrate.wait = sparrow->lag + 2;
 465   }
 466   sparrow->calibrate.wait--;
 467   return SPARROW_STATUS_QUO;
 468 }
 469
 470
 471 INVISIBLE sparrow_state
 472 mode_wait_for_grid(GstSparrow *sparrow, guint8 *in, guint8 *out){
 473   if (wait_for_blank(sparrow, in, out, WAIT_COUNTDOWN)){
 474     return SPARROW_NEXT_STATE;
 475   }
 476   return SPARROW_STATUS_QUO;
 477 }
 478
 479 /*init functions */
 480
 481 INVISIBLE void
 482 init_wait_for_grid(GstSparrow *sparrow){}
 483
 484 INVISIBLE void
 485 init_find_grid(GstSparrow *sparrow){}
 486
 487 INVISIBLE void
 488 init_find_self(GstSparrow *sparrow){
 489   sparrow->calibrate.incolour = sparrow->in.colours[SPARROW_WHITE];
 490   sparrow->calibrate.outcolour = sparrow->out.colours[SPARROW_WHITE];
 491   if (! sparrow->n_shapes){
 492     int i;
 493     for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
 494       init_one_square(sparrow, &(sparrow->shapes[i]));
 495     }
 496     sparrow->n_shapes = MAX_CALIBRATE_SHAPES;
 497     sparrow->countdown = CALIBRATE_INITIAL_WAIT;
 498   }
 499   else {
 500     sparrow->countdown = CALIBRATE_RETRY_WAIT;
 501   }
 502 }
 503
 504 INVISIBLE void
 505 init_pick_colour(GstSparrow *sparrow)
 506 {
 507   sparrow->countdown = WAIT_COUNTDOWN;
 508 }
 509
 510 INVISIBLE void
 511 init_find_screen(GstSparrow *sparrow){
 512   sparrow->countdown = sparrow->lag + 4;
 513   sparrow_find_screen_t *finder = &(sparrow->findscreen);
 514   CvSize size = {sparrow->in.width, sparrow->in.height};
 515   finder->green = cvCreateImage(size, IPL_DEPTH_8U, 1);
 516   finder->working = cvCreateImage(size, IPL_DEPTH_8U, 1);
 517   finder->mask = cvCreateImage(size, IPL_DEPTH_8U, 1);
 518 }
 519
 520 INVISIBLE void
 521 init_find_edges(GstSparrow *sparrow){}