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
;
579 sparrow_corner_t
*mesh_next
= fl
->mesh_next
;
581 memset(estimates
, 0, sizeof(estimates
)); /*just for clarity in debugging */
582 int prev_settled
= 0;
584 memcpy(mesh_next
, mesh
, width
* height
* sizeof(sparrow_corner_t
));
586 for (y
= 0; y
< height
; y
++){
587 for (x
= 0; x
< width
; x
++){
588 sparrow_corner_t
*corner
= &mesh
[y
* width
+ x
];
589 if (corner
->status
== CORNER_SETTLED
){
591 GST_DEBUG("ignoring settled corner %d, %d", x
, y
);
594 //memset(estimates, 0, sizeof(estimates));
596 for (guint j
= 0; j
< ESTIMATORS
; j
++){
597 sparrow_estimator_t
*e
= &estimators
[j
];
598 int x3
, y3
, x2
, y2
, x1
, y1
;
601 if (!(y3
>= 0 && y3
< height
&&
602 x3
>= 0 && x3
< width
&&
603 mesh
[y3
* width
+ x3
].status
!= CORNER_UNUSED
605 GST_DEBUG("not using estimator %d because corners aren't used, or are off screen\n"
606 "x3 %d, y3 %d", j
, x3
, y3
);
613 if (mesh
[y2
* width
+ x2
].status
== CORNER_UNUSED
||
614 mesh
[y1
* width
+ x1
].status
== CORNER_UNUSED
){
615 GST_DEBUG("not using estimator %d because corners aren't used", j
);
618 /*there are 3 points, and the unknown one.
619 They should all be in a line.
620 The ratio of the p3-p2:p2-p1 sould be the same as
623 This really has to be done in floating point.
625 collinearity, no division, but no useful error metric
626 x[0] * (y[1]-y[2]) + x[1] * (y[2]-y[0]) + x[2] * (y[0]-y[1]) == 0
627 (at least not without further division)
631 cos angle = dot product / product of euclidean lengths
633 (dx12 * dx23 + dy12 * dy23) /
634 (sqrt(dx12 * dx12 + dy12 * dy12) * sqrt(dx23 * dx23 + dy23 * dy23))
636 is costly up front (sqrt), but those distances need to be
637 calculated anyway (or at least they are handy). Not much gained by
638 short-circuiting on bad collinearity, though.
640 It also handlily catches all the division by zeros in one meaningful
643 sparrow_corner_t
*c1
= &mesh
[y1
* width
+ x1
];
644 sparrow_corner_t
*c2
= &mesh
[y2
* width
+ x2
];
645 sparrow_corner_t
*c3
= &mesh
[y3
* width
+ x3
];
647 double dx12
= c1
->x
- c2
->x
;
648 double dy12
= c1
->y
- c2
->y
;
649 double dx23
= c2
->x
- c3
->x
;
650 double dy23
= c2
->y
- c3
->y
;
651 double distance12
= sqrt(dx12
* dx12
+ dy12
* dy12
);
652 double distance23
= sqrt(dx23
* dx23
+ dy23
* dy23
);
654 double dp
= dx12
* dx23
+ dy12
* dy23
;
656 double distances
= distance12
* distance23
;
658 GST_DEBUG("mesh points: %d,%d, %d,%d, %d,%d\n"
659 "map points: %d,%d, %d,%d, %d,%d\n"
660 "diffs: 12: %0.3f,%0.3f, 23: %0.3f,%0.3f, \n"
661 "distances: 12: %0.3f, 32: %0.3f\n",
662 x1
, y1
, x2
, y2
, x3
, y3
,
663 C2I(c1
->x
), C2I(c1
->y
), C2I(c2
->x
), C2I(c2
->y
), C2I(c3
->x
), C2I(c3
->y
),
664 dx12
, dy12
, dx23
, dy23
, distance12
, distance23
670 if (distances
== 0.0){
671 GST_INFO("at least two points out of %d,%d, %d,%d, %d,%d are the same!",
672 x1
, y1
, x2
, y2
, x3
, y3
);
675 double line_error
= 1.0 - dp
/ distances
;
676 if (line_error
> MAX_NONCOLLINEARITY
){
677 GST_DEBUG("Points %d,%d, %d,%d, %d,%d are not in a line: non-collinearity: %3f",
678 x1
, y1
, x2
, y2
, x3
, y3
, line_error
);
681 //GST_DEBUG("GOOD collinearity: %3f", line_error);
684 double ratio
= distance12
/ distance23
;
685 /*so here's the estimate!*/
686 coord_t dx
= dx12
* ratio
;
687 coord_t dy
= dy12
* ratio
;
688 coord_t ex
= c1
->x
+ dx
;
689 coord_t ey
= c1
->y
+ dy
;
692 GST_DEBUG("dx, dy: %d,%d, ex, ey: %d,%d\n"
693 "dx raw: %0.3f,%0.3f, x1, x2: %0.3f,%0.3f,\n"
694 "distances: 12: %0.3f, 32: %0.3f\n"
696 C2I(dx
), C2I(dy
), C2I(ex
), C2I(ey
),
697 dx
, dy
, ex
, ey
, ratio
701 if (! coord_in_range(ey
, screen_height
) ||
702 ! coord_in_range(ex
, screen_width
)){
703 GST_DEBUG("rejecting estimate for %d, %d, due to ex, ey being %d, %d",
704 x
, y
, C2I(ex
), C2I(ey
));
708 GST_DEBUG("estimator %d,%d SUCCESSFULLY estimated that %d, %d will be %d, %d",
709 x1, x2, x, y, C2I(ex), C2I(ey));
714 debug
[coords_to_index(ex
, ey
, sparrow
->in
.width
, sparrow
->in
.height
)] = 0x00aa7700;
718 /*now there is an array of estimates.
719 The *_discard_cluster_outliers functions should fit here */
720 GST_INFO("got %d estimates for %d,%d", k
, x
, y
);
728 /*now find median values. If the number is even, add a copy of either
729 the original value, or a random element. */
731 if (corner
->status
!= CORNER_UNUSED
){
732 estimates
[k
].x
= corner
->x
;
733 estimates
[k
].y
= corner
->y
;
736 int r
= RANDINT(sparrow
, 0, r
);
737 estimates
[k
].x
= estimates
[r
].x
;
738 estimates
[k
].y
= estimates
[r
].y
;
742 sparrow_voter_t centre
= median_centre(estimates
, k
);
748 k
= euclidean_discard_cluster_outliers(estimates
, k
);
750 for (int j
= 0; j
< k
; j
++){
751 debug
[coords_to_index(estimates
[j
].x
, estimates
[j
].y
,
752 sparrow
->in
.width
, sparrow
->in
.height
)] = 0x00ffff00;
755 GST_INFO("After discard, left with %d estimates", k
);
756 /*now what? the mean? yes.*/
759 for (int j
= 0; j
< k
; j
++){
760 sumx
+= estimates
[j
].x
;
761 sumy
+= estimates
[j
].y
;
768 GST_INFO("estimating %d,%d", C2I(guess_x
), C2I(guess_y
));
770 if (corner
->status
== CORNER_EXACT
){
771 GST_INFO("using exact reading %d,%d", C2I(corner
->x
), C2I(corner
->y
));
773 debug
[coords_to_index(corner
->x
, corner
->y
,
774 sparrow
->in
.width
, sparrow
->in
.height
)] = 0xffff3300;
776 GST_DEBUG("exact corner");
777 if (abs(corner
->x
- guess_x
) < 2){
780 if (abs(corner
->y
- guess_y
) < 2){
785 GST_DEBUG("weak evidence, mark corner PROJECTED");
786 corner
->status
= CORNER_PROJECTED
;
788 debug
[coords_to_index(guess_x
, guess_y
,
789 sparrow
->in
.width
, sparrow
->in
.height
)] = 0xff0000ff;
793 GST_DEBUG("corner is SETTLED");
794 corner
->status
= CORNER_SETTLED
;
797 debug
[coords_to_index(guess_x
, guess_y
,
798 sparrow
->in
.width
, sparrow
->in
.height
)] = 0xffffffff;
805 GST_INFO("settled %d in that round. %d left to go",
806 settled
- prev_settled
, width
* height
- settled
);
807 if (settled
== width
* height
|| settled
== prev_settled
){
810 prev_settled
= settled
;
811 sparrow_corner_t
*tmp
= mesh_next
;
816 fl
->mesh_next
= mesh_next
;
817 MAYBE_DEBUG_IPL(fl
->debug
);
822 calculate_deltas(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
824 int width
= fl
->n_vlines
;
825 int height
= fl
->n_hlines
;
826 sparrow_corner_t
*mesh
= fl
->mesh
;
829 //DEBUG_FIND_LINES(fl);
830 /* calculate deltas toward adjacent corners */
831 /* try to extrapolate left and up, if possible, so need to go backwards. */
832 i
= width
* height
- 1;
833 for (y
= height
- 1; y
>= 0; y
--){
834 for (x
= width
- 1; x
>= 0; x
--, i
--){
835 sparrow_corner_t
*corner
= &mesh
[i
];
836 /* calculate the delta to next corner. If this corner is on edge, delta is
837 0 and next is this.*/
838 sparrow_corner_t
*right
= (x
== width
- 1) ? corner
: corner
+ 1;
839 sparrow_corner_t
*down
= (y
== height
- 1) ? corner
: corner
+ width
;
840 GST_DEBUG("i %d xy %d,%d width %d. in_xy %d,%d; down in_xy %d,%d; right in_xy %d,%d\n",
841 i
, x
, y
, width
, C2I(corner
->x
), C2I(corner
->y
), C2I(down
->x
),
842 C2I(down
->y
), C2I(right
->x
), C2I(right
->y
));
843 if (corner
->status
!= CORNER_UNUSED
){
844 corner
->dxr
= QUANTISE_DELTA(right
->x
- corner
->x
);
845 corner
->dyr
= QUANTISE_DELTA(right
->y
- corner
->y
);
846 corner
->dxd
= QUANTISE_DELTA(down
->x
- corner
->x
);
847 corner
->dyd
= QUANTISE_DELTA(down
->y
- corner
->y
);
852 debug_corners_image(sparrow
, fl
);
858 look_for_line(GstSparrow
*sparrow
, guint8
*in
, sparrow_find_lines_t
*fl
,
859 sparrow_line_t
*line
){
862 guint32 cmask
= sparrow
->out
.colours
[sparrow
->colour
];
865 /* subtract background noise */
866 fl
->input
->imageData
= (char *)in
;
867 cvSub(fl
->input
, fl
->threshold
, fl
->working
, NULL
);
868 guint32
*in32
= (guint32
*)fl
->working
->imageData
;
870 for (i
= 0; i
< sparrow
->in
.pixcount
; i
++){
871 colour
= in32
[i
] & cmask
;
872 signal
= (((colour
>> fl
->shift1
) & COLOUR_MASK
) +
873 ((colour
>> fl
->shift2
) & COLOUR_MASK
));
875 if (fl
->map
[i
].lines
[line
->dir
]){
876 /*assume the pixel is on for everyone and will just confuse
880 if (fl
->map
[i
].lines
[line
->dir
] != BAD_PIXEL
){
882 GST_DEBUG("HEY, expected point %d to be in line %d (dir %d) "
883 "and thus empty, but it is also in line %d\n"
884 "old signal %d, new signal %d, marking as BAD\n",
885 i, line->index, line->dir, fl->map[i].lines[line->dir],
886 fl->map[i].signal[line->dir], signal);
888 fl
->map
[i
].lines
[line
->dir
] = BAD_PIXEL
;
889 fl
->map
[i
].signal
[line
->dir
] = 0;
893 fl
->map
[i
].lines
[line
->dir
] = line
->index
;
894 fl
->map
[i
].signal
[line
->dir
] = signal
;
901 debug_map_image(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
902 guint32
*data
= (guint32
*)fl
->debug
->imageData
;
903 memset(data
, 0, sparrow
->in
.size
);
904 for (guint i
= 0; i
< sparrow
->in
.pixcount
; i
++){
905 data
[i
] |= fl
->map
[i
].signal
[SPARROW_HORIZONTAL
] << sparrow
->in
.gshift
;
906 data
[i
] |= fl
->map
[i
].signal
[SPARROW_VERTICAL
] << sparrow
->in
.rshift
;
907 data
[i
] |= ((fl
->map
[i
].lines
[SPARROW_VERTICAL
] == BAD_PIXEL
) ||
908 (fl
->map
[i
].lines
[SPARROW_HORIZONTAL
] == BAD_PIXEL
)) ? 255 << sparrow
->in
.bshift
: 0;
910 MAYBE_DEBUG_IPL(fl
->debug
);
913 /* draw the line (in sparrow->colour) */
915 draw_line(GstSparrow
* sparrow
, sparrow_line_t
*line
, guint8
*out
){
916 guint32
*p
= (guint32
*)out
;
917 guint32 colour
= sparrow
->out
.colours
[sparrow
->colour
];
919 if (line
->dir
== SPARROW_HORIZONTAL
){
920 p
+= line
->offset
* sparrow
->out
.width
;
921 for (i
= 0; i
< sparrow
->out
.width
; i
++){
926 guint32
*p
= (guint32
*)out
;
928 for(i
= 0; i
< sparrow
->out
.height
; i
++){
930 p
+= sparrow
->out
.width
;
936 jump_state(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
, edges_state_t state
){
937 if (state
== EDGES_NEXT_STATE
){
944 case EDGES_FIND_NOISE
:
945 sparrow
->countdown
= MAX(sparrow
->lag
, 1) + SAFETY_LAG
;
947 case EDGES_FIND_LINES
:
948 sparrow
->countdown
= MAX(sparrow
->lag
, 1) + SAFETY_LAG
;
950 case EDGES_FIND_CORNERS
:
951 sparrow
->countdown
= 7;
953 case EDGES_WAIT_FOR_PLAY
:
954 global_number_of_edge_finders
--;
955 sparrow
->countdown
= 300;
958 GST_DEBUG("jumped to non-existent state %d\n", fl
->state
);
963 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
967 draw_lines(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
, guint8
*in
, guint8
*out
)
969 sparrow_line_t
*line
= fl
->shuffled_lines
[fl
->current
];
970 sparrow
->countdown
--;
971 memset(out
, 0, sparrow
->out
.size
);
972 if (sparrow
->countdown
){
973 draw_line(sparrow
, line
, out
);
976 /*show nothing, look for result */
977 look_for_line(sparrow
, in
, fl
, line
);
979 debug_map_image(sparrow
, fl
);
982 if (fl
->current
== fl
->n_lines
){
983 jump_state(sparrow
, fl
, EDGES_NEXT_STATE
);
986 sparrow
->countdown
= MAX(sparrow
->lag
, 1) + SAFETY_LAG
;
991 #define LINE_THRESHOLD 32
994 find_threshold(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
, guint8
*in
, guint8
*out
)
996 memset(out
, 0, sparrow
->out
.size
);
997 /*XXX should average/median over a range of frames */
998 if (sparrow
->countdown
== 0){
999 memcpy(fl
->threshold
->imageData
, in
, sparrow
->in
.size
);
1000 /*add a constant, and smooth */
1001 cvAddS(fl
->threshold
, cvScalarAll(LINE_THRESHOLD
), fl
->working
, NULL
);
1002 cvSmooth(fl
->working
, fl
->threshold
, CV_GAUSSIAN
, 3, 0, 0, 0);
1003 //cvSmooth(fl->working, fl->threshold, CV_MEDIAN, 3, 0, 0, 0);
1004 jump_state(sparrow
, fl
, EDGES_NEXT_STATE
);
1006 sparrow
->countdown
--;
1009 /*match up lines and find corners */
1011 find_corners(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
)
1013 sparrow
->countdown
--;
1014 switch(sparrow
->countdown
){
1016 make_clusters(sparrow
, fl
);
1019 make_corners(sparrow
, fl
);
1022 complete_map(sparrow
, fl
);
1025 calculate_deltas(sparrow
, fl
);
1029 corners_to_full_lut(sparrow
, fl
);
1031 corners_to_lut(sparrow
, fl
);
1033 jump_state(sparrow
, fl
, EDGES_NEXT_STATE
);
1036 GST_DEBUG("how did sparrow->countdown get to be %d?", sparrow
->countdown
);
1037 sparrow
->countdown
= 5;
1039 return sparrow
->countdown
;
1042 /*use a dirty shared variable*/
1044 wait_for_play(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
1045 if (global_number_of_edge_finders
== 0 ||
1046 sparrow
->countdown
== 0){
1049 sparrow
->countdown
--;
1053 INVISIBLE sparrow_state
1054 mode_find_edges(GstSparrow
*sparrow
, guint8
*in
, guint8
*out
){
1055 sparrow_find_lines_t
*fl
= (sparrow_find_lines_t
*)sparrow
->helper_struct
;
1057 case EDGES_FIND_NOISE
:
1058 find_threshold(sparrow
, fl
, in
, out
);
1060 case EDGES_FIND_LINES
:
1061 draw_lines(sparrow
, fl
, in
, out
);
1063 case EDGES_FIND_CORNERS
:
1064 memset(out
, 0, sparrow
->out
.size
);
1065 find_corners(sparrow
, fl
);
1067 case EDGES_WAIT_FOR_PLAY
:
1068 memset(out
, 0, sparrow
->out
.size
);
1069 if (wait_for_play(sparrow
, fl
)){
1070 return SPARROW_NEXT_STATE
;
1074 GST_WARNING("strange state in mode_find_edges: %d", fl
->state
);
1075 memset(out
, 0, sparrow
->out
.size
);
1077 return SPARROW_STATUS_QUO
;
1081 finalise_find_edges(GstSparrow
*sparrow
){
1082 sparrow_find_lines_t
*fl
= (sparrow_find_lines_t
*)sparrow
->helper_struct
;
1083 //DEBUG_FIND_LINES(fl);
1084 if (sparrow
->save
&& *(sparrow
->save
)){
1085 GST_DEBUG("about to save to %s\n", sparrow
->save
);
1086 dump_edges_info(sparrow
, fl
, sparrow
->save
);
1088 if (sparrow
->debug
){
1089 cvReleaseImage(&fl
->debug
);
1092 free(fl
->shuffled_lines
);
1096 cvReleaseImage(&fl
->threshold
);
1097 cvReleaseImage(&fl
->working
);
1098 cvReleaseImageHeader(&fl
->input
);
1100 GST_DEBUG("freed everything\n");
1101 sparrow
->helper_struct
= NULL
;
1105 setup_colour_shifts(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
1106 /*COLOUR_QUANT reduces the signal a little bit more, avoiding overflow
1108 switch (sparrow
->colour
){
1111 fl
->shift1
= sparrow
->in
.gshift
+ COLOUR_QUANT
;
1112 fl
->shift2
= sparrow
->in
.gshift
+ COLOUR_QUANT
;
1114 case SPARROW_MAGENTA
:
1115 fl
->shift1
= sparrow
->in
.rshift
+ COLOUR_QUANT
;
1116 fl
->shift2
= sparrow
->in
.bshift
+ COLOUR_QUANT
;
1122 init_find_edges(GstSparrow
*sparrow
){
1124 sparrow_find_lines_t
*fl
= zalloc_aligned_or_die(sizeof(sparrow_find_lines_t
));
1125 sparrow
->helper_struct
= (void *)fl
;
1127 gint h_lines
= (sparrow
->out
.height
+ LINE_PERIOD
- 1) / LINE_PERIOD
;
1128 gint v_lines
= (sparrow
->out
.width
+ LINE_PERIOD
- 1) / LINE_PERIOD
;
1129 gint n_lines_max
= (h_lines
+ v_lines
);
1130 gint n_corners
= (h_lines
* v_lines
);
1131 fl
->n_hlines
= h_lines
;
1132 fl
->n_vlines
= v_lines
;
1134 fl
->h_lines
= malloc_aligned_or_die(sizeof(sparrow_line_t
) * n_lines_max
);
1135 fl
->shuffled_lines
= malloc_aligned_or_die(sizeof(sparrow_line_t
*) * n_lines_max
);
1136 GST_DEBUG("shuffled lines, malloced %p\n", fl
->shuffled_lines
);
1138 GST_DEBUG("map is going to be %d * %d \n", sizeof(sparrow_intersect_t
), sparrow
->in
.pixcount
);
1139 fl
->map
= zalloc_aligned_or_die(sizeof(sparrow_intersect_t
) * sparrow
->in
.pixcount
);
1140 fl
->clusters
= zalloc_or_die(n_corners
* sizeof(sparrow_cluster_t
));
1141 fl
->mesh_mem
= zalloc_aligned_or_die(n_corners
* sizeof(sparrow_corner_t
) * 2);
1142 fl
->mesh
= fl
->mesh_mem
;
1143 fl
->mesh_next
= fl
->mesh
+ n_corners
;
1145 sparrow_line_t
*line
= fl
->h_lines
;
1146 sparrow_line_t
**sline
= fl
->shuffled_lines
;
1149 for (i
= 0, offset
= H_LINE_OFFSET
; offset
< sparrow
->out
.height
;
1150 i
++, offset
+= LINE_PERIOD
){
1151 line
->offset
= offset
;
1152 line
->dir
= SPARROW_HORIZONTAL
;
1157 //GST_DEBUG("line %d h has offset %d\n", i, offset);
1160 /*now add the vertical lines */
1162 for (i
= 0, offset
= V_LINE_OFFSET
; offset
< sparrow
->out
.width
;
1163 i
++, offset
+= LINE_PERIOD
){
1164 line
->offset
= offset
;
1165 line
->dir
= SPARROW_VERTICAL
;
1170 //GST_DEBUG("line %d v has offset %d\n", i, offset);
1172 //DEBUG_FIND_LINES(fl);
1173 fl
->n_lines
= line
- fl
->h_lines
;
1174 GST_DEBUG("allocated %d lines, made %d\n", n_lines_max
, fl
->n_lines
);
1177 for (i
= 0; i
< fl
->n_lines
; i
++){
1178 int j
= RANDINT(sparrow
, 0, fl
->n_lines
);
1179 sparrow_line_t
*tmp
= fl
->shuffled_lines
[j
];
1180 fl
->shuffled_lines
[j
] = fl
->shuffled_lines
[i
];
1181 fl
->shuffled_lines
[i
] = tmp
;
1184 setup_colour_shifts(sparrow
, fl
);
1186 /* opencv images for threshold finding */
1187 CvSize size
= {sparrow
->in
.width
, sparrow
->in
.height
};
1188 fl
->working
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);
1189 fl
->threshold
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);
1191 /*input has no data allocated -- it uses latest frame*/
1192 fl
->input
= init_ipl_image(&sparrow
->in
, PIXSIZE
);
1193 //DEBUG_FIND_LINES(fl);
1194 if (sparrow
->debug
){
1195 fl
->debug
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);
1198 if (sparrow
->reload
){
1199 if (access(sparrow
->reload
, R_OK
)){
1200 GST_DEBUG("sparrow->reload is '%s' and it is UNREADABLE\n", sparrow
->reload
);
1203 read_edges_info(sparrow
, fl
, sparrow
->reload
);
1204 memset(fl
->map
, 0, sizeof(sparrow_intersect_t
) * sparrow
->in
.pixcount
);
1205 //memset(fl->clusters, 0, n_corners * sizeof(sparrow_cluster_t));
1206 memset(fl
->mesh
, 0, n_corners
* sizeof(sparrow_corner_t
));
1207 jump_state(sparrow
, fl
, EDGES_FIND_CORNERS
);
1210 jump_state(sparrow
, fl
, EDGES_FIND_NOISE
);
1213 global_number_of_edge_finders
++;