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;
91 return (int)(x
+ 0.5);
95 coord_to_int_clamp_dither(sparrow_find_lines_t
*fl
, coord_t x
,
96 const int max_plus_one
, const int i
){
100 if (x
>= max_plus_one
)
101 return max_plus_one
- 1;
107 coord_in_range(coord_t x
, const int max_plus_one
){
108 return x
>= 0 && (x
+ 0.5 < max_plus_one
);
113 #define COORD_TO_INT(x)((x) / (1 << SPARROW_FIXED_POINT))
114 #define COORD_TO_FLOAT(x)(((double)(x)) / (1 << SPARROW_FIXED_POINT))
115 #define INT_TO_COORD(x)((x) * (1 << SPARROW_FIXED_POINT))
118 coord_to_int_clamp(coord_t x
, const int max_plus_one
){
121 x
>>= SPARROW_FIXED_POINT
;
122 if (x
>= max_plus_one
)
123 return max_plus_one
- 1;
128 coord_in_range(coord_t x
, const int max_plus_one
){
129 return x
>= 0 && (x
< max_plus_one
<< SPARROW_FIXED_POINT
);
134 //these ones are common
136 coords_to_index(coord_t x
, coord_t y
, int w
, int h
){
137 int iy
= coord_to_int_clamp(y
, h
);
138 int ix
= coord_to_int_clamp(x
, w
);
142 #define C2I COORD_TO_INT
144 /********************************************/
147 corners_to_full_lut(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
148 DEBUG_FIND_LINES(fl
);
149 sparrow_corner_t
*mesh
= fl
->mesh
; /*maps regular points in ->out to points in ->in */
150 sparrow_map_lut_t
*map_lut
= sparrow
->map_lut
;
151 int mesh_w
= fl
->n_vlines
;
152 int mesh_h
= fl
->n_hlines
;
153 int mcy
, mmy
, mcx
, mmx
; /*Mesh Corner|Modulus X|Y*/
154 int y
= H_LINE_OFFSET
;
155 sparrow_corner_t
*mesh_row
= mesh
;
157 for(mcy
= 0; mcy
< mesh_h
- 1; mcy
++){
158 for (mmy
= 0; mmy
< LINE_PERIOD
; mmy
++, y
++){
159 sparrow_corner_t
*mesh_square
= mesh_row
;
160 int i
= y
* sparrow
->out
.width
+ V_LINE_OFFSET
;
161 for(mcx
= 0; mcx
< mesh_w
- 1; mcx
++){
162 coord_t iy
= mesh_square
->y
+ mmy
* mesh_square
->dyd
;
163 coord_t ix
= mesh_square
->x
+ mmy
* mesh_square
->dxd
;
164 for (mmx
= 0; mmx
< LINE_PERIOD
; mmx
++, i
++){
165 int ixx
= coord_to_int_clamp_dither(fl
, ix
, sparrow
->in
.width
, i
);
166 int iyy
= coord_to_int_clamp_dither(fl
, iy
, sparrow
->in
.height
, i
);
167 if(sparrow
->screenmask
[iyy
* sparrow
->in
.width
+ ixx
]){
171 ix
+= mesh_square
->dxr
;
172 iy
+= mesh_square
->dyr
;
179 sparrow
->map_lut
= map_lut
;
180 debug_map_lut(sparrow
, fl
);
184 debug_corners_image(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
185 sparrow_corner_t
*mesh
= fl
->mesh
;
186 guint32
*data
= (guint32
*)fl
->debug
->imageData
;
187 guint w
= fl
->debug
->width
;
188 guint h
= fl
->debug
->height
;
189 memset(data
, 0, sparrow
->in
.size
);
190 guint32 colours
[4] = {0xff0000ff, 0x00ff0000, 0x0000ff00, 0xffffffff};
191 for (int i
= 0; i
< fl
->n_vlines
* fl
->n_hlines
; i
++){
192 sparrow_corner_t
*c
= &mesh
[i
];
199 for (int j
= 1; j
< LINE_PERIOD
; j
+= 2){
204 guint hl
= coords_to_index(txr
, tyr
, w
, h
);
205 data
[hl
] = 0x88000088;
206 guint vl
= coords_to_index(txd
, tyd
, w
, h
);
207 data
[vl
] = 0x00663300;
209 data
[coords_to_index(x
, y
, w
, h
)] = colours
[c
->status
];
211 MAYBE_DEBUG_IPL(fl
->debug
);
216 debug_clusters(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
217 guint32
*data
= (guint32
*)fl
->debug
->imageData
;
218 memset(data
, 0, sparrow
->in
.size
);
219 int width
= fl
->n_vlines
;
220 int height
= fl
->n_hlines
;
221 sparrow_cluster_t
*clusters
= fl
->clusters
;
224 guint32 colours
[4] = {0xff0000ff, 0x0000ff00, 0x00ff0000,
226 for (i
= 0; i
< width
* height
; i
++){
227 colour
= colours
[i
% 5];
228 sparrow_voter_t
*v
= clusters
[i
].voters
;
229 for (j
= 0; j
< clusters
[i
].n
; j
++){
230 data
[coords_to_index(v
[j
].x
, v
[j
].y
,
231 sparrow
->in
.width
, sparrow
->in
.height
)] = (colour
* (v
[j
].signal
/ 2)) / 256;
234 MAYBE_DEBUG_IPL(fl
->debug
);
238 #define SIGNAL_QUANT 1
240 /*maximum number of pixels in a cluster */
241 #define CLUSTER_SIZE 8
244 /*find map points with common intersection data, and collect them into clusters */
246 make_clusters(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
247 sparrow_cluster_t
*clusters
= fl
->clusters
;
249 /*special case: spurious values collect up at 0,0 */
250 fl
->map
[0].signal
[SPARROW_VERTICAL
] = 0;
251 fl
->map
[0].signal
[SPARROW_HORIZONTAL
] = 0;
252 /*each point in fl->map is in a vertical line, a horizontal line, both, or
253 neither. Only the "both" case matters. */
254 for (y
= 0; y
< sparrow
->in
.height
; y
++){
255 for (x
= 0; x
< sparrow
->in
.width
; x
++){
256 sparrow_intersect_t
*p
= &fl
->map
[y
* sparrow
->in
.width
+ x
];
257 guint vsig
= p
->signal
[SPARROW_VERTICAL
];
258 guint hsig
= p
->signal
[SPARROW_HORIZONTAL
];
259 /*remembering that 0 is valid as a line number, but not as a signal */
260 if (! (vsig
&& hsig
)){
263 /*This one is lobbying for the position of a corner.*/
264 int vline
= p
->lines
[SPARROW_VERTICAL
];
265 int hline
= p
->lines
[SPARROW_HORIZONTAL
];
266 if (vline
== BAD_PIXEL
|| hline
== BAD_PIXEL
){
267 GST_DEBUG("ignoring bad pixel %d, %d\n", x
, y
);
270 sparrow_cluster_t
*cluster
= &clusters
[hline
* fl
->n_vlines
+ vline
];
271 sparrow_voter_t
*voters
= cluster
->voters
;
273 guint signal
= (vsig
* hsig
) / SIGNAL_QUANT
;
274 GST_DEBUG("signal at %p (%d, %d): %dv %dh, product %u, lines: %dv %dh\n"
275 "cluster is %p, n is %d\n", p
, x
, y
,
276 vsig
, hsig
, signal
, vline
, hline
, cluster
, n
);
278 GST_WARNING("signal at %p (%d, %d) is %d following quantisation!\n",
282 if (n
< CLUSTER_SIZE
){
283 voters
[n
].x
= INT_TO_COORD(x
);
284 voters
[n
].y
= INT_TO_COORD(y
);
285 voters
[n
].signal
= signal
;
289 /*duplicate x, y, signal, so they aren't mucked up */
293 /*replaced one ends up here */
297 for (int j
= 0; j
< CLUSTER_SIZE
; j
++){
298 if (voters
[j
].signal
< ts
){
299 ts2
= voters
[j
].signal
;
302 voters
[j
].signal
= ts
;
310 GST_DEBUG("more than %d pixels at cluster for corner %d, %d."
311 "Dropped %u for %u\n",
312 CLUSTER_SIZE
, vline
, hline
, ts2
, signal
);
317 debug_clusters(sparrow
, fl
);
323 drop_cluster_voter(sparrow_voter_t
*voters
, int n
, int k
)
328 for (i
= k
; i
< n
; i
++){
329 voters
[i
] = voters
[i
+ 1];
335 static inline int sort_median(coord_t
*a
, guint n
)
338 /*stupid sort, but n is very small*/
339 for (i
= 0; i
< n
; i
++){
340 for (j
= i
+ 1; j
< n
; j
++){
348 guint middle
= n
/ 2;
349 coord_t answer
= a
[middle
];
352 answer
+= a
[middle
- 1];
358 #define EUCLIDEAN_D2(ax, ay, bx, by)((ax - bx) * (ax - bx) + (ay - by) * (ay - by))
359 #define EUCLIDEAN_THRESHOLD 7
362 euclidean_discard_cluster_outliers(sparrow_voter_t
*voters
, int n
)
364 /* Calculate distance between each pair. Discard points with maximum sum,
365 then recalculate until all are within threshold.
367 GST_DEBUG("cleansing a cluster of size %d using sum of distances", n
);
370 for (i
= 0; i
< n
; i
++){
372 for (j
= i
+ 1; j
< n
; j
++){
373 coord_t d
= EUCLIDEAN_D2(voters
[i
].x
, voters
[i
].y
,
374 voters
[j
].x
, voters
[j
].y
);
381 coord_t worst_d
, threshold
;
383 threshold
= EUCLIDEAN_THRESHOLD
* n
;
386 for (i
= 0; i
< n
; i
++){
387 if (dsums
[i
] > worst_d
){
392 if (worst_d
> threshold
){
393 GST_DEBUG("failing point %d, distance sq %d, threshold %d\n",
394 worst_i
, C2I(worst_d
), C2I(threshold
));
395 //subtract this one from the sums, or they'll all go
396 for (i
= 0; i
< n
; i
++){
397 dsums
[i
] -= EUCLIDEAN_D2(voters
[i
].x
, voters
[i
].y
,
398 voters
[worst_i
].x
, voters
[worst_i
].y
);
400 n
= drop_cluster_voter(voters
, n
, worst_i
);
403 GST_DEBUG("worst %d, was only %d, threshold %d\n",
404 worst_i
, C2I(worst_d
), C2I(threshold
));
412 median_discard_cluster_outliers(sparrow_voter_t
*voters
, int n
)
417 for (i
= 0; i
< n
; i
++){
418 /*XXX could sort here*/
419 xvals
[i
] = voters
[i
].x
;
420 yvals
[i
] = voters
[i
].y
;
422 const coord_t xmed
= sort_median(xvals
, n
);
423 const coord_t ymed
= sort_median(yvals
, n
);
425 for (i
= 0; i
< n
; i
++){
426 coord_t dx
= voters
[i
].x
- xmed
;
427 coord_t dy
= voters
[i
].y
- ymed
;
428 if (dx
* dx
+ dy
* dy
> OUTLIER_THRESHOLD
){
429 n
= drop_cluster_voter(voters
, n
, i
);
437 make_corners(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
438 //DEBUG_FIND_LINES(fl);
439 int width
= fl
->n_vlines
;
440 int height
= fl
->n_hlines
;
441 sparrow_cluster_t
*clusters
= fl
->clusters
;
442 sparrow_corner_t
*mesh
= fl
->mesh
;
446 for (y
= 0; y
< height
; y
++){
447 for (x
= 0; x
< width
; x
++, i
++){
448 sparrow_cluster_t
*cluster
= clusters
+ i
;
449 if (cluster
->n
== 0){
453 /*discard outliers based on sum of squared distances: good points should
454 be in a cluster, and have lowest sum*/
455 cluster
->n
= euclidean_discard_cluster_outliers(cluster
->voters
, cluster
->n
);
457 /*discard values away from median x, y values.
458 (each dimension is calculated independently)*/
459 cluster
->n
= median_discard_cluster_outliers(cluster
->voters
, cluster
->n
);
461 /* now find a weighted average position */
462 /*With int coord_t, coord_sum_t is
463 64 bit to avoid overflow -- should probably just use floating point
465 coord_sum_t xsum
, ysum
;
466 coord_t xmean
, ymean
;
472 for (j
= 0; j
< cluster
->n
; j
++){
473 votes
+= cluster
->voters
[j
].signal
;
474 ysum
+= cluster
->voters
[j
].y
* cluster
->voters
[j
].signal
;
475 xsum
+= cluster
->voters
[j
].x
* cluster
->voters
[j
].signal
;
478 xmean
= xsum
/ votes
;
479 ymean
= ysum
/ votes
;
482 GST_WARNING("corner %d, %d voters, sum %d,%d, somehow has no votes\n",
483 i
, cluster
->n
, xsum
, ysum
);
486 GST_DEBUG("corner %d: %d voters, %d votes, sum %d,%d, mean %d,%d\n",
487 i
, cluster
->n
, votes
, C2I(xsum
), C2I(ysum
), C2I(xmean
), C2I(ymean
));
491 mesh
[i
].status
= CORNER_EXACT
;
492 GST_DEBUG("found corner %d at (%3f, %3f)\n",
493 i
, COORD_TO_FLOAT(xmean
), COORD_TO_FLOAT(ymean
));
498 static sparrow_point_t
499 median_centre(sparrow_voter_t
*estimates
, int n
){
500 /*X and Y arevcalculated independently, which is really not right.
501 on the other hand, it probably works. */
503 sparrow_point_t result
;
505 for (i
= 0; i
< n
; i
++){
506 vals
[i
] = estimates
[i
].x
;
508 result
.x
= coord_median(vals
, n
);
510 for (i
= 0; i
< n
; i
++){
511 vals
[i
] = estimates
[i
].y
;
513 result
.y
= coord_median(vals
, n
);
517 static const sparrow_estimator_t base_estimators
[] = {
532 #define BASE_ESTIMATORS (sizeof(base_estimators) / sizeof(sparrow_estimator_t))
533 #define ESTIMATORS (BASE_ESTIMATORS * 4)
536 calculate_estimator_tables(sparrow_estimator_t
*estimators
){
538 sparrow_estimator_t
*e
= estimators
;
539 for (i
= 0; i
< BASE_ESTIMATORS
; i
++){
540 for (j
= 0; j
< 4; j
++){
541 *e
= base_estimators
[i
];
558 GST_DEBUG("estimator: %-d,%-d %-d,%-d %-d,%-d",
559 e
->x1
, e
->y1
, e
->x2
, e
->y2
, e
->x3
, e
->y3
);
565 /* nice big word. acos(1.0 - MAX_NONCOLLINEARITY) = angle of deviation.
566 0.005: 5.7 degrees, 0.01: 8.1, 0.02: 11.5, 0.04: 16.3, 0.08: 23.1
567 1 pixel deviation in 32 -> ~ 1/33 == 0.03 (if I understand correctly)
569 #define MAX_NONCOLLINEARITY 0.02
571 /*the map made above is likely to be full of errors. Fix them, and add in
574 complete_map(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
575 sparrow_voter_t estimates
[ESTIMATORS
+ 1];
576 sparrow_estimator_t estimators
[ESTIMATORS
];
577 calculate_estimator_tables(estimators
);
579 guint32
*debug
= NULL
;
581 debug
= (guint32
*)fl
->debug
->imageData
;
582 memset(debug
, 0, sparrow
->in
.size
);
586 int width
= fl
->n_vlines
;
587 int height
= fl
->n_hlines
;
588 int screen_width
= sparrow
->in
.width
;
589 int screen_height
= sparrow
->in
.height
;
590 sparrow_corner_t
*mesh
= fl
->mesh
;
591 sparrow_corner_t
*mesh_next
= fl
->mesh_next
;
593 memset(estimates
, 0, sizeof(estimates
)); /*just for clarity in debugging */
594 int prev_settled
= 0;
596 memcpy(mesh_next
, mesh
, width
* height
* sizeof(sparrow_corner_t
));
598 for (y
= 0; y
< height
; y
++){
599 for (x
= 0; x
< width
; x
++){
600 sparrow_corner_t
*corner
= &mesh
[y
* width
+ x
];
601 if (corner
->status
== CORNER_SETTLED
){
603 GST_DEBUG("ignoring settled corner %d, %d", x
, y
);
607 for (guint j
= 0; j
< ESTIMATORS
; j
++){
608 sparrow_estimator_t
*e
= &estimators
[j
];
609 int x3
, y3
, x2
, y2
, x1
, y1
;
612 if (!(y3
>= 0 && y3
< height
&&
613 x3
>= 0 && x3
< width
&&
614 mesh
[y3
* width
+ x3
].status
!= CORNER_UNUSED
616 GST_DEBUG("not using estimator %d because corners aren't used, or are off screen\n"
617 "x3 %d, y3 %d", j
, x3
, y3
);
624 if (mesh
[y2
* width
+ x2
].status
== CORNER_UNUSED
||
625 mesh
[y1
* width
+ x1
].status
== CORNER_UNUSED
){
626 GST_DEBUG("not using estimator %d because corners aren't used", j
);
629 /*there are 3 points, and the unknown one.
630 They should all be in a line.
631 The ratio of the p3-p2:p2-p1 sould be the same as
634 This really has to be done in floating point.
636 collinearity, no division, but no useful error metric
637 x[0] * (y[1]-y[2]) + x[1] * (y[2]-y[0]) + x[2] * (y[0]-y[1]) == 0
638 (at least not without further division)
642 cos angle = dot product / product of euclidean lengths
644 (dx12 * dx23 + dy12 * dy23) /
645 (sqrt(dx12 * dx12 + dy12 * dy12) * sqrt(dx23 * dx23 + dy23 * dy23))
647 is costly up front (sqrt), but those distances need to be
648 calculated anyway (or at least they are handy). Not much gained by
649 short-circuiting on bad collinearity, though.
651 It also handlily catches all the division by zeros in one meaningful
654 sparrow_corner_t
*c1
= &mesh
[y1
* width
+ x1
];
655 sparrow_corner_t
*c2
= &mesh
[y2
* width
+ x2
];
656 sparrow_corner_t
*c3
= &mesh
[y3
* width
+ x3
];
658 double dx12
= c1
->x
- c2
->x
;
659 double dy12
= c1
->y
- c2
->y
;
660 double dx23
= c2
->x
- c3
->x
;
661 double dy23
= c2
->y
- c3
->y
;
662 double distance12
= sqrt(dx12
* dx12
+ dy12
* dy12
);
663 double distance23
= sqrt(dx23
* dx23
+ dy23
* dy23
);
665 double dp
= dx12
* dx23
+ dy12
* dy23
;
667 double distances
= distance12
* distance23
;
669 GST_DEBUG("mesh points: %d,%d, %d,%d, %d,%d\n"
670 "map points: %d,%d, %d,%d, %d,%d\n"
671 "diffs: 12: %0.3f,%0.3f, 23: %0.3f,%0.3f, \n"
672 "distances: 12: %0.3f, 32: %0.3f\n",
673 x1
, y1
, x2
, y2
, x3
, y3
,
674 C2I(c1
->x
), C2I(c1
->y
), C2I(c2
->x
), C2I(c2
->y
), C2I(c3
->x
), C2I(c3
->y
),
675 dx12
, dy12
, dx23
, dy23
, distance12
, distance23
681 if (distances
== 0.0){
682 GST_INFO("at least two points out of %d,%d, %d,%d, %d,%d are the same!",
683 x1
, y1
, x2
, y2
, x3
, y3
);
686 double line_error
= 1.0 - dp
/ distances
;
687 if (line_error
> MAX_NONCOLLINEARITY
){
688 GST_DEBUG("Points %d,%d, %d,%d, %d,%d are not in a line: non-collinearity: %3f",
689 x1
, y1
, x2
, y2
, x3
, y3
, line_error
);
692 //GST_DEBUG("GOOD collinearity: %3f", line_error);
695 double ratio
= distance12
/ distance23
;
696 /*so here's the estimate!*/
697 coord_t dx
= dx12
* ratio
;
698 coord_t dy
= dy12
* ratio
;
699 coord_t ex
= c1
->x
+ dx
;
700 coord_t ey
= c1
->y
+ dy
;
703 GST_DEBUG("dx, dy: %d,%d, ex, ey: %d,%d\n"
704 "dx raw: %0.3f,%0.3f, x1, x2: %0.3f,%0.3f,\n"
705 "distances: 12: %0.3f, 32: %0.3f\n"
707 C2I(dx
), C2I(dy
), C2I(ex
), C2I(ey
),
708 dx
, dy
, ex
, ey
, ratio
712 if (! coord_in_range(ey
, screen_height
) ||
713 ! coord_in_range(ex
, screen_width
)){
714 GST_DEBUG("rejecting estimate for %d, %d, due to ex, ey being %d, %d",
715 x
, y
, C2I(ex
), C2I(ey
));
719 GST_DEBUG("estimator %d,%d SUCCESSFULLY estimated that %d, %d will be %d, %d",
720 x1, x2, x, y, C2I(ex), C2I(ey));
725 debug
[coords_to_index(ex
, ey
, sparrow
->in
.width
, sparrow
->in
.height
)] = 0x00aa7700;
729 /*now there is an array of estimates.
730 The *_discard_cluster_outliers functions should fit here */
731 GST_INFO("got %d estimates for %d,%d", k
, x
, y
);
739 /*now find median values. If the number is even, add a copy of either
740 the original value, or a random element. */
742 if (corner
->status
!= CORNER_UNUSED
){
743 estimates
[k
].x
= corner
->x
;
744 estimates
[k
].y
= corner
->y
;
747 int r
= RANDINT(sparrow
, 0, r
);
748 estimates
[k
].x
= estimates
[r
].x
;
749 estimates
[k
].y
= estimates
[r
].y
;
753 sparrow_point_t centre
= median_centre(estimates
, k
);
759 k
= euclidean_discard_cluster_outliers(estimates
, k
);
761 for (int j
= 0; j
< k
; j
++){
762 debug
[coords_to_index(estimates
[j
].x
, estimates
[j
].y
,
763 sparrow
->in
.width
, sparrow
->in
.height
)] = 0x00ffff00;
766 GST_INFO("After discard, left with %d estimates", k
);
767 /*now what? the mean? yes.*/
770 for (int j
= 0; j
< k
; j
++){
771 sumx
+= estimates
[j
].x
;
772 sumy
+= estimates
[j
].y
;
779 GST_INFO("estimating %d,%d", C2I(guess_x
), C2I(guess_y
));
781 if (corner
->status
== CORNER_EXACT
){
782 GST_INFO("using exact reading %d,%d", C2I(corner
->x
), C2I(corner
->y
));
784 debug
[coords_to_index(corner
->x
, corner
->y
,
785 sparrow
->in
.width
, sparrow
->in
.height
)] = 0xffff3300;
787 if (abs(corner
->x
- guess_x
) < 3){
790 if (abs(corner
->y
- guess_y
) < 3){
795 GST_DEBUG("weak evidence, mark corner PROJECTED");
796 corner
->status
= CORNER_PROJECTED
;
798 debug
[coords_to_index(guess_x
, guess_y
,
799 sparrow
->in
.width
, sparrow
->in
.height
)] = 0xff0000ff;
803 GST_DEBUG("corner is SETTLED");
804 corner
->status
= CORNER_SETTLED
;
807 debug
[coords_to_index(guess_x
, guess_y
,
808 sparrow
->in
.width
, sparrow
->in
.height
)] = 0xffffffff;
815 GST_INFO("settled %d in that round. %d left to go",
816 settled
- prev_settled
, width
* height
- settled
);
817 if (settled
== width
* height
|| settled
== prev_settled
){
820 prev_settled
= settled
;
821 sparrow_corner_t
*tmp
= mesh_next
;
826 fl
->mesh_next
= mesh_next
;
827 MAYBE_DEBUG_IPL(fl
->debug
);
832 calculate_deltas(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
834 int width
= fl
->n_vlines
;
835 int height
= fl
->n_hlines
;
836 sparrow_corner_t
*mesh
= fl
->mesh
;
839 //DEBUG_FIND_LINES(fl);
840 /* calculate deltas toward adjacent corners */
841 /* try to extrapolate left and up, if possible, so need to go backwards. */
842 i
= width
* height
- 1;
843 for (y
= height
- 1; y
>= 0; y
--){
844 for (x
= width
- 1; x
>= 0; x
--, i
--){
845 sparrow_corner_t
*corner
= &mesh
[i
];
846 /* calculate the delta to next corner. If this corner is on edge, delta is
847 0 and next is this.*/
848 sparrow_corner_t
*right
= (x
== width
- 1) ? corner
: corner
+ 1;
849 sparrow_corner_t
*down
= (y
== height
- 1) ? corner
: corner
+ width
;
850 GST_DEBUG("i %d xy %d,%d width %d. in_xy %d,%d; down in_xy %d,%d; right in_xy %d,%d\n",
851 i
, x
, y
, width
, C2I(corner
->x
), C2I(corner
->y
), C2I(down
->x
),
852 C2I(down
->y
), C2I(right
->x
), C2I(right
->y
));
853 if (corner
->status
!= CORNER_UNUSED
){
854 if (right
->status
!= CORNER_UNUSED
){
855 corner
->dxr
= QUANTISE_DELTA(right
->x
- corner
->x
);
856 corner
->dyr
= QUANTISE_DELTA(right
->y
- corner
->y
);
858 if (down
->status
!= CORNER_UNUSED
){
859 corner
->dxd
= QUANTISE_DELTA(down
->x
- corner
->x
);
860 corner
->dyd
= QUANTISE_DELTA(down
->y
- corner
->y
);
866 debug_corners_image(sparrow
, fl
);
872 look_for_line(GstSparrow
*sparrow
, guint8
*in
, sparrow_find_lines_t
*fl
,
873 sparrow_line_t
*line
){
876 guint32 cmask
= sparrow
->out
.colours
[sparrow
->colour
];
879 /* subtract background noise */
880 fl
->input
->imageData
= (char *)in
;
881 cvSub(fl
->input
, fl
->threshold
, fl
->working
, NULL
);
882 guint32
*in32
= (guint32
*)fl
->working
->imageData
;
884 for (i
= 0; i
< sparrow
->in
.pixcount
; i
++){
885 colour
= in32
[i
] & cmask
;
886 signal
= (((colour
>> fl
->shift1
) & COLOUR_MASK
) +
887 ((colour
>> fl
->shift2
) & COLOUR_MASK
));
889 if (fl
->map
[i
].lines
[line
->dir
]){
890 /*assume the pixel is on for everyone and will just confuse
894 if (fl
->map
[i
].lines
[line
->dir
] != BAD_PIXEL
){
896 GST_DEBUG("HEY, expected point %d to be in line %d (dir %d) "
897 "and thus empty, but it is also in line %d\n"
898 "old signal %d, new signal %d, marking as BAD\n",
899 i, line->index, line->dir, fl->map[i].lines[line->dir],
900 fl->map[i].signal[line->dir], signal);
902 fl
->map
[i
].lines
[line
->dir
] = BAD_PIXEL
;
903 fl
->map
[i
].signal
[line
->dir
] = 0;
907 fl
->map
[i
].lines
[line
->dir
] = line
->index
;
908 fl
->map
[i
].signal
[line
->dir
] = signal
;
915 debug_map_image(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
916 guint32
*data
= (guint32
*)fl
->debug
->imageData
;
917 memset(data
, 0, sparrow
->in
.size
);
918 for (guint i
= 0; i
< sparrow
->in
.pixcount
; i
++){
919 data
[i
] |= fl
->map
[i
].signal
[SPARROW_HORIZONTAL
] << sparrow
->in
.gshift
;
920 data
[i
] |= fl
->map
[i
].signal
[SPARROW_VERTICAL
] << sparrow
->in
.rshift
;
921 data
[i
] |= ((fl
->map
[i
].lines
[SPARROW_VERTICAL
] == BAD_PIXEL
) ||
922 (fl
->map
[i
].lines
[SPARROW_HORIZONTAL
] == BAD_PIXEL
)) ? 255 << sparrow
->in
.bshift
: 0;
924 MAYBE_DEBUG_IPL(fl
->debug
);
927 /* draw the line (in sparrow->colour) */
929 draw_line(GstSparrow
* sparrow
, sparrow_line_t
*line
, guint8
*out
){
930 guint32
*p
= (guint32
*)out
;
931 guint32 colour
= sparrow
->out
.colours
[sparrow
->colour
];
933 if (line
->dir
== SPARROW_HORIZONTAL
){
934 p
+= line
->offset
* sparrow
->out
.width
;
935 for (i
= 0; i
< sparrow
->out
.width
; i
++){
940 guint32
*p
= (guint32
*)out
;
942 for(i
= 0; i
< sparrow
->out
.height
; i
++){
944 p
+= sparrow
->out
.width
;
950 jump_state(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
, edges_state_t state
){
951 if (state
== EDGES_NEXT_STATE
){
958 case EDGES_FIND_NOISE
:
959 sparrow
->countdown
= MAX(sparrow
->lag
, 1) + SAFETY_LAG
;
961 case EDGES_FIND_LINES
:
962 sparrow
->countdown
= MAX(sparrow
->lag
, 1) + SAFETY_LAG
;
964 case EDGES_FIND_CORNERS
:
965 sparrow
->countdown
= 7;
967 case EDGES_WAIT_FOR_PLAY
:
968 global_number_of_edge_finders
--;
969 sparrow
->countdown
= 300;
972 GST_DEBUG("jumped to non-existent state %d\n", fl
->state
);
977 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
981 draw_lines(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
, guint8
*in
, guint8
*out
)
983 sparrow_line_t
*line
= fl
->shuffled_lines
[fl
->current
];
984 sparrow
->countdown
--;
985 memset(out
, 0, sparrow
->out
.size
);
986 if (sparrow
->countdown
){
987 draw_line(sparrow
, line
, out
);
990 /*show nothing, look for result */
991 look_for_line(sparrow
, in
, fl
, line
);
993 debug_map_image(sparrow
, fl
);
996 if (fl
->current
== fl
->n_lines
){
997 jump_state(sparrow
, fl
, EDGES_NEXT_STATE
);
1000 sparrow
->countdown
= MAX(sparrow
->lag
, 1) + SAFETY_LAG
;
1005 #define LINE_THRESHOLD 32
1008 find_threshold(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
, guint8
*in
, guint8
*out
)
1010 memset(out
, 0, sparrow
->out
.size
);
1011 /*XXX should average/median over a range of frames */
1012 if (sparrow
->countdown
== 0){
1013 memcpy(fl
->threshold
->imageData
, in
, sparrow
->in
.size
);
1014 /*add a constant, and smooth */
1015 cvAddS(fl
->threshold
, cvScalarAll(LINE_THRESHOLD
), fl
->working
, NULL
);
1016 cvSmooth(fl
->working
, fl
->threshold
, CV_GAUSSIAN
, 3, 0, 0, 0);
1017 //cvSmooth(fl->working, fl->threshold, CV_MEDIAN, 3, 0, 0, 0);
1018 jump_state(sparrow
, fl
, EDGES_NEXT_STATE
);
1020 sparrow
->countdown
--;
1023 /*match up lines and find corners */
1025 find_corners(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
)
1027 sparrow
->countdown
--;
1028 switch(sparrow
->countdown
){
1030 make_clusters(sparrow
, fl
);
1033 make_corners(sparrow
, fl
);
1036 complete_map(sparrow
, fl
);
1039 calculate_deltas(sparrow
, fl
);
1043 corners_to_full_lut(sparrow
, fl
);
1045 corners_to_lut(sparrow
, fl
);
1047 jump_state(sparrow
, fl
, EDGES_NEXT_STATE
);
1050 GST_DEBUG("how did sparrow->countdown get to be %d?", sparrow
->countdown
);
1051 sparrow
->countdown
= 5;
1053 return sparrow
->countdown
;
1056 /*use a dirty shared variable*/
1058 wait_for_play(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
1059 if (global_number_of_edge_finders
== 0 ||
1060 sparrow
->countdown
== 0){
1063 sparrow
->countdown
--;
1067 INVISIBLE sparrow_state
1068 mode_find_edges(GstSparrow
*sparrow
, guint8
*in
, guint8
*out
){
1069 sparrow_find_lines_t
*fl
= (sparrow_find_lines_t
*)sparrow
->helper_struct
;
1071 case EDGES_FIND_NOISE
:
1072 find_threshold(sparrow
, fl
, in
, out
);
1074 case EDGES_FIND_LINES
:
1075 draw_lines(sparrow
, fl
, in
, out
);
1077 case EDGES_FIND_CORNERS
:
1078 memset(out
, 0, sparrow
->out
.size
);
1079 find_corners(sparrow
, fl
);
1081 case EDGES_WAIT_FOR_PLAY
:
1082 memset(out
, 0, sparrow
->out
.size
);
1083 if (wait_for_play(sparrow
, fl
)){
1084 return SPARROW_NEXT_STATE
;
1088 GST_WARNING("strange state in mode_find_edges: %d", fl
->state
);
1089 memset(out
, 0, sparrow
->out
.size
);
1091 return SPARROW_STATUS_QUO
;
1095 finalise_find_edges(GstSparrow
*sparrow
){
1096 sparrow_find_lines_t
*fl
= (sparrow_find_lines_t
*)sparrow
->helper_struct
;
1097 //DEBUG_FIND_LINES(fl);
1098 if (sparrow
->save
&& *(sparrow
->save
)){
1099 GST_DEBUG("about to save to %s\n", sparrow
->save
);
1100 dump_edges_info(sparrow
, fl
, sparrow
->save
);
1102 if (sparrow
->debug
){
1103 cvReleaseImage(&fl
->debug
);
1106 free(fl
->shuffled_lines
);
1111 cvReleaseImage(&fl
->threshold
);
1112 cvReleaseImage(&fl
->working
);
1113 cvReleaseImageHeader(&fl
->input
);
1115 GST_DEBUG("freed everything\n");
1116 sparrow
->helper_struct
= NULL
;
1120 setup_colour_shifts(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
1121 /*COLOUR_QUANT reduces the signal a little bit more, avoiding overflow
1123 switch (sparrow
->colour
){
1126 fl
->shift1
= sparrow
->in
.gshift
+ COLOUR_QUANT
;
1127 fl
->shift2
= sparrow
->in
.gshift
+ COLOUR_QUANT
;
1129 case SPARROW_MAGENTA
:
1130 fl
->shift1
= sparrow
->in
.rshift
+ COLOUR_QUANT
;
1131 fl
->shift2
= sparrow
->in
.bshift
+ COLOUR_QUANT
;
1137 init_find_edges(GstSparrow
*sparrow
){
1139 sparrow_find_lines_t
*fl
= zalloc_aligned_or_die(sizeof(sparrow_find_lines_t
));
1140 sparrow
->helper_struct
= (void *)fl
;
1142 gint h_lines
= (sparrow
->out
.height
+ LINE_PERIOD
- 1) / LINE_PERIOD
;
1143 gint v_lines
= (sparrow
->out
.width
+ LINE_PERIOD
- 1) / LINE_PERIOD
;
1144 gint n_lines_max
= (h_lines
+ v_lines
);
1145 gint n_corners
= (h_lines
* v_lines
);
1147 /*set up dither here, rather than in the busy time */
1148 fl
->dither
= malloc_aligned_or_die(sparrow
->out
.pixcount
* sizeof(double));
1149 dsfmt_fill_array_close_open(sparrow
->dsfmt
, fl
->dither
, sparrow
->out
.pixcount
);
1151 fl
->n_hlines
= h_lines
;
1152 fl
->n_vlines
= v_lines
;
1154 fl
->h_lines
= malloc_aligned_or_die(sizeof(sparrow_line_t
) * n_lines_max
);
1155 fl
->shuffled_lines
= malloc_aligned_or_die(sizeof(sparrow_line_t
*) * n_lines_max
);
1156 GST_DEBUG("shuffled lines, malloced %p\n", fl
->shuffled_lines
);
1158 GST_DEBUG("map is going to be %d * %d \n", sizeof(sparrow_intersect_t
), sparrow
->in
.pixcount
);
1159 fl
->map
= zalloc_aligned_or_die(sizeof(sparrow_intersect_t
) * sparrow
->in
.pixcount
);
1160 fl
->clusters
= zalloc_or_die(n_corners
* sizeof(sparrow_cluster_t
));
1161 fl
->mesh_mem
= zalloc_aligned_or_die(n_corners
* sizeof(sparrow_corner_t
) * 2);
1162 fl
->mesh
= fl
->mesh_mem
;
1163 fl
->mesh_next
= fl
->mesh
+ n_corners
;
1165 sparrow_line_t
*line
= fl
->h_lines
;
1166 sparrow_line_t
**sline
= fl
->shuffled_lines
;
1169 for (i
= 0, offset
= H_LINE_OFFSET
; offset
< sparrow
->out
.height
;
1170 i
++, offset
+= LINE_PERIOD
){
1171 line
->offset
= offset
;
1172 line
->dir
= SPARROW_HORIZONTAL
;
1177 //GST_DEBUG("line %d h has offset %d\n", i, offset);
1180 /*now add the vertical lines */
1182 for (i
= 0, offset
= V_LINE_OFFSET
; offset
< sparrow
->out
.width
;
1183 i
++, offset
+= LINE_PERIOD
){
1184 line
->offset
= offset
;
1185 line
->dir
= SPARROW_VERTICAL
;
1190 //GST_DEBUG("line %d v has offset %d\n", i, offset);
1192 //DEBUG_FIND_LINES(fl);
1193 fl
->n_lines
= line
- fl
->h_lines
;
1194 GST_DEBUG("allocated %d lines, made %d\n", n_lines_max
, fl
->n_lines
);
1197 for (i
= 0; i
< fl
->n_lines
; i
++){
1198 int j
= RANDINT(sparrow
, 0, fl
->n_lines
);
1199 sparrow_line_t
*tmp
= fl
->shuffled_lines
[j
];
1200 fl
->shuffled_lines
[j
] = fl
->shuffled_lines
[i
];
1201 fl
->shuffled_lines
[i
] = tmp
;
1204 setup_colour_shifts(sparrow
, fl
);
1206 /* opencv images for threshold finding */
1207 CvSize size
= {sparrow
->in
.width
, sparrow
->in
.height
};
1208 fl
->working
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);
1209 fl
->threshold
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);
1211 /*input has no data allocated -- it uses latest frame*/
1212 fl
->input
= init_ipl_image(&sparrow
->in
, PIXSIZE
);
1213 //DEBUG_FIND_LINES(fl);
1214 if (sparrow
->debug
){
1215 fl
->debug
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);
1218 if (sparrow
->reload
){
1219 if (access(sparrow
->reload
, R_OK
)){
1220 GST_DEBUG("sparrow->reload is '%s' and it is UNREADABLE\n", sparrow
->reload
);
1223 read_edges_info(sparrow
, fl
, sparrow
->reload
);
1224 memset(fl
->map
, 0, sizeof(sparrow_intersect_t
) * sparrow
->in
.pixcount
);
1225 //memset(fl->clusters, 0, n_corners * sizeof(sparrow_cluster_t));
1226 memset(fl
->mesh
, 0, n_corners
* sizeof(sparrow_corner_t
));
1227 jump_state(sparrow
, fl
, EDGES_FIND_CORNERS
);
1230 jump_state(sparrow
, fl
, EDGES_FIND_NOISE
);
1233 global_number_of_edge_finders
++;