4 * Camera image processing and filtering.
6 * gcc -Wall -O3 -ggdb3 -std=gnu99 -o brmbotimg brmbotimg.c apc.c -lm
15 #include <linux/videodev2.h>
18 #define PARITY (1) // 1: blue preference, -1: red preference
20 #define PUK_THRES 3500 /* 8x8 */
21 #define CORNER_THRES 200000 /* 40x40 */
22 #define PUSH_THRES 20000 /* 30x80 */
23 #define INCORN_THRES 100000 /* 28x28 */
29 static inline bool masked(int x
, int y
)
31 int ofs
= y
* mask_width
+ x
;
32 return mask_bits
[ofs
/ 8] & (1 << (7 - ofs
% 8));
35 FILE *debugout
= NULL
;
39 unsigned char y1
, u
, y2
, v
;
41 } __attribute__((packed
));
43 /* Negative value: likely red.
44 * Positive value: likely blue.
45 * Near zero: nothing. */
46 int assess_pixel(const struct dpix
*pixel
)
49 return 10*(pixel
->u
- 128) - 10*(pixel
->v
- 128); // - (pixel->y1 + pixel->y2)/2 + 138;
52 /* w,h is square radius, not diameter! */
53 int assess_square(const struct dpix
*image
, struct v4l2_format
*fmt
, int x
, int y
, int w
, int h
, int parity
)
55 x
/= 2; w
/= 2; // each dpix is two physical pixels
59 for (j
= y
- h
; j
< y
+ h
; j
++)
60 for (i
= x
- w
; i
< x
+ w
; i
++)
61 if (!masked(i
* 2, j
)) {
62 //const struct dpix *pixel = &image[j * fmt->fmt.pix.width / 2 + i];
63 //fprintf(stderr, "%d - %d,%d: Y %d, U %d, V %d\n", assess_pixel(pixel), i*2, j, (pixel->y1 + pixel->y2) / 2, pixel->u, pixel->v);
64 int a
= assess_pixel(&image
[j
* fmt
->fmt
.pix
.width
/ 2 + i
]);
65 if (__builtin_expect(!parity
, 1))
67 else if (a
* parity
> 0)
71 //return area > 0 ? assess / area : 0;
75 void color_square(struct dpix
*image
, struct v4l2_format
*fmt
, int x
, int y
, int w
, int h
, int u
, int v
)
77 x
/= 2; w
/= 2; // each dpix is two physical pixels
79 for (j
= y
- h
; j
< y
+ h
; j
++)
80 for (i
= x
- w
; i
< x
+ w
; i
++) {
81 struct dpix
*pixel
= &image
[j
* fmt
->fmt
.pix
.width
/ 2 + i
];
87 void relative_coords(struct v4l2_format
*fmt
, int x
, int y
, int *dx
, int *dy
)
89 *dx
= (fmt
->fmt
.pix
.width
/ 2) - x
;
90 *dy
= (fmt
->fmt
.pix
.height
/ 2 + CAM_Y_OFS
) - y
;
96 double angle_of(int dx
, int dy
)
98 double m
= 1.0f
, a
= 0.0f
;
99 if (dx
< 0) dx
= -dx
, a
= M_PI
, m
= -m
;
100 if (dy
< 0) dy
= -dy
, m
= -m
;
101 return m
* (a
+ atan((double) dy
/ dx
));
103 double weight_distance(int dx
, int dy
)
105 return pow((1 - 0.002), sqrt(dx
*dx
+ dy
*dy
));
109 /* Scan unmasked portion of image for blobs of given parity.
110 * Consider their relative positions, producing angle in the average
113 double blob_scan(struct dpix
*image
, struct v4l2_format
*fmt
, int parity
)
115 double avg_angle
= 0;
116 int avg_weight
= 100; // tend to go straight ahead
118 int square_radius
= 4;
119 int assess_any_thres
= 5000;
120 for (int x
= square_radius
; x
< fmt
->fmt
.pix
.width
; x
+= square_radius
* 2)
121 for (int y
= square_radius
; y
< fmt
->fmt
.pix
.height
; y
+= square_radius
* 2) {
122 int assess
= assess_square(image
, fmt
, x
, y
, square_radius
, square_radius
, 0);
123 if (abs(assess
) < assess_any_thres
) {
124 color_square(image
, fmt
, x
, y
, square_radius
, square_radius
, 0, 0);
129 color_square(image
, fmt
, x
, y
, square_radius
, square_radius
, 255, 255);
133 relative_coords(fmt
, x
, y
, &dx
, &dy
);
134 double angle
= angle_of(dx
, dy
);
135 double weight
= assess
* weight_distance(dx
, dy
);
137 fprintf(stderr
, "%d,%f blob @ %d,%d (%d,%d) angle %f\n", assess
, weight
, x
, y
, dx
, dy
, angle
);
139 avg_angle
+= (angle
- avg_angle
) * weight
/ avg_weight
;
140 avg_weight
+= weight
;
143 fprintf(stderr
, "total angle %f [%d]\n", avg_angle
, avg_weight
);
148 /* Scan unmasked portion of image for blobs of given parity.
149 * Find direction and distance of the nearest one. */
150 void nearest_blob_scan(struct dpix
*image
, struct v4l2_format
*fmt
, int parity
, double *best_angle
, double *best_dist
)
155 int square_radius
= 4;
156 for (int x
= square_radius
; x
< fmt
->fmt
.pix
.width
; x
+= square_radius
* 2)
157 for (int y
= square_radius
; y
< fmt
->fmt
.pix
.height
; y
+= square_radius
* 2) {
158 int assess
= assess_square(image
, fmt
, x
, y
, square_radius
, square_radius
, 0);
159 if (abs(assess
) < PUK_THRES
) {
164 color_square(image
, fmt
, x
, y
, square_radius
, square_radius
, 128, 255);
167 color_square(image
, fmt
, x
, y
, square_radius
, square_radius
, 128, 0);
169 relative_coords(fmt
, x
, y
, &dx
, &dy
);
170 double angle
= angle_of(dx
, dy
);
172 fprintf(stderr
, "%d>%d blob @ %d,%d (%d,%d) angle %f\n", assess
, PUK_THRES
, x
, y
, dx
, dy
, angle
);
174 double dist2
= dx
* dx
+ dy
* dy
;
175 if (dist2
< *best_dist
) {
181 fprintf(stderr
, "best blob angle %f dist %f\n", *best_angle
, *best_dist
);
182 *best_dist
= sqrt(*best_dist
);
185 int color_neighbor_blob_squares(struct dpix
*image
, struct v4l2_format
*fmt
, int parity
, int kx
, int ky
, int base_radius
, int dist
)
187 int square_radius
= 4;
188 int edgel
= dist
+ base_radius
/ square_radius
;
189 int qx
[4 * edgel
], qy
[4 * edgel
];
190 int sx
= kx
- base_radius
- dist
* square_radius
;
191 int sy
= ky
- base_radius
- dist
* square_radius
;
192 int mx
= kx
+ base_radius
+ dist
* square_radius
;
193 int my
= ky
+ base_radius
+ dist
* square_radius
;
198 while (x
< mx
) { qx
[i
] = x
; qy
[i
] = y
; x
+= square_radius
* 2; i
++; }
199 while (y
< my
) { qx
[i
] = x
; qy
[i
] = y
; y
+= square_radius
* 2; i
++; }
200 while (x
> sx
) { qx
[i
] = x
; qy
[i
] = y
; x
-= square_radius
* 2; i
++; }
201 while (y
> sy
) { qx
[i
] = x
; qy
[i
] = y
; y
-= square_radius
* 2; i
++; }
203 for (i
= 0; i
< 4 * edgel
; i
++) {
204 int x
= qx
[i
], y
= qy
[i
];
205 if (x
< square_radius
|| x
> fmt
->fmt
.pix
.width
- square_radius
) continue;
206 if (y
< square_radius
|| y
> fmt
->fmt
.pix
.height
- square_radius
) continue;
208 int assess
= assess_square(image
, fmt
, x
, y
, square_radius
, square_radius
, 0) * parity
;
209 if (assess
< PUK_THRES
)
211 // puk-like square near the corner is taboo!
212 color_square(image
, fmt
, x
, y
, square_radius
, square_radius
, 0, 0);
218 double corner_angle(struct dpix
*image
, struct v4l2_format
*fmt
, int parity
)
220 int square_radius
= 20;
221 for (int x
= square_radius
; x
< fmt
->fmt
.pix
.width
; x
+= square_radius
* 2)
222 for (int y
= square_radius
; y
< fmt
->fmt
.pix
.height
; y
+= square_radius
* 2) {
223 int assess
= assess_square(image
, fmt
,x
, y
, square_radius
, square_radius
, 0);
224 if (assess
* parity
> CORNER_THRES
) {
225 /* Color the square! This way, we will not confuse it with blobs. */
226 color_square(image
, fmt
, x
, y
, square_radius
, square_radius
, 0, 0);
228 relative_coords(fmt
, x
, y
, &dx
, &dy
);
229 double angle
= angle_of(dx
, dy
);
230 fprintf(stderr
, "%d>%d corner @ %d,%d (%d,%d) angle %f\n", assess
, CORNER_THRES
, x
, y
, dx
, dy
, angle
);
231 for (int dist
= 0; color_neighbor_blob_squares(image
, fmt
, parity
, x
, y
, square_radius
, dist
); dist
++);
235 fprintf(stderr
, "not seeing corner!\n");
239 bool pushing(struct dpix
*image
, struct v4l2_format
*fmt
, int parity
)
241 int x
= 250, y
= 240 + CAM_Y_OFS
, xr
= 15, yr
= 40;
242 double value
= assess_square(image
, fmt
, 250, 240 + CAM_Y_OFS
, 15, 40, parity
) * parity
;
243 color_square(image
, fmt
, x
, y
, xr
, yr
, 0, 0);
244 fprintf(stderr
, "Pushing check: %d,%d %dx%d %f >? %d\n", x
, y
, xr
, yr
, value
, PUSH_THRES
);
245 return value
> PUSH_THRES
;
248 bool in_corner(const struct dpix
*image
, struct v4l2_format
*fmt
, int parity
)
250 double value
= assess_square(image
, fmt
, 220, 240 + CAM_Y_OFS
, 14, 14, parity
) * parity
;
251 fprintf(stderr
, "In-corner check: 220,260 14x14 %f >? %d\n", value
, INCORN_THRES
);
252 return value
> INCORN_THRES
;
255 double randomize_angle(double angle
)
257 #define ANGLE_SPREAD (M_PI/5)
258 return angle
- ANGLE_SPREAD
/2 + ((double)random() / RAND_MAX
* ANGLE_SPREAD
);
262 void process_image(void *p
, int size
/* 614400 */, struct v4l2_format
*fmt
)
266 if (!f
) f
= fopen("stream.raw", "wb");
271 gettimeofday(&tv
, NULL
);
273 fprintf(stderr
, "\n----------------------------------------------------------------------\n"
274 "\n[%d %ld,%ld] frame (size %d w %d h %d bpl %d)\n",
275 framei
, tv
.tv_sec
, tv
.tv_usec
,
276 size
, fmt
->fmt
.pix
.width
, fmt
->fmt
.pix
.height
, fmt
->fmt
.pix
.bytesperline
);
279 /* Perhaps we have a goal? */
280 static int goal_timer
= 0;
281 if (goal_timer
> 0) {
282 /* In that case, do not override existing orders. */
285 goto next
; fflush(stderr
);
288 /* Each two bytes are (Y, U, Y, V) representing two
289 * adjecent pixels. */
290 struct dpix
*image
= p
;
299 /* Get basic navigation data - are we pushing and where is our home? */
300 bool are_in_corner
= in_corner(image
, fmt
, PARITY
);
301 double corner
= corner_angle(image
, fmt
, PARITY
);
302 bool are_pushing
= pushing(image
, fmt
, PARITY
);
305 bool backoff
= false;
307 /* First, test for emergency back-off. If, for the last N frames,
308 * the corner position has not changed (and is not ahead), back off
309 * in random (narrow) angle. */
310 if (!isnan(corner
) && abs(corner
) > 0.1) {
311 #define FRAME_TEST 12
312 static double corners
[FRAME_TEST
];
313 static int corners_k
= 0;
314 corners
[corners_k
++] = corner
;
315 if (corners_k
>= FRAME_TEST
)
318 for (int i
= 0; i
< FRAME_TEST
; i
++)
319 if (abs(corners
[i
] - corner
) > 0.1)
321 angle
= randomize_angle(M_PI
);
324 fprintf(stderr
, "Emergency backoff! Corner stays at %f\n", corner
);
330 /* Escape! If we know what to get away from, do that.
331 * Otherwise go in a random angle for a second. */
332 if (!isnan(corner
) && random() % 100 > 10) {
333 fprintf(stderr
, "Corner escape: %f\n", corner
);
334 angle
= randomize_angle(corner
);
335 fprintf(stderr
, "-> %f\n", angle
);
337 fprintf(stderr
, "Corner escape: at random\n");
338 angle
= (double)random() / RAND_MAX
* M_PI
* 2 - M_PI
;
339 fprintf(stderr
, "-> %f\n", angle
);
344 } else if (!isnan(corner
) && are_pushing
) {
346 fprintf(stderr
, "Pushing to corner: %f\n", corner
);
351 /* Look for blobs. */
353 nearest_blob_scan(image
, fmt
, PARITY
, &angle
, &dist
);
358 /* If we are closing in and know where the
359 * corner is, start evasion sequence so that
360 * we, the blob and the corner are lined up
361 * properly when we start pushing it. */
362 if (dist
< 150 && !isnan(corner
)) {
363 if (abs(angle
- corner
) > M_PI
/2) {
364 /* Go around the blob. */
365 angle
= angle
+ M_PI
/4 * (random() % 2 ? 1 : -1);
366 fprintf(stderr
, "evasion sequence: %f\n", angle
);
373 /* We are lost! Tilt around for a bit. */
374 angle
= randomize_angle(M_PI
);
380 if (!goal_timer
) goal_timer
= 3; // default decision granularity
381 if (angle
> M_PI
) angle
-= 2*M_PI
;
382 fprintf(stderr
, "Final angle (%d) %f, following for %d ticks\n", backoff
, (angle
* 180 / M_PI
), goal_timer
);
383 printf("%s %d\n", backoff
? "BACKOFF" : "ANGLE", ((int) (angle
* 180 / M_PI
)));
388 if (!debugout
) debugout
= fopen("out.raw", "wb");
389 // comment out below for no debugging
390 fwrite((char*) p
, size
, 1, debugout
);
396 struct v4l2_format fmt
= { .fmt
= { .pix
= { .width
= 640, .height
= 480, .bytesperline
= 1280 } } };
398 while (!feof(stdin
)) {
399 int i
= fread(frame
, 614400, 1, stdin
);
401 process_image(frame
, 614400, &fmt
);