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
*corners
= fl
->corners
;
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
];
224 sparrow_cluster_t
*cluster
= &clusters
[vline
* height
+ hline
];
228 cluster
->voters
[n
].x
= x
<< SPARROW_FIXED_POINT
;
229 cluster
->voters
[n
].y
= y
<< SPARROW_FIXED_POINT
;
230 cluster
->voters
[n
].signal
= (SIG_WEIGHT
+ p
->signal
[SPARROW_HORIZONTAL
]) *
231 (SIG_WEIGHT
+ p
->signal
[SPARROW_VERTICAL
]);
233 /*these next two could of course be computed from the offset */
236 GST_DEBUG("more than 8 pixels at cluster for corner %d, %d\n",
238 /*if this message ever turns up, replace the weakest signals or add
245 for (y
= 0; y
< height
; y
++){
246 for (x
= 0; x
< width
; x
++, i
++){
248 1. centre of gravity (x,y, weighted average)
249 2. discard outliers? look for connectedness? but if 2 are outliers?
251 sparrow_cluster_t
*cluster
= clusters
+ i
;
252 if (cluster
->n
== 0){
263 for (j
= 0; j
< cluster
->n
; j
++){
264 votes
+= cluster
->voters
[j
].signal
;
265 ysum
+= cluster
->voters
[j
].y
* cluster
->voters
[j
].signal
;
266 xsum
+= cluster
->voters
[j
].x
* cluster
->voters
[j
].signal
;
269 /* don't diminish signal altogether. The previous iteration's means
273 xmean
= xsum
/ votes
;
274 ymean
= ysum
/ votes
;
278 for (j
= 0; j
< cluster
->n
; j
++){
279 int xdiff
= abs(cluster
->voters
[j
].x
- xmean
);
280 int ydiff
= abs(cluster
->voters
[j
].y
- ymean
);
281 devsum
+= xdiff
+ ydiff
;
282 if (xdiff
+ ydiff
> worst
){
283 worst
= xdiff
+ ydiff
;
287 /*a bad outlier has significantly greater than average deviation
288 (but how much is bad? median deviation would be more useful)*/
289 if (worst
> 3 * devsum
/ cluster
->n
){
290 /* reduce the worst ones weight. it is a silly aberration. */
291 cluster
->voters
[worstn
].signal
/= OUTLIER_PENALTY
;
292 GST_DEBUG("dropping outlier at %d,%d (mean %d,%d)\n",
293 cluster
->voters
[worstn
].x
, cluster
->voters
[worstn
].y
, xmean
, ymean
);
298 corners
[i
].out_x
= x
* LINE_PERIOD
;
299 corners
[i
].out_y
= y
* LINE_PERIOD
;
300 corners
[i
].in_x
= xmean
;
301 corners
[i
].in_y
= ymean
;
302 corners
[i
].used
= TRUE
;
303 double div
= (double)(1 << SPARROW_FIXED_POINT
); /*for printf only*/
304 GST_DEBUG("found corner %d (%d,%d) at (%3f, %3f)\n",
305 i
, corners
[i
].out_x
, corners
[i
].out_y
,
306 xmean
/ div
, ymean
/ div
);
309 DEBUG_FIND_LINES(fl
);
310 /* calculate deltas toward adjacent corners */
311 /* try to extrapolate left and up, if possible, so need to go backwards. */
312 for (y
= height
- 2; y
>= 0; y
--){
313 for (x
= width
- 2; x
>= 0; x
--){
315 if (corners
[i
].used
){
316 corners
[i
].dxh
= (corners
[i
+ 1].in_x
- corners
[i
].in_x
) / LINE_PERIOD
;
317 corners
[i
].dyh
= (corners
[i
+ 1].in_y
- corners
[i
].in_y
) / LINE_PERIOD
;
318 corners
[i
].dxv
= (corners
[i
+ width
].in_x
- corners
[i
].in_x
) / LINE_PERIOD
;
319 corners
[i
].dyv
= (corners
[i
+ width
].in_y
- corners
[i
].in_y
) / LINE_PERIOD
;
322 /*prefer copy from left unless it is itself reconstructed,
323 (for no great reason)
324 A mixed copy would be possible and better */
325 if(corners
[i
+ 1].used
&&
326 (corners
[i
+ 1].used
< corners
[i
+ width
].used
)){
327 corners
[i
].dxh
= corners
[i
+ 1].dxh
;
328 corners
[i
].dyh
= corners
[i
+ 1].dyh
;
329 corners
[i
].dxv
= corners
[i
+ 1].dxv
;
330 corners
[i
].dyv
= corners
[i
+ 1].dyv
;
331 corners
[i
].in_x
= corners
[i
+ 1].in_x
- corners
[i
+ 1].dxh
* LINE_PERIOD
;
332 corners
[i
].in_y
= corners
[i
+ 1].in_y
- corners
[i
+ 1].dyh
* LINE_PERIOD
;
333 corners
[i
].out_x
= corners
[i
+ 1].out_x
- 1;
334 corners
[i
].out_y
= corners
[i
+ 1].out_y
;
335 corners
[i
].used
= corners
[i
+ 1].used
+ 1;
337 else if(corners
[i
+ width
].used
){
338 corners
[i
].dxh
= corners
[i
+ width
].dxh
;
339 corners
[i
].dyh
= corners
[i
+ width
].dyh
;
340 corners
[i
].dxv
= corners
[i
+ width
].dxv
;
341 corners
[i
].dyv
= corners
[i
+ width
].dyv
;
342 corners
[i
].in_x
= corners
[i
+ width
].in_x
- corners
[i
+ width
].dxv
* LINE_PERIOD
;
343 corners
[i
].in_y
= corners
[i
+ width
].in_y
- corners
[i
+ width
].dyv
* LINE_PERIOD
;
344 corners
[i
].out_x
= corners
[i
+ width
].out_x
;
345 corners
[i
].out_y
= corners
[i
+ width
].out_y
- 1;
346 corners
[i
].used
= corners
[i
+ width
].used
+ 1;
352 DEBUG_FIND_LINES(fl
);
353 debug_corners_image(sparrow
, fl
);
355 //DEBUG_FIND_LINES(fl);
359 /* With no line drawn (in our colour) look at the background noise. Any real
360 signal has to be stringer than this.
362 XXX looking for simple maximum -- maybe heap or histogram might be better,
363 so as to be less susceptible to wierd outliers (e.g., bad pixels). */
365 look_for_threshold(GstSparrow
*sparrow
, guint8
*in
, sparrow_find_lines_t
*fl
){
366 //DEBUG_FIND_LINES(fl);
370 guint32
*in32
= (guint32
*)in
;
372 for (i
= 0; i
< (int)sparrow
->in
.pixcount
; i
++){
373 colour
= in32
[i
] & sparrow
->colour
;
374 signal
= ((colour
>> fl
->shift1
) +
375 (colour
>> fl
->shift2
)) & 0x1ff;
376 if (signal
> highest
){
380 fl
->threshold
= highest
+ 10;
381 GST_DEBUG("found maximum noise of %d, using threshold %d\n", highest
, fl
->threshold
);
386 look_for_line(GstSparrow
*sparrow
, guint8
*in
, sparrow_find_lines_t
*fl
,
387 sparrow_line_t
*line
){
391 guint32
*in32
= (guint32
*)in
;
392 for (i
= 0; i
< sparrow
->in
.pixcount
; i
++){
393 colour
= in32
[i
] & sparrow
->colour
;
394 signal
= ((colour
>> fl
->shift1
) +
395 (colour
>> fl
->shift2
)) & 0x1ff;
396 if (signal
> fl
->threshold
){
397 if (fl
->map
[i
].lines
[line
->dir
]){
398 GST_DEBUG("HEY, expected point %d to be in line %d (dir %d)"
399 "and thus empty, but it is also in line %d\n",
400 i
, line
->index
, line
->dir
, fl
->map
[i
].lines
[line
->dir
]);
402 fl
->map
[i
].lines
[line
->dir
] = line
->index
;
403 fl
->map
[i
].signal
[line
->dir
] = signal
;
409 debug_map_image(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
410 guint32
*data
= (guint32
*)fl
->debug
->imageData
;
411 memset(data
, 0, sparrow
->in
.size
);
412 for (guint i
= 0; i
< sparrow
->in
.pixcount
; i
++){
413 data
[i
] |= fl
->map
[i
].signal
[SPARROW_HORIZONTAL
] << sparrow
->in
.gshift
;
414 data
[i
] |= fl
->map
[i
].signal
[SPARROW_VERTICAL
] << sparrow
->in
.rshift
;
416 MAYBE_DEBUG_IPL(fl
->debug
);
419 /* draw the line (in sparrow->colour) */
421 draw_line(GstSparrow
* sparrow
, sparrow_line_t
*line
, guint8
*out
){
422 guint32
*p
= (guint32
*)out
;
424 if (line
->dir
== SPARROW_HORIZONTAL
){
425 p
+= line
->offset
* sparrow
->out
.width
;
426 for (i
= 0; i
< sparrow
->out
.width
; i
++){
427 p
[i
] = sparrow
->colour
;
431 guint32
*p
= (guint32
*)out
;
433 for(i
= 0; i
< sparrow
->out
.height
; i
++){
434 *p
= sparrow
->colour
;
435 p
+= sparrow
->out
.width
;
441 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
445 INVISIBLE sparrow_state
446 mode_find_edges(GstSparrow
*sparrow
, guint8
*in
, guint8
*out
){
447 sparrow_find_lines_t
*fl
= (sparrow_find_lines_t
*)sparrow
->helper_struct
;
448 DEBUG_FIND_LINES(fl
);
449 if (fl
->current
== fl
->n_lines
){
452 sparrow_line_t
*line
= fl
->shuffled_lines
[fl
->current
];
454 sparrow
->countdown
--;
455 memset(out
, 0, sparrow
->out
.size
);
456 if (sparrow
->countdown
){
457 /* show the line except on the first round, when we find a threshold*/
459 GST_DEBUG("current %d line %p\n", fl
->current
, line
);
460 draw_line(sparrow
, line
, out
);
464 /*show nothing, look for result */
466 look_for_line(sparrow
, in
, fl
, line
);
470 look_for_threshold(sparrow
, in
, fl
);
472 sparrow
->countdown
= sparrow
->lag
+ 2;
475 debug_map_image(sparrow
, fl
);
477 return SPARROW_STATUS_QUO
;
479 /*match up lines and find corners */
480 find_corners(sparrow
, in
, fl
);
481 corners_to_lut(sparrow
, fl
);
483 return SPARROW_NEXT_STATE
;
487 finalise_find_edges(GstSparrow
*sparrow
){
488 sparrow_find_lines_t
*fl
= (sparrow_find_lines_t
*)sparrow
->helper_struct
;
489 //DEBUG_FIND_LINES(fl);
491 cvReleaseImage(&fl
->debug
);
494 free(fl
->shuffled_lines
);
504 setup_colour_shifts(GstSparrow
*sparrow
, sparrow_find_lines_t
*fl
){
505 //DEBUG_FIND_LINES(fl);
507 switch (sparrow
->colour
){
510 fl
->shift1
= sparrow
->in
.gshift
;
511 fl
->shift2
= sparrow
->in
.gshift
;
513 case SPARROW_MAGENTA
:
514 fl
->shift1
= sparrow
->in
.rshift
;
515 fl
->shift2
= sparrow
->in
.bshift
;
521 init_find_edges(GstSparrow
*sparrow
){
522 gint32 w
= sparrow
->out
.width
;
523 gint32 h
= sparrow
->out
.height
;
525 sparrow_find_lines_t
*fl
= zalloc_aligned_or_die(sizeof(sparrow_find_lines_t
));
526 sparrow
->helper_struct
= (void *)fl
;
528 gint h_lines
= (h
+ LINE_PERIOD
- 1) / LINE_PERIOD
;
529 gint v_lines
= (w
+ LINE_PERIOD
- 1) / LINE_PERIOD
;
530 gint n_lines
= (h_lines
+ v_lines
);
531 gint n_corners
= (h_lines
* v_lines
);
532 fl
->n_hlines
= h_lines
;
533 fl
->n_vlines
= v_lines
;
534 fl
->n_lines
= n_lines
;
536 fl
->h_lines
= malloc_aligned_or_die(sizeof(sparrow_line_t
) * n_lines
);
537 fl
->shuffled_lines
= malloc_aligned_or_die(sizeof(sparrow_line_t
*) * n_lines
);
538 GST_DEBUG("shuffled lines, malloced %p\n", fl
->shuffled_lines
);
539 fl
->map
= zalloc_aligned_or_die(sizeof(sparrow_intersect_t
) * sparrow
->in
.pixcount
);
540 fl
->clusters
= zalloc_or_die(n_corners
* sizeof(sparrow_cluster_t
));
541 fl
->corners
= zalloc_aligned_or_die(n_corners
* sizeof(sparrow_corner_t
));
542 fl
->mesh
= malloc_aligned_or_die(sizeof(sparrow_corner_t
) * h_lines
* v_lines
);
544 sparrow_line_t
*line
= fl
->h_lines
;
545 sparrow_line_t
**sline
= fl
->shuffled_lines
;
546 int offset
= LINE_PERIOD
/ 2;
548 for (i
= 0, offset
= LINE_PERIOD
/ 2; offset
< h
;
549 i
++, offset
+= LINE_PERIOD
){
550 line
->offset
= offset
;
551 line
->dir
= SPARROW_HORIZONTAL
;
558 /*now add the vertical lines */
560 for (i
= 0, offset
= LINE_PERIOD
/ 2; offset
< w
;
561 i
++, offset
+= LINE_PERIOD
){
562 line
->offset
= offset
;
563 line
->dir
= SPARROW_VERTICAL
;
569 //DEBUG_FIND_LINES(fl);
571 GST_DEBUG("allocated %d lines, used %d\n", n_lines
, line
- fl
->h_lines
);
574 for (i
= 0; i
< n_lines
- 1; i
++){
575 int j
= RANDINT(sparrow
, 0, n_lines
);
576 sparrow_line_t
*tmp
= fl
->shuffled_lines
[j
];
577 fl
->shuffled_lines
[j
] = fl
->shuffled_lines
[i
];
578 fl
->shuffled_lines
[i
] = tmp
;
581 setup_colour_shifts(sparrow
, fl
);
582 sparrow
->countdown
= sparrow
->lag
+ 2;
583 //DEBUG_FIND_LINES(fl);
585 CvSize size
= {sparrow
->in
.width
, sparrow
->in
.height
};
586 fl
->debug
= cvCreateImage(size
, IPL_DEPTH_8U
, PIXSIZE
);