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   guint y;
  49   guint stride = sparrow->out.width * PIXSIZE;
  50   guint8 *line = out + shape->y * stride + shape->x * PIXSIZE;
  51   for(y = 0; y < (guint)shape->h; y++){
  52     memset(line, 255, shape->w * PIXSIZE);
  53     line += stride;
  54   }
  55 }
  56
  57 static void draw_shapes(GstSparrow *sparrow, guint8 *out){
  58   int i;
  59   sparrow_shape_t *shape;
  60   for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
  61     shape = sparrow->shapes + i;
  62     switch (shape->shape){
  63     case NO_SHAPE:
  64       goto done; /* an empty one ends the list */
  65     case VERTICAL_LINE:
  66       vertical_line(sparrow, out, shape->x);
  67       break;
  68     case HORIZONTAL_LINE:
  69       horizontal_line(sparrow, out, shape->x);
  70       break;
  71     case RECTANGLE:
  72       rectangle(sparrow, out, shape);
  73       break;
  74     case FULLSCREEN:
  75       memset(out, 255, sparrow->out.size);
  76       break;
  77     }
  78   }
  79  done:
  80   return;
  81 }
  82
  83 static inline void
  84 init_one_square(GstSparrow *sparrow, sparrow_shape_t* shape){
  85     shape->shape = RECTANGLE;
  86     shape->w = CALIBRATE_SELF_SIZE;
  87     shape->h = CALIBRATE_SELF_SIZE;
  88     shape->x  = RANDINT(sparrow, sparrow->out.width / 8,
  89         sparrow->out.width * 7 / 8 - shape->w);
  90     shape->y  = RANDINT(sparrow, sparrow->out.height / 8,
  91         sparrow->out.height * 7 / 8 - shape->h);
  92 }
  93
  94 static void calibrate_init_squares(GstSparrow *sparrow){
  95   int i;
  96   for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
  97     init_one_square(sparrow, &(sparrow->shapes[i]));
  98   }
  99 }
 100
 101 /*fake other projection */
 102 static void add_random_signal(GstSparrow *sparrow, guint8 *out){
 103   int i;
 104   static sparrow_shape_t shapes[MAX_CALIBRATE_SHAPES];
 105   static int been_here = 0;
 106   static int countdown = 0;
 107   static int on = 0;
 108   if (! been_here){
 109     for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
 110       init_one_square(sparrow, &shapes[i]);
 111     }
 112     been_here = 1;
 113   }
 114   if (! countdown){
 115     on = ! on;
 116     countdown = on ? RANDINT(sparrow, CALIBRATE_ON_MIN_T, CALIBRATE_ON_MAX_T) :
 117       RANDINT(sparrow, CALIBRATE_ON_MIN_T, CALIBRATE_ON_MAX_T);
 118   }
 119   if (on){
 120     for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
 121       rectangle(sparrow, out, &shapes[i]);
 122     }
 123   }
 124   countdown--;
 125 }
 126
 127
 128 INVISIBLE void
 129 calibrate_init_lines(GstSparrow *sparrow){
 130   int i;
 131   sparrow_shape_t* shape = sparrow->shapes;
 132   for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
 133     shape[i].w = 1;
 134     shape[i].h = 1;
 135     shape[i].x = 0;
 136     shape[i].y = 0;
 137     shape[i].shape = NO_SHAPE;
 138   }
 139   /* shape[0] will be set to vertical or horizontal in due course */
 140 }
 141
 142
 143
 144
 145 static inline void
 146 record_calibration(GstSparrow *sparrow, gint32 offset, int signal){
 147   //signal = (signal != 0);
 148   sparrow->lag_table[offset].record <<= 1;
 149   sparrow->lag_table[offset].record |= signal;
 150 }
 151
 152 static inline void
 153 colour_coded_pixel(guint32* pixel, guint32 lag, guint32 shift){
 154 #define CCP_SCALE 2
 155   if (shift < 9 * CCP_SCALE){
 156     shift /= CCP_SCALE;
 157     if (shift == 0){
 158       *pixel = (guint32)-1;
 159     }
 160     else{
 161       shift--;
 162       guint32 c = lag_false_colour[lag];
 163       guint32 mask = (1 << (8 - shift)) - 1;
 164       mask |= (mask << 8);
 165       mask |= (mask << 16); //XXX LUT would be quicker
 166       c >>= shift;
 167       c &= mask;
 168       *pixel = c;
 169     }
 170   }
 171 }
 172
 173
 174 static inline char *
 175 int64_to_binary_string(char *s, guint64 n){
 176   /* s should be a *65* byte array */
 177   int i;
 178   for (i = 0; i < 64; i++){
 179     s[i] = (n & (1ULL << (63 - i))) ? '*' : '.';
 180   }
 181   s[64] = 0;
 182   return s;
 183 }
 184
 185
 186 /*return 1 if a reasonably likely lag has been found */
 187
 188 static inline int
 189 find_lag(GstSparrow *sparrow){
 190   int res = 0;
 191   guint i, j;
 192   guint32 *frame = (guint32 *)sparrow->debug_frame;
 193   if (sparrow->debug){
 194     memset(frame, 0, sparrow->in.size);
 195   }
 196   guint64 target_pattern = sparrow->lag_record;
 197   guint32 overall_best = (guint32)-1;
 198   guint32 overall_lag = 0;
 199   char pattern_debug[65];
 200   int votes[MAX_CALIBRATION_LAG] = {0};
 201
 202   GST_DEBUG("pattern: %s %llx\n", int64_to_binary_string(pattern_debug, target_pattern),
 203       target_pattern);
 204
 205   for (i = 0; i < sparrow->in.pixcount; i++){
 206     guint64 record = sparrow->lag_table[i].record;
 207     if (record == 0 || ~record == 0){
 208       /*ignore this one! it'll never usefully match. */
 209       //frame[i] = 0xffffffff;
 210       continue;
 211     }
 212
 213     guint64 mask = ((guint64)-1) >> MAX_CALIBRATION_LAG;
 214     guint32 best = hamming_distance64(record, target_pattern, mask);
 215     guint32 lag = 0;
 216
 217     for (j = 1; j < MAX_CALIBRATION_LAG; j++){
 218       /*latest frame is least significant bit
 219         >> pushes into future,
 220         << pushes into past
 221         record is presumed to be a few frames past
 222         relative to main record, so we push it back.
 223       */
 224       record <<= 1;
 225       mask <<= 1;
 226       guint32 d = hamming_distance64(record, target_pattern, mask);
 227       if (d < best){
 228         best = d;
 229         lag = j;
 230       }
 231     }
 232     if (sparrow->debug){
 233       colour_coded_pixel(&frame[i], lag, best);
 234     }
 235
 236     if (best <= CALIBRATE_MAX_VOTE_ERROR){
 237       votes[lag] += 1 >> (CALIBRATE_MAX_VOTE_ERROR - best);
 238     }
 239
 240     if (best < overall_best){
 241       overall_best = best;
 242       overall_lag = lag;
 243       char pattern_debug2[65];
 244       guint64 r = sparrow->lag_table[i].record;
 245       GST_DEBUG("Best now: lag  %u! error %u pixel %u\n"
 246           "record:  %s %llx\n"
 247           "pattern: %s %llx\n",
 248           overall_lag, overall_best, i,
 249           int64_to_binary_string(pattern_debug, r), r,
 250           int64_to_binary_string(pattern_debug2, target_pattern), target_pattern
 251       );
 252     }
 253   }
 254
 255   if (sparrow->debug){
 256     debug_frame(sparrow, sparrow->debug_frame, sparrow->in.width, sparrow->in.height);
 257   }
 258
 259   /*calculate votes winner, as a check for winner-takes-all */
 260   guint popular_lag;
 261   int popular_votes = -1;
 262   for (i = 0; i < MAX_CALIBRATION_LAG; i++){
 263     if(votes[i] > popular_votes){
 264       popular_votes = votes[i];
 265       popular_lag = i;
 266     }
 267     if (votes[i]){
 268       GST_DEBUG("%d votes for %d\n", votes[i], i);
 269     }
 270   }
 271   /*votes and best have to agree, and best has to be low */
 272   if (overall_best <= CALIBRATE_MAX_BEST_ERROR &&
 273       overall_lag == popular_lag){
 274     sparrow->lag = overall_lag;
 275     res = 1;
 276   }
 277   return res;
 278 }
 279
 280
 281 static inline void
 282 abs_diff(GstSparrow *sparrow, guint8 *a, guint8 *b, guint8 *target){
 283   sparrow->in_ipl[0]->imageData = (char*) a;
 284   sparrow->in_ipl[1]->imageData = (char*) b;
 285   sparrow->in_ipl[2]->imageData = (char*) target;
 286   cvAbsDiff(sparrow->in_ipl[0], sparrow->in_ipl[1], sparrow->in_ipl[2]);
 287 }
 288
 289 static inline void
 290 threshold(GstSparrow *sparrow, guint8 *frame, guint8 *target, guint threshold){
 291   sparrow->in_ipl[0]->imageData = (char*) frame;
 292   sparrow->in_ipl[1]->imageData = (char*) target;
 293   //cvAbsDiff(sparrow->in_ipl[0], sparrow->in_ipl[1], sparrow->in_ipl[2]);
 294   cvCmpS(sparrow->in_ipl[0], (double)threshold, sparrow->in_ipl[1], CV_CMP_GT);
 295 }
 296
 297
 298 void INVISIBLE
 299 reset_find_self(GstSparrow *sparrow, gint first){
 300   if (first){
 301     calibrate_init_squares(sparrow);
 302     sparrow->countdown = CALIBRATE_INITIAL_WAIT;
 303   }
 304   else {
 305     sparrow->countdown = CALIBRATE_RETRY_WAIT;
 306   }
 307 }
 308
 309 /*compare the frame to the new one. regions of change should indicate the
 310   square is about.
 311 */
 312 static inline int
 313 calibrate_find_self(GstSparrow *sparrow, guint8 *in){
 314   //GST_DEBUG("finding square\n");
 315   int res = 0;
 316   if(sparrow->prev_frame){
 317     //debug_frame(sparrow, sparrow->in_frame, sparrow->in.width, sparrow->in.height);
 318     guint32 i;
 319     guint32 *frame = (guint32 *)in;
 320     for (i = 0; i < sparrow->in.pixcount; i++){
 321       int signal = (((frame[i] >> sparrow->in.gshift) & 255) > CALIBRATE_SIGNAL_THRESHOLD);
 322       record_calibration(sparrow, i, signal);
 323     }
 324     if (sparrow->countdown == 0){
 325       res = find_lag(sparrow);
 326       if (res){
 327         GST_DEBUG("lag is set at %u! after %u cycles\n", sparrow->lag, sparrow->frame_count);
 328       }
 329       else {
 330         reset_find_self(sparrow, 0);
 331       }
 332     }
 333   }
 334   sparrow->countdown--;
 335   return res;
 336 }
 337
 338
 339 static gboolean cycle_pattern(GstSparrow *sparrow){
 340   gboolean on = sparrow->calibrate.on;
 341   if (sparrow->calibrate.wait == 0){
 342     on = !on;
 343     if (on){
 344       sparrow->calibrate.wait = RANDINT(sparrow, CALIBRATE_ON_MIN_T, CALIBRATE_ON_MAX_T);
 345     }
 346     else{
 347       sparrow->calibrate.wait = RANDINT(sparrow, CALIBRATE_OFF_MIN_T, CALIBRATE_OFF_MAX_T);
 348     }
 349     sparrow->calibrate.on = on;
 350     sparrow->calibrate.transitions++;
 351   }
 352   sparrow->calibrate.wait--;
 353   sparrow->lag_record = (sparrow->lag_record << 1) | on;
 354   //GST_DEBUG("lag record %llx, on %i\n", sparrow->lag_record, on);
 355   return on;
 356 }
 357
 358 static void
 359 see_grid(GstSparrow *sparrow, guint8 *in){
 360 }
 361
 362 static void
 363 find_grid(GstSparrow *sparrow, guint8 *in, guint8 *out){
 364   see_grid(sparrow, in);
 365   int on = cycle_pattern(sparrow);
 366   memset(out, 0, sparrow->out.size);
 367   if (on){
 368     draw_shapes(sparrow, out);
 369   }
 370 }
 371
 372 static void
 373 see_edges(GstSparrow *sparrow, guint8 *in){
 374   /* there is a big flash of white. or not.  look for the largest area of
 375      light.
 376
 377      perhaps, threshold at one or two levels.  or more. if they agree they are
 378      probably right.
 379
 380      or floodfill (and fill in)
 381
 382      canny edge detection:
 383      http://opencv.willowgarage.com/documentation/c/feature_detection.html#canny
 384
 385  */
 386   guint i;
 387   for (i = 0; i < sparrow->in.pixcount; i++){
 388
 389     guint32 signal = sparrow->work_frame[i * PIXSIZE + 2];
 390     if (signal > CALIBRATE_WAIT_SIGNAL_THRESHOLD){
 391       sparrow->countdown = WAIT_COUNTDOWN;
 392       break;
 393     }
 394   }
 395
 396
 397 }
 398
 399 INVISIBLE sparrow_state
 400 mode_find_edges(GstSparrow *sparrow, guint8 *in, guint8 *out){
 401   see_edges(sparrow, in);
 402   int on = cycle_pattern(sparrow);
 403   if (on){
 404     memset(out, 255, sparrow->out.size);
 405   }
 406   else {
 407     memset(out, 0, sparrow->out.size);
 408   }
 409   return SPARROW_STATUS_QUO;
 410 }
 411
 412 INVISIBLE void
 413 init_find_edges(GstSparrow *sparrow, guint8 *in, guint8 *out){
 414   //reset_pattern(GstSparrow *sparrow);
 415 }
 416
 417 INVISIBLE sparrow_state
 418 mode_find_self(GstSparrow *sparrow, guint8 *in, guint8 *out){
 419   if(calibrate_find_self(sparrow, in)){
 420     return SPARROW_WAIT_FOR_GRID;
 421   }
 422   gboolean on = cycle_pattern(sparrow);
 423   memset(out, 0, sparrow->out.size);
 424   if (on){
 425     draw_shapes(sparrow, out);
 426   }
 427 #if FAKE_OTHER_PROJECTION
 428   add_random_signal(sparrow, out);
 429 #endif
 430   return SPARROW_STATUS_QUO;
 431 }
 432
 433 /* wait for the other projector to stop changing: sufficient to look for no
 434    significant changes for as long as the longest pattern interval */
 435
 436 static int
 437 wait_for_blank(GstSparrow *sparrow, guint8 *in, guint8 *out){
 438   guint32 i;
 439   abs_diff(sparrow, in, sparrow->prev_frame, sparrow->work_frame);
 440   for (i = 0; i < sparrow->in.pixcount; i++){
 441     guint32 signal = sparrow->work_frame[i * PIXSIZE + 2];  //possibly R, G, or B, but never A
 442     if (signal > CALIBRATE_WAIT_SIGNAL_THRESHOLD){
 443       sparrow->countdown = WAIT_COUNTDOWN;
 444       break;
 445     }
 446   }
 447   memset(out, 0, sparrow->out.size);
 448   sparrow->countdown--;
 449   return (sparrow->countdown == 0);
 450 }
 451
 452
 453 INVISIBLE sparrow_state
 454 mode_wait_for_grid(GstSparrow *sparrow, guint8 *in, guint8 *out){
 455   if (wait_for_blank(sparrow, in, out)){
 456     return SPARROW_FIND_EDGES;
 457   }
 458   return SPARROW_STATUS_QUO;
 459 }
 460
 461 INVISIBLE calibrate_init_grid(GstSparrow *sparrow){}