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"
34 /*3 pixels manhatten distance makes you an outlier */
35 #define OUTLIER_THRESHOLD 3 << (SPARROW_FIXED_POINT)
36 #define OUTLIER_PENALTY 8
42 #define SPARROW_MAP_LUT_SHIFT 1
43 #define SPARROW_FP_2_LUT (SPARROW_FIXED_POINT - SPARROW_MAP_LUT_SHIFT)
45 #define OFFSET(x, y, w)((((y) * (w)) >> SPARROW_FIXED_POINT) + ((x) >> SPARROW_FIXED_POINT))
47 static void corners_to_lut(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
48 //DEBUG_FIND_LINES(fl);
49 sparrow_map_t
*map
= &sparrow
->map
; /*rows in sparrow->out */
50 guint8
*mask
= sparrow
->screenmask
; /*mask in sparrow->in */
51 sparrow_corner_t
*mesh
= fl
->mesh
; /*maps regular points in ->out to points in ->in */
53 int mesh_w
= fl
->n_vlines
;
54 int mesh_h
= fl
->n_hlines
;
55 int in_w
= sparrow
->in
.width
;
56 int mcy
, mmy
, mcx
; /*Mesh Corner|Modulus X|Y*/
59 sparrow_map_row_t
*row
= map
->rows
;
60 sparrow_map_point_t
*p
= map
->point_mem
;
61 sparrow_corner_t
*mesh_row
= mesh
;
62 for(mcy
= 0; mcy
< mesh_h
; mcy
++){
63 for (mmy
= 0; mmy
< LINE_PERIOD
; mmy
++){
64 sparrow_corner_t
*mesh_square
= mesh_row
;
68 for(mcx
= 0; mcx
< mesh_w
; mcx
++){
69 if (mesh_square
->used
){
70 int iy
= mesh_square
->in_y
+ mmy
* mesh_square
->dyv
;
71 int ix
= mesh_square
->in_x
+ mmy
* mesh_square
->dxv
;
72 int ii
= OFFSET(ix
, iy
, in_w
);
73 int ii_end
= OFFSET(ix
+ (LINE_PERIOD
- 1) * mesh_square
->dxh
,
74 iy
+ (LINE_PERIOD
- 1) * mesh_square
->dyh
, in_w
);
75 int start_on
= mask
[ii
];
76 int end_on
= mask
[ii_end
];
77 if(start_on
&& end_on
){
78 /*add the point, maybe switch on */
79 if (row
->start
== row
->end
){/* if both are 0 */
80 row
->start
= mcx
* LINE_PERIOD
;
84 p
->dx
= mesh_square
->dxh
;
85 p
->dy
= mesh_square
->dyh
;
89 /*add the point, switch off somewhere in the middle*/
90 for (x
= 1; x
< LINE_PERIOD
; x
++){
91 iy
+= mesh_square
->dyh
;
92 ix
+= mesh_square
->dxh
;
93 ii
= OFFSET(ix
, iy
, in_w
);
95 /*point is not in the same column with the others,
96 but sparrow knows this because the row->start says so */
100 p
->dx
= mesh_square
->dxh
;
101 p
->dy
= mesh_square
->dyh
;
108 /* add some, switch off */
109 for (x
= 1; x
< LINE_PERIOD
; x
++){
110 iy
+= mesh_square
->dyh
;
111 ix
+= mesh_square
->dxh
;
112 ii
= OFFSET(ix
, iy
, in_w
);
121 start > end: this is first off pixel.
122 start == end: row hasn't started (both 0)
123 start < end: both are set -- row is done
125 if (row
->start
> row
->end
){
126 row
->end
= mcx
* LINE_PERIOD
;
128 else if (row
->start
< row
->end
){
142 corners_to_full_lut(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
143 //DEBUG_FIND_LINES(fl);
144 sparrow_corner_t
*mesh
= fl
->mesh
; /*maps regular points in ->out to points in ->in */
145 sparrow_map_lut_t
*map_lut
= sparrow
->map_lut
;
146 int mesh_w
= fl
->n_vlines
;
147 int mesh_h
= fl
->n_hlines
;
148 int mcy
, mmy
, mcx
, mmx
; /*Mesh Corner|Modulus X|Y*/
150 sparrow_corner_t
*mesh_row
= mesh
;
151 for(mcy
= 0; mcy
< mesh_h
; mcy
++){
152 for (mmy
= 0; mmy
< LINE_PERIOD
; mmy
++){
153 sparrow_corner_t
*mesh_square
= mesh_row
;
154 for(mcx
= 0; mcx
< mesh_w
; mcx
++){
155 int iy
= mesh_square
->in_y
+ mmy
* mesh_square
->dyv
;
156 int ix
= mesh_square
->in_x
+ mmy
* mesh_square
->dxv
;
157 for (mmx
= 0; mmx
< LINE_PERIOD
; mmx
++, i
++){
158 map_lut
[i
].x
= ix
>> SPARROW_FP_2_LUT
;
159 map_lut
[i
].y
= iy
>> SPARROW_FP_2_LUT
;
160 ix
+= mesh_square
->dxh
;
161 iy
+= mesh_square
->dyh
;
168 sparrow
->map_lut
= map_lut
;
171 #define INTXY(x)((x)>> SPARROW_FIXED_POINT)
172 #define FLOATXY(x)(((double)(x)) / (1 << SPARROW_FIXED_POINT))
174 debug_corners_image(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
175 guint32
*data
= (guint32
*)fl
->debug
->imageData
;
176 guint w
= fl
->debug
->width
;
177 memset(data
, 0, sparrow
->in
.size
);
178 guint32 colours
[4] = {0xaaaaaaaa, 0x0000ff00, 0x00ff0000, 0xff0000ff};
179 for (int i
= 0; i
< fl
->n_vlines
* fl
->n_hlines
; i
++){
180 sparrow_corner_t
*c
= &fl
->mesh
[i
];
183 GST_DEBUG("i %d used %d x: %f, y: %f dxh %f dyh %f dxv %f dyv %f\n",
184 i
, c
->used
, FLOATXY(x
), FLOATXY(y
),
185 FLOATXY(c
->dxh
), FLOATXY(c
->dyh
), FLOATXY(c
->dxv
), FLOATXY(c
->dxh
));
186 data
[INTXY(y
+ c
->dyh
) * w
+ INTXY(x
+ c
->dxh
)] = 0x0000aaaa;
187 data
[INTXY(y
+ c
->dyh
+ c
->dyh
) * w
+ INTXY(x
+ c
->dxh
+ c
->dxh
)] = 0x0000aaaa;
188 data
[INTXY(y
+ c
->dyv
) * w
+ INTXY(x
+ c
->dxv
)] = 0x00aaaa00;
189 data
[INTXY(y
+ c
->dyv
+ c
->dyv
) * w
+ INTXY(x
+ c
->dxv
+ c
->dxv
)] = 0x00aaaa00;
190 data
[INTXY(y
) * w
+ INTXY(x
)] = colours
[c
->used
];
192 MAYBE_DEBUG_IPL(fl
->debug
);
198 find_corners(GstSparrow
*sparrow
, guint8
*in
, sparrow_find_lines_t
*fl
){
199 //DEBUG_FIND_LINES(fl);
201 int width
= fl
->n_vlines
;
202 int height
= fl
->n_hlines
;
203 sparrow_cluster_t
*clusters
= fl
->clusters
;
204 sparrow_corner_t
*mesh
= fl
->mesh
;
206 /*each point in fl->map is in a vertical line, a horizontal line, both, or
207 neither. Only the "both" case matters. */
208 for (y
= 0; y
< sparrow
->in
.height
; y
++){
209 for (x
= 0; x
< sparrow
->in
.width
; x
++){
210 sparrow_intersect_t
*p
= &fl
->map
[y
* sparrow
->in
.width
+ x
];
211 /*remembering that 0 is valid as a line no, but not as a signal */
212 if (! p
->signal
[SPARROW_HORIZONTAL
] ||
213 ! p
->signal
[SPARROW_VERTICAL
]){
216 /*This one is lobbying for the position of a corner.*/
218 /*XXX what to do in the case that there is no intersection? often cases
219 this will happen in the dark bits and be OK. But if it happens in the
221 /*linearise the xy coordinates*/
222 int vline
= p
->lines
[SPARROW_VERTICAL
];
223 int hline
= p
->lines
[SPARROW_HORIZONTAL
];
225 GST_DEBUG("signal at %p (%d, %d): %dv %dh, lines: %dh %dv\n", p
, x
, y
,
226 p
->signal
[SPARROW_HORIZONTAL
], p
->signal
[SPARROW_VERTICAL
],
229 sparrow_cluster_t
*cluster
= &clusters
[vline
* height
+ hline
];
231 GST_DEBUG("cluster is %p, n is %d\n", cluster
, n
);
233 cluster
->voters
[n
].x
= x
<< SPARROW_FIXED_POINT
;
234 cluster
->voters
[n
].y
= y
<< SPARROW_FIXED_POINT
;
235 cluster
->voters
[n
].signal
= (SIG_WEIGHT
+ p
->signal
[SPARROW_HORIZONTAL
]) *
236 (SIG_WEIGHT
+ p
->signal
[SPARROW_VERTICAL
]);
238 /*these next two could of course be computed from the offset */
241 GST_DEBUG("more than 8 pixels at cluster for corner %d, %d\n",
243 /*if this message ever turns up, replace the weakest signals or add
248 //DEBUG_FIND_LINES(fl);
250 for (y
= 0; y
< height
; y
++){
251 for (x
= 0; x
< width
; x
++, i
++){
253 1. centre of gravity (x,y, weighted average)
254 2. discard outliers? look for connectedness? but if 2 are outliers?
256 sparrow_cluster_t
*cluster
= clusters
+ i
;
257 if (cluster
->n
== 0){
268 for (j
= 0; j
< cluster
->n
; j
++){
269 votes
+= cluster
->voters
[j
].signal
;
270 ysum
+= cluster
->voters
[j
].y
* cluster
->voters
[j
].signal
;
271 xsum
+= cluster
->voters
[j
].x
* cluster
->voters
[j
].signal
;
274 /* don't diminish signal altogether. The previous iteration's means
278 xmean
= xsum
/ votes
;
279 ymean
= ysum
/ votes
;
283 for (j
= 0; j
< cluster
->n
; j
++){
284 int xdiff
= abs(cluster
->voters
[j
].x
- xmean
);
285 int ydiff
= abs(cluster
->voters
[j
].y
- ymean
);
286 devsum
+= xdiff
+ ydiff
;
287 if (xdiff
+ ydiff
> worst
){
288 worst
= xdiff
+ ydiff
;
292 /*a bad outlier has significantly greater than average deviation
293 (but how much is bad? median deviation would be more useful)*/
294 if (worst
> 3 * devsum
/ cluster
->n
){
295 /* reduce the worst ones weight. it is a silly aberration. */
296 cluster
->voters
[worstn
].signal
/= OUTLIER_PENALTY
;
297 GST_DEBUG("dropping outlier at %d,%d (mean %d,%d)\n",
298 cluster
->voters
[worstn
].x
, cluster
->voters
[worstn
].y
, xmean
, ymean
);
303 mesh
[i
].out_x
= x
* LINE_PERIOD
;
304 mesh
[i
].out_y
= y
* LINE_PERIOD
;
305 mesh
[i
].in_x
= xmean
;
306 mesh
[i
].in_y
= ymean
;
308 double div
= (double)(1 << SPARROW_FIXED_POINT
); /*for printf only*/
309 GST_DEBUG("found corner %d (%d,%d) at (%3f, %3f)\n",
310 i
, mesh
[i
].out_x
, mesh
[i
].out_y
,
311 xmean
/ div
, ymean
/ div
);
314 //DEBUG_FIND_LINES(fl);
315 /* calculate deltas toward adjacent corners */
316 /* try to extrapolate left and up, if possible, so need to go backwards. */
317 for (y
= height
- 2; y
>= 0; y
--){
318 for (x
= width
- 2; x
>= 0; x
--){
321 mesh
[i
].dxh
= (mesh
[i
+ 1].in_x
- mesh
[i
].in_x
) / LINE_PERIOD
;
322 mesh
[i
].dyh
= (mesh
[i
+ 1].in_y
- mesh
[i
].in_y
) / LINE_PERIOD
;
323 mesh
[i
].dxv
= (mesh
[i
+ width
].in_x
- mesh
[i
].in_x
) / LINE_PERIOD
;
324 mesh
[i
].dyv
= (mesh
[i
+ width
].in_y
- mesh
[i
].in_y
) / LINE_PERIOD
;
327 /*prefer copy from left unless it is itself reconstructed,
328 (for no great reason)
329 A mixed copy would be possible and better */
330 if(mesh
[i
+ 1].used
&&
331 (mesh
[i
+ 1].used
< mesh
[i
+ width
].used
)){
332 mesh
[i
].dxh
= mesh
[i
+ 1].dxh
;
333 mesh
[i
].dyh
= mesh
[i
+ 1].dyh
;
334 mesh
[i
].dxv
= mesh
[i
+ 1].dxv
;
335 mesh
[i
].dyv
= mesh
[i
+ 1].dyv
;
336 mesh
[i
].in_x
= mesh
[i
+ 1].in_x
- mesh
[i
+ 1].dxh
* LINE_PERIOD
;
337 mesh
[i
].in_y
= mesh
[i
+ 1].in_y
- mesh
[i
+ 1].dyh
* LINE_PERIOD
;
338 mesh
[i
].out_x
= mesh
[i
+ 1].out_x
- 1;
339 mesh
[i
].out_y
= mesh
[i
+ 1].out_y
;
340 mesh
[i
].used
= mesh
[i
+ 1].used
+ 1;
342 else if(mesh
[i
+ width
].used
){
343 mesh
[i
].dxh
= mesh
[i
+ width
].dxh
;
344 mesh
[i
].dyh
= mesh
[i
+ width
].dyh
;
345 mesh
[i
].dxv
= mesh
[i
+ width
].dxv
;
346 mesh
[i
].dyv
= mesh
[i
+ width
].dyv
;
347 mesh
[i
].in_x
= mesh
[i
+ width
].in_x
- mesh
[i
+ width
].dxv
* LINE_PERIOD
;
348 mesh
[i
].in_y
= mesh
[i
+ width
].in_y
- mesh
[i
+ width
].dyv
* LINE_PERIOD
;
349 mesh
[i
].out_x
= mesh
[i
+ width
].out_x
;
350 mesh
[i
].out_y
= mesh
[i
+ width
].out_y
- 1;
351 mesh
[i
].used
= mesh
[i
+ width
].used
+ 1;
357 DEBUG_FIND_LINES(fl
);
358 debug_corners_image(sparrow
, fl
);
360 //DEBUG_FIND_LINES(fl);
364 /* With no line drawn (in our colour) look at the background noise. Any real
365 signal has to be stringer than this.
367 XXX looking for simple maximum -- maybe heap or histogram might be better,
368 so as to be less susceptible to wierd outliers (e.g., bad pixels). */
370 look_for_threshold(GstSparrow
*sparrow
, guint8
*in
, sparrow_find_lines_t
*fl
){
371 //DEBUG_FIND_LINES(fl);
375 guint32
*in32
= (guint32
*)in
;
377 for (i
= 0; i
< (int)sparrow
->in
.pixcount
; i
++){
378 colour
= in32
[i
] & sparrow
->colour
;
379 signal
= ((colour
>> fl
->shift1
) +
380 (colour
>> fl
->shift2
)) & 0x1ff;
381 if (signal
> highest
){
385 fl
->threshold
= highest
+ 1;
386 GST_DEBUG("found maximum noise of %d, using threshold %d\n", highest
, fl
->threshold
);
391 look_for_line(GstSparrow
*sparrow
, guint8
*in
, sparrow_find_lines_t
*fl
,
392 sparrow_line_t
*line
){
396 guint32
*in32
= (guint32
*)in
;
397 for (i
= 0; i
< sparrow
->in
.pixcount
; i
++){
398 colour
= in32
[i
] & sparrow
->colour
;
399 signal
= ((colour
>> fl
->shift1
) +
400 (colour
>> fl
->shift2
)) & 0x1ff;
401 if (signal
> fl
->threshold
){
402 if (fl
->map
[i
].lines
[line
->dir
]){
403 GST_DEBUG("HEY, expected point %d to be in line %d (dir %d)"
404 "and thus empty, but it is also in line %d\n",
405 i
, line
->index
, line
->dir
, fl
->map
[i
].lines
[line
->dir
]);
407 fl
->map
[i
].lines
[line
->dir
] = line
->index
;
408 fl
->map
[i
].signal
[line
->dir
] = signal
;
414 debug_map_image(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
415 guint32
*data
= (guint32
*)fl
->debug
->imageData
;
416 memset(data
, 0, sparrow
->in
.size
);
417 for (guint i
= 0; i
< sparrow
->in
.pixcount
; i
++){
418 data
[i
] |= fl
->map
[i
].signal
[SPARROW_HORIZONTAL
] << sparrow
->in
.gshift
;
419 data
[i
] |= fl
->map
[i
].signal
[SPARROW_VERTICAL
] << sparrow
->in
.rshift
;
421 MAYBE_DEBUG_IPL(fl
->debug
);
424 /* draw the line (in sparrow->colour) */
426 draw_line(GstSparrow
* sparrow
, sparrow_line_t
*line
, guint8
*out
){
427 guint32
*p
= (guint32
*)out
;
429 if (line
->dir
== SPARROW_HORIZONTAL
){
430 p
+= line
->offset
* sparrow
->out
.width
;
431 for (i
= 0; i
< sparrow
->out
.width
; i
++){
432 p
[i
] = sparrow
->colour
;
436 guint32
*p
= (guint32
*)out
;
438 for(i
= 0; i
< sparrow
->out
.height
; i
++){
439 *p
= sparrow
->colour
;
440 p
+= sparrow
->out
.width
;
446 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
450 INVISIBLE sparrow_state
451 mode_find_edges(GstSparrow
*sparrow
, guint8
*in
, guint8
*out
){
452 sparrow_find_lines_t
*fl
= (sparrow_find_lines_t
*)sparrow
->helper_struct
;
453 //DEBUG_FIND_LINES(fl);
454 if (fl
->current
== fl
->n_lines
){
457 sparrow_line_t
*line
= fl
->shuffled_lines
[fl
->current
];
459 sparrow
->countdown
--;
460 memset(out
, 0, sparrow
->out
.size
);
461 if (sparrow
->countdown
){
462 /* show the line except on the first round, when we find a threshold*/
464 GST_DEBUG("current %d line %p\n", fl
->current
, line
);
465 draw_line(sparrow
, line
, out
);
469 /*show nothing, look for result */
471 look_for_line(sparrow
, in
, fl
, line
);
475 look_for_threshold(sparrow
, in
, fl
);
477 sparrow
->countdown
= sparrow
->lag
+ 2;
480 debug_map_image(sparrow
, fl
);
482 return SPARROW_STATUS_QUO
;
484 /*match up lines and find corners */
485 find_corners(sparrow
, in
, fl
);
486 corners_to_lut(sparrow
, fl
);
488 return SPARROW_NEXT_STATE
;
492 finalise_find_edges(GstSparrow
*sparrow
){
493 sparrow_find_lines_t
*fl
= (sparrow_find_lines_t
*)sparrow
->helper_struct
;
494 //DEBUG_FIND_LINES(fl);
496 cvReleaseImage(&fl
->debug
);
499 //DEBUG_FIND_LINES(fl);
500 free(fl
->shuffled_lines
);
502 //DEBUG_FIND_LINES(fl);
505 //DEBUG_FIND_LINES(fl);
507 sparrow
->helper_struct
= NULL
;
512 setup_colour_shifts(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
513 //DEBUG_FIND_LINES(fl);
515 switch (sparrow
->colour
){
518 fl
->shift1
= sparrow
->in
.gshift
;
519 fl
->shift2
= sparrow
->in
.gshift
;
521 case SPARROW_MAGENTA
:
522 fl
->shift1
= sparrow
->in
.rshift
;
523 fl
->shift2
= sparrow
->in
.bshift
;
529 init_find_edges(GstSparrow
*sparrow
){
530 gint32 w
= sparrow
->out
.width
;
531 gint32 h
= sparrow
->out
.height
;
533 sparrow_find_lines_t
*fl
= zalloc_aligned_or_die(sizeof(sparrow_find_lines_t
));
534 sparrow
->helper_struct
= (void *)fl
;
536 gint h_lines
= (h
+ LINE_PERIOD
- 1) / LINE_PERIOD
;
537 gint v_lines
= (w
+ LINE_PERIOD
- 1) / LINE_PERIOD
;
538 gint n_lines
= (h_lines
+ v_lines
);
539 gint n_corners
= (h_lines
* v_lines
);
540 fl
->n_hlines
= h_lines
;
541 fl
->n_vlines
= v_lines
;
542 fl
->n_lines
= n_lines
;
544 fl
->h_lines
= malloc_aligned_or_die(sizeof(sparrow_line_t
) * n_lines
);
545 fl
->shuffled_lines
= malloc_aligned_or_die(sizeof(sparrow_line_t
*) * n_lines
);
546 GST_DEBUG("shuffled lines, malloced %p\n", fl
->shuffled_lines
);
547 fl
->map
= zalloc_aligned_or_die(sizeof(sparrow_intersect_t
) * sparrow
->in
.pixcount
);
548 fl
->clusters
= zalloc_or_die(n_corners
* sizeof(sparrow_cluster_t
));
549 fl
->mesh
= zalloc_aligned_or_die(n_corners
* sizeof(sparrow_corner_t
));
551 sparrow_line_t
*line
= fl
->h_lines
;
552 sparrow_line_t
**sline
= fl
->shuffled_lines
;
553 int offset
= LINE_PERIOD
/ 2;
555 for (i
= 0, offset
= LINE_PERIOD
/ 2; offset
< h
;
556 i
++, offset
+= LINE_PERIOD
){
557 line
->offset
= offset
;
558 line
->dir
= SPARROW_HORIZONTAL
;
565 /*now add the vertical lines */
567 for (i
= 0, offset
= LINE_PERIOD
/ 2; offset
< w
;
568 i
++, offset
+= LINE_PERIOD
){
569 line
->offset
= offset
;
570 line
->dir
= SPARROW_VERTICAL
;
576 //DEBUG_FIND_LINES(fl);
578 GST_DEBUG("allocated %d lines, used %d\n", n_lines
, line
- fl
->h_lines
);
581 for (i
= 0; i
< n_lines
- 1; i
++){
582 int j
= RANDINT(sparrow
, 0, n_lines
);
583 sparrow_line_t
*tmp
= fl
->shuffled_lines
[j
];
584 fl
->shuffled_lines
[j
] = fl
->shuffled_lines
[i
];
585 fl
->shuffled_lines
[i
] = tmp
;
588 setup_colour_shifts(sparrow
, fl
);
589 sparrow
->countdown
= sparrow
->lag
+ 2;
590 //DEBUG_FIND_LINES(fl);
592 CvSize size
= {sparrow
->in
.width
, sparrow
->in
.height
};
593 fl
->debug
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);