store exact index, not x, y in map_lut, as subpixel precision is unused
[sparrow.git] / edges.c
blob7008b8a08a160debf30a202436190f53b888c23b
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.
20 #include "sparrow.h"
21 #include "gstsparrow.h"
22 #include "edges.h"
24 #include <string.h>
25 #include <math.h>
26 #include <unistd.h>
28 #include "cv.h"
29 #include "median.h"
31 static int global_number_of_edge_finders = 0;
33 static void dump_edges_info(GstSparrow *sparrow, sparrow_find_lines_t *fl, const char *filename){
34 GST_DEBUG("about to save to %s\n", filename);
35 FILE *f = fopen(filename, "w");
36 sparrow_fl_condensed_t condensed;
37 condensed.n_vlines = fl->n_vlines;
38 condensed.n_hlines = fl->n_hlines;
40 /* simply write fl, map, clusters and mesh in sequence */
41 GST_DEBUG("fl is %p, file is %p\n", fl, f);
42 GST_DEBUG("fl: %d x %d\n", sizeof(sparrow_find_lines_t), 1);
43 fwrite(&condensed, sizeof(sparrow_fl_condensed_t), 1, f);
44 GST_DEBUG("fl->map %d x %d\n", sizeof(sparrow_intersect_t), sparrow->in.pixcount);
45 fwrite(fl->map, sizeof(sparrow_intersect_t), sparrow->in.pixcount, f);
46 GST_DEBUG("fl->clusters %d x %d\n", sizeof(sparrow_cluster_t), fl->n_hlines * fl->n_vlines);
47 fwrite(fl->clusters, sizeof(sparrow_cluster_t), fl->n_hlines * fl->n_vlines, f);
48 GST_DEBUG("fl->mesh %d x %d\n", sizeof(sparrow_corner_t), fl->n_hlines * fl->n_vlines);
49 fwrite(fl->mesh, sizeof(sparrow_corner_t), fl->n_hlines * fl->n_vlines, f);
50 /*and write the mask too */
51 GST_DEBUG("sparrow->screenmask\n");
52 fwrite(sparrow->screenmask, 1, sparrow->in.pixcount, f);
53 fclose(f);
56 static void read_edges_info(GstSparrow *sparrow, sparrow_find_lines_t *fl, const char *filename){
57 FILE *f = fopen(filename, "r");
58 sparrow_fl_condensed_t condensed;
59 size_t read = fread(&condensed, sizeof(sparrow_fl_condensed_t), 1, f);
60 assert(condensed.n_hlines == fl->n_hlines);
61 assert(condensed.n_vlines == fl->n_vlines);
63 guint n_corners = fl->n_hlines * fl->n_vlines;
64 read += fread(fl->map, sizeof(sparrow_intersect_t), sparrow->in.pixcount, f);
65 read += fread(fl->clusters, sizeof(sparrow_cluster_t), n_corners, f);
66 read += fread(fl->mesh, sizeof(sparrow_corner_t), n_corners, f);
67 read += fread(sparrow->screenmask, 1, sparrow->in.pixcount, f);
68 fclose(f);
71 static void
72 debug_map_lut(GstSparrow *sparrow, sparrow_find_lines_t *fl){
73 guint32 *map_lut = sparrow->map_lut;
74 if (sparrow->debug){
75 debug_frame(sparrow, (guint8*)map_lut, sparrow->out.width, sparrow->out.height, PIXSIZE);
79 #if USE_FLOAT_COORDS
81 #define COORD_TO_INT(x)((int)((x) + 0.5))
82 #define COORD_TO_FLOAT(x)((double)(x))
83 #define INT_TO_COORD(x)((coord_t)(x))
85 static inline int
86 coord_to_int_clamp(coord_t x, const int max_plus_one){
87 if (x < 0)
88 return 0;
89 if (x >= max_plus_one - 1.5)
90 return max_plus_one - 1;
91 return (int)(x + 0.5);
94 static inline int
95 coord_to_int_clamp_dither(sparrow_find_lines_t *fl, coord_t x,
96 const int max_plus_one, const int i){
97 if (x < 0)
98 return 0;
99 x += fl->dither[i];
100 if (x >= max_plus_one)
101 return max_plus_one - 1;
102 return (int)x;
106 static inline int
107 coord_in_range(coord_t x, const int max_plus_one){
108 return x >= 0 && (x + 0.5 < max_plus_one);
111 #else
113 #define COORD_TO_INT(x)((x) / (1 << SPARROW_FIXED_POINT))
114 #define COORD_TO_FLOAT(x)(((double)(x)) / (1 << SPARROW_FIXED_POINT))
115 #define INT_TO_COORD(x)((x) * (1 << SPARROW_FIXED_POINT))
117 static inline int
118 coord_to_int_clamp(coord_t x, const int max_plus_one){
119 if (x < 0)
120 return 0;
121 x >>= SPARROW_FIXED_POINT;
122 if (x >= max_plus_one)
123 return max_plus_one - 1;
124 return x;
127 static inline int
128 coord_in_range(coord_t x, const int max_plus_one){
129 return x >= 0 && (x < max_plus_one << SPARROW_FIXED_POINT);
132 #endif
134 //these ones are common
135 static inline int
136 coords_to_index(coord_t x, coord_t y, int w, int h){
137 int iy = coord_to_int_clamp(y, h);
138 int ix = coord_to_int_clamp(x, w);
139 return iy * w + ix;
142 #define C2I COORD_TO_INT
143 #define C2F COORD_TO_FLOAT
145 /********************************************/
147 static void
148 corners_to_full_lut(GstSparrow *sparrow, sparrow_find_lines_t *fl){
149 DEBUG_FIND_LINES(fl);
150 sparrow_corner_t *mesh = fl->mesh; /*maps regular points in ->out to points in ->in */
151 guint32 *map_lut = sparrow->map_lut;
152 int mesh_w = fl->n_vlines;
153 int mesh_h = fl->n_hlines;
154 int mcy, mmy, mcx, mmx; /*Mesh Corner|Modulus X|Y*/
155 int y = H_LINE_OFFSET;
156 sparrow_corner_t *mesh_row = mesh;
158 for(mcy = 0; mcy < mesh_h - 1; mcy++){
159 for (mmy = 0; mmy < LINE_PERIOD; mmy++, y++){
160 sparrow_corner_t *mesh_square = mesh_row;
161 int i = y * sparrow->out.width + V_LINE_OFFSET;
162 for(mcx = 0; mcx < mesh_w - 1; mcx++){
163 coord_t iy = mesh_square->y + mmy * mesh_square->dyd;
164 coord_t ix = mesh_square->x + mmy * mesh_square->dxd;
165 for (mmx = 0; mmx < LINE_PERIOD; mmx++, i++){
166 int ixx = coord_to_int_clamp_dither(fl, ix, sparrow->in.width, i);
167 int iyy = coord_to_int_clamp_dither(fl, iy, sparrow->in.height, i);
168 guint32 inpos = iyy * sparrow->in.width + ixx;
169 if(sparrow->screenmask[inpos]){
170 map_lut[i] = inpos;
172 ix += mesh_square->dxr;
173 iy += mesh_square->dyr;
175 mesh_square++;
178 mesh_row += mesh_w;
180 sparrow->map_lut = map_lut;
181 debug_map_lut(sparrow, fl);
184 static void
185 debug_corners_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
186 sparrow_corner_t *mesh = fl->mesh;
187 guint32 *data = (guint32*)fl->debug->imageData;
188 guint w = fl->debug->width;
189 guint h = fl->debug->height;
190 memset(data, 0, sparrow->in.size);
191 guint32 colours[4] = {0xff0000ff, 0x00ff0000, 0x0000ff00, 0xffffffff};
192 for (int i = 0; i < fl->n_vlines * fl->n_hlines; i++){
193 sparrow_corner_t *c = &mesh[i];
194 coord_t x = c->x;
195 coord_t y = c->y;
196 coord_t txr = x;
197 coord_t txd = x;
198 coord_t tyr = y;
199 coord_t tyd = y;
200 for (int j = 1; j < LINE_PERIOD; j+= 2){
201 txr += c->dxr * 2;
202 txd += c->dxd * 2;
203 tyr += c->dyr * 2;
204 tyd += c->dyd * 2;
205 guint hl = coords_to_index(txr, tyr, w, h);
206 data[hl] = 0x88000088;
207 guint vl = coords_to_index(txd, tyd, w, h);
208 data[vl] = 0x00663300;
210 data[coords_to_index(x, y, w, h)] = colours[c->status];
212 MAYBE_DEBUG_IPL(fl->debug);
216 static void
217 debug_clusters(GstSparrow *sparrow, sparrow_find_lines_t *fl){
218 guint32 *data = (guint32*)fl->debug->imageData;
219 memset(data, 0, sparrow->in.size);
220 int width = fl->n_vlines;
221 int height = fl->n_hlines;
222 sparrow_cluster_t *clusters = fl->clusters;
223 int i, j;
224 guint32 colour;
225 guint32 colours[4] = {0xff0000ff, 0x0000ff00, 0x00ff0000,
226 0x00ff00ff};
227 for (i = 0; i < width * height; i++){
228 colour = colours[i % 5];
229 sparrow_voter_t *v = clusters[i].voters;
230 for (j = 0; j < clusters[i].n; j++){
231 data[coords_to_index(v[j].x, v[j].y,
232 sparrow->in.width, sparrow->in.height)] = (colour * (v[j].signal / 2)) / 256;
235 MAYBE_DEBUG_IPL(fl->debug);
239 #define SIGNAL_QUANT 1
241 /*maximum number of pixels in a cluster */
242 #define CLUSTER_SIZE 8
245 /*find map points with common intersection data, and collect them into clusters */
246 static void
247 make_clusters(GstSparrow *sparrow, sparrow_find_lines_t *fl){
248 sparrow_cluster_t *clusters = fl->clusters;
249 int x, y;
250 /*special case: spurious values collect up at 0,0 */
251 fl->map[0].signal[SPARROW_VERTICAL] = 0;
252 fl->map[0].signal[SPARROW_HORIZONTAL] = 0;
253 /*each point in fl->map is in a vertical line, a horizontal line, both, or
254 neither. Only the "both" case matters. */
255 for (y = 0; y < sparrow->in.height; y++){
256 for (x = 0; x < sparrow->in.width; x++){
257 sparrow_intersect_t *p = &fl->map[y * sparrow->in.width + x];
258 guint vsig = p->signal[SPARROW_VERTICAL];
259 guint hsig = p->signal[SPARROW_HORIZONTAL];
260 /*remembering that 0 is valid as a line number, but not as a signal */
261 if (! (vsig && hsig)){
262 continue;
264 /*This one is lobbying for the position of a corner.*/
265 int vline = p->lines[SPARROW_VERTICAL];
266 int hline = p->lines[SPARROW_HORIZONTAL];
267 if (vline == BAD_PIXEL || hline == BAD_PIXEL){
268 GST_DEBUG("ignoring bad pixel %d, %d\n", x, y);
269 continue;
271 sparrow_cluster_t *cluster = &clusters[hline * fl->n_vlines + vline];
272 sparrow_voter_t *voters = cluster->voters;
273 int n = cluster->n;
274 guint signal = (vsig * hsig) / SIGNAL_QUANT;
275 GST_DEBUG("signal at %p (%d, %d): %dv %dh, product %u, lines: %dv %dh\n"
276 "cluster is %p, n is %d\n", p, x, y,
277 vsig, hsig, signal, vline, hline, cluster, n);
278 if (signal == 0){
279 GST_WARNING("signal at %p (%d, %d) is %d following quantisation!\n",
280 p, x, y, signal);
283 if (n < CLUSTER_SIZE){
284 voters[n].x = INT_TO_COORD(x);
285 voters[n].y = INT_TO_COORD(y);
286 voters[n].signal = signal;
287 cluster->n++;
289 else {
290 /*duplicate x, y, signal, so they aren't mucked up */
291 guint ts = signal;
292 coord_t tx = x;
293 coord_t ty = y;
294 /*replaced one ends up here */
295 guint ts2;
296 coord_t tx2;
297 coord_t ty2;
298 for (int j = 0; j < CLUSTER_SIZE; j++){
299 if (voters[j].signal < ts){
300 ts2 = voters[j].signal;
301 tx2 = voters[j].x;
302 ty2 = voters[j].y;
303 voters[j].signal = ts;
304 voters[j].x = tx;
305 voters[j].y = ty;
306 ts = ts2;
307 tx = tx2;
308 ty = ty2;
311 GST_DEBUG("more than %d pixels at cluster for corner %d, %d."
312 "Dropped %u for %u\n",
313 CLUSTER_SIZE, vline, hline, ts2, signal);
317 if (sparrow->debug){
318 debug_clusters(sparrow, fl);
323 static inline int
324 drop_cluster_voter(sparrow_voter_t *voters, int n, int k)
326 int i;
327 if (k < n){
328 n--;
329 for (i = k; i < n; i++){
330 voters[i] = voters[i + 1];
333 return n;
336 static inline int sort_median(coord_t *a, guint n)
338 guint i, j;
339 /*stupid sort, but n is very small*/
340 for (i = 0; i < n; i++){
341 for (j = i + 1; j < n; j++){
342 if (a[i] > a[j]){
343 coord_t tmp = a[j];
344 a[j] = a[i];
345 a[i] = tmp;
349 guint middle = n / 2;
350 coord_t answer = a[middle];
352 if ((n & 1) == 0){
353 answer += a[middle - 1];
354 answer /= 2;
356 return answer;
359 #define EUCLIDEAN_D2(ax, ay, bx, by)((ax - bx) * (ax - bx) + (ay - by) * (ay - by))
360 #define EUCLIDEAN_THRESHOLD 7
362 static inline int
363 euclidean_discard_cluster_outliers(sparrow_voter_t *voters, int n)
365 /* Calculate distance between each pair. Discard points with maximum sum,
366 then recalculate until all are within threshold.
368 GST_DEBUG("cleansing a cluster of size %d using sum of distances", n);
369 int i, j;
370 coord_t dsums[n];
371 for (i = 0; i < n; i++){
372 dsums[i] = 0;
373 for (j = i + 1; j < n; j++){
374 coord_t d = EUCLIDEAN_D2(voters[i].x, voters[i].y,
375 voters[j].x, voters[j].y);
376 dsums[i] += d;
377 dsums[j] += d;
381 int worst_i;
382 coord_t worst_d, threshold;
383 while (n > 1){
384 threshold = EUCLIDEAN_THRESHOLD * n;
385 worst_i = 0;
386 worst_d = 0;
387 for (i = 0; i < n; i++){
388 if (dsums[i] > worst_d){
389 worst_d = dsums[i];
390 worst_i = i;
393 if (worst_d > threshold){
394 GST_DEBUG("failing point %d, distance sq %d, threshold %d\n",
395 worst_i, C2I(worst_d), C2I(threshold));
396 //subtract this one from the sums, or they'll all go
397 for (i = 0; i < n; i++){
398 dsums[i] -= EUCLIDEAN_D2(voters[i].x, voters[i].y,
399 voters[worst_i].x, voters[worst_i].y);
401 n = drop_cluster_voter(voters, n, worst_i);
403 else{
404 GST_DEBUG("worst %d, was only %d, threshold %d\n",
405 worst_i, C2I(worst_d), C2I(threshold));
406 break;
409 return n;
412 static inline int
413 median_discard_cluster_outliers(sparrow_voter_t *voters, int n)
415 coord_t xvals[n];
416 coord_t yvals[n];
417 int i;
418 for (i = 0; i < n; i++){
419 /*XXX could sort here*/
420 xvals[i] = voters[i].x;
421 yvals[i] = voters[i].y;
423 const coord_t xmed = sort_median(xvals, n);
424 const coord_t ymed = sort_median(yvals, n);
426 for (i = 0; i < n; i++){
427 coord_t dx = voters[i].x - xmed;
428 coord_t dy = voters[i].y - ymed;
429 if (dx * dx + dy * dy > OUTLIER_THRESHOLD){
430 n = drop_cluster_voter(voters, n, i);
433 return n;
436 /* */
437 static inline void
438 make_corners(GstSparrow *sparrow, sparrow_find_lines_t *fl){
439 //DEBUG_FIND_LINES(fl);
440 int width = fl->n_vlines;
441 int height = fl->n_hlines;
442 sparrow_cluster_t *clusters = fl->clusters;
443 sparrow_corner_t *mesh = fl->mesh;
444 int x, y, i;
446 i = 0;
447 for (y = 0; y < height; y++){
448 for (x = 0; x < width; x++, i++){
449 sparrow_cluster_t *cluster = clusters + i;
450 if (cluster->n == 0){
451 continue;
453 #if 1
454 /*discard outliers based on sum of squared distances: good points should
455 be in a cluster, and have lowest sum*/
456 cluster->n = euclidean_discard_cluster_outliers(cluster->voters, cluster->n);
457 #else
458 /*discard values away from median x, y values.
459 (each dimension is calculated independently)*/
460 cluster->n = median_discard_cluster_outliers(cluster->voters, cluster->n);
461 #endif
462 /* now find a weighted average position */
463 /*With int coord_t, coord_sum_t is
464 64 bit to avoid overflow -- should probably just use floating point
465 (or reduce signal)*/
466 coord_sum_t xsum, ysum;
467 coord_t xmean, ymean;
468 guint64 votes;
469 int j;
470 xsum = 0;
471 ysum = 0;
472 votes = 0;
473 for (j = 0; j < cluster->n; j++){
474 votes += cluster->voters[j].signal;
475 ysum += cluster->voters[j].y * cluster->voters[j].signal;
476 xsum += cluster->voters[j].x * cluster->voters[j].signal;
478 if (votes){
479 xmean = xsum / votes;
480 ymean = ysum / votes;
482 else {
483 GST_WARNING("corner %d, %d voters, sum %d,%d, somehow has no votes\n",
484 i, cluster->n, xsum, ysum);
487 GST_DEBUG("corner %d: %d voters, %d votes, sum %d,%d, mean %d,%d\n",
488 i, cluster->n, votes, C2I(xsum), C2I(ysum), C2I(xmean), C2I(ymean));
490 mesh[i].x = xmean;
491 mesh[i].y = ymean;
492 mesh[i].status = CORNER_EXACT;
493 GST_DEBUG("found corner %d at (%3f, %3f)\n",
494 i, COORD_TO_FLOAT(xmean), COORD_TO_FLOAT(ymean));
499 static sparrow_point_t
500 median_centre(sparrow_voter_t *estimates, int n){
501 /*X and Y arevcalculated independently, which is really not right.
502 on the other hand, it probably works. */
503 int i;
504 sparrow_point_t result;
505 coord_t vals[n];
506 for (i = 0; i < n; i++){
507 vals[i] = estimates[i].x;
509 result.x = coord_median(vals, n);
511 for (i = 0; i < n; i++){
512 vals[i] = estimates[i].y;
514 result.y = coord_median(vals, n);
515 return result;
518 static const sparrow_estimator_t base_estimators[] = {
519 { 0, 1, 0, 2, 0, 3},
520 { 0, 2, 0, 4, 0, 6},
521 { 1, 0, 2, 0, 3, 0},
522 { 1, 1, 2, 2, 3, 3},
523 { 1, 2, 2, 4, 3, 6},
524 { 1, 3, 2, 6, 3, 9},
525 { 2, 0, 4, 0, 6, 0},
526 { 2, 1, 4, 2, 6, 3},
527 { 2, 2, 4, 4, 6, 6},
528 { 2, 3, 4, 6, 6, 9},
529 { 3, 1, 6, 2, 9, 3},
530 { 3, 2, 6, 4, 9, 6},
533 #define BASE_ESTIMATORS (sizeof(base_estimators) / sizeof(sparrow_estimator_t))
534 #define ESTIMATORS (BASE_ESTIMATORS * 4)
536 static inline guint
537 calculate_estimator_tables(sparrow_estimator_t *estimators){
538 guint i, j;
539 sparrow_estimator_t *e = estimators;
540 for (i = 0; i < BASE_ESTIMATORS; i++){
541 for (j = 0; j < 4; j++){
542 *e = base_estimators[i];
543 if (j & 1){
544 if (! e->x1){
545 continue;
547 e->x1 = -e->x1;
548 e->x2 = -e->x2;
549 e->x3 = -e->x3;
551 if (j & 2){
552 if (! e->y1){
553 continue;
555 e->y1 = -e->y1;
556 e->y2 = -e->y2;
557 e->y3 = -e->y3;
559 GST_DEBUG("estimator: %-d,%-d %-d,%-d %-d,%-d",
560 e->x1, e->y1, e->x2, e->y2, e->x3, e->y3);
561 e++;
564 return e - estimators;
568 /*the map made above is likely to be full of errors. Fix them, and add in
569 missing points */
570 static void
571 complete_map(GstSparrow *sparrow, sparrow_find_lines_t *fl){
572 sparrow_voter_t estimates[ESTIMATORS + 1]; /* 1 extra for trick simplifying median */
573 sparrow_estimator_t estimators[ESTIMATORS];
574 guint est_count = calculate_estimator_tables(estimators);
575 GST_DEBUG("made %d estimators", est_count);
576 guint32 *debug = NULL;
577 if (sparrow->debug){
578 debug = (guint32*)fl->debug->imageData;
579 memset(debug, 0, sparrow->in.size);
582 int x, y;
583 int width = fl->n_vlines;
584 int height = fl->n_hlines;
585 int screen_width = sparrow->in.width;
586 int screen_height = sparrow->in.height;
587 sparrow_corner_t *mesh = fl->mesh;
588 sparrow_corner_t *mesh_next = fl->mesh_next;
590 memset(estimates, 0, sizeof(estimates)); /*just for clarity in debugging */
591 int prev_settled = 0;
592 while (1){
593 memcpy(mesh_next, mesh, width * height * sizeof(sparrow_corner_t));
594 int settled = 0;
595 for (y = 0; y < height; y++){
596 for (x = 0; x < width; x++){
597 sparrow_corner_t *corner = &mesh[y * width + x];
598 if (corner->status == CORNER_SETTLED){
599 settled ++;
600 GST_DEBUG("ignoring settled corner %d, %d", x, y);
601 continue;
603 int k = 0;
604 for (guint j = 0; j < est_count; j++){
605 sparrow_estimator_t *e = &estimators[j];
606 int x3, y3, x2, y2, x1, y1;
607 y3 = y + e->y3;
608 x3 = x + e->x3;
609 if (!(y3 >= 0 && y3 < height &&
610 x3 >= 0 && x3 < width &&
611 mesh[y3 * width + x3].status != CORNER_UNUSED
613 GST_DEBUG("not using estimator %d because corners aren't used, or are off screen\n"
614 "x3 %d, y3 %d", j, x3, y3);
615 continue;
617 y2 = y + e->y2;
618 x2 = x + e->x2;
619 y1 = y + e->y1;
620 x1 = x + e->x1;
621 if (mesh[y2 * width + x2].status == CORNER_UNUSED ||
622 mesh[y1 * width + x1].status == CORNER_UNUSED){
623 GST_DEBUG("not using estimator %d because corners aren't used", j);
624 continue;
626 /*there are 3 points, and the unknown one.
627 They should all be in a line.
628 The ratio of the p3-p2:p2-p1 sould be the same as
629 p2-p1:p1:p0.
631 This really has to be done in floating point.
633 collinearity, no division, but no useful error metric
634 x[0] * (y[1]-y[2]) + x[1] * (y[2]-y[0]) + x[2] * (y[0]-y[1]) == 0
635 (at least not without further division)
637 This way:
639 cos angle = dot product / product of euclidean lengths
641 (dx12 * dx23 + dy12 * dy23) /
642 (sqrt(dx12 * dx12 + dy12 * dy12) * sqrt(dx23 * dx23 + dy23 * dy23))
644 is costly up front (sqrt), but those distances need to be
645 calculated anyway (or at least they are handy). Not much gained by
646 short-circuiting on bad collinearity, though.
648 It also handlily catches all the division by zeros in one meaningful
651 sparrow_corner_t *c1 = &mesh[y1 * width + x1];
652 sparrow_corner_t *c2 = &mesh[y2 * width + x2];
653 sparrow_corner_t *c3 = &mesh[y3 * width + x3];
655 double dx12 = c1->x - c2->x;
656 double dy12 = c1->y - c2->y;
657 double dx23 = c2->x - c3->x;
658 double dy23 = c2->y - c3->y;
659 double distance12 = sqrt(dx12 * dx12 + dy12 * dy12);
660 double distance23 = sqrt(dx23 * dx23 + dy23 * dy23);
662 double dp = dx12 * dx23 + dy12 * dy23;
664 double distances = distance12 * distance23;
666 GST_LOG("mesh points: %d,%d, %d,%d, %d,%d\n"
667 "map points: %d,%d, %d,%d, %d,%d\n"
668 "diffs: 12: %0.3f,%0.3f, 23: %0.3f,%0.3f, \n"
669 "distances: 12: %0.3f, 32: %0.3f\n",
670 x1, y1, x2, y2, x3, y3,
671 C2I(c1->x), C2I(c1->y), C2I(c2->x), C2I(c2->y), C2I(c3->x), C2I(c3->y),
672 dx12, dy12, dx23, dy23, distance12, distance23
675 if (distances == 0.0){
676 GST_INFO("at least two points out of %d,%d, %d,%d, %d,%d are the same!",
677 x1, y1, x2, y2, x3, y3);
678 continue;
680 double line_error = 1.0 - dp / distances;
681 if (line_error > MAX_NONCOLLINEARITY){
682 GST_DEBUG("Points %d,%d, %d,%d, %d,%d are not in a line: non-collinearity: %3f",
683 x1, y1, x2, y2, x3, y3, line_error);
684 continue;
686 GST_LOG("GOOD collinearity: %3f", line_error);
689 double ratio = distance12 / distance23;
690 /*so here's the estimate!*/
691 coord_t dx = dx12 * ratio;
692 coord_t dy = dy12 * ratio;
693 coord_t ex = c1->x + dx;
694 coord_t ey = c1->y + dy;
696 GST_LOG("dx, dy: %d,%d, ex, ey: %d,%d\n"
697 "dx raw: %0.3f,%0.3f, x1, x2: %0.3f,%0.3f,\n"
698 "distances: 12: %0.3f, 32: %0.3f\n"
699 "ratio: %0.3f\n",
700 C2I(dx), C2I(dy), C2I(ex), C2I(ey),
701 dx, dy, ex, ey, ratio
704 if (! coord_in_range(ey, screen_height) ||
705 ! coord_in_range(ex, screen_width)){
706 GST_DEBUG("rejecting estimate for %d, %d, due to ex, ey being %d, %d",
707 x, y, C2I(ex), C2I(ey));
708 continue;
711 GST_LOG("estimator %d,%d SUCCESSFULLY estimated that %d, %d will be %d, %d",
712 x1, x2, x, y, C2I(ex), C2I(ey));
714 estimates[k].x = ex;
715 estimates[k].y = ey;
716 if (sparrow->debug){
717 debug[coords_to_index(ex, ey, sparrow->in.width, sparrow->in.height)] = 0x00aa7700;
719 k++;
721 /*now there is an array of estimates.
722 The *_discard_cluster_outliers functions should fit here */
723 GST_INFO("got %d estimates for %d,%d", k, x, y);
724 if(! k){
725 continue;
727 coord_t guess_x;
728 coord_t guess_y;
730 #if 1
731 /*now find median values. If the number is even, add a copy of either
732 the original value, or a random element. */
733 if (! k & 1){
734 if (corner->status != CORNER_UNUSED){
735 estimates[k].x = corner->x;
736 estimates[k].y = corner->y;
738 else {
739 int r = RANDINT(sparrow, 0, r);
740 estimates[k].x = estimates[r].x;
741 estimates[k].y = estimates[r].y;
743 k++;
745 sparrow_point_t centre = median_centre(estimates, k);
746 guess_x = centre.x;
747 guess_y = centre.y;
749 #else
750 k = euclidean_discard_cluster_outliers(estimates, k);
751 if (sparrow->debug){
752 for (int j = 0; j < k; j++){
753 debug[coords_to_index(estimates[j].x, estimates[j].y,
754 sparrow->in.width, sparrow->in.height)] = 0x00ffff00;
757 GST_INFO("After discard, left with %d estimates", k);
758 /*now what? the mean? yes.*/
759 coord_t sumx = 0;
760 coord_t sumy = 0;
761 for (int j = 0; j < k; j++){
762 sumx += estimates[j].x;
763 sumy += estimates[j].y;
765 guess_x = sumx / k;
766 guess_y = sumy / k;
767 #endif
769 GST_INFO("estimating %d,%d", C2I(guess_x), C2I(guess_y));
771 if (corner->status == CORNER_EXACT){
772 if (sparrow->debug){
773 debug[coords_to_index(corner->x, corner->y,
774 sparrow->in.width, sparrow->in.height)] = 0xffff3300;
776 if ((guess_x - corner->x) * (guess_x - corner->x) +
777 (guess_y - corner->y) * (guess_y - corner->y)
778 < CORNER_EXACT_THRESHOLD){
779 guess_x = corner->x;
780 guess_y = corner->y;
781 corner->status = CORNER_SETTLED;
782 GST_INFO("using exact reading %0.3f, %0.3f", C2F(corner->x), C2F(corner->y));
784 else{
785 GST_INFO("REJECTING exact reading %0.3f,%0.3f: too far from median %0.3f,%0.3f",
786 C2F(corner->x), C2F(corner->y), C2F(corner->x), C2F(corner->y));
787 corner->status = CORNER_PROJECTED;
790 else if (k < MIN_CORNER_ESTIMATES){
791 GST_INFO("weak evidence (%d estimates) for corner %d,%d, marking it PROJECTED",
792 k, x, y);
793 corner->status = CORNER_PROJECTED;
794 if (sparrow->debug){
795 debug[coords_to_index(guess_x, guess_y,
796 sparrow->in.width, sparrow->in.height)] = 0xff0000ff;
799 else{
800 GST_DEBUG("corner %d, %d is SETTLED", x, y);
801 corner->status = CORNER_SETTLED;
802 settled ++;
803 if (sparrow->debug){
804 debug[coords_to_index(guess_x, guess_y,
805 sparrow->in.width, sparrow->in.height)] = 0xffffffff;
808 corner->x = guess_x;
809 corner->y = guess_y;
812 GST_INFO("settled %d in that round. %d left to go",
813 settled - prev_settled, width * height - settled);
814 if (settled == width * height || settled == prev_settled){
815 break;
817 prev_settled = settled;
818 sparrow_corner_t *tmp = mesh_next;
819 mesh_next = mesh;
820 mesh = tmp;
822 fl->mesh = mesh;
823 fl->mesh_next = mesh_next;
824 MAYBE_DEBUG_IPL(fl->debug);
828 static void
829 calculate_deltas(GstSparrow *sparrow, sparrow_find_lines_t *fl){
830 int i;
831 int width = fl->n_vlines;
832 int height = fl->n_hlines;
833 sparrow_corner_t *mesh = fl->mesh;
834 gint x, y;
836 //DEBUG_FIND_LINES(fl);
837 /* calculate deltas toward adjacent corners */
838 /* try to extrapolate left and up, if possible, so need to go backwards. */
839 i = width * height - 1;
840 for (y = height - 1; y >= 0; y--){
841 for (x = width - 1; x >= 0; x--, i--){
842 sparrow_corner_t *corner = &mesh[i];
843 /* calculate the delta to next corner. If this corner is on edge, delta is
844 0 and next is this.*/
845 sparrow_corner_t *right = (x == width - 1) ? corner : corner + 1;
846 sparrow_corner_t *down = (y == height - 1) ? corner : corner + width;
847 GST_DEBUG("i %d xy %d,%d width %d. in_xy %d,%d; down in_xy %d,%d; right in_xy %d,%d\n",
848 i, x, y, width, C2I(corner->x), C2I(corner->y), C2I(down->x),
849 C2I(down->y), C2I(right->x), C2I(right->y));
850 if (corner->status != CORNER_UNUSED){
851 if (right->status != CORNER_UNUSED){
852 corner->dxr = QUANTISE_DELTA(right->x - corner->x);
853 corner->dyr = QUANTISE_DELTA(right->y - corner->y);
855 if (down->status != CORNER_UNUSED){
856 corner->dxd = QUANTISE_DELTA(down->x - corner->x);
857 corner->dyd = QUANTISE_DELTA(down->y - corner->y);
862 if (sparrow->debug){
863 debug_corners_image(sparrow, fl);
868 static void
869 look_for_line(GstSparrow *sparrow, guint8 *in, sparrow_find_lines_t *fl,
870 sparrow_line_t *line){
871 guint i;
872 guint32 colour;
873 guint32 cmask = sparrow->out.colours[sparrow->colour];
874 int signal;
876 /* subtract background noise */
877 fl->input->imageData = (char *)in;
878 cvSub(fl->input, fl->threshold, fl->working, NULL);
879 guint32 *in32 = (guint32 *)fl->working->imageData;
881 for (i = 0; i < sparrow->in.pixcount; i++){
882 colour = in32[i] & cmask;
883 signal = (((colour >> fl->shift1) & COLOUR_MASK) +
884 ((colour >> fl->shift2) & COLOUR_MASK));
885 if (signal){
886 if (fl->map[i].lines[line->dir]){
887 /*assume the pixel is on for everyone and will just confuse
888 matters. ignore it.
891 if (fl->map[i].lines[line->dir] != BAD_PIXEL){
893 GST_DEBUG("HEY, expected point %d to be in line %d (dir %d) "
894 "and thus empty, but it is also in line %d\n"
895 "old signal %d, new signal %d, marking as BAD\n",
896 i, line->index, line->dir, fl->map[i].lines[line->dir],
897 fl->map[i].signal[line->dir], signal);
899 fl->map[i].lines[line->dir] = BAD_PIXEL;
900 fl->map[i].signal[line->dir] = 0;
903 else{
904 fl->map[i].lines[line->dir] = line->index;
905 fl->map[i].signal[line->dir] = signal;
911 static void
912 debug_map_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
913 guint32 *data = (guint32*)fl->debug->imageData;
914 memset(data, 0, sparrow->in.size);
915 for (guint i = 0; i < sparrow->in.pixcount; i++){
916 data[i] |= fl->map[i].signal[SPARROW_HORIZONTAL] << sparrow->in.gshift;
917 data[i] |= fl->map[i].signal[SPARROW_VERTICAL] << sparrow->in.rshift;
918 data[i] |= ((fl->map[i].lines[SPARROW_VERTICAL] == BAD_PIXEL) ||
919 (fl->map[i].lines[SPARROW_HORIZONTAL] == BAD_PIXEL)) ? 255 << sparrow->in.bshift : 0;
921 MAYBE_DEBUG_IPL(fl->debug);
924 /* draw the line (in sparrow->colour) */
925 static inline void
926 draw_line(GstSparrow * sparrow, sparrow_line_t *line, guint8 *out){
927 guint32 *p = (guint32 *)out;
928 guint32 colour = sparrow->out.colours[sparrow->colour];
929 int i;
930 if (line->dir == SPARROW_HORIZONTAL){
931 p += line->offset * sparrow->out.width;
932 for (i = 0; i < sparrow->out.width; i++){
933 p[i] = colour;
936 else {
937 guint32 *p = (guint32 *)out;
938 p += line->offset;
939 for(i = 0; i < sparrow->out.height; i++){
940 *p = colour;
941 p += sparrow->out.width;
946 static void
947 jump_state(GstSparrow *sparrow, sparrow_find_lines_t *fl, edges_state_t state){
948 if (state == EDGES_NEXT_STATE){
949 fl->state++;
951 else {
952 fl->state = state;
954 switch (fl->state){
955 case EDGES_FIND_NOISE:
956 sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG + CAMERA_ADJUST_TIME;
957 break;
958 case EDGES_FIND_LINES:
959 sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
960 break;
961 case EDGES_FIND_CORNERS:
962 sparrow->countdown = 7;
963 break;
964 case EDGES_WAIT_FOR_PLAY:
965 global_number_of_edge_finders--;
966 sparrow->countdown = 300;
967 break;
968 default:
969 GST_DEBUG("jumped to non-existent state %d\n", fl->state);
970 break;
974 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
975 until last one.
977 static inline void
978 draw_lines(GstSparrow *sparrow, sparrow_find_lines_t *fl, guint8 *in, guint8 *out)
980 sparrow_line_t *line = fl->shuffled_lines[fl->current];
981 sparrow->countdown--;
982 memset(out, 0, sparrow->out.size);
983 if (sparrow->countdown){
984 draw_line(sparrow, line, out);
986 else{
987 /*show nothing, look for result */
988 look_for_line(sparrow, in, fl, line);
989 if (sparrow->debug){
990 debug_map_image(sparrow, fl);
992 fl->current++;
993 if (fl->current == fl->n_lines){
994 jump_state(sparrow, fl, EDGES_NEXT_STATE);
996 else{
997 sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
1002 #define LINE_THRESHOLD 32
1004 static inline void
1005 find_threshold(GstSparrow *sparrow, sparrow_find_lines_t *fl, guint8 *in, guint8 *out)
1007 memset(out, 0, sparrow->out.size);
1008 /*XXX should average/median over a range of frames */
1009 if (sparrow->countdown == 0){
1010 memcpy(fl->threshold->imageData, in, sparrow->in.size);
1011 /*add a constant, and smooth */
1012 cvAddS(fl->threshold, cvScalarAll(LINE_THRESHOLD), fl->working, NULL);
1013 cvSmooth(fl->working, fl->threshold, CV_GAUSSIAN, 3, 0, 0, 0);
1014 //cvSmooth(fl->working, fl->threshold, CV_MEDIAN, 3, 0, 0, 0);
1015 jump_state(sparrow, fl, EDGES_NEXT_STATE);
1017 sparrow->countdown--;
1020 /*match up lines and find corners */
1021 static inline int
1022 find_corners(GstSparrow *sparrow, sparrow_find_lines_t *fl)
1024 sparrow->countdown--;
1025 switch(sparrow->countdown){
1026 case 4:
1027 make_clusters(sparrow, fl);
1028 break;
1029 case 3:
1030 make_corners(sparrow, fl);
1031 break;
1032 case 2:
1033 complete_map(sparrow, fl);
1034 break;
1035 case 1:
1036 calculate_deltas(sparrow, fl);
1037 break;
1038 case 0:
1039 #if USE_FULL_LUT
1040 corners_to_full_lut(sparrow, fl);
1041 #else
1042 corners_to_lut(sparrow, fl);
1043 #endif
1044 jump_state(sparrow, fl, EDGES_NEXT_STATE);
1045 break;
1046 default:
1047 GST_DEBUG("how did sparrow->countdown get to be %d?", sparrow->countdown);
1048 sparrow->countdown = 5;
1050 return sparrow->countdown;
1053 /*use a dirty shared variable*/
1054 static gboolean
1055 wait_for_play(GstSparrow *sparrow, sparrow_find_lines_t *fl){
1056 if (global_number_of_edge_finders == 0 ||
1057 sparrow->countdown == 0){
1058 return TRUE;
1060 sparrow->countdown--;
1061 return FALSE;
1064 INVISIBLE sparrow_state
1065 mode_find_edges(GstSparrow *sparrow, GstBuffer *inbuf, GstBuffer *outbuf){
1066 guint8 *in = GST_BUFFER_DATA(inbuf);
1067 guint8 *out = GST_BUFFER_DATA(outbuf);
1068 sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
1069 switch (fl->state){
1070 case EDGES_FIND_NOISE:
1071 find_threshold(sparrow, fl, in, out);
1072 break;
1073 case EDGES_FIND_LINES:
1074 draw_lines(sparrow, fl, in, out);
1075 break;
1076 case EDGES_FIND_CORNERS:
1077 memset(out, 0, sparrow->out.size);
1078 find_corners(sparrow, fl);
1079 break;
1080 case EDGES_WAIT_FOR_PLAY:
1081 memset(out, 0, sparrow->out.size);
1082 if (wait_for_play(sparrow, fl)){
1083 return SPARROW_NEXT_STATE;
1085 break;
1086 default:
1087 GST_WARNING("strange state in mode_find_edges: %d", fl->state);
1088 memset(out, 0, sparrow->out.size);
1090 return SPARROW_STATUS_QUO;
1093 INVISIBLE void
1094 finalise_find_edges(GstSparrow *sparrow){
1095 sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
1096 //DEBUG_FIND_LINES(fl);
1097 if (sparrow->save && *(sparrow->save)){
1098 GST_DEBUG("about to save to %s\n", sparrow->save);
1099 dump_edges_info(sparrow, fl, sparrow->save);
1101 if (sparrow->debug){
1102 cvReleaseImage(&fl->debug);
1104 free(fl->h_lines);
1105 free(fl->shuffled_lines);
1106 free(fl->map);
1107 free(fl->mesh_mem);
1108 free(fl->clusters);
1109 free(fl->dither);
1110 cvReleaseImage(&fl->threshold);
1111 cvReleaseImage(&fl->working);
1112 cvReleaseImageHeader(&fl->input);
1113 free(fl);
1114 GST_DEBUG("freed everything\n");
1115 sparrow->helper_struct = NULL;
1118 static void
1119 setup_colour_shifts(GstSparrow *sparrow, sparrow_find_lines_t *fl){
1120 /*COLOUR_QUANT reduces the signal a little bit more, avoiding overflow
1121 later */
1122 switch (sparrow->colour){
1123 case SPARROW_WHITE:
1124 case SPARROW_GREEN:
1125 fl->shift1 = sparrow->in.gshift + COLOUR_QUANT;
1126 fl->shift2 = sparrow->in.gshift + COLOUR_QUANT;
1127 break;
1128 case SPARROW_MAGENTA:
1129 fl->shift1 = sparrow->in.rshift + COLOUR_QUANT;
1130 fl->shift2 = sparrow->in.bshift + COLOUR_QUANT;
1131 break;
1135 INVISIBLE void
1136 init_find_edges(GstSparrow *sparrow){
1137 gint i;
1138 sparrow_find_lines_t *fl = zalloc_aligned_or_die(sizeof(sparrow_find_lines_t));
1139 sparrow->helper_struct = (void *)fl;
1141 gint h_lines = (sparrow->out.height + LINE_PERIOD - 1) / LINE_PERIOD;
1142 gint v_lines = (sparrow->out.width + LINE_PERIOD - 1) / LINE_PERIOD;
1143 gint n_lines_max = (h_lines + v_lines);
1144 gint n_corners = (h_lines * v_lines);
1146 /*set up dither here, rather than in the busy time */
1147 fl->dither = malloc_aligned_or_die(sparrow->out.pixcount * sizeof(double));
1148 dsfmt_fill_array_close_open(sparrow->dsfmt, fl->dither, sparrow->out.pixcount);
1150 fl->n_hlines = h_lines;
1151 fl->n_vlines = v_lines;
1153 fl->h_lines = malloc_aligned_or_die(sizeof(sparrow_line_t) * n_lines_max);
1154 fl->shuffled_lines = malloc_aligned_or_die(sizeof(sparrow_line_t *) * n_lines_max);
1155 GST_DEBUG("shuffled lines, malloced %p\n", fl->shuffled_lines);
1157 GST_DEBUG("map is going to be %d * %d \n", sizeof(sparrow_intersect_t), sparrow->in.pixcount);
1158 fl->map = zalloc_aligned_or_die(sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
1159 fl->clusters = zalloc_or_die(n_corners * sizeof(sparrow_cluster_t));
1160 fl->mesh_mem = zalloc_aligned_or_die(n_corners * sizeof(sparrow_corner_t) * 2);
1161 fl->mesh = fl->mesh_mem;
1162 fl->mesh_next = fl->mesh + n_corners;
1164 sparrow_line_t *line = fl->h_lines;
1165 sparrow_line_t **sline = fl->shuffled_lines;
1166 int offset;
1168 for (i = 0, offset = H_LINE_OFFSET; offset < sparrow->out.height;
1169 i++, offset += LINE_PERIOD){
1170 line->offset = offset;
1171 line->dir = SPARROW_HORIZONTAL;
1172 line->index = i;
1173 *sline = line;
1174 line++;
1175 sline++;
1176 //GST_DEBUG("line %d h has offset %d\n", i, offset);
1179 /*now add the vertical lines */
1180 fl->v_lines = line;
1181 for (i = 0, offset = V_LINE_OFFSET; offset < sparrow->out.width;
1182 i++, offset += LINE_PERIOD){
1183 line->offset = offset;
1184 line->dir = SPARROW_VERTICAL;
1185 line->index = i;
1186 *sline = line;
1187 line++;
1188 sline++;
1189 //GST_DEBUG("line %d v has offset %d\n", i, offset);
1191 //DEBUG_FIND_LINES(fl);
1192 fl->n_lines = line - fl->h_lines;
1193 GST_DEBUG("allocated %d lines, made %d\n", n_lines_max, fl->n_lines);
1195 /*now shuffle */
1196 for (i = 0; i < fl->n_lines; i++){
1197 int j = RANDINT(sparrow, 0, fl->n_lines);
1198 sparrow_line_t *tmp = fl->shuffled_lines[j];
1199 fl->shuffled_lines[j] = fl->shuffled_lines[i];
1200 fl->shuffled_lines[i] = tmp;
1203 setup_colour_shifts(sparrow, fl);
1205 /* opencv images for threshold finding */
1206 CvSize size = {sparrow->in.width, sparrow->in.height};
1207 fl->working = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
1208 fl->threshold = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
1210 /*input has no data allocated -- it uses latest frame*/
1211 fl->input = init_ipl_image(&sparrow->in, PIXSIZE);
1212 //DEBUG_FIND_LINES(fl);
1213 if (sparrow->debug){
1214 fl->debug = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
1217 if (sparrow->reload){
1218 if (access(sparrow->reload, R_OK)){
1219 GST_DEBUG("sparrow->reload is '%s' and it is UNREADABLE\n", sparrow->reload);
1220 exit(1);
1222 read_edges_info(sparrow, fl, sparrow->reload);
1223 memset(fl->map, 0, sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
1224 //memset(fl->clusters, 0, n_corners * sizeof(sparrow_cluster_t));
1225 memset(fl->mesh, 0, n_corners * sizeof(sparrow_corner_t));
1226 jump_state(sparrow, fl, EDGES_FIND_CORNERS);
1228 else {
1229 jump_state(sparrow, fl, EDGES_FIND_NOISE);
1232 global_number_of_edge_finders++;