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 #if 0
  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 #endif
 102
 103 static gboolean cycle_pattern(GstSparrow *sparrow){
 104   sparrow_calibrate_t *calibrate = (sparrow_calibrate_t *)sparrow->helper_struct;
 105   gboolean on = calibrate->on;
 106   if (calibrate->wait == 0){
 107     on = !on;
 108     if (on){
 109       calibrate->wait = RANDINT(sparrow, CALIBRATE_ON_MIN_T, CALIBRATE_ON_MAX_T);
 110     }
 111     else{
 112       calibrate->wait = RANDINT(sparrow, CALIBRATE_OFF_MIN_T, CALIBRATE_OFF_MAX_T);
 113     }
 114     calibrate->on = on;
 115     calibrate->transitions++;
 116   }
 117   calibrate->wait--;
 118   calibrate->lag_record = (calibrate->lag_record << 1) | on;
 119   //GST_DEBUG("lag record %llx, on %i\n", sparrow->lag_record, on);
 120   return on;
 121 }
 122
 123
 124 static inline void
 125 colour_coded_pixel(guint32* pixel, guint32 lag, guint32 shift){
 126 #define CCP_SCALE 2
 127   if (shift < 9 * CCP_SCALE){
 128     shift /= CCP_SCALE;
 129     if (shift == 0){
 130       *pixel = (guint32)-1;
 131     }
 132     else{
 133       shift--;
 134       guint32 c = lag_false_colour[lag];
 135       guint32 mask = (1 << (8 - shift)) - 1;
 136       mask |= (mask << 8);
 137       mask |= (mask << 16); //XXX LUT would be quicker
 138       c >>= shift;
 139       c &= mask;
 140       *pixel = c;
 141     }
 142   }
 143 }
 144
 145
 146 static inline char *
 147 int64_to_binary_string(char *s, guint64 n){
 148   /* s should be a *65* byte array */
 149   int i;
 150   for (i = 0; i < 64; i++){
 151     s[i] = (n & (1ULL << (63 - i))) ? '*' : '.';
 152   }
 153   s[64] = 0;
 154   return s;
 155 }
 156
 157
 158 /*return 1 if a reasonably likely lag has been found */
 159
 160 static inline int
 161 find_lag(GstSparrow *sparrow){
 162   sparrow_calibrate_t *calibrate = (sparrow_calibrate_t *)sparrow->helper_struct;
 163   int res = 0;
 164   guint i, j;
 165   guint32 *frame = (guint32 *)sparrow->debug_frame;
 166   if (sparrow->debug){
 167     memset(frame, 0, sparrow->in.size);
 168   }
 169   guint64 target_pattern = calibrate->lag_record;
 170   guint32 overall_best = (guint32)-1;
 171   guint32 overall_lag = 0;
 172   char pattern_debug[65];
 173   int votes[MAX_CALIBRATION_LAG] = {0};
 174
 175   GST_DEBUG("pattern: %s %llx\n", int64_to_binary_string(pattern_debug, target_pattern),
 176       target_pattern);
 177
 178   for (i = 0; i < sparrow->in.pixcount; i++){
 179     guint64 record = calibrate->lag_table[i].record;
 180     if (record == 0 || ~record == 0){
 181       /*ignore this one! it'll never usefully match. */
 182       //frame[i] = 0xffffffff;
 183       continue;
 184     }
 185
 186     guint64 mask = ((guint64)-1) >> MAX_CALIBRATION_LAG;
 187     guint32 best = hamming_distance64(record, target_pattern, mask);
 188     guint32 lag = 0;
 189
 190     for (j = 1; j < MAX_CALIBRATION_LAG; j++){
 191       /*latest frame is least significant bit
 192         >> pushes into future,
 193         << pushes into past
 194         record is presumed to be a few frames past
 195         relative to main record, so we push it back.
 196       */
 197       record <<= 1;
 198       mask <<= 1;
 199       guint32 d = hamming_distance64(record, target_pattern, mask);
 200       if (d < best){
 201         best = d;
 202         lag = j;
 203       }
 204     }
 205     if (sparrow->debug){
 206       colour_coded_pixel(&frame[i], lag, best);
 207     }
 208
 209     if (best <= CALIBRATE_MAX_VOTE_ERROR){
 210       votes[lag] += 1 >> (CALIBRATE_MAX_VOTE_ERROR - best);
 211     }
 212
 213     if (best < overall_best){
 214       overall_best = best;
 215       overall_lag = lag;
 216       char pattern_debug2[65];
 217       guint64 r = calibrate->lag_table[i].record;
 218       GST_DEBUG("Best now: lag  %u! error %u pixel %u\n"
 219           "record:  %s %llx\n"
 220           "pattern: %s %llx\n",
 221           overall_lag, overall_best, i,
 222           int64_to_binary_string(pattern_debug, r), r,
 223           int64_to_binary_string(pattern_debug2, target_pattern), target_pattern
 224       );
 225     }
 226   }
 227
 228   if (sparrow->debug){
 229     debug_frame(sparrow, sparrow->debug_frame, sparrow->in.width, sparrow->in.height, PIXSIZE);
 230   }
 231
 232   /*calculate votes winner, as a check for winner-takes-all */
 233   guint popular_lag;
 234   int popular_votes = -1;
 235   for (i = 0; i < MAX_CALIBRATION_LAG; i++){
 236     if(votes[i] > popular_votes){
 237       popular_votes = votes[i];
 238       popular_lag = i;
 239     }
 240     if (votes[i]){
 241       GST_DEBUG("%d votes for %d\n", votes[i], i);
 242     }
 243   }
 244   /*votes and best have to agree, and best has to be low */
 245   if (overall_best <= CALIBRATE_MAX_BEST_ERROR &&
 246       overall_lag == popular_lag){
 247     sparrow->lag = overall_lag;
 248     res = 1;
 249   }
 250   return res;
 251 }
 252
 253 static inline void
 254 record_calibration(GstSparrow *sparrow, gint32 offset, int signal){
 255   //signal = (signal != 0);
 256   sparrow_calibrate_t *calibrate = (sparrow_calibrate_t *)sparrow->helper_struct;
 257   calibrate->lag_table[offset].record <<= 1;
 258   calibrate->lag_table[offset].record |= signal;
 259 }
 260
 261
 262 INVISIBLE sparrow_state
 263 mode_find_self(GstSparrow *sparrow, GstBuffer *inbuf, GstBuffer *outbuf){
 264   guint8 *in = GST_BUFFER_DATA(inbuf);
 265   guint8 *out = GST_BUFFER_DATA(outbuf);
 266
 267   int ret = SPARROW_STATUS_QUO;
 268   guint32 i;
 269   guint32 *frame = (guint32 *)in;
 270   /* record the current signal */
 271   for (i = 0; i < sparrow->in.pixcount; i++){
 272     int signal = (((frame[i] >> sparrow->in.gshift) & 255) > CALIBRATE_SIGNAL_THRESHOLD);
 273     record_calibration(sparrow, i, signal);
 274   }
 275   if (sparrow->countdown == 0){
 276     /* analyse the signal */
 277     int r = find_lag(sparrow);
 278     if (r){
 279       GST_DEBUG("lag is set at %u! after %u cycles\n", sparrow->lag, sparrow->frame_count);
 280       ret = SPARROW_NEXT_STATE;
 281     }
 282     else {
 283       sparrow->countdown = CALIBRATE_RETRY_WAIT;
 284     }
 285   }
 286   memset(out, 0, sparrow->out.size);
 287   gboolean on = cycle_pattern(sparrow);
 288   if (on){
 289     draw_shapes(sparrow, out);
 290   }
 291 #if FAKE_OTHER_PROJECTION
 292   add_random_signal(sparrow, out);
 293 #endif
 294   sparrow->countdown--;
 295   return ret;
 296 }
 297
 298
 299
 300
 301
 302 /*init functions */
 303
 304
 305 INVISIBLE void
 306 finalise_find_self(GstSparrow *sparrow)
 307 {
 308   sparrow_calibrate_t *calibrate = (sparrow_calibrate_t *)sparrow->helper_struct;
 309   free(calibrate->lag_table);
 310   free(calibrate);
 311 }
 312
 313
 314 INVISIBLE void
 315 init_find_self(GstSparrow *sparrow){
 316   sparrow_calibrate_t *calibrate = zalloc_aligned_or_die(sizeof(sparrow_calibrate_t));
 317   sparrow->helper_struct = (void *)calibrate;
 318   GST_DEBUG("allocating %u * %u for lag_table\n", sparrow->in.pixcount, sizeof(lag_times_t));
 319   calibrate->lag_table = zalloc_aligned_or_die(sparrow->in.pixcount * sizeof(lag_times_t));
 320
 321   calibrate->incolour = sparrow->in.colours[SPARROW_WHITE];
 322   calibrate->outcolour = sparrow->out.colours[SPARROW_WHITE];
 323
 324   /*initialise IPL structs for openCV */
 325   for (int i = 0; i < SPARROW_N_IPL_IN; i++){
 326     calibrate->in_ipl[i] = init_ipl_image(&sparrow->in, PIXSIZE);
 327   }
 328
 329   int i;
 330   for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
 331     init_one_square(sparrow, &(calibrate->shapes[i]));
 332   }
 333   calibrate->n_shapes = MAX_CALIBRATE_SHAPES;
 334   sparrow->countdown = CALIBRATE_INITIAL_WAIT;
 335 }
 336