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
);
595 for (guint j
= 0; j
< ESTIMATORS
; j
++){
596 sparrow_estimator_t
*e
= &estimators
[j
];
597 int x3
, y3
, x2
, y2
, x1
, y1
;
600 if (!(y3
>= 0 && y3
< height
&&
601 x3
>= 0 && x3
< width
&&
602 mesh
[y3
* width
+ x3
].status
!= CORNER_UNUSED
604 GST_DEBUG("not using estimator %d because corners aren't used, or are off screen\n"
605 "x3 %d, y3 %d", j
, x3
, y3
);
612 if (mesh
[y2
* width
+ x2
].status
== CORNER_UNUSED
||
613 mesh
[y1
* width
+ x1
].status
== CORNER_UNUSED
){
614 GST_DEBUG("not using estimator %d because corners aren't used", j
);
617 /*there are 3 points, and the unknown one.
618 They should all be in a line.
619 The ratio of the p3-p2:p2-p1 sould be the same as
622 This really has to be done in floating point.
624 collinearity, no division, but no useful error metric
625 x[0] * (y[1]-y[2]) + x[1] * (y[2]-y[0]) + x[2] * (y[0]-y[1]) == 0
626 (at least not without further division)
630 cos angle = dot product / product of euclidean lengths
632 (dx12 * dx23 + dy12 * dy23) /
633 (sqrt(dx12 * dx12 + dy12 * dy12) * sqrt(dx23 * dx23 + dy23 * dy23))
635 is costly up front (sqrt), but those distances need to be
636 calculated anyway (or at least they are handy). Not much gained by
637 short-circuiting on bad collinearity, though.
639 It also handlily catches all the division by zeros in one meaningful
642 sparrow_corner_t
*c1
= &mesh
[y1
* width
+ x1
];
643 sparrow_corner_t
*c2
= &mesh
[y2
* width
+ x2
];
644 sparrow_corner_t
*c3
= &mesh
[y3
* width
+ x3
];
646 double dx12
= c1
->x
- c2
->x
;
647 double dy12
= c1
->y
- c2
->y
;
648 double dx23
= c2
->x
- c3
->x
;
649 double dy23
= c2
->y
- c3
->y
;
650 double distance12
= sqrt(dx12
* dx12
+ dy12
* dy12
);
651 double distance23
= sqrt(dx23
* dx23
+ dy23
* dy23
);
653 double dp
= dx12
* dx23
+ dy12
* dy23
;
655 double distances
= distance12
* distance23
;
657 GST_DEBUG("mesh points: %d,%d, %d,%d, %d,%d\n"
658 "map points: %d,%d, %d,%d, %d,%d\n"
659 "diffs: 12: %0.3f,%0.3f, 23: %0.3f,%0.3f, \n"
660 "distances: 12: %0.3f, 32: %0.3f\n",
661 x1
, y1
, x2
, y2
, x3
, y3
,
662 C2I(c1
->x
), C2I(c1
->y
), C2I(c2
->x
), C2I(c2
->y
), C2I(c3
->x
), C2I(c3
->y
),
663 dx12
, dy12
, dx23
, dy23
, distance12
, distance23
669 if (distances
== 0.0){
670 GST_INFO("at least two points out of %d,%d, %d,%d, %d,%d are the same!",
671 x1
, y1
, x2
, y2
, x3
, y3
);
674 double line_error
= 1.0 - dp
/ distances
;
675 if (line_error
> MAX_NONCOLLINEARITY
){
676 GST_DEBUG("Points %d,%d, %d,%d, %d,%d are not in a line: non-collinearity: %3f",
677 x1
, y1
, x2
, y2
, x3
, y3
, line_error
);
680 //GST_DEBUG("GOOD collinearity: %3f", line_error);
683 double ratio
= distance12
/ distance23
;
684 /*so here's the estimate!*/
685 coord_t dx
= dx12
* ratio
;
686 coord_t dy
= dy12
* ratio
;
687 coord_t ex
= c1
->x
+ dx
;
688 coord_t ey
= c1
->y
+ dy
;
691 GST_DEBUG("dx, dy: %d,%d, ex, ey: %d,%d\n"
692 "dx raw: %0.3f,%0.3f, x1, x2: %0.3f,%0.3f,\n"
693 "distances: 12: %0.3f, 32: %0.3f\n"
695 C2I(dx
), C2I(dy
), C2I(ex
), C2I(ey
),
696 dx
, dy
, ex
, ey
, ratio
700 if (! coord_in_range(ey
, screen_height
) ||
701 ! coord_in_range(ex
, screen_width
)){
702 GST_DEBUG("rejecting estimate for %d, %d, due to ex, ey being %d, %d",
703 x
, y
, C2I(ex
), C2I(ey
));
707 GST_DEBUG("estimator %d,%d SUCCESSFULLY estimated that %d, %d will be %d, %d",
708 x1, x2, x, y, C2I(ex), C2I(ey));
713 debug
[coords_to_index(ex
, ey
, sparrow
->in
.width
, sparrow
->in
.height
)] = 0x00aa7700;
717 /*now there is an array of estimates.
718 The *_discard_cluster_outliers functions should fit here */
719 GST_INFO("got %d estimates for %d,%d", k
, x
, y
);
727 /*now find median values. If the number is even, add a copy of either
728 the original value, or a random element. */
730 if (corner
->status
!= CORNER_UNUSED
){
731 estimates
[k
].x
= corner
->x
;
732 estimates
[k
].y
= corner
->y
;
735 int r
= RANDINT(sparrow
, 0, r
);
736 estimates
[k
].x
= estimates
[r
].x
;
737 estimates
[k
].y
= estimates
[r
].y
;
741 sparrow_voter_t centre
= median_centre(estimates
, k
);
747 k
= euclidean_discard_cluster_outliers(estimates
, k
);
749 for (int j
= 0; j
< k
; j
++){
750 debug
[coords_to_index(estimates
[j
].x
, estimates
[j
].y
,
751 sparrow
->in
.width
, sparrow
->in
.height
)] = 0x00ffff00;
754 GST_INFO("After discard, left with %d estimates", k
);
755 /*now what? the mean? yes.*/
758 for (int j
= 0; j
< k
; j
++){
759 sumx
+= estimates
[j
].x
;
760 sumy
+= estimates
[j
].y
;
767 GST_INFO("estimating %d,%d", C2I(guess_x
), C2I(guess_y
));
769 if (corner
->status
== CORNER_EXACT
){
770 GST_INFO("using exact reading %d,%d", C2I(corner
->x
), C2I(corner
->y
));
772 debug
[coords_to_index(corner
->x
, corner
->y
,
773 sparrow
->in
.width
, sparrow
->in
.height
)] = 0xffff3300;
775 if (abs(corner
->x
- guess_x
) < 3){
778 if (abs(corner
->y
- guess_y
) < 3){
783 GST_DEBUG("weak evidence, mark corner PROJECTED");
784 corner
->status
= CORNER_PROJECTED
;
786 debug
[coords_to_index(guess_x
, guess_y
,
787 sparrow
->in
.width
, sparrow
->in
.height
)] = 0xff0000ff;
791 GST_DEBUG("corner is SETTLED");
792 corner
->status
= CORNER_SETTLED
;
795 debug
[coords_to_index(guess_x
, guess_y
,
796 sparrow
->in
.width
, sparrow
->in
.height
)] = 0xffffffff;
803 GST_INFO("settled %d in that round. %d left to go",
804 settled
- prev_settled
, width
* height
- settled
);
805 if (settled
== width
* height
|| settled
== prev_settled
){
808 prev_settled
= settled
;
809 sparrow_corner_t
*tmp
= mesh_next
;
814 fl
->mesh_next
= mesh_next
;
815 MAYBE_DEBUG_IPL(fl
->debug
);
820 calculate_deltas(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
822 int width
= fl
->n_vlines
;
823 int height
= fl
->n_hlines
;
824 sparrow_corner_t
*mesh
= fl
->mesh
;
827 //DEBUG_FIND_LINES(fl);
828 /* calculate deltas toward adjacent corners */
829 /* try to extrapolate left and up, if possible, so need to go backwards. */
830 i
= width
* height
- 1;
831 for (y
= height
- 1; y
>= 0; y
--){
832 for (x
= width
- 1; x
>= 0; x
--, i
--){
833 sparrow_corner_t
*corner
= &mesh
[i
];
834 /* calculate the delta to next corner. If this corner is on edge, delta is
835 0 and next is this.*/
836 sparrow_corner_t
*right
= (x
== width
- 1) ? corner
: corner
+ 1;
837 sparrow_corner_t
*down
= (y
== height
- 1) ? corner
: corner
+ width
;
838 GST_DEBUG("i %d xy %d,%d width %d. in_xy %d,%d; down in_xy %d,%d; right in_xy %d,%d\n",
839 i
, x
, y
, width
, C2I(corner
->x
), C2I(corner
->y
), C2I(down
->x
),
840 C2I(down
->y
), C2I(right
->x
), C2I(right
->y
));
841 if (corner
->status
!= CORNER_UNUSED
){
842 corner
->dxr
= QUANTISE_DELTA(right
->x
- corner
->x
);
843 corner
->dyr
= QUANTISE_DELTA(right
->y
- corner
->y
);
844 corner
->dxd
= QUANTISE_DELTA(down
->x
- corner
->x
);
845 corner
->dyd
= QUANTISE_DELTA(down
->y
- corner
->y
);
850 debug_corners_image(sparrow
, fl
);
856 look_for_line(GstSparrow
*sparrow
, guint8
*in
, sparrow_find_lines_t
*fl
,
857 sparrow_line_t
*line
){
860 guint32 cmask
= sparrow
->out
.colours
[sparrow
->colour
];
863 /* subtract background noise */
864 fl
->input
->imageData
= (char *)in
;
865 cvSub(fl
->input
, fl
->threshold
, fl
->working
, NULL
);
866 guint32
*in32
= (guint32
*)fl
->working
->imageData
;
868 for (i
= 0; i
< sparrow
->in
.pixcount
; i
++){
869 colour
= in32
[i
] & cmask
;
870 signal
= (((colour
>> fl
->shift1
) & COLOUR_MASK
) +
871 ((colour
>> fl
->shift2
) & COLOUR_MASK
));
873 if (fl
->map
[i
].lines
[line
->dir
]){
874 /*assume the pixel is on for everyone and will just confuse
878 if (fl
->map
[i
].lines
[line
->dir
] != BAD_PIXEL
){
880 GST_DEBUG("HEY, expected point %d to be in line %d (dir %d) "
881 "and thus empty, but it is also in line %d\n"
882 "old signal %d, new signal %d, marking as BAD\n",
883 i, line->index, line->dir, fl->map[i].lines[line->dir],
884 fl->map[i].signal[line->dir], signal);
886 fl
->map
[i
].lines
[line
->dir
] = BAD_PIXEL
;
887 fl
->map
[i
].signal
[line
->dir
] = 0;
891 fl
->map
[i
].lines
[line
->dir
] = line
->index
;
892 fl
->map
[i
].signal
[line
->dir
] = signal
;
899 debug_map_image(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
900 guint32
*data
= (guint32
*)fl
->debug
->imageData
;
901 memset(data
, 0, sparrow
->in
.size
);
902 for (guint i
= 0; i
< sparrow
->in
.pixcount
; i
++){
903 data
[i
] |= fl
->map
[i
].signal
[SPARROW_HORIZONTAL
] << sparrow
->in
.gshift
;
904 data
[i
] |= fl
->map
[i
].signal
[SPARROW_VERTICAL
] << sparrow
->in
.rshift
;
905 data
[i
] |= ((fl
->map
[i
].lines
[SPARROW_VERTICAL
] == BAD_PIXEL
) ||
906 (fl
->map
[i
].lines
[SPARROW_HORIZONTAL
] == BAD_PIXEL
)) ? 255 << sparrow
->in
.bshift
: 0;
908 MAYBE_DEBUG_IPL(fl
->debug
);
911 /* draw the line (in sparrow->colour) */
913 draw_line(GstSparrow
* sparrow
, sparrow_line_t
*line
, guint8
*out
){
914 guint32
*p
= (guint32
*)out
;
915 guint32 colour
= sparrow
->out
.colours
[sparrow
->colour
];
917 if (line
->dir
== SPARROW_HORIZONTAL
){
918 p
+= line
->offset
* sparrow
->out
.width
;
919 for (i
= 0; i
< sparrow
->out
.width
; i
++){
924 guint32
*p
= (guint32
*)out
;
926 for(i
= 0; i
< sparrow
->out
.height
; i
++){
928 p
+= sparrow
->out
.width
;
934 jump_state(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
, edges_state_t state
){
935 if (state
== EDGES_NEXT_STATE
){
942 case EDGES_FIND_NOISE
:
943 sparrow
->countdown
= MAX(sparrow
->lag
, 1) + SAFETY_LAG
;
945 case EDGES_FIND_LINES
:
946 sparrow
->countdown
= MAX(sparrow
->lag
, 1) + SAFETY_LAG
;
948 case EDGES_FIND_CORNERS
:
949 sparrow
->countdown
= 7;
951 case EDGES_WAIT_FOR_PLAY
:
952 global_number_of_edge_finders
--;
953 sparrow
->countdown
= 300;
956 GST_DEBUG("jumped to non-existent state %d\n", fl
->state
);
961 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
965 draw_lines(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
, guint8
*in
, guint8
*out
)
967 sparrow_line_t
*line
= fl
->shuffled_lines
[fl
->current
];
968 sparrow
->countdown
--;
969 memset(out
, 0, sparrow
->out
.size
);
970 if (sparrow
->countdown
){
971 draw_line(sparrow
, line
, out
);
974 /*show nothing, look for result */
975 look_for_line(sparrow
, in
, fl
, line
);
977 debug_map_image(sparrow
, fl
);
980 if (fl
->current
== fl
->n_lines
){
981 jump_state(sparrow
, fl
, EDGES_NEXT_STATE
);
984 sparrow
->countdown
= MAX(sparrow
->lag
, 1) + SAFETY_LAG
;
989 #define LINE_THRESHOLD 32
992 find_threshold(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
, guint8
*in
, guint8
*out
)
994 memset(out
, 0, sparrow
->out
.size
);
995 /*XXX should average/median over a range of frames */
996 if (sparrow
->countdown
== 0){
997 memcpy(fl
->threshold
->imageData
, in
, sparrow
->in
.size
);
998 /*add a constant, and smooth */
999 cvAddS(fl
->threshold
, cvScalarAll(LINE_THRESHOLD
), fl
->working
, NULL
);
1000 cvSmooth(fl
->working
, fl
->threshold
, CV_GAUSSIAN
, 3, 0, 0, 0);
1001 //cvSmooth(fl->working, fl->threshold, CV_MEDIAN, 3, 0, 0, 0);
1002 jump_state(sparrow
, fl
, EDGES_NEXT_STATE
);
1004 sparrow
->countdown
--;
1007 /*match up lines and find corners */
1009 find_corners(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
)
1011 sparrow
->countdown
--;
1012 switch(sparrow
->countdown
){
1014 make_clusters(sparrow
, fl
);
1017 make_corners(sparrow
, fl
);
1020 complete_map(sparrow
, fl
);
1023 calculate_deltas(sparrow
, fl
);
1027 corners_to_full_lut(sparrow
, fl
);
1029 corners_to_lut(sparrow
, fl
);
1031 jump_state(sparrow
, fl
, EDGES_NEXT_STATE
);
1034 GST_DEBUG("how did sparrow->countdown get to be %d?", sparrow
->countdown
);
1035 sparrow
->countdown
= 5;
1037 return sparrow
->countdown
;
1040 /*use a dirty shared variable*/
1042 wait_for_play(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
1043 if (global_number_of_edge_finders
== 0 ||
1044 sparrow
->countdown
== 0){
1047 sparrow
->countdown
--;
1051 INVISIBLE sparrow_state
1052 mode_find_edges(GstSparrow
*sparrow
, guint8
*in
, guint8
*out
){
1053 sparrow_find_lines_t
*fl
= (sparrow_find_lines_t
*)sparrow
->helper_struct
;
1055 case EDGES_FIND_NOISE
:
1056 find_threshold(sparrow
, fl
, in
, out
);
1058 case EDGES_FIND_LINES
:
1059 draw_lines(sparrow
, fl
, in
, out
);
1061 case EDGES_FIND_CORNERS
:
1062 memset(out
, 0, sparrow
->out
.size
);
1063 find_corners(sparrow
, fl
);
1065 case EDGES_WAIT_FOR_PLAY
:
1066 memset(out
, 0, sparrow
->out
.size
);
1067 if (wait_for_play(sparrow
, fl
)){
1068 return SPARROW_NEXT_STATE
;
1072 GST_WARNING("strange state in mode_find_edges: %d", fl
->state
);
1073 memset(out
, 0, sparrow
->out
.size
);
1075 return SPARROW_STATUS_QUO
;
1079 finalise_find_edges(GstSparrow
*sparrow
){
1080 sparrow_find_lines_t
*fl
= (sparrow_find_lines_t
*)sparrow
->helper_struct
;
1081 //DEBUG_FIND_LINES(fl);
1082 if (sparrow
->save
&& *(sparrow
->save
)){
1083 GST_DEBUG("about to save to %s\n", sparrow
->save
);
1084 dump_edges_info(sparrow
, fl
, sparrow
->save
);
1086 if (sparrow
->debug
){
1087 cvReleaseImage(&fl
->debug
);
1090 free(fl
->shuffled_lines
);
1094 cvReleaseImage(&fl
->threshold
);
1095 cvReleaseImage(&fl
->working
);
1096 cvReleaseImageHeader(&fl
->input
);
1098 GST_DEBUG("freed everything\n");
1099 sparrow
->helper_struct
= NULL
;
1103 setup_colour_shifts(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
1104 /*COLOUR_QUANT reduces the signal a little bit more, avoiding overflow
1106 switch (sparrow
->colour
){
1109 fl
->shift1
= sparrow
->in
.gshift
+ COLOUR_QUANT
;
1110 fl
->shift2
= sparrow
->in
.gshift
+ COLOUR_QUANT
;
1112 case SPARROW_MAGENTA
:
1113 fl
->shift1
= sparrow
->in
.rshift
+ COLOUR_QUANT
;
1114 fl
->shift2
= sparrow
->in
.bshift
+ COLOUR_QUANT
;
1120 init_find_edges(GstSparrow
*sparrow
){
1122 sparrow_find_lines_t
*fl
= zalloc_aligned_or_die(sizeof(sparrow_find_lines_t
));
1123 sparrow
->helper_struct
= (void *)fl
;
1125 gint h_lines
= (sparrow
->out
.height
+ LINE_PERIOD
- 1) / LINE_PERIOD
;
1126 gint v_lines
= (sparrow
->out
.width
+ LINE_PERIOD
- 1) / LINE_PERIOD
;
1127 gint n_lines_max
= (h_lines
+ v_lines
);
1128 gint n_corners
= (h_lines
* v_lines
);
1129 fl
->n_hlines
= h_lines
;
1130 fl
->n_vlines
= v_lines
;
1132 fl
->h_lines
= malloc_aligned_or_die(sizeof(sparrow_line_t
) * n_lines_max
);
1133 fl
->shuffled_lines
= malloc_aligned_or_die(sizeof(sparrow_line_t
*) * n_lines_max
);
1134 GST_DEBUG("shuffled lines, malloced %p\n", fl
->shuffled_lines
);
1136 GST_DEBUG("map is going to be %d * %d \n", sizeof(sparrow_intersect_t
), sparrow
->in
.pixcount
);
1137 fl
->map
= zalloc_aligned_or_die(sizeof(sparrow_intersect_t
) * sparrow
->in
.pixcount
);
1138 fl
->clusters
= zalloc_or_die(n_corners
* sizeof(sparrow_cluster_t
));
1139 fl
->mesh_mem
= zalloc_aligned_or_die(n_corners
* sizeof(sparrow_corner_t
) * 2);
1140 fl
->mesh
= fl
->mesh_mem
;
1141 fl
->mesh_next
= fl
->mesh
+ n_corners
;
1143 sparrow_line_t
*line
= fl
->h_lines
;
1144 sparrow_line_t
**sline
= fl
->shuffled_lines
;
1147 for (i
= 0, offset
= H_LINE_OFFSET
; offset
< sparrow
->out
.height
;
1148 i
++, offset
+= LINE_PERIOD
){
1149 line
->offset
= offset
;
1150 line
->dir
= SPARROW_HORIZONTAL
;
1155 //GST_DEBUG("line %d h has offset %d\n", i, offset);
1158 /*now add the vertical lines */
1160 for (i
= 0, offset
= V_LINE_OFFSET
; offset
< sparrow
->out
.width
;
1161 i
++, offset
+= LINE_PERIOD
){
1162 line
->offset
= offset
;
1163 line
->dir
= SPARROW_VERTICAL
;
1168 //GST_DEBUG("line %d v has offset %d\n", i, offset);
1170 //DEBUG_FIND_LINES(fl);
1171 fl
->n_lines
= line
- fl
->h_lines
;
1172 GST_DEBUG("allocated %d lines, made %d\n", n_lines_max
, fl
->n_lines
);
1175 for (i
= 0; i
< fl
->n_lines
; i
++){
1176 int j
= RANDINT(sparrow
, 0, fl
->n_lines
);
1177 sparrow_line_t
*tmp
= fl
->shuffled_lines
[j
];
1178 fl
->shuffled_lines
[j
] = fl
->shuffled_lines
[i
];
1179 fl
->shuffled_lines
[i
] = tmp
;
1182 setup_colour_shifts(sparrow
, fl
);
1184 /* opencv images for threshold finding */
1185 CvSize size
= {sparrow
->in
.width
, sparrow
->in
.height
};
1186 fl
->working
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);
1187 fl
->threshold
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);
1189 /*input has no data allocated -- it uses latest frame*/
1190 fl
->input
= init_ipl_image(&sparrow
->in
, PIXSIZE
);
1191 //DEBUG_FIND_LINES(fl);
1192 if (sparrow
->debug
){
1193 fl
->debug
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);
1196 if (sparrow
->reload
){
1197 if (access(sparrow
->reload
, R_OK
)){
1198 GST_DEBUG("sparrow->reload is '%s' and it is UNREADABLE\n", sparrow
->reload
);
1201 read_edges_info(sparrow
, fl
, sparrow
->reload
);
1202 memset(fl
->map
, 0, sizeof(sparrow_intersect_t
) * sparrow
->in
.pixcount
);
1203 //memset(fl->clusters, 0, n_corners * sizeof(sparrow_cluster_t));
1204 memset(fl
->mesh
, 0, n_corners
* sizeof(sparrow_corner_t
));
1205 jump_state(sparrow
, fl
, EDGES_FIND_CORNERS
);
1208 jump_state(sparrow
, fl
, EDGES_FIND_NOISE
);
1211 global_number_of_edge_finders
++;