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 static inline void
  28 rectangle(GstSparrow *sparrow, guint8 *out, sparrow_shape_t *shape){
  29   int y, x;
  30   guint stride = sparrow->out.width;
  31   guint32 *line = ((guint32 *)out) + shape->y * stride + shape->x;
  32   for (x = 0; x < shape->w; x++){
  33     line[x] = sparrow->calibrate.outcolour;
  34   }
  35   guint32 *line2 = line + stride;
  36   for(y = 1; y < shape->h; y++){
  37     memcpy(line2, line, shape->w * PIXSIZE);
  38     line2 += stride;
  39   }
  40 }
  41
  42 static void draw_shapes(GstSparrow *sparrow, guint8 *out){
  43   int i;
  44   sparrow_shape_t *shape;
  45   for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
  46     shape = sparrow->shapes + i;
  47     switch (shape->shape){
  48     case NO_SHAPE:
  49       goto done; /* an empty one ends the list */
  50     case RECTANGLE:
  51       rectangle(sparrow, out, shape);
  52       break;
  53     default:
  54       break;
  55     }
  56   }
  57  done:
  58   return;
  59 }
  60
  61 static inline void
  62 init_one_square(GstSparrow *sparrow, sparrow_shape_t* shape){
  63     shape->shape = RECTANGLE;
  64     shape->w = CALIBRATE_SELF_SIZE;
  65     shape->h = CALIBRATE_SELF_SIZE;
  66     shape->x  = RANDINT(sparrow, sparrow->out.width / 8,
  67         sparrow->out.width * 7 / 8 - shape->w);
  68     shape->y  = RANDINT(sparrow, sparrow->out.height / 8,
  69         sparrow->out.height * 7 / 8 - shape->h);
  70 }
  71
  72
  73 /*fake other projection */
  74 static void add_random_signal(GstSparrow *sparrow, guint8 *out){
  75   int i;
  76   static sparrow_shape_t shapes[MAX_CALIBRATE_SHAPES];
  77   static int been_here = 0;
  78   static int countdown = 0;
  79   static int on = 0;
  80   if (! been_here){
  81     for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
  82       init_one_square(sparrow, &shapes[i]);
  83     }
  84     been_here = 1;
  85   }
  86   if (! countdown){
  87     on = ! on;
  88     countdown = on ? RANDINT(sparrow, CALIBRATE_ON_MIN_T, CALIBRATE_ON_MAX_T) :
  89       RANDINT(sparrow, CALIBRATE_ON_MIN_T, CALIBRATE_ON_MAX_T);
  90   }
  91   if (on){
  92     for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
  93       rectangle(sparrow, out, &shapes[i]);
  94     }
  95   }
  96   countdown--;
  97 }
  98
  99 static gboolean cycle_pattern(GstSparrow *sparrow){
 100   gboolean on = sparrow->calibrate.on;
 101   if (sparrow->calibrate.wait == 0){
 102     on = !on;
 103     if (on){
 104       sparrow->calibrate.wait = RANDINT(sparrow, CALIBRATE_ON_MIN_T, CALIBRATE_ON_MAX_T);
 105     }
 106     else{
 107       sparrow->calibrate.wait = RANDINT(sparrow, CALIBRATE_OFF_MIN_T, CALIBRATE_OFF_MAX_T);
 108     }
 109     sparrow->calibrate.on = on;
 110     sparrow->calibrate.transitions++;
 111   }
 112   sparrow->calibrate.wait--;
 113   sparrow->lag_record = (sparrow->lag_record << 1) | on;
 114   //GST_DEBUG("lag record %llx, on %i\n", sparrow->lag_record, on);
 115   return on;
 116 }
 117
 118
 119 static inline void
 120 colour_coded_pixel(guint32* pixel, guint32 lag, guint32 shift){
 121 #define CCP_SCALE 2
 122   if (shift < 9 * CCP_SCALE){
 123     shift /= CCP_SCALE;
 124     if (shift == 0){
 125       *pixel = (guint32)-1;
 126     }
 127     else{
 128       shift--;
 129       guint32 c = lag_false_colour[lag];
 130       guint32 mask = (1 << (8 - shift)) - 1;
 131       mask |= (mask << 8);
 132       mask |= (mask << 16); //XXX LUT would be quicker
 133       c >>= shift;
 134       c &= mask;
 135       *pixel = c;
 136     }
 137   }
 138 }
 139
 140
 141 static inline char *
 142 int64_to_binary_string(char *s, guint64 n){
 143   /* s should be a *65* byte array */
 144   int i;
 145   for (i = 0; i < 64; i++){
 146     s[i] = (n & (1ULL << (63 - i))) ? '*' : '.';
 147   }
 148   s[64] = 0;
 149   return s;
 150 }
 151
 152
 153 /*return 1 if a reasonably likely lag has been found */
 154
 155 static inline int
 156 find_lag(GstSparrow *sparrow){
 157   int res = 0;
 158   guint i, j;
 159   guint32 *frame = (guint32 *)sparrow->debug_frame;
 160   if (sparrow->debug){
 161     memset(frame, 0, sparrow->in.size);
 162   }
 163   guint64 target_pattern = sparrow->lag_record;
 164   guint32 overall_best = (guint32)-1;
 165   guint32 overall_lag = 0;
 166   char pattern_debug[65];
 167   int votes[MAX_CALIBRATION_LAG] = {0};
 168
 169   GST_DEBUG("pattern: %s %llx\n", int64_to_binary_string(pattern_debug, target_pattern),
 170       target_pattern);
 171
 172   for (i = 0; i < sparrow->in.pixcount; i++){
 173     guint64 record = sparrow->lag_table[i].record;
 174     if (record == 0 || ~record == 0){
 175       /*ignore this one! it'll never usefully match. */
 176       //frame[i] = 0xffffffff;
 177       continue;
 178     }
 179
 180     guint64 mask = ((guint64)-1) >> MAX_CALIBRATION_LAG;
 181     guint32 best = hamming_distance64(record, target_pattern, mask);
 182     guint32 lag = 0;
 183
 184     for (j = 1; j < MAX_CALIBRATION_LAG; j++){
 185       /*latest frame is least significant bit
 186         >> pushes into future,
 187         << pushes into past
 188         record is presumed to be a few frames past
 189         relative to main record, so we push it back.
 190       */
 191       record <<= 1;
 192       mask <<= 1;
 193       guint32 d = hamming_distance64(record, target_pattern, mask);
 194       if (d < best){
 195         best = d;
 196         lag = j;
 197       }
 198     }
 199     if (sparrow->debug){
 200       colour_coded_pixel(&frame[i], lag, best);
 201     }
 202
 203     if (best <= CALIBRATE_MAX_VOTE_ERROR){
 204       votes[lag] += 1 >> (CALIBRATE_MAX_VOTE_ERROR - best);
 205     }
 206
 207     if (best < overall_best){
 208       overall_best = best;
 209       overall_lag = lag;
 210       char pattern_debug2[65];
 211       guint64 r = sparrow->lag_table[i].record;
 212       GST_DEBUG("Best now: lag  %u! error %u pixel %u\n"
 213           "record:  %s %llx\n"
 214           "pattern: %s %llx\n",
 215           overall_lag, overall_best, i,
 216           int64_to_binary_string(pattern_debug, r), r,
 217           int64_to_binary_string(pattern_debug2, target_pattern), target_pattern
 218       );
 219     }
 220   }
 221
 222   if (sparrow->debug){
 223     debug_frame(sparrow, sparrow->debug_frame, sparrow->in.width, sparrow->in.height);
 224   }
 225
 226   /*calculate votes winner, as a check for winner-takes-all */
 227   guint popular_lag;
 228   int popular_votes = -1;
 229   for (i = 0; i < MAX_CALIBRATION_LAG; i++){
 230     if(votes[i] > popular_votes){
 231       popular_votes = votes[i];
 232       popular_lag = i;
 233     }
 234     if (votes[i]){
 235       GST_DEBUG("%d votes for %d\n", votes[i], i);
 236     }
 237   }
 238   /*votes and best have to agree, and best has to be low */
 239   if (overall_best <= CALIBRATE_MAX_BEST_ERROR &&
 240       overall_lag == popular_lag){
 241     sparrow->lag = overall_lag;
 242     res = 1;
 243   }
 244   return res;
 245 }
 246
 247 static inline void
 248 record_calibration(GstSparrow *sparrow, gint32 offset, int signal){
 249   //signal = (signal != 0);
 250   sparrow->lag_table[offset].record <<= 1;
 251   sparrow->lag_table[offset].record |= signal;
 252 }
 253
 254
 255 INVISIBLE sparrow_state
 256 mode_find_self(GstSparrow *sparrow, guint8 *in, guint8 *out){
 257   int ret = SPARROW_STATUS_QUO;
 258   guint32 i;
 259   guint32 *frame = (guint32 *)in;
 260   /* record the current signal */
 261   for (i = 0; i < sparrow->in.pixcount; i++){
 262     int signal = (((frame[i] >> sparrow->in.gshift) & 255) > CALIBRATE_SIGNAL_THRESHOLD);
 263     record_calibration(sparrow, i, signal);
 264   }
 265   if (sparrow->countdown == 0){
 266     /* analyse the signal */
 267     int r = find_lag(sparrow);
 268     if (r){
 269       GST_DEBUG("lag is set at %u! after %u cycles\n", sparrow->lag, sparrow->frame_count);
 270       ret = SPARROW_NEXT_STATE;
 271     }
 272     else {
 273       init_find_self(sparrow);
 274     }
 275   }
 276
 277   memset(out, 0, sparrow->out.size);
 278   gboolean on = cycle_pattern(sparrow);
 279   if (on){
 280     draw_shapes(sparrow, out);
 281   }
 282 #if FAKE_OTHER_PROJECTION
 283   add_random_signal(sparrow, out);
 284 #endif
 285   sparrow->countdown--;
 286   return ret;
 287 }
 288
 289
 290 static inline void
 291 abs_diff(GstSparrow *sparrow, guint8 *a, guint8 *b, guint8 *target){
 292   sparrow->in_ipl[0]->imageData = (char*) a;
 293   sparrow->in_ipl[1]->imageData = (char*) b;
 294   sparrow->in_ipl[2]->imageData = (char*) target;
 295   cvAbsDiff(sparrow->in_ipl[0], sparrow->in_ipl[1], sparrow->in_ipl[2]);
 296 }
 297
 298
 299 /* wait for the other projector to stop changing, for as many frames as are
 300    set in sparrow->countdown.  When sparrow->countdown reaches 0, return 1.
 301
 302    If sparrow->countdown is already 0, do nothing.
 303
 304    If something happens, reset sparrow->countdown to <blanktime>.
 305 */
 306 static int
 307 wait_for_blank(GstSparrow *sparrow, guint8 *in, guint8 *out, int blanktime){
 308   if (sparrow->countdown){
 309     guint32 i;
 310     abs_diff(sparrow, in, sparrow->prev_frame, sparrow->work_frame);
 311     //Use the green channel
 312     for (i = (sparrow->in.gshift >> 8);
 313          i < sparrow->in.pixcount * PIXSIZE;
 314          i += 4){
 315       guint8 signal = sparrow->work_frame[i];
 316       if (signal > CALIBRATE_WAIT_SIGNAL_THRESHOLD){
 317         sparrow->countdown = blanktime;
 318         break;
 319       }
 320     }
 321     memset(out, 0, sparrow->out.size);
 322     sparrow->countdown--;
 323   }
 324   return (sparrow->countdown == 0);
 325 }
 326
 327
 328 static inline void
 329 new_calibration_colour(GstSparrow *sparrow){
 330   int c = RANDINT(sparrow, 1, 3);
 331   sparrow->calibrate.incolour = sparrow->in.colours[c];
 332   sparrow->calibrate.outcolour = sparrow->out.colours[c];
 333   //sparrow->calibrate.wait == 0;
 334 }
 335
 336 /* Choose between green or magenta, randomly and iteratively, until the
 337    other one chooses something else.  But first wait for blank? */
 338
 339 INVISIBLE sparrow_state
 340 mode_pick_colour(GstSparrow *sparrow, guint8 *in, guint8 *out){
 341   if(wait_for_blank(sparrow, in, out, WAIT_COUNTDOWN)){
 342     new_calibration_colour(sparrow);
 343     sparrow->calibrate.wait = sparrow->lag + 2;
 344   }
 345   sparrow->calibrate.wait--;
 346   return SPARROW_STATUS_QUO;
 347 }
 348
 349
 350 INVISIBLE sparrow_state
 351 mode_wait_for_grid(GstSparrow *sparrow, guint8 *in, guint8 *out){
 352   if (wait_for_blank(sparrow, in, out, WAIT_COUNTDOWN)){
 353     return SPARROW_NEXT_STATE;
 354   }
 355   return SPARROW_STATUS_QUO;
 356 }
 357
 358 /*init functions */
 359
 360 INVISIBLE void
 361 init_wait_for_grid(GstSparrow *sparrow){}
 362
 363 INVISIBLE void
 364 init_find_grid(GstSparrow *sparrow){}
 365
 366 INVISIBLE void
 367 init_find_self(GstSparrow *sparrow){
 368   sparrow->calibrate.incolour = sparrow->in.colours[SPARROW_WHITE];
 369   sparrow->calibrate.outcolour = sparrow->out.colours[SPARROW_WHITE];
 370   if (! sparrow->n_shapes){
 371     int i;
 372     for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
 373       init_one_square(sparrow, &(sparrow->shapes[i]));
 374     }
 375     sparrow->n_shapes = MAX_CALIBRATE_SHAPES;
 376     sparrow->countdown = CALIBRATE_INITIAL_WAIT;
 377   }
 378   else {
 379     sparrow->countdown = CALIBRATE_RETRY_WAIT;
 380   }
 381 }
 382
 383 INVISIBLE void
 384 init_pick_colour(GstSparrow *sparrow)
 385 {
 386   sparrow->countdown = WAIT_COUNTDOWN;
 387 }
 388