mark bad pixels in debug images (blue)
[sparrow.git] / edges.c
blob3f437f3b45820b739a460138b46527b1a556c37e
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.
20 #include "sparrow.h"
21 #include "gstsparrow.h"
22 #include "edges.h"
24 #include <string.h>
25 #include <math.h>
26 #include <unistd.h>
28 #include "cv.h"
31 static void dump_edges_info(GstSparrow *sparrow, sparrow_find_lines_t *fl, const char *filename){
32 GST_DEBUG("about to save to %s\n", filename);
33 FILE *f = fopen(filename, "w");
34 sparrow_fl_condensed_t condensed;
35 condensed.n_vlines = fl->n_vlines;
36 condensed.n_hlines = fl->n_hlines;
38 /* simply write fl, map, clusters and mesh in sequence */
39 GST_DEBUG("fl is %p, file is %p\n", fl, f);
40 GST_DEBUG("fl: %d x %d\n", sizeof(sparrow_find_lines_t), 1);
41 fwrite(&condensed, sizeof(sparrow_fl_condensed_t), 1, f);
42 GST_DEBUG("fl->map %d x %d\n", sizeof(sparrow_intersect_t), sparrow->in.pixcount);
43 fwrite(fl->map, sizeof(sparrow_intersect_t), sparrow->in.pixcount, f);
44 GST_DEBUG("fl->clusters %d x %d\n", sizeof(sparrow_cluster_t), fl->n_hlines * fl->n_vlines);
45 fwrite(fl->clusters, sizeof(sparrow_cluster_t), fl->n_hlines * fl->n_vlines, f);
46 GST_DEBUG("fl->mesh %d x %d\n", sizeof(sparrow_corner_t), fl->n_hlines * fl->n_vlines);
47 fwrite(fl->mesh, sizeof(sparrow_corner_t), fl->n_hlines * fl->n_vlines, f);
48 /*and write the mask too */
49 GST_DEBUG("sparrow->screenmask\n");
50 fwrite(sparrow->screenmask, 1, sparrow->out.pixcount, f);
51 fclose(f);
54 static void read_edges_info(GstSparrow *sparrow, sparrow_find_lines_t *fl, const char *filename){
55 FILE *f = fopen(filename, "r");
56 sparrow_fl_condensed_t condensed;
57 size_t read = fread(&condensed, sizeof(sparrow_fl_condensed_t), 1, f);
58 assert(condensed.n_hlines == fl->n_hlines);
59 assert(condensed.n_vlines == fl->n_vlines);
61 guint n_corners = fl->n_hlines * fl->n_vlines;
62 read += fread(fl->map, sizeof(sparrow_intersect_t), sparrow->in.pixcount, f);
63 read += fread(fl->clusters, sizeof(sparrow_cluster_t), n_corners, f);
64 read += fread(fl->mesh, sizeof(sparrow_corner_t), n_corners, f);
65 read += fread(sparrow->screenmask, 1, sparrow->out.pixcount, f);
66 fclose(f);
71 static void
72 debug_lut(GstSparrow *sparrow, sparrow_find_lines_t *fl){
75 #define OFFSET(x, y, w)((((y) * (w)) >> SPARROW_FIXED_POINT) + ((x) >> SPARROW_FIXED_POINT))
77 #define QUANTISE_DELTA(d)(((d) + LINE_PERIOD / 2) / LINE_PERIOD)
79 /*tolerate up to 1/8 of a pixel drift */
80 #define MAX_DRIFT (1 << (SPARROW_FIXED_POINT - 3))
83 static inline sparrow_map_path_t*
84 possibly_new_point(sparrow_map_path_t *p, int dx, int dy){
85 if (dx != p->dx && dy != p->dy){
86 p++;
87 p->dx = dx;
88 p->dy = dy;
89 p->n = 0;
91 return p;
94 static void UNUSED
95 corners_to_lut(GstSparrow *sparrow, sparrow_find_lines_t *fl){
96 //DEBUG_FIND_LINES(fl);
97 sparrow_map_t *map = &sparrow->map; /*rows in sparrow->out */
98 guint8 *mask = sparrow->screenmask; /*mask in sparrow->in */
99 sparrow_corner_t *mesh = fl->mesh; /*maps regular points in ->out to points in ->in */
100 int mesh_w = fl->n_vlines;
101 int mesh_h = fl->n_hlines;
102 int in_w = sparrow->in.width;
103 int mcy, mmy, mcx; /*Mesh Corner|Modulus X|Y*/
105 int y;
106 sparrow_corner_t *mesh_row = mesh;
108 for (y = 0; y < H_LINE_OFFSET; y++){
109 map->rows[y].start = 0;
110 map->rows[y].end = 0;
111 map->rows[y].points = NULL;
114 sparrow_map_row_t *row = map->rows + H_LINE_OFFSET;
115 row->points = map->point_mem;
116 sparrow_map_path_t *p = row->points;
118 for(mcy = 0; mcy < mesh_h - 1; mcy++){ /* for each mesh row */
119 for (mmy = 0; mmy < LINE_PERIOD; mmy++){ /* for each output line */
120 int ix, iy; /* input x, y at mesh points, interpolated vertically */
121 int rx, ry; /* running x, y; approximates ix, iy */
122 int dx, dy;
123 int on = 0;
124 sparrow_corner_t *mesh_square = mesh_row;
125 GST_DEBUG("row %p, y %d, row offset %d\n", row, y, row - map->rows);
126 y++;
127 row->points = NULL;
128 row->start = 0;
129 row->end = 0;
130 for(mcx = 0; mcx < mesh_w - 1; mcx++){
131 /*for each mesh block except the last, which has no dx,dy.
132 Thus the mesh blocks are referenced in LINE_PERIOD passes.*/
133 if (mesh_square->status == CORNER_UNUSED){
134 if (! on){
135 mesh_square++;
136 continue;
138 /*lordy! continue with previous deltas*/
139 ix = rx;
140 iy = ry;
142 else {
143 /* starting point for this row in this block. */
144 iy = mesh_square->in_y + mmy * (mesh_square->dyd / 1);
145 ix = mesh_square->in_x + mmy * (mesh_square->dxd / 1);
146 /*incremental delta going left to right in this block */
147 dy = (mesh_square->dyr / 1);
148 dx = (mesh_square->dxr / 1);
151 /*index of the last point in this block
152 NB: calculating from ix, iy, which may differ slightly from rx, ry*/
153 int lasti = OFFSET(
154 ix + (LINE_PERIOD - 1) * dx,
155 iy + (LINE_PERIOD - 1) * dy,
156 in_w);
157 //GST_DEBUG("lasti is %d, ix %d, iy %d\n", lasti, ix, iy);
158 if (! on){
159 if (! mask[lasti]){
160 /*it doesn't turn on within this block (or it is of ignorably
161 short length). */
162 mesh_square++;
163 continue;
165 /*it does turn on. so step through and find it. This happens once
166 per line.*/
167 rx = ix;
168 ry = iy;
169 int j;
170 for (j = 0; j < LINE_PERIOD; j++){
171 if (mask[OFFSET(rx, ry, in_w)]){
172 break;
174 rx += dx;
175 ry += dy;
177 row->start = mcx * LINE_PERIOD + j;
178 row->in_x = rx;
179 row->in_y = ry;
180 p = possibly_new_point(p, dx, dy);
181 row->points = p;
182 p->n = LINE_PERIOD - j;
183 on = 1;
184 mesh_square++;
185 continue;
187 /*it is on. */
188 /*maybe rx, ry are drifting badly, in which case, we need to recalculate dx, dy*/
189 if (abs(rx - ix) > MAX_DRIFT ||
190 abs(ry - iy) > MAX_DRIFT){
191 int y = mcy * LINE_PERIOD + mmy;
192 int x = mcx * LINE_PERIOD;
193 GST_DEBUG("output point %d %d, rx, ry %d, %d have got %d, %d away from target %d, %d."
194 " dx, dy is %d, %d\n",
195 x, y, rx, ry, rx - ix, ry - iy, ix, iy, dx, dy);
196 sparrow_corner_t *next = mesh_square + 1;
197 if(next->status != CORNER_UNUSED){
198 int niy = next->in_y + mmy * (next->dyd / 1);
199 int nix = next->in_x + mmy * (next->dxd / 1);
200 dx = QUANTISE_DELTA(nix - ix);
201 dy = QUANTISE_DELTA(niy - iy);
202 GST_DEBUG("new dx, dy is %d, %d\n", dx, dy);
204 else{
205 GST_DEBUG("next corner is UNUSED. dx, dy unchanged\n");
209 /*Probably dx/dy are different, so we need a new point */
210 p = possibly_new_point(p, dx, dy);
212 /*does it end it this one? */
213 if (! mask[lasti]){
214 int j;
215 for (j = 0; j < LINE_PERIOD; j++){
216 if (! mask[OFFSET(rx, ry, in_w)]){
217 break;
219 rx += dx;
220 ry += dy;
222 p->n += j;
223 row->end = mcx * LINE_PERIOD + j;
224 /*this row is done! */
225 break;
227 p->n += LINE_PERIOD;
228 rx += LINE_PERIOD * dx;
229 ry += LINE_PERIOD * dy;
230 mesh_square++;
232 row++;
234 mesh_row += mesh_w;
237 /*blank lines for the last few */
238 for (y = sparrow->out.height - H_LINE_OFFSET; y < sparrow->out.height; y++){
239 map->rows[y].start = 0;
240 map->rows[y].end = 0;
241 map->rows[y].points = NULL;
244 debug_lut(sparrow, fl);
247 UNUSED static void
248 corners_to_full_lut(GstSparrow *sparrow, sparrow_find_lines_t *fl){
249 DEBUG_FIND_LINES(fl);
250 sparrow_corner_t *mesh = fl->mesh; /*maps regular points in ->out to points in ->in */
251 sparrow_map_lut_t *map_lut = sparrow->map_lut;
252 int mesh_w = fl->n_vlines;
253 int mesh_h = fl->n_hlines;
254 int mcy, mmy, mcx, mmx; /*Mesh Corner|Modulus X|Y*/
255 int y = H_LINE_OFFSET;
256 sparrow_corner_t *mesh_row = mesh;
257 for(mcy = 0; mcy < mesh_h - 1; mcy++){
258 for (mmy = 0; mmy < LINE_PERIOD; mmy++, y++){
259 sparrow_corner_t *mesh_square = mesh_row;
260 int i = y * sparrow->out.width + V_LINE_OFFSET;
261 for(mcx = 0; mcx < mesh_w - 1; mcx++){
262 int iy = mesh_square->in_y + mmy * mesh_square->dyd;
263 int ix = mesh_square->in_x + mmy * mesh_square->dxd;
264 for (mmx = 0; mmx < LINE_PERIOD; mmx++, i++){
265 map_lut[i].x = MAX(ix >> SPARROW_FP_2_LUT, 0);
266 map_lut[i].y = MAX(iy >> SPARROW_FP_2_LUT, 0);
267 //GST_DEBUG("ix, iy %d, %d x, y %d, %d i: %d, y%d\n", ix, iy,
268 // map_lut[i].x, map_lut[i].y, i, y);
270 ix += mesh_square->dxr;
271 iy += mesh_square->dyr;
273 mesh_square++;
276 mesh_row += mesh_w;
278 sparrow->map_lut = map_lut;
281 #define INTXY(x)((x) / (1 << SPARROW_FIXED_POINT))
282 #define FLOATXY(x)(((double)(x)) / (1 << SPARROW_FIXED_POINT))
283 static void
284 debug_corners_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
285 sparrow_corner_t *mesh = fl->mesh;
286 guint32 *data = (guint32*)fl->debug->imageData;
287 guint w = fl->debug->width;
288 memset(data, 0, sparrow->in.size);
289 guint32 colours[4] = {0xff0000ff, 0x00ff0000, 0x0000ff00, 0xcccccccc};
290 for (int i = 0; i < fl->n_vlines * fl->n_hlines; i++){
291 sparrow_corner_t *c = &mesh[i];
292 int x = c->in_x;
293 int y = c->in_y;
294 GST_DEBUG("i %d status %d x: %f, y: %f dxr %f dyr %f dxd %f dyd %f\n"
295 "int x, y %d,%d (raw %d,%d) data %p\n",
296 i, c->status, FLOATXY(x), FLOATXY(y),
297 FLOATXY(c->dxr), FLOATXY(c->dyr), FLOATXY(c->dxd), FLOATXY(c->dyd),
298 INTXY(x), INTXY(y), x, y, data);
299 int txr = x;
300 int txd = x;
301 int tyr = y;
302 int tyd = y;
303 for (int j = 1; j < LINE_PERIOD; j+= 2){
304 txr += c->dxr * 2;
305 txd += c->dxd * 2;
306 tyr += c->dyr * 2;
307 tyd += c->dyd * 2;
308 data[INTXY(tyr) * w + INTXY(txr)] = 0x000088;
309 data[INTXY(tyd) * w + INTXY(txd)] = 0x663300;
311 data[INTXY(y) * w + INTXY(x)] = colours[c->status];
313 MAYBE_DEBUG_IPL(fl->debug);
317 static void
318 debug_clusters(GstSparrow *sparrow, sparrow_find_lines_t *fl){
319 guint32 *data = (guint32*)fl->debug->imageData;
320 memset(data, 0, sparrow->in.size);
321 int width = fl->n_vlines;
322 int height = fl->n_hlines;
323 sparrow_cluster_t *clusters = fl->clusters;
324 int i, j;
325 guint32 colour;
326 guint32 colours[4] = {0xff0000ff, 0x0000ff00, 0x00ff0000,
327 0x00ff00ff};
328 for (i = 0; i < width * height; i++){
329 colour = colours[i % 5];
330 sparrow_voter_t *v = clusters[i].voters;
331 for (j = 0; j < clusters[i].n; j++){
332 data[v[j].y * sparrow->in.width +
333 v[j].x] = (colour * (v[j].signal / 2)) / 256;
336 MAYBE_DEBUG_IPL(fl->debug);
340 #define SIGNAL_QUANT 1
342 /*maximum number of pixels in a cluster */
343 #define CLUSTER_SIZE 8
346 /*create the mesh */
347 static void
348 make_clusters(GstSparrow *sparrow, sparrow_find_lines_t *fl){
349 sparrow_cluster_t *clusters = fl->clusters;
350 int x, y;
351 /*special case: spurious values collect up at 0,0 */
352 fl->map[0].signal[SPARROW_VERTICAL] = 0;
353 fl->map[0].signal[SPARROW_HORIZONTAL] = 0;
354 /*each point in fl->map is in a vertical line, a horizontal line, both, or
355 neither. Only the "both" case matters. */
356 for (y = 0; y < sparrow->in.height; y++){
357 for (x = 0; x < sparrow->in.width; x++){
358 sparrow_intersect_t *p = &fl->map[y * sparrow->in.width + x];
359 guint vsig = p->signal[SPARROW_VERTICAL];
360 guint hsig = p->signal[SPARROW_HORIZONTAL];
361 /*remembering that 0 is valid as a line number, but not as a signal */
362 if (! (vsig && hsig)){
363 continue;
365 /*This one is lobbying for the position of a corner.*/
366 int vline = p->lines[SPARROW_VERTICAL];
367 int hline = p->lines[SPARROW_HORIZONTAL];
368 if (vline == BAD_PIXEL || hline == BAD_PIXEL){
369 GST_DEBUG("ignoring bad pixel %d, %d\n", x, y);
370 continue;
372 sparrow_cluster_t *cluster = &clusters[hline * fl->n_vlines + vline];
373 sparrow_voter_t *voters = cluster->voters;
374 int n = cluster->n;
375 guint signal = (vsig * hsig) / SIGNAL_QUANT;
376 GST_DEBUG("signal at %p (%d, %d): %dv %dh, product %u, lines: %dv %dh\n"
377 "cluster is %p, n is %d\n", p, x, y,
378 vsig, hsig, signal, vline, hline, cluster, n);
379 if (signal == 0){
380 GST_WARNING("signal at %p (%d, %d) is %d following quantisation!\n",
381 p, x, y, signal);
384 if (n < CLUSTER_SIZE){
385 voters[n].x = x;
386 voters[n].y = y;
387 voters[n].signal = signal;
388 cluster->n++;
390 else {
391 /*duplicate x, y, signal, so they aren't mucked up */
392 guint ts = signal;
393 int tx = x;
394 int ty = y;
395 /*replaced one ends up here */
396 int ts2;
397 int tx2;
398 int ty2;
399 for (int j = 0; j < CLUSTER_SIZE; j++){
400 if (voters[j].signal < ts){
401 ts2 = voters[j].signal;
402 tx2 = voters[j].x;
403 ty2 = voters[j].y;
404 voters[j].signal = ts;
405 voters[j].x = tx;
406 voters[j].y = ty;
407 ts = ts2;
408 tx = tx2;
409 ty = ty2;
412 GST_DEBUG("more than %d pixels at cluster for corner %d, %d."
413 "Dropped %u for %u\n",
414 CLUSTER_SIZE, vline, hline, ts2, signal);
418 if (sparrow->debug){
419 debug_clusters(sparrow, fl);
424 static inline void
425 drop_cluster_voter(sparrow_cluster_t *cluster, int n)
427 if (n < cluster->n){
428 for (int i = n; i < cluster->n - 1; i++){
429 cluster->voters[i] = cluster->voters[i + 1];
431 cluster->n--;
435 static inline int sort_median(int *a, guint n)
437 guint i, j;
438 /*stupid sort, but n is very small*/
439 for (i = 0; i < n; i++){
440 for (j = i + 1; j < n; j++){
441 if (a[i] > a[j]){
442 int tmp = a[j];
443 a[j] = a[i];
444 a[i] = tmp;
448 guint middle = n / 2;
449 int answer = a[middle];
451 if ((n & 1) == 0){
452 answer += a[middle - 1];
453 answer /= 2;
455 return answer;
458 #define EUCLIDEAN_D2(ax, ay, bx, by)((ax - bx) * (ax - bx) + (ay - by) * (ay - by))
459 #define NEIGHBOURLY_THRESHOLD (LINE_PERIOD * 4)
461 static inline void
462 neighbourly_discard_cluster_outliers(GstSparrow *sparrow, sparrow_cluster_t *cluster,
463 sparrow_corner_t *neighbour)
465 /* assuming the output mesh entirely fits in the input window (which is
466 required for sparrow to work) the neighbours should be at most
467 LINE_PERIOD * input resolution / output resolution apart. But set the
468 threshold higher, just in case. */
469 const int threshold = NEIGHBOURLY_THRESHOLD * sparrow->in.height / sparrow->out.height;
470 int i;
471 int neighbour_d[CLUSTER_SIZE];
472 int close = 0;
473 for (i = 0; i < cluster->n; i++){
474 int d = EUCLIDEAN_D2(neighbour->in_x, neighbour->in_y,
475 cluster->voters[i].x, cluster->voters[i].y);
476 int pass = d > threshold;
477 neighbour_d[i] = pass;
478 close += pass;
479 GST_DEBUG("failing point %d, distance sq %d, threshold %d\n", i, d, threshold);
481 if (close > 1){
482 for (i = 0; i < cluster->n; i++){
483 if (! neighbour_d[i]){
484 drop_cluster_voter(cluster, i);
490 static inline void
491 median_discard_cluster_outliers(sparrow_cluster_t *cluster)
493 int xvals[CLUSTER_SIZE];
494 int yvals[CLUSTER_SIZE];
495 int i;
496 for (i = 0; i < cluster->n; i++){
497 /*XXX could sort here*/
498 xvals[i] = cluster->voters[i].x;
499 yvals[i] = cluster->voters[i].y;
501 const int xmed = sort_median(xvals, cluster->n);
502 const int ymed = sort_median(yvals, cluster->n);
504 for (i = 0; i < cluster->n; i++){
505 int dx = cluster->voters[i].x - xmed;
506 int dy = cluster->voters[i].y - ymed;
507 if (dx * dx + dy * dy > OUTLIER_THRESHOLD){
508 drop_cluster_voter(cluster, i);
513 /*create the mesh */
514 static inline void
515 make_corners(GstSparrow *sparrow, sparrow_find_lines_t *fl){
516 //DEBUG_FIND_LINES(fl);
517 int width = fl->n_vlines;
518 int height = fl->n_hlines;
519 sparrow_cluster_t *clusters = fl->clusters;
520 sparrow_corner_t *mesh = fl->mesh;
521 int x, y, i;
523 i = 0;
524 for (y = 0; y < height; y++){
525 for (x = 0; x < width; x++, i++){
526 sparrow_cluster_t *cluster = clusters + i;
527 if (cluster->n == 0){
528 continue;
531 /*the good points should all be adjacent; distant ones are spurious, so
532 are discarded.
534 This fails if all the cluster are way off. Obviously it would be good
535 to include information about the grid in the decision, but that is not
536 there yet. (needs iteration, really).
538 Here's a slight attempt:*/
539 #if 0
540 sparrow_corner_t *neighbour;
541 if (x){
542 neighbour = &mesh[i - 1];
543 neighbourly_discard_cluster_outliers(sparrow, cluster, neighbour);
545 else if (y){
546 neighbour = &mesh[i - width];
547 neighbourly_discard_cluster_outliers(sparrow, cluster, neighbour);
549 #endif
550 median_discard_cluster_outliers(cluster);
552 /* now find a weighted average position */
553 /*64 bit to avoid overflow -- should probably just use floating point
554 (or reduce signal)*/
555 guint64 xsum, ysum;
556 guint xmean, ymean;
557 guint64 votes;
558 int j;
559 xsum = 0;
560 ysum = 0;
561 votes = 0;
562 for (j = 0; j < cluster->n; j++){
563 votes += cluster->voters[j].signal;
564 ysum += cluster->voters[j].y * cluster->voters[j].signal;
565 xsum += cluster->voters[j].x * cluster->voters[j].signal;
567 if (votes){
568 xmean = (xsum << SPARROW_FIXED_POINT) / votes;
569 ymean = (ysum << SPARROW_FIXED_POINT) / votes;
571 else {
572 GST_WARNING("corner %d, %d voters, sum %d,%d, somehow has no votes\n",
573 i, cluster->n, xsum, ysum);
576 GST_DEBUG("corner %d: %d voters, %d votes, sum %d,%d, mean %d,%d\n",
577 i, cluster->n, votes, xsum, ysum, xmean, ymean);
579 mesh[i].in_x = xmean;
580 mesh[i].in_y = ymean;
581 mesh[i].status = CORNER_EXACT;
582 GST_DEBUG("found corner %d at (%3f, %3f)\n",
583 i, FLOATXY(xmean), FLOATXY(ymean));
589 static inline void
590 make_map(GstSparrow *sparrow, sparrow_find_lines_t *fl){
591 int i;
592 int width = fl->n_vlines;
593 int height = fl->n_hlines;
594 sparrow_corner_t *mesh = fl->mesh;
595 gint x, y;
597 DEBUG_FIND_LINES(fl);
598 /* calculate deltas toward adjacent corners */
599 /* try to extrapolate left and up, if possible, so need to go backwards. */
600 i = width * height - 1;
601 for (y = height - 1; y >= 0; y--){
602 for (x = width - 1; x >= 0; x--, i--){
603 sparrow_corner_t *corner = &mesh[i];
604 /* calculate the delta to next corner. If this corner is on edge, delta is
605 0 and next is this.*/
606 sparrow_corner_t *right = (x == width - 1) ? corner : corner + 1;
607 sparrow_corner_t *down = (y == height - 1) ? corner : corner + width;
608 GST_DEBUG("i %d xy %d,%d width %d. in_xy %d,%d; down in_xy %d,%d; right in_xy %d,%d\n",
609 i, x, y, width, corner->in_x, corner->in_y, down->in_x,
610 down->in_y, right->in_x, right->in_y);
611 if (corner->status != CORNER_UNUSED){
612 corner->dxr = (right->in_x - corner->in_x);
613 corner->dyr = (right->in_y - corner->in_y);
614 corner->dxd = (down->in_x - corner->in_x);
615 corner->dyd = (down->in_y - corner->in_y);
617 else {
618 /*copy from both right and down, if they both exist. */
619 struct {
620 int dxr;
621 int dyr;
622 int dxd;
623 int dyd;
624 } dividends = {0, 0, 0, 0};
625 struct {
626 int r;
627 int d;
628 } divisors = {0, 0};
630 if (right != corner){
631 if (right->dxr || right->dyr){
632 dividends.dxr += right->dxr;
633 dividends.dyr += right->dyr;
634 divisors.r++;
636 if (right->dxd || right->dyd){
637 dividends.dxd += right->dxd;
638 dividends.dyd += right->dyd;
639 divisors.d++;
642 if (down != corner){
643 if (down->dxr || down->dyr){
644 dividends.dxr += down->dxr;
645 dividends.dyr += down->dyr;
646 divisors.r++;
648 if (down->dxd || down->dyd){
649 dividends.dxd += down->dxd;
650 dividends.dyd += down->dyd;
651 divisors.d++;
654 corner->dxr = divisors.r ? dividends.dxr / divisors.r : 0;
655 corner->dyr = divisors.r ? dividends.dyr / divisors.r : 0;
656 corner->dxd = divisors.d ? dividends.dxd / divisors.d : 0;
657 corner->dyd = divisors.d ? dividends.dyd / divisors.d : 0;
659 /*now extrapolate position, preferably from both left and right */
660 if (right == corner){
661 if (down != corner){ /*use down only */
662 corner->in_x = down->in_x - corner->dxd;
663 corner->in_y = down->in_y - corner->dyd;
665 else {/*oh no*/
666 GST_DEBUG("can't reconstruct corner %d, %d: no useable neighbours\n", x, y);
667 /*it would be easy enough to look further, but hopefully of no
668 practical use */
671 else if (down == corner){ /*use right only */
672 corner->in_x = right->in_x - corner->dxr;
673 corner->in_y = right->in_y - corner->dyr;
675 else { /* use both */
676 corner->in_x = right->in_x - corner->dxr;
677 corner->in_y = right->in_y - corner->dyr;
678 corner->in_x += down->in_x - corner->dxd;
679 corner->in_y += down->in_y - corner->dyd;
680 corner->in_x >>= 1;
681 corner->in_y >>= 1;
683 corner->status = CORNER_PROJECTED;
687 /*now quantise delta values. It would be wrong to do it earlier, when they
688 are being used to calculate whole mesh jumps, but from now they are
689 primarily going to used for pixel (mesh / LINE_PERIOD) jumps. To do this in
690 corners_to_lut puts a whole lot of division in a tight loop.*/
691 for (i = 0; i < width * height; i++){
692 sparrow_corner_t *corner = &mesh[i];
693 corner->dxr = QUANTISE_DELTA(corner->dxr);
694 corner->dyr = QUANTISE_DELTA(corner->dyr);
695 corner->dxd = QUANTISE_DELTA(corner->dxd);
696 corner->dyd = QUANTISE_DELTA(corner->dyd);
698 DEBUG_FIND_LINES(fl);
699 if (sparrow->debug){
700 debug_corners_image(sparrow, fl);
706 static void
707 look_for_line(GstSparrow *sparrow, guint8 *in, sparrow_find_lines_t *fl,
708 sparrow_line_t *line){
709 guint i;
710 guint32 colour;
711 guint32 cmask = sparrow->out.colours[sparrow->colour];
712 int signal;
714 /* subtract background noise */
715 fl->input->imageData = (char *)in;
716 cvSub(fl->input, fl->threshold, fl->working, NULL);
717 guint32 *in32 = (guint32 *)fl->working->imageData;
719 for (i = 0; i < sparrow->in.pixcount; i++){
720 colour = in32[i] & cmask;
721 signal = (((colour >> fl->shift1) +
722 (colour >> fl->shift2))) & 0xff;
723 if (signal){
724 if (fl->map[i].lines[line->dir]){
725 /*assume the pixel is on for everyone and will just confuse
726 matters. ignore it.
727 XXX maybe threshold, so if signal is hugely bigger in one, don't ban it.
729 if (fl->map[i].lines[line->dir] != BAD_PIXEL){
730 GST_DEBUG("HEY, expected point %d to be in line %d (dir %d) "
731 "and thus empty, but it is also in line %d\n"
732 "old signal %d, new signal %d, marking as BAD\n",
733 i, line->index, line->dir, fl->map[i].lines[line->dir],
734 fl->map[i].signal[line->dir], signal);
735 fl->map[i].lines[line->dir] = BAD_PIXEL;
736 fl->map[i].signal[line->dir] = 0;
739 else{
740 fl->map[i].lines[line->dir] = line->index;
741 fl->map[i].signal[line->dir] = signal;
747 static void
748 debug_map_image(GstSparrow *sparrow, sparrow_find_lines_t *fl){
749 guint32 *data = (guint32*)fl->debug->imageData;
750 memset(data, 0, sparrow->in.size);
751 for (guint i = 0; i < sparrow->in.pixcount; i++){
752 data[i] |= fl->map[i].signal[SPARROW_HORIZONTAL] << sparrow->in.gshift;
753 data[i] |= fl->map[i].signal[SPARROW_VERTICAL] << sparrow->in.rshift;
754 data[i] |= ((fl->map[i].lines[SPARROW_VERTICAL] == BAD_PIXEL) ||
755 (fl->map[i].lines[SPARROW_HORIZONTAL] == BAD_PIXEL)) ? 255 << sparrow->in.bshift : 0;
757 MAYBE_DEBUG_IPL(fl->debug);
760 /* draw the line (in sparrow->colour) */
761 static inline void
762 draw_line(GstSparrow * sparrow, sparrow_line_t *line, guint8 *out){
763 guint32 *p = (guint32 *)out;
764 guint32 colour = sparrow->out.colours[sparrow->colour];
765 int i;
766 if (line->dir == SPARROW_HORIZONTAL){
767 p += line->offset * sparrow->out.width;
768 for (i = 0; i < sparrow->out.width; i++){
769 p[i] = colour;
772 else {
773 guint32 *p = (guint32 *)out;
774 p += line->offset;
775 for(i = 0; i < sparrow->out.height; i++){
776 *p = colour;
777 p += sparrow->out.width;
782 static void
783 jump_state(GstSparrow *sparrow, sparrow_find_lines_t *fl, edges_state_t state){
784 if (state == EDGES_NEXT_STATE){
785 fl->state++;
787 else {
788 fl->state = state;
790 switch (fl->state){
791 case EDGES_FIND_NOISE:
792 sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
793 break;
794 case EDGES_FIND_LINES:
795 sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
796 break;
797 case EDGES_FIND_CORNERS:
798 sparrow->countdown = 4;
799 break;
800 default:
801 GST_DEBUG("jumped to non-existent state %d\n", fl->state);
802 break;
806 /* show each line for 2 frames, then wait sparrow->lag frames, leaving line on
807 until last one.
809 static inline void
810 draw_lines(GstSparrow *sparrow, sparrow_find_lines_t *fl, guint8 *in, guint8 *out)
812 sparrow_line_t *line = fl->shuffled_lines[fl->current];
813 sparrow->countdown--;
814 memset(out, 0, sparrow->out.size);
815 if (sparrow->countdown){
816 draw_line(sparrow, line, out);
818 else{
819 /*show nothing, look for result */
820 look_for_line(sparrow, in, fl, line);
821 if (sparrow->debug){
822 debug_map_image(sparrow, fl);
824 fl->current++;
825 if (fl->current == fl->n_lines){
826 jump_state(sparrow, fl, EDGES_NEXT_STATE);
828 else{
829 sparrow->countdown = MAX(sparrow->lag, 1) + SAFETY_LAG;
834 #define LINE_THRESHOLD 32
836 static inline void
837 find_threshold(GstSparrow *sparrow, sparrow_find_lines_t *fl, guint8 *in, guint8 *out)
839 memset(out, 0, sparrow->out.size);
840 /*XXX should average/median over a range of frames */
841 if (sparrow->countdown == 0){
842 memcpy(fl->threshold->imageData, in, sparrow->in.size);
843 /*add a constant, and smooth */
844 cvAddS(fl->threshold, cvScalarAll(LINE_THRESHOLD), fl->working, NULL);
845 cvSmooth(fl->working, fl->threshold, CV_GAUSSIAN, 3, 0, 0, 0);
846 //cvSmooth(fl->working, fl->threshold, CV_MEDIAN, 3, 0, 0, 0);
847 jump_state(sparrow, fl, EDGES_NEXT_STATE);
849 sparrow->countdown--;
852 /*match up lines and find corners */
853 static inline int
854 find_corners(GstSparrow *sparrow, sparrow_find_lines_t *fl)
856 sparrow->countdown--;
857 switch(sparrow->countdown){
858 case 3:
859 make_clusters(sparrow, fl);
860 break;
861 case 2:
862 make_corners(sparrow, fl);
863 break;
864 case 1:
865 make_map(sparrow, fl);
866 break;
867 case 0:
868 #if USE_FULL_LUT
869 corners_to_full_lut(sparrow, fl);
870 #else
871 corners_to_lut(sparrow, fl);
872 #endif
873 break;
874 default:
875 GST_DEBUG("how did sparrow->countdown get to be %d?", sparrow->countdown);
876 sparrow->countdown = 4;
878 return sparrow->countdown;
882 INVISIBLE sparrow_state
883 mode_find_edges(GstSparrow *sparrow, guint8 *in, guint8 *out){
884 sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
885 switch (fl->state){
886 case EDGES_FIND_NOISE:
887 find_threshold(sparrow, fl, in, out);
888 break;
889 case EDGES_FIND_LINES:
890 draw_lines(sparrow, fl, in, out);
891 break;
892 case EDGES_FIND_CORNERS:
893 if (find_corners(sparrow, fl))
894 break;
895 return SPARROW_NEXT_STATE;
896 case EDGES_NEXT_STATE:
897 break; /*shush gcc */
899 return SPARROW_STATUS_QUO;
902 INVISIBLE void
903 finalise_find_edges(GstSparrow *sparrow){
904 sparrow_find_lines_t *fl = (sparrow_find_lines_t *)sparrow->helper_struct;
905 //DEBUG_FIND_LINES(fl);
906 if (sparrow->save && *(sparrow->save)){
907 GST_DEBUG("about to save to %s\n", sparrow->save);
908 dump_edges_info(sparrow, fl, sparrow->save);
910 if (sparrow->debug){
911 cvReleaseImage(&fl->debug);
913 free(fl->h_lines);
914 free(fl->shuffled_lines);
915 free(fl->map);
916 free(fl->mesh);
917 free(fl->clusters);
918 cvReleaseImage(&fl->threshold);
919 cvReleaseImage(&fl->working);
920 cvReleaseImageHeader(&fl->input);
921 free(fl);
922 GST_DEBUG("freed everything\n");
923 sparrow->helper_struct = NULL;
926 /*reduce the signal a little bit more, avoiding overflow later */
927 #define COLOUR_QUANT 1
929 static void
930 setup_colour_shifts(GstSparrow *sparrow, sparrow_find_lines_t *fl){
931 switch (sparrow->colour){
932 case SPARROW_WHITE:
933 case SPARROW_GREEN:
934 fl->shift1 = sparrow->in.gshift + COLOUR_QUANT;
935 fl->shift2 = sparrow->in.gshift + COLOUR_QUANT;
936 break;
937 case SPARROW_MAGENTA:
938 fl->shift1 = sparrow->in.rshift + COLOUR_QUANT;
939 fl->shift2 = sparrow->in.bshift + COLOUR_QUANT;
940 break;
944 INVISIBLE void
945 init_find_edges(GstSparrow *sparrow){
946 gint32 w = sparrow->out.width;
947 gint32 h = sparrow->out.height;
948 gint i;
949 sparrow_find_lines_t *fl = zalloc_aligned_or_die(sizeof(sparrow_find_lines_t));
950 sparrow->helper_struct = (void *)fl;
952 gint h_lines = (h + LINE_PERIOD - 1) / LINE_PERIOD;
953 gint v_lines = (w + LINE_PERIOD - 1) / LINE_PERIOD;
954 gint n_lines = (h_lines + v_lines);
955 gint n_corners = (h_lines * v_lines);
956 fl->n_hlines = h_lines;
957 fl->n_vlines = v_lines;
958 fl->n_lines = n_lines;
960 fl->h_lines = malloc_aligned_or_die(sizeof(sparrow_line_t) * n_lines);
961 fl->shuffled_lines = malloc_aligned_or_die(sizeof(sparrow_line_t*) * n_lines);
962 GST_DEBUG("shuffled lines, malloced %p\n", fl->shuffled_lines);
964 fl->map = zalloc_aligned_or_die(sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
965 fl->clusters = zalloc_or_die(n_corners * sizeof(sparrow_cluster_t));
966 fl->mesh = zalloc_aligned_or_die(n_corners * sizeof(sparrow_corner_t));
968 sparrow_line_t *line = fl->h_lines;
969 sparrow_line_t **sline = fl->shuffled_lines;
970 int offset;
972 for (i = 0, offset = H_LINE_OFFSET; offset < h;
973 i++, offset += LINE_PERIOD){
974 line->offset = offset;
975 line->dir = SPARROW_HORIZONTAL;
976 line->index = i;
977 *sline = line;
978 line++;
979 sline++;
982 /*now add the vertical lines */
983 fl->v_lines = line;
984 for (i = 0, offset = V_LINE_OFFSET; offset < w;
985 i++, offset += LINE_PERIOD){
986 line->offset = offset;
987 line->dir = SPARROW_VERTICAL;
988 line->index = i;
989 *sline = line;
990 line++;
991 sline++;
993 //DEBUG_FIND_LINES(fl);
995 GST_DEBUG("allocated %d lines, status %d\n", n_lines, line - fl->h_lines);
997 /*now shuffle */
998 for (i = 0; i < n_lines; i++){
999 int j = RANDINT(sparrow, 0, n_lines);
1000 sparrow_line_t *tmp = fl->shuffled_lines[j];
1001 fl->shuffled_lines[j] = fl->shuffled_lines[i];
1002 fl->shuffled_lines[i] = tmp;
1005 setup_colour_shifts(sparrow, fl);
1007 if (sparrow->reload){
1008 if (access(sparrow->reload, R_OK)){
1009 GST_DEBUG("sparrow>reload is '%s' and it is UNREADABLE\n", sparrow->reload);
1010 exit(1);
1012 read_edges_info(sparrow, fl, sparrow->reload);
1013 memset(fl->map, 0, sizeof(sparrow_intersect_t) * sparrow->in.pixcount);
1014 //memset(fl->clusters, 0, n_corners * sizeof(sparrow_cluster_t));
1015 memset(fl->mesh, 0, n_corners * sizeof(sparrow_corner_t));
1016 jump_state(sparrow, fl, EDGES_FIND_CORNERS);
1018 else {
1019 jump_state(sparrow, fl, EDGES_FIND_NOISE);
1021 /* opencv images for threshold finding */
1022 CvSize size = {sparrow->in.width, sparrow->in.height};
1023 fl->working = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
1024 fl->threshold = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);
1026 /*input has no data allocated -- it uses latest frame*/
1027 fl->input = init_ipl_image(&sparrow->in, PIXSIZE);
1029 //DEBUG_FIND_LINES(fl);
1030 if (sparrow->debug){
1031 fl->debug = cvCreateImage(size, IPL_DEPTH_8U, PIXSIZE);