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