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 static void
 357 see_screen(GstSparrow *sparrow, guint8 *in){
 358   /* there is a big flash of white. or not.  look for the largest area of
 359      light.
 360
 361      perhaps, threshold at one or two levels.  or more. if they agree they are
 362      probably right.
 363
 364      or floodfill (and fill in)
 365
 366      canny edge detection:
 367      http://opencv.willowgarage.com/documentation/c/feature_detection.html#canny
 368
 369  */
 370   guint i;
 371   for (i = 0; i < sparrow->in.pixcount; i++){
 372
 373     guint32 signal = sparrow->work_frame[i * PIXSIZE + 2];
 374     if (signal > CALIBRATE_WAIT_SIGNAL_THRESHOLD){
 375       sparrow->countdown = WAIT_COUNTDOWN;
 376       break;
 377     }
 378   }
 379 }
 380
 381 INVISIBLE sparrow_state
 382 mode_find_edges(GstSparrow *sparrow, guint8 *in, guint8 *out){
 383   return SPARROW_STATUS_QUO;
 384 }
 385
 386 INVISIBLE sparrow_state
 387 mode_find_screen(GstSparrow *sparrow, guint8 *in, guint8 *out){
 388   see_screen(sparrow, in);
 389   int on = cycle_pattern(sparrow);
 390   if (on){
 391     memset(out, 255, sparrow->out.size);
 392   }
 393   else {
 394     memset(out, 0, sparrow->out.size);
 395   }
 396   return SPARROW_STATUS_QUO;
 397 }
 398
 399 /* wait for the other projector to stop changing, for as many frames as are
 400    set in sparrow->countdown.  When sparrow->countdown reaches 0, return 1.
 401
 402    If sparrow->countdown is already 0, do nothing.
 403
 404    If something happens, reset sparrow->countdown to <blanktime>.
 405 */
 406 static int
 407 wait_for_blank(GstSparrow *sparrow, guint8 *in, guint8 *out, int blanktime){
 408   if (sparrow->countdown){
 409     guint32 i;
 410     abs_diff(sparrow, in, sparrow->prev_frame, sparrow->work_frame);
 411     //Use the green channel
 412     for (i = (sparrow->in.gshift >> 8);
 413          i < sparrow->in.pixcount * PIXSIZE;
 414          i += 4){
 415       guint8 signal = sparrow->work_frame[i];
 416       if (signal > CALIBRATE_WAIT_SIGNAL_THRESHOLD){
 417         sparrow->countdown = blanktime;
 418         break;
 419       }
 420     }
 421     memset(out, 0, sparrow->out.size);
 422     sparrow->countdown--;
 423   }
 424   return (sparrow->countdown == 0);
 425 }
 426
 427
 428 static inline void
 429 new_calibration_colour(GstSparrow *sparrow){
 430   int c = RANDINT(sparrow, 1, 3);
 431   sparrow->calibrate.incolour = sparrow->in.colours[c];
 432   sparrow->calibrate.outcolour = sparrow->out.colours[c];
 433   //sparrow->calibrate.wait == 0;
 434 }
 435
 436 /* Choose between green or magenta, randomly and iteratively, until the
 437    other one chooses something else.  But first wait for blank? */
 438
 439 INVISIBLE sparrow_state
 440 mode_pick_colour(GstSparrow *sparrow, guint8 *in, guint8 *out){
 441   if(wait_for_blank(sparrow, in, out, WAIT_COUNTDOWN)){
 442     new_calibration_colour(sparrow);
 443     sparrow->calibrate.wait = sparrow->lag + 2;
 444   }
 445   sparrow->calibrate.wait--;
 446   return SPARROW_STATUS_QUO;
 447 }
 448
 449
 450 INVISIBLE sparrow_state
 451 mode_wait_for_grid(GstSparrow *sparrow, guint8 *in, guint8 *out){
 452   if (wait_for_blank(sparrow, in, out, WAIT_COUNTDOWN)){
 453     return SPARROW_NEXT_STATE;
 454   }
 455   return SPARROW_STATUS_QUO;
 456 }
 457
 458 /*init functions */
 459
 460 INVISIBLE void
 461 init_wait_for_grid(GstSparrow *sparrow){}
 462
 463 INVISIBLE void
 464 init_find_grid(GstSparrow *sparrow){}
 465
 466 void INVISIBLE
 467 init_find_self(GstSparrow *sparrow){
 468   sparrow->calibrate.incolour = sparrow->in.colours[SPARROW_WHITE];
 469   sparrow->calibrate.outcolour = sparrow->out.colours[SPARROW_WHITE];
 470   if (! sparrow->n_shapes){
 471     int i;
 472     for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
 473       init_one_square(sparrow, &(sparrow->shapes[i]));
 474     }
 475     sparrow->n_shapes = MAX_CALIBRATE_SHAPES;
 476     sparrow->countdown = CALIBRATE_INITIAL_WAIT;
 477   }
 478   else {
 479     sparrow->countdown = CALIBRATE_RETRY_WAIT;
 480   }
 481 }
 482
 483 INVISIBLE void
 484 init_pick_colour(GstSparrow *sparrow)
 485 {
 486   sparrow->countdown = WAIT_COUNTDOWN;
 487 }
 488
 489 INVISIBLE void
 490 init_find_screen(GstSparrow *sparrow){
 491   //reset_pattern(GstSparrow *sparrow);
 492 }
 493
 494 INVISIBLE void
 495 init_find_edges(GstSparrow *sparrow){}