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
143 #define C2F COORD_TO_FLOAT
145 /********************************************/
148 corners_to_full_lut(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
149 DEBUG_FIND_LINES(fl
);
150 sparrow_corner_t
*mesh
= fl
->mesh
; /*maps regular points in ->out to points in ->in */
151 sparrow_map_lut_t
*map_lut
= sparrow
->map_lut
;
152 int mesh_w
= fl
->n_vlines
;
153 int mesh_h
= fl
->n_hlines
;
154 int mcy
, mmy
, mcx
, mmx
; /*Mesh Corner|Modulus X|Y*/
155 int y
= H_LINE_OFFSET
;
156 sparrow_corner_t
*mesh_row
= mesh
;
157 int *prefetch
= sparrow
->in_prefetch
;
159 for(mcy
= 0; mcy
< mesh_h
- 1; mcy
++){
160 for (mmy
= 0; mmy
< LINE_PERIOD
; mmy
++, y
++){
161 int pfi
= 0; /*prefetch index*/
163 sparrow_corner_t
*mesh_square
= mesh_row
;
164 int i
= y
* sparrow
->out
.width
+ V_LINE_OFFSET
;
165 for(mcx
= 0; mcx
< mesh_w
- 1; mcx
++){
166 coord_t iy
= mesh_square
->y
+ mmy
* mesh_square
->dyd
;
167 coord_t ix
= mesh_square
->x
+ mmy
* mesh_square
->dxd
;
168 for (mmx
= 0; mmx
< LINE_PERIOD
; mmx
++, i
++){
169 int ixx
= coord_to_int_clamp_dither(fl
, ix
, sparrow
->in
.width
, i
);
170 int iyy
= coord_to_int_clamp_dither(fl
, iy
, sparrow
->in
.height
, i
);
171 int ipp
= iyy
* sparrow
->in
.width
+ ixx
;
172 if(sparrow
->screenmask
[ipp
]){
175 if (abs(ipp
- old_ipp
) > IMAGINED_CACHE_SIZE
/ PIXSIZE
&&
176 pfi
< CACHE_PREFETCH_PER_ROW
){
177 prefetch
[y
* CACHE_PREFETCH_PER_ROW
+ pfi
] = ipp
;
182 ix
+= mesh_square
->dxr
;
183 iy
+= mesh_square
->dyr
;
190 sparrow
->map_lut
= map_lut
;
191 debug_map_lut(sparrow
, fl
);
195 debug_corners_image(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
196 sparrow_corner_t
*mesh
= fl
->mesh
;
197 guint32
*data
= (guint32
*)fl
->debug
->imageData
;
198 guint w
= fl
->debug
->width
;
199 guint h
= fl
->debug
->height
;
200 memset(data
, 0, sparrow
->in
.size
);
201 guint32 colours
[4] = {0xff0000ff, 0x00ff0000, 0x0000ff00, 0xffffffff};
202 for (int i
= 0; i
< fl
->n_vlines
* fl
->n_hlines
; i
++){
203 sparrow_corner_t
*c
= &mesh
[i
];
210 for (int j
= 1; j
< LINE_PERIOD
; j
+= 2){
215 guint hl
= coords_to_index(txr
, tyr
, w
, h
);
216 data
[hl
] = 0x88000088;
217 guint vl
= coords_to_index(txd
, tyd
, w
, h
);
218 data
[vl
] = 0x00663300;
220 data
[coords_to_index(x
, y
, w
, h
)] = colours
[c
->status
];
222 MAYBE_DEBUG_IPL(fl
->debug
);
227 debug_clusters(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
228 guint32
*data
= (guint32
*)fl
->debug
->imageData
;
229 memset(data
, 0, sparrow
->in
.size
);
230 int width
= fl
->n_vlines
;
231 int height
= fl
->n_hlines
;
232 sparrow_cluster_t
*clusters
= fl
->clusters
;
235 guint32 colours
[4] = {0xff0000ff, 0x0000ff00, 0x00ff0000,
237 for (i
= 0; i
< width
* height
; i
++){
238 colour
= colours
[i
% 5];
239 sparrow_voter_t
*v
= clusters
[i
].voters
;
240 for (j
= 0; j
< clusters
[i
].n
; j
++){
241 data
[coords_to_index(v
[j
].x
, v
[j
].y
,
242 sparrow
->in
.width
, sparrow
->in
.height
)] = (colour
* (v
[j
].signal
/ 2)) / 256;
245 MAYBE_DEBUG_IPL(fl
->debug
);
249 #define SIGNAL_QUANT 1
251 /*maximum number of pixels in a cluster */
252 #define CLUSTER_SIZE 8
255 /*find map points with common intersection data, and collect them into clusters */
257 make_clusters(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
258 sparrow_cluster_t
*clusters
= fl
->clusters
;
260 /*special case: spurious values collect up at 0,0 */
261 fl
->map
[0].signal
[SPARROW_VERTICAL
] = 0;
262 fl
->map
[0].signal
[SPARROW_HORIZONTAL
] = 0;
263 /*each point in fl->map is in a vertical line, a horizontal line, both, or
264 neither. Only the "both" case matters. */
265 for (y
= 0; y
< sparrow
->in
.height
; y
++){
266 for (x
= 0; x
< sparrow
->in
.width
; x
++){
267 sparrow_intersect_t
*p
= &fl
->map
[y
* sparrow
->in
.width
+ x
];
268 guint vsig
= p
->signal
[SPARROW_VERTICAL
];
269 guint hsig
= p
->signal
[SPARROW_HORIZONTAL
];
270 /*remembering that 0 is valid as a line number, but not as a signal */
271 if (! (vsig
&& hsig
)){
274 /*This one is lobbying for the position of a corner.*/
275 int vline
= p
->lines
[SPARROW_VERTICAL
];
276 int hline
= p
->lines
[SPARROW_HORIZONTAL
];
277 if (vline
== BAD_PIXEL
|| hline
== BAD_PIXEL
){
278 GST_DEBUG("ignoring bad pixel %d, %d\n", x
, y
);
281 sparrow_cluster_t
*cluster
= &clusters
[hline
* fl
->n_vlines
+ vline
];
282 sparrow_voter_t
*voters
= cluster
->voters
;
284 guint signal
= (vsig
* hsig
) / SIGNAL_QUANT
;
285 GST_DEBUG("signal at %p (%d, %d): %dv %dh, product %u, lines: %dv %dh\n"
286 "cluster is %p, n is %d\n", p
, x
, y
,
287 vsig
, hsig
, signal
, vline
, hline
, cluster
, n
);
289 GST_WARNING("signal at %p (%d, %d) is %d following quantisation!\n",
293 if (n
< CLUSTER_SIZE
){
294 voters
[n
].x
= INT_TO_COORD(x
);
295 voters
[n
].y
= INT_TO_COORD(y
);
296 voters
[n
].signal
= signal
;
300 /*duplicate x, y, signal, so they aren't mucked up */
304 /*replaced one ends up here */
308 for (int j
= 0; j
< CLUSTER_SIZE
; j
++){
309 if (voters
[j
].signal
< ts
){
310 ts2
= voters
[j
].signal
;
313 voters
[j
].signal
= ts
;
321 GST_DEBUG("more than %d pixels at cluster for corner %d, %d."
322 "Dropped %u for %u\n",
323 CLUSTER_SIZE
, vline
, hline
, ts2
, signal
);
328 debug_clusters(sparrow
, fl
);
334 drop_cluster_voter(sparrow_voter_t
*voters
, int n
, int k
)
339 for (i
= k
; i
< n
; i
++){
340 voters
[i
] = voters
[i
+ 1];
346 static inline int sort_median(coord_t
*a
, guint n
)
349 /*stupid sort, but n is very small*/
350 for (i
= 0; i
< n
; i
++){
351 for (j
= i
+ 1; j
< n
; j
++){
359 guint middle
= n
/ 2;
360 coord_t answer
= a
[middle
];
363 answer
+= a
[middle
- 1];
369 #define EUCLIDEAN_D2(ax, ay, bx, by)((ax - bx) * (ax - bx) + (ay - by) * (ay - by))
370 #define EUCLIDEAN_THRESHOLD 7
373 euclidean_discard_cluster_outliers(sparrow_voter_t
*voters
, int n
)
375 /* Calculate distance between each pair. Discard points with maximum sum,
376 then recalculate until all are within threshold.
378 GST_DEBUG("cleansing a cluster of size %d using sum of distances", n
);
381 for (i
= 0; i
< n
; i
++){
383 for (j
= i
+ 1; j
< n
; j
++){
384 coord_t d
= EUCLIDEAN_D2(voters
[i
].x
, voters
[i
].y
,
385 voters
[j
].x
, voters
[j
].y
);
392 coord_t worst_d
, threshold
;
394 threshold
= EUCLIDEAN_THRESHOLD
* n
;
397 for (i
= 0; i
< n
; i
++){
398 if (dsums
[i
] > worst_d
){
403 if (worst_d
> threshold
){
404 GST_DEBUG("failing point %d, distance sq %d, threshold %d\n",
405 worst_i
, C2I(worst_d
), C2I(threshold
));
406 //subtract this one from the sums, or they'll all go
407 for (i
= 0; i
< n
; i
++){
408 dsums
[i
] -= EUCLIDEAN_D2(voters
[i
].x
, voters
[i
].y
,
409 voters
[worst_i
].x
, voters
[worst_i
].y
);
411 n
= drop_cluster_voter(voters
, n
, worst_i
);
414 GST_DEBUG("worst %d, was only %d, threshold %d\n",
415 worst_i
, C2I(worst_d
), C2I(threshold
));
423 median_discard_cluster_outliers(sparrow_voter_t
*voters
, int n
)
428 for (i
= 0; i
< n
; i
++){
429 /*XXX could sort here*/
430 xvals
[i
] = voters
[i
].x
;
431 yvals
[i
] = voters
[i
].y
;
433 const coord_t xmed
= sort_median(xvals
, n
);
434 const coord_t ymed
= sort_median(yvals
, n
);
436 for (i
= 0; i
< n
; i
++){
437 coord_t dx
= voters
[i
].x
- xmed
;
438 coord_t dy
= voters
[i
].y
- ymed
;
439 if (dx
* dx
+ dy
* dy
> OUTLIER_THRESHOLD
){
440 n
= drop_cluster_voter(voters
, n
, i
);
448 make_corners(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
449 //DEBUG_FIND_LINES(fl);
450 int width
= fl
->n_vlines
;
451 int height
= fl
->n_hlines
;
452 sparrow_cluster_t
*clusters
= fl
->clusters
;
453 sparrow_corner_t
*mesh
= fl
->mesh
;
457 for (y
= 0; y
< height
; y
++){
458 for (x
= 0; x
< width
; x
++, i
++){
459 sparrow_cluster_t
*cluster
= clusters
+ i
;
460 if (cluster
->n
== 0){
464 /*discard outliers based on sum of squared distances: good points should
465 be in a cluster, and have lowest sum*/
466 cluster
->n
= euclidean_discard_cluster_outliers(cluster
->voters
, cluster
->n
);
468 /*discard values away from median x, y values.
469 (each dimension is calculated independently)*/
470 cluster
->n
= median_discard_cluster_outliers(cluster
->voters
, cluster
->n
);
472 /* now find a weighted average position */
473 /*With int coord_t, coord_sum_t is
474 64 bit to avoid overflow -- should probably just use floating point
476 coord_sum_t xsum
, ysum
;
477 coord_t xmean
, ymean
;
483 for (j
= 0; j
< cluster
->n
; j
++){
484 votes
+= cluster
->voters
[j
].signal
;
485 ysum
+= cluster
->voters
[j
].y
* cluster
->voters
[j
].signal
;
486 xsum
+= cluster
->voters
[j
].x
* cluster
->voters
[j
].signal
;
489 xmean
= xsum
/ votes
;
490 ymean
= ysum
/ votes
;
493 GST_WARNING("corner %d, %d voters, sum %d,%d, somehow has no votes\n",
494 i
, cluster
->n
, xsum
, ysum
);
497 GST_DEBUG("corner %d: %d voters, %d votes, sum %d,%d, mean %d,%d\n",
498 i
, cluster
->n
, votes
, C2I(xsum
), C2I(ysum
), C2I(xmean
), C2I(ymean
));
502 mesh
[i
].status
= CORNER_EXACT
;
503 GST_DEBUG("found corner %d at (%3f, %3f)\n",
504 i
, COORD_TO_FLOAT(xmean
), COORD_TO_FLOAT(ymean
));
509 static sparrow_point_t
510 median_centre(sparrow_voter_t
*estimates
, int n
){
511 /*X and Y arevcalculated independently, which is really not right.
512 on the other hand, it probably works. */
514 sparrow_point_t result
;
516 for (i
= 0; i
< n
; i
++){
517 vals
[i
] = estimates
[i
].x
;
519 result
.x
= coord_median(vals
, n
);
521 for (i
= 0; i
< n
; i
++){
522 vals
[i
] = estimates
[i
].y
;
524 result
.y
= coord_median(vals
, n
);
528 static const sparrow_estimator_t base_estimators
[] = {
543 #define BASE_ESTIMATORS (sizeof(base_estimators) / sizeof(sparrow_estimator_t))
544 #define ESTIMATORS (BASE_ESTIMATORS * 4)
547 calculate_estimator_tables(sparrow_estimator_t
*estimators
){
549 sparrow_estimator_t
*e
= estimators
;
550 for (i
= 0; i
< BASE_ESTIMATORS
; i
++){
551 for (j
= 0; j
< 4; j
++){
552 *e
= base_estimators
[i
];
569 GST_DEBUG("estimator: %-d,%-d %-d,%-d %-d,%-d",
570 e
->x1
, e
->y1
, e
->x2
, e
->y2
, e
->x3
, e
->y3
);
577 /*the map made above is likely to be full of errors. Fix them, and add in
580 complete_map(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
581 sparrow_voter_t estimates
[ESTIMATORS
+ 1]; /* 1 extra for trick simplifying median */
582 sparrow_estimator_t estimators
[ESTIMATORS
];
583 calculate_estimator_tables(estimators
);
585 guint32
*debug
= NULL
;
587 debug
= (guint32
*)fl
->debug
->imageData
;
588 memset(debug
, 0, sparrow
->in
.size
);
592 int width
= fl
->n_vlines
;
593 int height
= fl
->n_hlines
;
594 int screen_width
= sparrow
->in
.width
;
595 int screen_height
= sparrow
->in
.height
;
596 sparrow_corner_t
*mesh
= fl
->mesh
;
597 sparrow_corner_t
*mesh_next
= fl
->mesh_next
;
599 memset(estimates
, 0, sizeof(estimates
)); /*just for clarity in debugging */
600 int prev_settled
= 0;
602 memcpy(mesh_next
, mesh
, width
* height
* sizeof(sparrow_corner_t
));
604 for (y
= 0; y
< height
; y
++){
605 for (x
= 0; x
< width
; x
++){
606 sparrow_corner_t
*corner
= &mesh
[y
* width
+ x
];
607 if (corner
->status
== CORNER_SETTLED
){
609 GST_DEBUG("ignoring settled corner %d, %d", x
, y
);
613 for (guint j
= 0; j
< ESTIMATORS
; j
++){
614 sparrow_estimator_t
*e
= &estimators
[j
];
615 int x3
, y3
, x2
, y2
, x1
, y1
;
618 if (!(y3
>= 0 && y3
< height
&&
619 x3
>= 0 && x3
< width
&&
620 mesh
[y3
* width
+ x3
].status
!= CORNER_UNUSED
622 GST_DEBUG("not using estimator %d because corners aren't used, or are off screen\n"
623 "x3 %d, y3 %d", j
, x3
, y3
);
630 if (mesh
[y2
* width
+ x2
].status
== CORNER_UNUSED
||
631 mesh
[y1
* width
+ x1
].status
== CORNER_UNUSED
){
632 GST_DEBUG("not using estimator %d because corners aren't used", j
);
635 /*there are 3 points, and the unknown one.
636 They should all be in a line.
637 The ratio of the p3-p2:p2-p1 sould be the same as
640 This really has to be done in floating point.
642 collinearity, no division, but no useful error metric
643 x[0] * (y[1]-y[2]) + x[1] * (y[2]-y[0]) + x[2] * (y[0]-y[1]) == 0
644 (at least not without further division)
648 cos angle = dot product / product of euclidean lengths
650 (dx12 * dx23 + dy12 * dy23) /
651 (sqrt(dx12 * dx12 + dy12 * dy12) * sqrt(dx23 * dx23 + dy23 * dy23))
653 is costly up front (sqrt), but those distances need to be
654 calculated anyway (or at least they are handy). Not much gained by
655 short-circuiting on bad collinearity, though.
657 It also handlily catches all the division by zeros in one meaningful
660 sparrow_corner_t
*c1
= &mesh
[y1
* width
+ x1
];
661 sparrow_corner_t
*c2
= &mesh
[y2
* width
+ x2
];
662 sparrow_corner_t
*c3
= &mesh
[y3
* width
+ x3
];
664 double dx12
= c1
->x
- c2
->x
;
665 double dy12
= c1
->y
- c2
->y
;
666 double dx23
= c2
->x
- c3
->x
;
667 double dy23
= c2
->y
- c3
->y
;
668 double distance12
= sqrt(dx12
* dx12
+ dy12
* dy12
);
669 double distance23
= sqrt(dx23
* dx23
+ dy23
* dy23
);
671 double dp
= dx12
* dx23
+ dy12
* dy23
;
673 double distances
= distance12
* distance23
;
675 GST_LOG("mesh points: %d,%d, %d,%d, %d,%d\n"
676 "map points: %d,%d, %d,%d, %d,%d\n"
677 "diffs: 12: %0.3f,%0.3f, 23: %0.3f,%0.3f, \n"
678 "distances: 12: %0.3f, 32: %0.3f\n",
679 x1
, y1
, x2
, y2
, x3
, y3
,
680 C2I(c1
->x
), C2I(c1
->y
), C2I(c2
->x
), C2I(c2
->y
), C2I(c3
->x
), C2I(c3
->y
),
681 dx12
, dy12
, dx23
, dy23
, distance12
, distance23
684 if (distances
== 0.0){
685 GST_INFO("at least two points out of %d,%d, %d,%d, %d,%d are the same!",
686 x1
, y1
, x2
, y2
, x3
, y3
);
689 double line_error
= 1.0 - dp
/ distances
;
690 if (line_error
> MAX_NONCOLLINEARITY
){
691 GST_DEBUG("Points %d,%d, %d,%d, %d,%d are not in a line: non-collinearity: %3f",
692 x1
, y1
, x2
, y2
, x3
, y3
, line_error
);
695 GST_LOG("GOOD collinearity: %3f", line_error
);
698 double ratio
= distance12
/ distance23
;
699 /*so here's the estimate!*/
700 coord_t dx
= dx12
* ratio
;
701 coord_t dy
= dy12
* ratio
;
702 coord_t ex
= c1
->x
+ dx
;
703 coord_t ey
= c1
->y
+ dy
;
705 GST_LOG("dx, dy: %d,%d, ex, ey: %d,%d\n"
706 "dx raw: %0.3f,%0.3f, x1, x2: %0.3f,%0.3f,\n"
707 "distances: 12: %0.3f, 32: %0.3f\n"
709 C2I(dx
), C2I(dy
), C2I(ex
), C2I(ey
),
710 dx
, dy
, ex
, ey
, ratio
713 if (! coord_in_range(ey
, screen_height
) ||
714 ! coord_in_range(ex
, screen_width
)){
715 GST_DEBUG("rejecting estimate for %d, %d, due to ex, ey being %d, %d",
716 x
, y
, C2I(ex
), C2I(ey
));
720 GST_LOG("estimator %d,%d SUCCESSFULLY estimated that %d, %d will be %d, %d",
721 x1
, x2
, x
, y
, C2I(ex
), C2I(ey
));
726 debug
[coords_to_index(ex
, ey
, sparrow
->in
.width
, sparrow
->in
.height
)] = 0x00aa7700;
730 /*now there is an array of estimates.
731 The *_discard_cluster_outliers functions should fit here */
732 GST_INFO("got %d estimates for %d,%d", k
, x
, y
);
740 /*now find median values. If the number is even, add a copy of either
741 the original value, or a random element. */
743 if (corner
->status
!= CORNER_UNUSED
){
744 estimates
[k
].x
= corner
->x
;
745 estimates
[k
].y
= corner
->y
;
748 int r
= RANDINT(sparrow
, 0, r
);
749 estimates
[k
].x
= estimates
[r
].x
;
750 estimates
[k
].y
= estimates
[r
].y
;
754 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
;
778 GST_INFO("estimating %d,%d", C2I(guess_x
), C2I(guess_y
));
780 if (corner
->status
== CORNER_EXACT
){
782 debug
[coords_to_index(corner
->x
, corner
->y
,
783 sparrow
->in
.width
, sparrow
->in
.height
)] = 0xffff3300;
785 if ((guess_x
- corner
->x
) * (guess_x
- corner
->x
) +
786 (guess_y
- corner
->y
) * (guess_y
- corner
->y
)
787 < CORNER_EXACT_THRESHOLD
){
790 corner
->status
= CORNER_SETTLED
;
791 GST_INFO("using exact reading %0.3f, %0.3f", C2F(corner
->x
), C2F(corner
->y
));
794 GST_INFO("REJECTING exact reading %0.3f,%0.3f: too far from median %0.3f,%0.3f",
795 C2F(corner
->x
), C2F(corner
->y
), C2F(corner
->x
), C2F(corner
->y
));
796 corner
->status
= CORNER_PROJECTED
;
799 else if (k
< MIN_CORNER_ESTIMATES
){
800 GST_INFO("weak evidence (%d estimates) for corner %d,%d, marking it PROJECTED",
802 corner
->status
= CORNER_PROJECTED
;
804 debug
[coords_to_index(guess_x
, guess_y
,
805 sparrow
->in
.width
, sparrow
->in
.height
)] = 0xff0000ff;
809 GST_DEBUG("corner %d, %d is SETTLED", x
, y
);
810 corner
->status
= CORNER_SETTLED
;
813 debug
[coords_to_index(guess_x
, guess_y
,
814 sparrow
->in
.width
, sparrow
->in
.height
)] = 0xffffffff;
821 GST_INFO("settled %d in that round. %d left to go",
822 settled
- prev_settled
, width
* height
- settled
);
823 if (settled
== width
* height
|| settled
== prev_settled
){
826 prev_settled
= settled
;
827 sparrow_corner_t
*tmp
= mesh_next
;
832 fl
->mesh_next
= mesh_next
;
833 MAYBE_DEBUG_IPL(fl
->debug
);
838 calculate_deltas(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
840 int width
= fl
->n_vlines
;
841 int height
= fl
->n_hlines
;
842 sparrow_corner_t
*mesh
= fl
->mesh
;
845 //DEBUG_FIND_LINES(fl);
846 /* calculate deltas toward adjacent corners */
847 /* try to extrapolate left and up, if possible, so need to go backwards. */
848 i
= width
* height
- 1;
849 for (y
= height
- 1; y
>= 0; y
--){
850 for (x
= width
- 1; x
>= 0; x
--, i
--){
851 sparrow_corner_t
*corner
= &mesh
[i
];
852 /* calculate the delta to next corner. If this corner is on edge, delta is
853 0 and next is this.*/
854 sparrow_corner_t
*right
= (x
== width
- 1) ? corner
: corner
+ 1;
855 sparrow_corner_t
*down
= (y
== height
- 1) ? corner
: corner
+ width
;
856 GST_DEBUG("i %d xy %d,%d width %d. in_xy %d,%d; down in_xy %d,%d; right in_xy %d,%d\n",
857 i
, x
, y
, width
, C2I(corner
->x
), C2I(corner
->y
), C2I(down
->x
),
858 C2I(down
->y
), C2I(right
->x
), C2I(right
->y
));
859 if (corner
->status
!= CORNER_UNUSED
){
860 if (right
->status
!= CORNER_UNUSED
){
861 corner
->dxr
= QUANTISE_DELTA(right
->x
- corner
->x
);
862 corner
->dyr
= QUANTISE_DELTA(right
->y
- corner
->y
);
864 if (down
->status
!= CORNER_UNUSED
){
865 corner
->dxd
= QUANTISE_DELTA(down
->x
- corner
->x
);
866 corner
->dyd
= QUANTISE_DELTA(down
->y
- corner
->y
);
872 debug_corners_image(sparrow
, fl
);
878 look_for_line(GstSparrow
*sparrow
, guint8
*in
, sparrow_find_lines_t
*fl
,
879 sparrow_line_t
*line
){
882 guint32 cmask
= sparrow
->out
.colours
[sparrow
->colour
];
885 /* subtract background noise */
886 fl
->input
->imageData
= (char *)in
;
887 cvSub(fl
->input
, fl
->threshold
, fl
->working
, NULL
);
888 guint32
*in32
= (guint32
*)fl
->working
->imageData
;
890 for (i
= 0; i
< sparrow
->in
.pixcount
; i
++){
891 colour
= in32
[i
] & cmask
;
892 signal
= (((colour
>> fl
->shift1
) & COLOUR_MASK
) +
893 ((colour
>> fl
->shift2
) & COLOUR_MASK
));
895 if (fl
->map
[i
].lines
[line
->dir
]){
896 /*assume the pixel is on for everyone and will just confuse
900 if (fl
->map
[i
].lines
[line
->dir
] != BAD_PIXEL
){
902 GST_DEBUG("HEY, expected point %d to be in line %d (dir %d) "
903 "and thus empty, but it is also in line %d\n"
904 "old signal %d, new signal %d, marking as BAD\n",
905 i, line->index, line->dir, fl->map[i].lines[line->dir],
906 fl->map[i].signal[line->dir], signal);
908 fl
->map
[i
].lines
[line
->dir
] = BAD_PIXEL
;
909 fl
->map
[i
].signal
[line
->dir
] = 0;
913 fl
->map
[i
].lines
[line
->dir
] = line
->index
;
914 fl
->map
[i
].signal
[line
->dir
] = signal
;
921 debug_map_image(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
922 guint32
*data
= (guint32
*)fl
->debug
->imageData
;
923 memset(data
, 0, sparrow
->in
.size
);
924 for (guint i
= 0; i
< sparrow
->in
.pixcount
; i
++){
925 data
[i
] |= fl
->map
[i
].signal
[SPARROW_HORIZONTAL
] << sparrow
->in
.gshift
;
926 data
[i
] |= fl
->map
[i
].signal
[SPARROW_VERTICAL
] << sparrow
->in
.rshift
;
927 data
[i
] |= ((fl
->map
[i
].lines
[SPARROW_VERTICAL
] == BAD_PIXEL
) ||
928 (fl
->map
[i
].lines
[SPARROW_HORIZONTAL
] == BAD_PIXEL
)) ? 255 << sparrow
->in
.bshift
: 0;
930 MAYBE_DEBUG_IPL(fl
->debug
);
933 /* draw the line (in sparrow->colour) */
935 draw_line(GstSparrow
* sparrow
, sparrow_line_t
*line
, guint8
*out
){
936 guint32
*p
= (guint32
*)out
;
937 guint32 colour
= sparrow
->out
.colours
[sparrow
->colour
];
939 if (line
->dir
== SPARROW_HORIZONTAL
){
940 p
+= line
->offset
* sparrow
->out
.width
;
941 for (i
= 0; i
< sparrow
->out
.width
; i
++){
946 guint32
*p
= (guint32
*)out
;
948 for(i
= 0; i
< sparrow
->out
.height
; i
++){
950 p
+= sparrow
->out
.width
;
956 jump_state(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
, edges_state_t state
){
957 if (state
== EDGES_NEXT_STATE
){
964 case EDGES_FIND_NOISE
:
965 sparrow
->countdown
= MAX(sparrow
->lag
, 1) + SAFETY_LAG
;
967 case EDGES_FIND_LINES
:
968 sparrow
->countdown
= MAX(sparrow
->lag
, 1) + SAFETY_LAG
;
970 case EDGES_FIND_CORNERS
:
971 sparrow
->countdown
= 7;
973 case EDGES_WAIT_FOR_PLAY
:
974 global_number_of_edge_finders
--;
975 sparrow
->countdown
= 300;
978 GST_DEBUG("jumped to non-existent state %d\n", fl
->state
);
983 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
987 draw_lines(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
, guint8
*in
, guint8
*out
)
989 sparrow_line_t
*line
= fl
->shuffled_lines
[fl
->current
];
990 sparrow
->countdown
--;
991 memset(out
, 0, sparrow
->out
.size
);
992 if (sparrow
->countdown
){
993 draw_line(sparrow
, line
, out
);
996 /*show nothing, look for result */
997 look_for_line(sparrow
, in
, fl
, line
);
999 debug_map_image(sparrow
, fl
);
1002 if (fl
->current
== fl
->n_lines
){
1003 jump_state(sparrow
, fl
, EDGES_NEXT_STATE
);
1006 sparrow
->countdown
= MAX(sparrow
->lag
, 1) + SAFETY_LAG
;
1011 #define LINE_THRESHOLD 32
1014 find_threshold(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
, guint8
*in
, guint8
*out
)
1016 memset(out
, 0, sparrow
->out
.size
);
1017 /*XXX should average/median over a range of frames */
1018 if (sparrow
->countdown
== 0){
1019 memcpy(fl
->threshold
->imageData
, in
, sparrow
->in
.size
);
1020 /*add a constant, and smooth */
1021 cvAddS(fl
->threshold
, cvScalarAll(LINE_THRESHOLD
), fl
->working
, NULL
);
1022 cvSmooth(fl
->working
, fl
->threshold
, CV_GAUSSIAN
, 3, 0, 0, 0);
1023 //cvSmooth(fl->working, fl->threshold, CV_MEDIAN, 3, 0, 0, 0);
1024 jump_state(sparrow
, fl
, EDGES_NEXT_STATE
);
1026 sparrow
->countdown
--;
1029 /*match up lines and find corners */
1031 find_corners(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
)
1033 sparrow
->countdown
--;
1034 switch(sparrow
->countdown
){
1036 make_clusters(sparrow
, fl
);
1039 make_corners(sparrow
, fl
);
1042 complete_map(sparrow
, fl
);
1045 calculate_deltas(sparrow
, fl
);
1049 corners_to_full_lut(sparrow
, fl
);
1051 corners_to_lut(sparrow
, fl
);
1053 jump_state(sparrow
, fl
, EDGES_NEXT_STATE
);
1056 GST_DEBUG("how did sparrow->countdown get to be %d?", sparrow
->countdown
);
1057 sparrow
->countdown
= 5;
1059 return sparrow
->countdown
;
1062 /*use a dirty shared variable*/
1064 wait_for_play(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
1065 if (global_number_of_edge_finders
== 0 ||
1066 sparrow
->countdown
== 0){
1069 sparrow
->countdown
--;
1073 INVISIBLE sparrow_state
1074 mode_find_edges(GstSparrow
*sparrow
, guint8
*in
, guint8
*out
){
1075 sparrow_find_lines_t
*fl
= (sparrow_find_lines_t
*)sparrow
->helper_struct
;
1077 case EDGES_FIND_NOISE
:
1078 find_threshold(sparrow
, fl
, in
, out
);
1080 case EDGES_FIND_LINES
:
1081 draw_lines(sparrow
, fl
, in
, out
);
1083 case EDGES_FIND_CORNERS
:
1084 memset(out
, 0, sparrow
->out
.size
);
1085 find_corners(sparrow
, fl
);
1087 case EDGES_WAIT_FOR_PLAY
:
1088 memset(out
, 0, sparrow
->out
.size
);
1089 if (wait_for_play(sparrow
, fl
)){
1090 return SPARROW_NEXT_STATE
;
1094 GST_WARNING("strange state in mode_find_edges: %d", fl
->state
);
1095 memset(out
, 0, sparrow
->out
.size
);
1097 return SPARROW_STATUS_QUO
;
1101 finalise_find_edges(GstSparrow
*sparrow
){
1102 sparrow_find_lines_t
*fl
= (sparrow_find_lines_t
*)sparrow
->helper_struct
;
1103 //DEBUG_FIND_LINES(fl);
1104 if (sparrow
->save
&& *(sparrow
->save
)){
1105 GST_DEBUG("about to save to %s\n", sparrow
->save
);
1106 dump_edges_info(sparrow
, fl
, sparrow
->save
);
1108 if (sparrow
->debug
){
1109 cvReleaseImage(&fl
->debug
);
1112 free(fl
->shuffled_lines
);
1117 cvReleaseImage(&fl
->threshold
);
1118 cvReleaseImage(&fl
->working
);
1119 cvReleaseImageHeader(&fl
->input
);
1121 GST_DEBUG("freed everything\n");
1122 sparrow
->helper_struct
= NULL
;
1126 setup_colour_shifts(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
1127 /*COLOUR_QUANT reduces the signal a little bit more, avoiding overflow
1129 switch (sparrow
->colour
){
1132 fl
->shift1
= sparrow
->in
.gshift
+ COLOUR_QUANT
;
1133 fl
->shift2
= sparrow
->in
.gshift
+ COLOUR_QUANT
;
1135 case SPARROW_MAGENTA
:
1136 fl
->shift1
= sparrow
->in
.rshift
+ COLOUR_QUANT
;
1137 fl
->shift2
= sparrow
->in
.bshift
+ COLOUR_QUANT
;
1143 init_find_edges(GstSparrow
*sparrow
){
1145 sparrow_find_lines_t
*fl
= zalloc_aligned_or_die(sizeof(sparrow_find_lines_t
));
1146 sparrow
->helper_struct
= (void *)fl
;
1148 gint h_lines
= (sparrow
->out
.height
+ LINE_PERIOD
- 1) / LINE_PERIOD
;
1149 gint v_lines
= (sparrow
->out
.width
+ LINE_PERIOD
- 1) / LINE_PERIOD
;
1150 gint n_lines_max
= (h_lines
+ v_lines
);
1151 gint n_corners
= (h_lines
* v_lines
);
1153 /*set up dither here, rather than in the busy time */
1154 fl
->dither
= malloc_aligned_or_die(sparrow
->out
.pixcount
* sizeof(double));
1155 dsfmt_fill_array_close_open(sparrow
->dsfmt
, fl
->dither
, sparrow
->out
.pixcount
);
1157 fl
->n_hlines
= h_lines
;
1158 fl
->n_vlines
= v_lines
;
1160 fl
->h_lines
= malloc_aligned_or_die(sizeof(sparrow_line_t
) * n_lines_max
);
1161 fl
->shuffled_lines
= malloc_aligned_or_die(sizeof(sparrow_line_t
*) * n_lines_max
);
1162 GST_DEBUG("shuffled lines, malloced %p\n", fl
->shuffled_lines
);
1164 GST_DEBUG("map is going to be %d * %d \n", sizeof(sparrow_intersect_t
), sparrow
->in
.pixcount
);
1165 fl
->map
= zalloc_aligned_or_die(sizeof(sparrow_intersect_t
) * sparrow
->in
.pixcount
);
1166 fl
->clusters
= zalloc_or_die(n_corners
* sizeof(sparrow_cluster_t
));
1167 fl
->mesh_mem
= zalloc_aligned_or_die(n_corners
* sizeof(sparrow_corner_t
) * 2);
1168 fl
->mesh
= fl
->mesh_mem
;
1169 fl
->mesh_next
= fl
->mesh
+ n_corners
;
1171 sparrow_line_t
*line
= fl
->h_lines
;
1172 sparrow_line_t
**sline
= fl
->shuffled_lines
;
1175 for (i
= 0, offset
= H_LINE_OFFSET
; offset
< sparrow
->out
.height
;
1176 i
++, offset
+= LINE_PERIOD
){
1177 line
->offset
= offset
;
1178 line
->dir
= SPARROW_HORIZONTAL
;
1183 //GST_DEBUG("line %d h has offset %d\n", i, offset);
1186 /*now add the vertical lines */
1188 for (i
= 0, offset
= V_LINE_OFFSET
; offset
< sparrow
->out
.width
;
1189 i
++, offset
+= LINE_PERIOD
){
1190 line
->offset
= offset
;
1191 line
->dir
= SPARROW_VERTICAL
;
1196 //GST_DEBUG("line %d v has offset %d\n", i, offset);
1198 //DEBUG_FIND_LINES(fl);
1199 fl
->n_lines
= line
- fl
->h_lines
;
1200 GST_DEBUG("allocated %d lines, made %d\n", n_lines_max
, fl
->n_lines
);
1203 for (i
= 0; i
< fl
->n_lines
; i
++){
1204 int j
= RANDINT(sparrow
, 0, fl
->n_lines
);
1205 sparrow_line_t
*tmp
= fl
->shuffled_lines
[j
];
1206 fl
->shuffled_lines
[j
] = fl
->shuffled_lines
[i
];
1207 fl
->shuffled_lines
[i
] = tmp
;
1210 setup_colour_shifts(sparrow
, fl
);
1212 /* opencv images for threshold finding */
1213 CvSize size
= {sparrow
->in
.width
, sparrow
->in
.height
};
1214 fl
->working
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);
1215 fl
->threshold
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);
1217 /*input has no data allocated -- it uses latest frame*/
1218 fl
->input
= init_ipl_image(&sparrow
->in
, PIXSIZE
);
1219 //DEBUG_FIND_LINES(fl);
1220 if (sparrow
->debug
){
1221 fl
->debug
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);
1224 if (sparrow
->reload
){
1225 if (access(sparrow
->reload
, R_OK
)){
1226 GST_DEBUG("sparrow->reload is '%s' and it is UNREADABLE\n", sparrow
->reload
);
1229 read_edges_info(sparrow
, fl
, sparrow
->reload
);
1230 memset(fl
->map
, 0, sizeof(sparrow_intersect_t
) * sparrow
->in
.pixcount
);
1231 //memset(fl->clusters, 0, n_corners * sizeof(sparrow_cluster_t));
1232 memset(fl
->mesh
, 0, n_corners
* sizeof(sparrow_corner_t
));
1233 jump_state(sparrow
, fl
, EDGES_FIND_CORNERS
);
1236 jump_state(sparrow
, fl
, EDGES_FIND_NOISE
);
1239 global_number_of_edge_finders
++;