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.
21 #include "gstsparrow.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
);
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
);
72 debug_map_lut(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
73 sparrow_map_lut_t
*map_lut
= sparrow
->map_lut
;
75 debug_frame(sparrow
, (guint8
*)map_lut
, sparrow
->out
.width
, sparrow
->out
.height
, PIXSIZE
);
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))
86 coord_to_int_clamp(coord_t x
, const int max_plus_one
){
89 if (x
>= max_plus_one
- 1.5)
90 return max_plus_one
- 1;
95 coord_in_range(coord_t x
, const int max_plus_one
){
96 return x
>= 0 && (x
+ 0.5 < max_plus_one
);
101 #define COORD_TO_INT(x)((x) / (1 << SPARROW_FIXED_POINT))
102 #define COORD_TO_FLOAT(x)(((double)(x)) / (1 << SPARROW_FIXED_POINT))
103 #define INT_TO_COORD(x)((x) * (1 << SPARROW_FIXED_POINT))
106 coord_to_int_clamp(coord_t x
, const int max_plus_one
){
109 x
>>= SPARROW_FIXED_POINT
;
110 if (x
>= max_plus_one
)
111 return max_plus_one
- 1;
116 coord_in_range(coord_t x
, const int max_plus_one
){
117 return x
>= 0 && (x
< max_plus_one
<< SPARROW_FIXED_POINT
);
122 //these ones are common
124 coords_to_index(coord_t x
, coord_t y
, int w
, int h
){
125 int iy
= coord_to_int_clamp(y
, h
);
126 int ix
= coord_to_int_clamp(x
, w
);
130 #define C2I COORD_TO_INT
132 /********************************************/
135 corners_to_full_lut(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
136 DEBUG_FIND_LINES(fl
);
137 sparrow_corner_t
*mesh
= fl
->mesh
; /*maps regular points in ->out to points in ->in */
138 sparrow_map_lut_t
*map_lut
= sparrow
->map_lut
;
139 int mesh_w
= fl
->n_vlines
;
140 int mesh_h
= fl
->n_hlines
;
141 int mcy
, mmy
, mcx
, mmx
; /*Mesh Corner|Modulus X|Y*/
142 int y
= H_LINE_OFFSET
;
143 sparrow_corner_t
*mesh_row
= mesh
;
145 for(mcy
= 0; mcy
< mesh_h
- 1; mcy
++){
146 for (mmy
= 0; mmy
< LINE_PERIOD
; mmy
++, y
++){
147 sparrow_corner_t
*mesh_square
= mesh_row
;
148 int i
= y
* sparrow
->out
.width
+ V_LINE_OFFSET
;
149 for(mcx
= 0; mcx
< mesh_w
- 1; mcx
++){
150 coord_t iy
= mesh_square
->y
+ mmy
* mesh_square
->dyd
;
151 coord_t ix
= mesh_square
->x
+ mmy
* mesh_square
->dxd
;
152 for (mmx
= 0; mmx
< LINE_PERIOD
; mmx
++, i
++){
153 int ixx
= coord_to_int_clamp(iy
, sparrow
->in
.width
);
154 int iyy
= coord_to_int_clamp(iy
, sparrow
->in
.height
);
155 if(sparrow
->screenmask
[iyy
* sparrow
->in
.width
+ ixx
]){
159 ix
+= mesh_square
->dxr
;
160 iy
+= mesh_square
->dyr
;
167 sparrow
->map_lut
= map_lut
;
168 debug_map_lut(sparrow
, fl
);
172 debug_corners_image(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
173 sparrow_corner_t
*mesh
= fl
->mesh
;
174 guint32
*data
= (guint32
*)fl
->debug
->imageData
;
175 guint w
= fl
->debug
->width
;
176 guint h
= fl
->debug
->height
;
177 memset(data
, 0, sparrow
->in
.size
);
178 guint32 colours
[4] = {0xff0000ff, 0x00ff0000, 0x0000ff00, 0xffffffff};
179 for (int i
= 0; i
< fl
->n_vlines
* fl
->n_hlines
; i
++){
180 sparrow_corner_t
*c
= &mesh
[i
];
187 for (int j
= 1; j
< LINE_PERIOD
; j
+= 2){
192 guint hl
= coords_to_index(txr
, tyr
, w
, h
);
193 data
[hl
] = 0x88000088;
194 guint vl
= coords_to_index(txd
, tyd
, w
, h
);
195 data
[vl
] = 0x00663300;
197 data
[coords_to_index(x
, y
, w
, h
)] = colours
[c
->status
];
199 MAYBE_DEBUG_IPL(fl
->debug
);
204 debug_clusters(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
205 guint32
*data
= (guint32
*)fl
->debug
->imageData
;
206 memset(data
, 0, sparrow
->in
.size
);
207 int width
= fl
->n_vlines
;
208 int height
= fl
->n_hlines
;
209 sparrow_cluster_t
*clusters
= fl
->clusters
;
212 guint32 colours
[4] = {0xff0000ff, 0x0000ff00, 0x00ff0000,
214 for (i
= 0; i
< width
* height
; i
++){
215 colour
= colours
[i
% 5];
216 sparrow_voter_t
*v
= clusters
[i
].voters
;
217 for (j
= 0; j
< clusters
[i
].n
; j
++){
218 data
[coords_to_index(v
[j
].x
, v
[j
].y
,
219 sparrow
->in
.width
, sparrow
->in
.height
)] = (colour
* (v
[j
].signal
/ 2)) / 256;
222 MAYBE_DEBUG_IPL(fl
->debug
);
226 #define SIGNAL_QUANT 1
228 /*maximum number of pixels in a cluster */
229 #define CLUSTER_SIZE 8
232 /*find map points with common intersection data, and collect them into clusters */
234 make_clusters(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
235 sparrow_cluster_t
*clusters
= fl
->clusters
;
237 /*special case: spurious values collect up at 0,0 */
238 fl
->map
[0].signal
[SPARROW_VERTICAL
] = 0;
239 fl
->map
[0].signal
[SPARROW_HORIZONTAL
] = 0;
240 /*each point in fl->map is in a vertical line, a horizontal line, both, or
241 neither. Only the "both" case matters. */
242 for (y
= 0; y
< sparrow
->in
.height
; y
++){
243 for (x
= 0; x
< sparrow
->in
.width
; x
++){
244 sparrow_intersect_t
*p
= &fl
->map
[y
* sparrow
->in
.width
+ x
];
245 guint vsig
= p
->signal
[SPARROW_VERTICAL
];
246 guint hsig
= p
->signal
[SPARROW_HORIZONTAL
];
247 /*remembering that 0 is valid as a line number, but not as a signal */
248 if (! (vsig
&& hsig
)){
251 /*This one is lobbying for the position of a corner.*/
252 int vline
= p
->lines
[SPARROW_VERTICAL
];
253 int hline
= p
->lines
[SPARROW_HORIZONTAL
];
254 if (vline
== BAD_PIXEL
|| hline
== BAD_PIXEL
){
255 GST_DEBUG("ignoring bad pixel %d, %d\n", x
, y
);
258 sparrow_cluster_t
*cluster
= &clusters
[hline
* fl
->n_vlines
+ vline
];
259 sparrow_voter_t
*voters
= cluster
->voters
;
261 guint signal
= (vsig
* hsig
) / SIGNAL_QUANT
;
262 GST_DEBUG("signal at %p (%d, %d): %dv %dh, product %u, lines: %dv %dh\n"
263 "cluster is %p, n is %d\n", p
, x
, y
,
264 vsig
, hsig
, signal
, vline
, hline
, cluster
, n
);
266 GST_WARNING("signal at %p (%d, %d) is %d following quantisation!\n",
270 if (n
< CLUSTER_SIZE
){
271 voters
[n
].x
= INT_TO_COORD(x
);
272 voters
[n
].y
= INT_TO_COORD(y
);
273 voters
[n
].signal
= signal
;
277 /*duplicate x, y, signal, so they aren't mucked up */
281 /*replaced one ends up here */
285 for (int j
= 0; j
< CLUSTER_SIZE
; j
++){
286 if (voters
[j
].signal
< ts
){
287 ts2
= voters
[j
].signal
;
290 voters
[j
].signal
= ts
;
298 GST_DEBUG("more than %d pixels at cluster for corner %d, %d."
299 "Dropped %u for %u\n",
300 CLUSTER_SIZE
, vline
, hline
, ts2
, signal
);
305 debug_clusters(sparrow
, fl
);
311 drop_cluster_voter(sparrow_voter_t
*voters
, int n
, int k
)
316 for (i
= k
; i
< n
; i
++){
317 voters
[i
] = voters
[i
+ 1];
323 static inline int sort_median(coord_t
*a
, guint n
)
326 /*stupid sort, but n is very small*/
327 for (i
= 0; i
< n
; i
++){
328 for (j
= i
+ 1; j
< n
; j
++){
336 guint middle
= n
/ 2;
337 coord_t answer
= a
[middle
];
340 answer
+= a
[middle
- 1];
346 #define EUCLIDEAN_D2(ax, ay, bx, by)((ax - bx) * (ax - bx) + (ay - by) * (ay - by))
347 #define EUCLIDEAN_THRESHOLD 7
350 euclidean_discard_cluster_outliers(sparrow_voter_t
*voters
, int n
)
352 /* Calculate distance between each pair. Discard points with maximum sum,
353 then recalculate until all are within threshold.
355 GST_DEBUG("cleansing a cluster of size %d using sum of distances", n
);
358 for (i
= 0; i
< n
; i
++){
360 for (j
= i
+ 1; j
< n
; j
++){
361 coord_t d
= EUCLIDEAN_D2(voters
[i
].x
, voters
[i
].y
,
362 voters
[j
].x
, voters
[j
].y
);
369 coord_t worst_d
, threshold
;
371 threshold
= EUCLIDEAN_THRESHOLD
* n
;
374 for (i
= 0; i
< n
; i
++){
375 if (dsums
[i
] > worst_d
){
380 if (worst_d
> threshold
){
381 GST_DEBUG("failing point %d, distance sq %d, threshold %d\n",
382 worst_i
, C2I(worst_d
), C2I(threshold
));
383 //subtract this one from the sums, or they'll all go
384 for (i
= 0; i
< n
; i
++){
385 dsums
[i
] -= EUCLIDEAN_D2(voters
[i
].x
, voters
[i
].y
,
386 voters
[worst_i
].x
, voters
[worst_i
].y
);
388 n
= drop_cluster_voter(voters
, n
, worst_i
);
391 GST_DEBUG("worst %d, was only %d, threshold %d\n",
392 worst_i
, C2I(worst_d
), C2I(threshold
));
400 median_discard_cluster_outliers(sparrow_voter_t
*voters
, int n
)
405 for (i
= 0; i
< n
; i
++){
406 /*XXX could sort here*/
407 xvals
[i
] = voters
[i
].x
;
408 yvals
[i
] = voters
[i
].y
;
410 const coord_t xmed
= sort_median(xvals
, n
);
411 const coord_t ymed
= sort_median(yvals
, n
);
413 for (i
= 0; i
< n
; i
++){
414 coord_t dx
= voters
[i
].x
- xmed
;
415 coord_t dy
= voters
[i
].y
- ymed
;
416 if (dx
* dx
+ dy
* dy
> OUTLIER_THRESHOLD
){
417 n
= drop_cluster_voter(voters
, n
, i
);
425 make_corners(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
426 //DEBUG_FIND_LINES(fl);
427 int width
= fl
->n_vlines
;
428 int height
= fl
->n_hlines
;
429 sparrow_cluster_t
*clusters
= fl
->clusters
;
430 sparrow_corner_t
*mesh
= fl
->mesh
;
434 for (y
= 0; y
< height
; y
++){
435 for (x
= 0; x
< width
; x
++, i
++){
436 sparrow_cluster_t
*cluster
= clusters
+ i
;
437 if (cluster
->n
== 0){
441 /*discard outliers based on sum of squared distances: good points should
442 be in a cluster, and have lowest sum*/
443 cluster
->n
= euclidean_discard_cluster_outliers(cluster
->voters
, cluster
->n
);
445 /*discard values away from median x, y values.
446 (each dimension is calculated independently)*/
447 cluster
->n
= median_discard_cluster_outliers(cluster
->voters
, cluster
->n
);
449 /* now find a weighted average position */
450 /*With int coord_t, coord_sum_t is
451 64 bit to avoid overflow -- should probably just use floating point
453 coord_sum_t xsum
, ysum
;
454 coord_t xmean
, ymean
;
460 for (j
= 0; j
< cluster
->n
; j
++){
461 votes
+= cluster
->voters
[j
].signal
;
462 ysum
+= cluster
->voters
[j
].y
* cluster
->voters
[j
].signal
;
463 xsum
+= cluster
->voters
[j
].x
* cluster
->voters
[j
].signal
;
466 xmean
= xsum
/ votes
;
467 ymean
= ysum
/ votes
;
470 GST_WARNING("corner %d, %d voters, sum %d,%d, somehow has no votes\n",
471 i
, cluster
->n
, xsum
, ysum
);
474 GST_DEBUG("corner %d: %d voters, %d votes, sum %d,%d, mean %d,%d\n",
475 i
, cluster
->n
, votes
, C2I(xsum
), C2I(ysum
), C2I(xmean
), C2I(ymean
));
479 mesh
[i
].status
= CORNER_EXACT
;
480 GST_DEBUG("found corner %d at (%3f, %3f)\n",
481 i
, COORD_TO_FLOAT(xmean
), COORD_TO_FLOAT(ymean
));
486 static sparrow_voter_t
487 median_centre(sparrow_voter_t
*estimates
, int n
){
488 /*X and Y arevcalculated independently, which is really not right.
489 on the other hand, it probably works. */
491 sparrow_voter_t result
;
493 for (i
= 0; i
< n
; i
++){
494 vals
[i
] = estimates
[i
].x
;
496 result
.x
= coord_median(vals
, n
);
498 for (i
= 0; i
< n
; i
++){
499 vals
[i
] = estimates
[i
].y
;
501 result
.y
= coord_median(vals
, n
);
505 static const sparrow_estimator_t base_estimators
[] = {
520 #define BASE_ESTIMATORS (sizeof(base_estimators) / sizeof(sparrow_estimator_t))
521 #define ESTIMATORS (BASE_ESTIMATORS * 4)
524 calculate_estimator_tables(sparrow_estimator_t
*estimators
){
526 sparrow_estimator_t
*e
= estimators
;
527 for (i
= 0; i
< BASE_ESTIMATORS
; i
++){
528 for (j
= 0; j
< 4; j
++){
529 *e
= base_estimators
[i
];
546 GST_DEBUG("estimator: %-d,%-d %-d,%-d %-d,%-d",
547 e
->x1
, e
->y1
, e
->x2
, e
->y2
, e
->x3
, e
->y3
);
553 /* nice big word. acos(1.0 - MAX_NONCOLLINEARITY) = angle of deviation.
554 0.005: 5.7 degrees, 0.01: 8.1, 0.02: 11.5, 0.04: 16.3, 0.08: 23.1
555 1 pixel deviation in 32 -> ~ 1/33 == 0.03 (if I understand correctly)
557 #define MAX_NONCOLLINEARITY 0.02
559 /*the map made above is likely to be full of errors. Fix them, and add in
562 complete_map(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
563 sparrow_voter_t estimates
[ESTIMATORS
+ 1];
564 sparrow_estimator_t estimators
[ESTIMATORS
];
565 calculate_estimator_tables(estimators
);
567 guint32
*debug
= NULL
;
569 debug
= (guint32
*)fl
->debug
->imageData
;
570 memset(debug
, 0, sparrow
->in
.size
);
574 int width
= fl
->n_vlines
;
575 int height
= fl
->n_hlines
;
576 int screen_width
= sparrow
->in
.width
;
577 int screen_height
= sparrow
->in
.height
;
578 sparrow_corner_t
*mesh
= fl
->mesh
;
580 int prev_settled
= 0;
583 for (y
= 0; y
< height
; y
++){
584 for (x
= 0; x
< width
; x
++){
585 sparrow_corner_t
*corner
= &mesh
[y
* width
+ x
];
586 if (corner
->status
== CORNER_SETTLED
){
588 GST_DEBUG("ignoring settled corner %d, %d", x
, y
);
591 //memset(estimates, 0, sizeof(estimates));
593 for (guint j
= 0; j
< ESTIMATORS
; j
++){
594 sparrow_estimator_t
*e
= &estimators
[j
];
595 int x3
, y3
, x2
, y2
, x1
, y1
;
598 if (!(y3
>= 0 && y3
< height
&&
599 x3
>= 0 && x3
< width
&&
600 mesh
[y3
* width
+ x3
].status
!= CORNER_UNUSED
602 GST_DEBUG("not using estimator %d because corners aren't used, or are off screen\n"
603 "x3 %d, y3 %d", j
, x3
, y3
);
610 if (mesh
[y2
* width
+ x2
].status
== CORNER_UNUSED
||
611 mesh
[y1
* width
+ x1
].status
== CORNER_UNUSED
){
612 GST_DEBUG("not using estimator %d because corners aren't used", j
);
615 /*there are 3 points, and the unknown one.
616 They should all be in a line.
617 The ratio of the p3-p2:p2-p1 sould be the same as
620 This really has to be done in floating point.
622 collinearity, no division, but no useful error metric
623 x[0] * (y[1]-y[2]) + x[1] * (y[2]-y[0]) + x[2] * (y[0]-y[1]) == 0
624 (at least not without further division)
628 cos angle = dot product / product of euclidean lengths
630 (dx12 * dx23 + dy12 * dy23) /
631 (sqrt(dx12 * dx12 + dy12 * dy12) * sqrt(dx23 * dx23 + dy23 * dy23))
633 is costly up front (sqrt), but those distances need to be
634 calculated anyway (or at least they are handy). Not much gained by
635 short-circuiting on bad collinearity, though.
637 It also handlily catches all the division by zeros in one meaningful
640 sparrow_corner_t
*c1
= &mesh
[y1
* width
+ x1
];
641 sparrow_corner_t
*c2
= &mesh
[y2
* width
+ x2
];
642 sparrow_corner_t
*c3
= &mesh
[y3
* width
+ x3
];
644 double dx12
= c1
->x
- c2
->x
;
645 double dy12
= c1
->y
- c2
->y
;
646 double dx23
= c2
->x
- c3
->x
;
647 double dy23
= c2
->y
- c3
->y
;
648 double distance12
= sqrt(dx12
* dx12
+ dy12
* dy12
);
649 double distance23
= sqrt(dx23
* dx23
+ dy23
* dy23
);
651 double dp
= dx12
* dx23
+ dy12
* dy23
;
653 double distances
= distance12
* distance23
;
655 GST_DEBUG("mesh points: %d,%d, %d,%d, %d,%d\n"
656 "map points: %d,%d, %d,%d, %d,%d\n"
657 "diffs: 12: %0.3f,%0.3f, 23: %0.3f,%0.3f, \n"
658 "distances: 12: %0.3f, 32: %0.3f\n",
659 x1
, y1
, x2
, y2
, x3
, y3
,
660 C2I(c1
->x
), C2I(c1
->y
), C2I(c2
->x
), C2I(c2
->y
), C2I(c3
->x
), C2I(c3
->y
),
661 dx12
, dy12
, dx23
, dy23
, distance12
, distance23
667 if (distances
== 0.0){
668 GST_INFO("at least two points out of %d,%d, %d,%d, %d,%d are the same!",
669 x1
, y1
, x2
, y2
, x3
, y3
);
672 double line_error
= 1.0 - dp
/ distances
;
673 if (line_error
> MAX_NONCOLLINEARITY
){
674 GST_DEBUG("Points %d,%d, %d,%d, %d,%d are not in a line: non-collinearity: %3f",
675 x1
, y1
, x2
, y2
, x3
, y3
, line_error
);
678 //GST_DEBUG("GOOD collinearity: %3f", line_error);
681 double ratio
= distance12
/ distance23
;
682 /*so here's the estimate!*/
683 coord_t dx
= dx12
* ratio
;
684 coord_t dy
= dy12
* ratio
;
685 coord_t ex
= c1
->x
+ dx
;
686 coord_t ey
= c1
->y
+ dy
;
689 GST_DEBUG("dx, dy: %d,%d, ex, ey: %d,%d\n"
690 "dx raw: %0.3f,%0.3f, x1, x2: %0.3f,%0.3f,\n"
691 "distances: 12: %0.3f, 32: %0.3f\n"
693 C2I(dx
), C2I(dy
), C2I(ex
), C2I(ey
),
694 dx
, dy
, ex
, ey
, ratio
698 if (! coord_in_range(ey
, screen_height
) ||
699 ! coord_in_range(ex
, screen_width
)){
700 GST_DEBUG("rejecting estimate for %d, %d, due to ex, ey being %d, %d",
701 x
, y
, C2I(ex
), C2I(ey
));
705 GST_DEBUG("estimator %d,%d SUCCESSFULLY estimated that %d, %d will be %d, %d",
706 x1, x2, x, y, C2I(ex), C2I(ey));
711 debug
[coords_to_index(ex
, ey
, sparrow
->in
.width
, sparrow
->in
.height
)] = 0x00aa7700;
715 /*now there is an array of estimates.
716 The *_discard_cluster_outliers functions should fit here */
717 GST_INFO("got %d estimates for %d,%d", k
, x
, y
);
725 /*now find median values. If the number is even, add a copy of either
726 the original value, or a random element. */
728 if (corner
->status
!= CORNER_UNUSED
){
729 estimates
[k
].x
= corner
->x
;
730 estimates
[k
].y
= corner
->y
;
733 int r
= RANDINT(sparrow
, 0, r
);
734 estimates
[k
].x
= estimates
[r
].x
;
735 estimates
[k
].y
= estimates
[r
].y
;
739 sparrow_voter_t centre
= median_centre(estimates
, k
);
745 k
= euclidean_discard_cluster_outliers(estimates
, k
);
747 for (int j
= 0; j
< k
; j
++){
748 debug
[coords_to_index(estimates
[j
].x
, estimates
[j
].y
,
749 sparrow
->in
.width
, sparrow
->in
.height
)] = 0x00ffff00;
752 GST_INFO("After discard, left with %d estimates", k
);
753 /*now what? the mean? yes.*/
756 for (int j
= 0; j
< k
; j
++){
757 sumx
+= estimates
[j
].x
;
758 sumy
+= estimates
[j
].y
;
765 GST_INFO("estimating %d,%d", C2I(guess_x
), C2I(guess_y
));
767 if (corner
->status
== CORNER_EXACT
){
768 GST_INFO("using exact reading %d,%d", C2I(corner
->x
), C2I(corner
->y
));
770 debug
[coords_to_index(corner
->x
, corner
->y
,
771 sparrow
->in
.width
, sparrow
->in
.height
)] = 0xffff3300;
773 GST_DEBUG("exact corner");
774 if (abs(corner
->x
- guess_x
) < 2){
777 if (abs(corner
->y
- guess_y
) < 2){
782 GST_DEBUG("weak evidence, mark corner PROJECTED");
783 corner
->status
= CORNER_PROJECTED
;
785 debug
[coords_to_index(guess_x
, guess_y
,
786 sparrow
->in
.width
, sparrow
->in
.height
)] = 0xff0000ff;
790 GST_DEBUG("corner is SETTLED");
791 corner
->status
= CORNER_SETTLED
;
794 debug
[coords_to_index(guess_x
, guess_y
,
795 sparrow
->in
.width
, sparrow
->in
.height
)] = 0xffffffff;
802 GST_INFO("settled %d in that round. %d left to go",
803 settled
- prev_settled
, width
* height
- settled
);
804 if (settled
== width
* height
|| settled
== prev_settled
){
807 prev_settled
= settled
;
809 MAYBE_DEBUG_IPL(fl
->debug
);
814 calculate_deltas(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
816 int width
= fl
->n_vlines
;
817 int height
= fl
->n_hlines
;
818 sparrow_corner_t
*mesh
= fl
->mesh
;
821 //DEBUG_FIND_LINES(fl);
822 /* calculate deltas toward adjacent corners */
823 /* try to extrapolate left and up, if possible, so need to go backwards. */
824 i
= width
* height
- 1;
825 for (y
= height
- 1; y
>= 0; y
--){
826 for (x
= width
- 1; x
>= 0; x
--, i
--){
827 sparrow_corner_t
*corner
= &mesh
[i
];
828 /* calculate the delta to next corner. If this corner is on edge, delta is
829 0 and next is this.*/
830 sparrow_corner_t
*right
= (x
== width
- 1) ? corner
: corner
+ 1;
831 sparrow_corner_t
*down
= (y
== height
- 1) ? corner
: corner
+ width
;
832 GST_DEBUG("i %d xy %d,%d width %d. in_xy %d,%d; down in_xy %d,%d; right in_xy %d,%d\n",
833 i
, x
, y
, width
, C2I(corner
->x
), C2I(corner
->y
), C2I(down
->x
),
834 C2I(down
->y
), C2I(right
->x
), C2I(right
->y
));
835 if (corner
->status
!= CORNER_UNUSED
){
836 corner
->dxr
= QUANTISE_DELTA(right
->x
- corner
->x
);
837 corner
->dyr
= QUANTISE_DELTA(right
->y
- corner
->y
);
838 corner
->dxd
= QUANTISE_DELTA(down
->x
- corner
->x
);
839 corner
->dyd
= QUANTISE_DELTA(down
->y
- corner
->y
);
844 debug_corners_image(sparrow
, fl
);
850 look_for_line(GstSparrow
*sparrow
, guint8
*in
, sparrow_find_lines_t
*fl
,
851 sparrow_line_t
*line
){
854 guint32 cmask
= sparrow
->out
.colours
[sparrow
->colour
];
857 /* subtract background noise */
858 fl
->input
->imageData
= (char *)in
;
859 cvSub(fl
->input
, fl
->threshold
, fl
->working
, NULL
);
860 guint32
*in32
= (guint32
*)fl
->working
->imageData
;
862 for (i
= 0; i
< sparrow
->in
.pixcount
; i
++){
863 colour
= in32
[i
] & cmask
;
864 signal
= (((colour
>> fl
->shift1
) & COLOUR_MASK
) +
865 ((colour
>> fl
->shift2
) & COLOUR_MASK
));
867 if (fl
->map
[i
].lines
[line
->dir
]){
868 /*assume the pixel is on for everyone and will just confuse
872 if (fl
->map
[i
].lines
[line
->dir
] != BAD_PIXEL
){
874 GST_DEBUG("HEY, expected point %d to be in line %d (dir %d) "
875 "and thus empty, but it is also in line %d\n"
876 "old signal %d, new signal %d, marking as BAD\n",
877 i, line->index, line->dir, fl->map[i].lines[line->dir],
878 fl->map[i].signal[line->dir], signal);
880 fl
->map
[i
].lines
[line
->dir
] = BAD_PIXEL
;
881 fl
->map
[i
].signal
[line
->dir
] = 0;
885 fl
->map
[i
].lines
[line
->dir
] = line
->index
;
886 fl
->map
[i
].signal
[line
->dir
] = signal
;
893 debug_map_image(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
894 guint32
*data
= (guint32
*)fl
->debug
->imageData
;
895 memset(data
, 0, sparrow
->in
.size
);
896 for (guint i
= 0; i
< sparrow
->in
.pixcount
; i
++){
897 data
[i
] |= fl
->map
[i
].signal
[SPARROW_HORIZONTAL
] << sparrow
->in
.gshift
;
898 data
[i
] |= fl
->map
[i
].signal
[SPARROW_VERTICAL
] << sparrow
->in
.rshift
;
899 data
[i
] |= ((fl
->map
[i
].lines
[SPARROW_VERTICAL
] == BAD_PIXEL
) ||
900 (fl
->map
[i
].lines
[SPARROW_HORIZONTAL
] == BAD_PIXEL
)) ? 255 << sparrow
->in
.bshift
: 0;
902 MAYBE_DEBUG_IPL(fl
->debug
);
905 /* draw the line (in sparrow->colour) */
907 draw_line(GstSparrow
* sparrow
, sparrow_line_t
*line
, guint8
*out
){
908 guint32
*p
= (guint32
*)out
;
909 guint32 colour
= sparrow
->out
.colours
[sparrow
->colour
];
911 if (line
->dir
== SPARROW_HORIZONTAL
){
912 p
+= line
->offset
* sparrow
->out
.width
;
913 for (i
= 0; i
< sparrow
->out
.width
; i
++){
918 guint32
*p
= (guint32
*)out
;
920 for(i
= 0; i
< sparrow
->out
.height
; i
++){
922 p
+= sparrow
->out
.width
;
928 jump_state(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
, edges_state_t state
){
929 if (state
== EDGES_NEXT_STATE
){
936 case EDGES_FIND_NOISE
:
937 sparrow
->countdown
= MAX(sparrow
->lag
, 1) + SAFETY_LAG
;
939 case EDGES_FIND_LINES
:
940 sparrow
->countdown
= MAX(sparrow
->lag
, 1) + SAFETY_LAG
;
942 case EDGES_FIND_CORNERS
:
943 sparrow
->countdown
= 7;
945 case EDGES_WAIT_FOR_PLAY
:
946 global_number_of_edge_finders
--;
947 sparrow
->countdown
= 300;
950 GST_DEBUG("jumped to non-existent state %d\n", fl
->state
);
955 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
959 draw_lines(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
, guint8
*in
, guint8
*out
)
961 sparrow_line_t
*line
= fl
->shuffled_lines
[fl
->current
];
962 sparrow
->countdown
--;
963 memset(out
, 0, sparrow
->out
.size
);
964 if (sparrow
->countdown
){
965 draw_line(sparrow
, line
, out
);
968 /*show nothing, look for result */
969 look_for_line(sparrow
, in
, fl
, line
);
971 debug_map_image(sparrow
, fl
);
974 if (fl
->current
== fl
->n_lines
){
975 jump_state(sparrow
, fl
, EDGES_NEXT_STATE
);
978 sparrow
->countdown
= MAX(sparrow
->lag
, 1) + SAFETY_LAG
;
983 #define LINE_THRESHOLD 32
986 find_threshold(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
, guint8
*in
, guint8
*out
)
988 memset(out
, 0, sparrow
->out
.size
);
989 /*XXX should average/median over a range of frames */
990 if (sparrow
->countdown
== 0){
991 memcpy(fl
->threshold
->imageData
, in
, sparrow
->in
.size
);
992 /*add a constant, and smooth */
993 cvAddS(fl
->threshold
, cvScalarAll(LINE_THRESHOLD
), fl
->working
, NULL
);
994 cvSmooth(fl
->working
, fl
->threshold
, CV_GAUSSIAN
, 3, 0, 0, 0);
995 //cvSmooth(fl->working, fl->threshold, CV_MEDIAN, 3, 0, 0, 0);
996 jump_state(sparrow
, fl
, EDGES_NEXT_STATE
);
998 sparrow
->countdown
--;
1001 /*match up lines and find corners */
1003 find_corners(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
)
1005 sparrow
->countdown
--;
1006 switch(sparrow
->countdown
){
1008 make_clusters(sparrow
, fl
);
1011 make_corners(sparrow
, fl
);
1014 complete_map(sparrow
, fl
);
1017 calculate_deltas(sparrow
, fl
);
1021 corners_to_full_lut(sparrow
, fl
);
1023 corners_to_lut(sparrow
, fl
);
1025 jump_state(sparrow
, fl
, EDGES_NEXT_STATE
);
1028 GST_DEBUG("how did sparrow->countdown get to be %d?", sparrow
->countdown
);
1029 sparrow
->countdown
= 5;
1031 return sparrow
->countdown
;
1034 /*use a dirty shared variable*/
1036 wait_for_play(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
1037 if (global_number_of_edge_finders
== 0 ||
1038 sparrow
->countdown
== 0){
1041 sparrow
->countdown
--;
1045 INVISIBLE sparrow_state
1046 mode_find_edges(GstSparrow
*sparrow
, guint8
*in
, guint8
*out
){
1047 sparrow_find_lines_t
*fl
= (sparrow_find_lines_t
*)sparrow
->helper_struct
;
1049 case EDGES_FIND_NOISE
:
1050 find_threshold(sparrow
, fl
, in
, out
);
1052 case EDGES_FIND_LINES
:
1053 draw_lines(sparrow
, fl
, in
, out
);
1055 case EDGES_FIND_CORNERS
:
1056 memset(out
, 0, sparrow
->out
.size
);
1057 find_corners(sparrow
, fl
);
1059 case EDGES_WAIT_FOR_PLAY
:
1060 memset(out
, 0, sparrow
->out
.size
);
1061 if (wait_for_play(sparrow
, fl
)){
1062 return SPARROW_NEXT_STATE
;
1066 GST_WARNING("strange state in mode_find_edges: %d", fl
->state
);
1067 memset(out
, 0, sparrow
->out
.size
);
1069 return SPARROW_STATUS_QUO
;
1073 finalise_find_edges(GstSparrow
*sparrow
){
1074 sparrow_find_lines_t
*fl
= (sparrow_find_lines_t
*)sparrow
->helper_struct
;
1075 //DEBUG_FIND_LINES(fl);
1076 if (sparrow
->save
&& *(sparrow
->save
)){
1077 GST_DEBUG("about to save to %s\n", sparrow
->save
);
1078 dump_edges_info(sparrow
, fl
, sparrow
->save
);
1080 if (sparrow
->debug
){
1081 cvReleaseImage(&fl
->debug
);
1084 free(fl
->shuffled_lines
);
1088 cvReleaseImage(&fl
->threshold
);
1089 cvReleaseImage(&fl
->working
);
1090 cvReleaseImageHeader(&fl
->input
);
1092 GST_DEBUG("freed everything\n");
1093 sparrow
->helper_struct
= NULL
;
1097 setup_colour_shifts(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
1098 /*COLOUR_QUANT reduces the signal a little bit more, avoiding overflow
1100 switch (sparrow
->colour
){
1103 fl
->shift1
= sparrow
->in
.gshift
+ COLOUR_QUANT
;
1104 fl
->shift2
= sparrow
->in
.gshift
+ COLOUR_QUANT
;
1106 case SPARROW_MAGENTA
:
1107 fl
->shift1
= sparrow
->in
.rshift
+ COLOUR_QUANT
;
1108 fl
->shift2
= sparrow
->in
.bshift
+ COLOUR_QUANT
;
1114 init_find_edges(GstSparrow
*sparrow
){
1116 sparrow_find_lines_t
*fl
= zalloc_aligned_or_die(sizeof(sparrow_find_lines_t
));
1117 sparrow
->helper_struct
= (void *)fl
;
1119 gint h_lines
= (sparrow
->out
.height
+ LINE_PERIOD
- 1) / LINE_PERIOD
;
1120 gint v_lines
= (sparrow
->out
.width
+ LINE_PERIOD
- 1) / LINE_PERIOD
;
1121 gint n_lines_max
= (h_lines
+ v_lines
);
1122 gint n_corners
= (h_lines
* v_lines
);
1123 fl
->n_hlines
= h_lines
;
1124 fl
->n_vlines
= v_lines
;
1126 fl
->h_lines
= malloc_aligned_or_die(sizeof(sparrow_line_t
) * n_lines_max
);
1127 fl
->shuffled_lines
= malloc_aligned_or_die(sizeof(sparrow_line_t
*) * n_lines_max
);
1128 GST_DEBUG("shuffled lines, malloced %p\n", fl
->shuffled_lines
);
1130 GST_DEBUG("map is going to be %d * %d \n", sizeof(sparrow_intersect_t
), sparrow
->in
.pixcount
);
1131 fl
->map
= zalloc_aligned_or_die(sizeof(sparrow_intersect_t
) * sparrow
->in
.pixcount
);
1132 fl
->clusters
= zalloc_or_die(n_corners
* sizeof(sparrow_cluster_t
));
1133 fl
->mesh
= zalloc_aligned_or_die(n_corners
* sizeof(sparrow_corner_t
));
1135 sparrow_line_t
*line
= fl
->h_lines
;
1136 sparrow_line_t
**sline
= fl
->shuffled_lines
;
1139 for (i
= 0, offset
= H_LINE_OFFSET
; offset
< sparrow
->out
.height
;
1140 i
++, offset
+= LINE_PERIOD
){
1141 line
->offset
= offset
;
1142 line
->dir
= SPARROW_HORIZONTAL
;
1147 //GST_DEBUG("line %d h has offset %d\n", i, offset);
1150 /*now add the vertical lines */
1152 for (i
= 0, offset
= V_LINE_OFFSET
; offset
< sparrow
->out
.width
;
1153 i
++, offset
+= LINE_PERIOD
){
1154 line
->offset
= offset
;
1155 line
->dir
= SPARROW_VERTICAL
;
1160 //GST_DEBUG("line %d v has offset %d\n", i, offset);
1162 //DEBUG_FIND_LINES(fl);
1163 fl
->n_lines
= line
- fl
->h_lines
;
1164 GST_DEBUG("allocated %d lines, made %d\n", n_lines_max
, fl
->n_lines
);
1167 for (i
= 0; i
< fl
->n_lines
; i
++){
1168 int j
= RANDINT(sparrow
, 0, fl
->n_lines
);
1169 sparrow_line_t
*tmp
= fl
->shuffled_lines
[j
];
1170 fl
->shuffled_lines
[j
] = fl
->shuffled_lines
[i
];
1171 fl
->shuffled_lines
[i
] = tmp
;
1174 setup_colour_shifts(sparrow
, fl
);
1176 /* opencv images for threshold finding */
1177 CvSize size
= {sparrow
->in
.width
, sparrow
->in
.height
};
1178 fl
->working
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);
1179 fl
->threshold
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);
1181 /*input has no data allocated -- it uses latest frame*/
1182 fl
->input
= init_ipl_image(&sparrow
->in
, PIXSIZE
);
1183 //DEBUG_FIND_LINES(fl);
1184 if (sparrow
->debug
){
1185 fl
->debug
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);
1188 if (sparrow
->reload
){
1189 if (access(sparrow
->reload
, R_OK
)){
1190 GST_DEBUG("sparrow->reload is '%s' and it is UNREADABLE\n", sparrow
->reload
);
1193 read_edges_info(sparrow
, fl
, sparrow
->reload
);
1194 memset(fl
->map
, 0, sizeof(sparrow_intersect_t
) * sparrow
->in
.pixcount
);
1195 //memset(fl->clusters, 0, n_corners * sizeof(sparrow_cluster_t));
1196 memset(fl
->mesh
, 0, n_corners
* sizeof(sparrow_corner_t
));
1197 jump_state(sparrow
, fl
, EDGES_FIND_CORNERS
);
1200 jump_state(sparrow
, fl
, EDGES_FIND_NOISE
);
1203 global_number_of_edge_finders
++;