store exact index, not x, y in map_lut, as subpixel precision is unused
[sparrow.git] / calibrate.c
blob694ef87d8f40db77480aee8ee4a7032f7f8600b8
1 /* Copyright (C) <2010> Douglas Bagnall <douglas@halo.gen.nz>
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.
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.
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.
19 #include "sparrow.h"
20 #include "gstsparrow.h"
21 #include "calibrate.h"
23 #include <string.h>
24 #include <math.h>
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;
36 guint32 *line2 = line + stride;
37 for(y = 1; y < shape->h; y++){
38 memcpy(line2, line, shape->w * PIXSIZE);
39 line2 += stride;
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;
59 done:
60 return;
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);
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]);
87 been_here = 1;
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);
94 if (on){
95 for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
96 rectangle(sparrow, out, &shapes[i], calibrate->outcolour);
99 countdown--;
101 #endif
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);
111 else{
112 calibrate->wait = RANDINT(sparrow, CALIBRATE_OFF_MIN_T, CALIBRATE_OFF_MAX_T);
114 calibrate->on = on;
115 calibrate->transitions++;
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;
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;
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;
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))) ? '*' : '.';
153 s[64] = 0;
154 return s;
158 /*return 1 if a reasonably likely lag has been found */
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);
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};
175 GST_DEBUG("pattern: %s %llx\n", int64_to_binary_string(pattern_debug, target_pattern),
176 target_pattern);
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;
186 guint64 mask = ((guint64)-1) >> MAX_CALIBRATION_LAG;
187 guint32 best = hamming_distance64(record, target_pattern, mask);
188 guint32 lag = 0;
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.
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;
205 if (sparrow->debug){
206 colour_coded_pixel(&frame[i], lag, best);
209 if (best <= CALIBRATE_MAX_VOTE_ERROR){
210 votes[lag] += 1 >> (CALIBRATE_MAX_VOTE_ERROR - best);
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
228 if (sparrow->debug){
229 debug_frame(sparrow, sparrow->debug_frame, sparrow->in.width, sparrow->in.height, PIXSIZE);
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;
240 if (votes[i]){
241 GST_DEBUG("%d votes for %d\n", votes[i], i);
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;
250 return res;
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;
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);
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);
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;
282 else {
283 sparrow->countdown = CALIBRATE_RETRY_WAIT;
286 memset(out, 0, sparrow->out.size);
287 gboolean on = cycle_pattern(sparrow);
288 if (on){
289 draw_shapes(sparrow, out);
291 #if FAKE_OTHER_PROJECTION
292 add_random_signal(sparrow, out);
293 #endif
294 sparrow->countdown--;
295 return ret;
302 /*init functions */
305 INVISIBLE void
306 finalise_find_self(GstSparrow *sparrow)
308 sparrow_calibrate_t *calibrate = (sparrow_calibrate_t *)sparrow->helper_struct;
309 free(calibrate->lag_table);
310 free(calibrate);
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));
321 calibrate->incolour = sparrow->in.colours[SPARROW_WHITE];
322 calibrate->outcolour = sparrow->out.colours[SPARROW_WHITE];
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);
329 int i;
330 for (i = 0; i < MAX_CALIBRATE_SHAPES; i++){
331 init_one_square(sparrow, &(calibrate->shapes[i]));
333 calibrate->n_shapes = MAX_CALIBRATE_SHAPES;
334 sparrow->countdown = CALIBRATE_INITIAL_WAIT;