host/camnav/brmbotimg.c

   1 /*
   2  * brmbotimg.c
   3  *
   4  * Camera image processing and filtering.
   5  *
   6  * gcc -Wall -O3 -ggdb3 -std=gnu99 -o brmbotimg brmbotimg.c apc.c -lm
   7  */
   8 #include <math.h>
   9 #include <stdbool.h>
  10 #include <stdio.h>
  11 #include <stdlib.h>
  12 #include <sys/time.h>
  13 #include <time.h>
  14 #include <unistd.h>
  15 #include <linux/videodev2.h>
  16
  17
  18 #define PARITY (1) // 1: blue preference, -1: red preference
  19
  20 #define PUK_THRES 3500 /* 8x8 */
  21 #define CORNER_THRES 200000 /* 40x40 */
  22 #define PUSH_THRES 20000 /* 30x80 */
  23 #define INCORN_THRES 100000 /* 28x28 */
  24
  25 #define CAM_Y_OFS 25
  26
  27 #include "mask.xbm"
  28
  29 static inline bool masked(int x, int y)
  30 {
  31         int ofs = y * mask_width + x;
  32         return mask_bits[ofs / 8] & (1 << (7 - ofs % 8));
  33 }
  34
  35 FILE *debugout = NULL;
  36
  37
  38 struct dpix {
  39         unsigned char y1, u, y2, v;
  40         //char u, y1, v, y2;
  41 } __attribute__((packed));
  42
  43 /* Negative value: likely red.
  44  * Positive value: likely blue.
  45  * Near zero: nothing. */
  46 int assess_pixel(const struct dpix *pixel)
  47 {
  48         /* TODO */
  49         return 10*(pixel->u - 128) - 10*(pixel->v - 128); // - (pixel->y1 + pixel->y2)/2 + 138;
  50 }
  51
  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)
  54 {
  55         x /= 2; w /= 2; // each dpix is two physical pixels
  56         int assess = 0;
  57         int area = h * w;
  58         int j, i;
  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))
  66                                         assess += a;
  67                                 else if (a * parity > 0)
  68                                         assess += a;
  69                         } else
  70                                 area--;
  71         //return area > 0 ? assess / area : 0;
  72         return assess;
  73 }
  74
  75 void color_square(struct dpix *image, struct v4l2_format *fmt, int x, int y, int w, int h, int u, int v)
  76 {
  77         x /= 2; w /= 2; // each dpix is two physical pixels
  78         int j, i;
  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];
  82                         pixel->u = u;
  83                         pixel->v = v;
  84                 }
  85 }
  86
  87 void relative_coords(struct v4l2_format *fmt, int x, int y, int *dx, int *dy)
  88 {
  89         *dx = (fmt->fmt.pix.width / 2) - x;
  90         *dy = (fmt->fmt.pix.height / 2 + CAM_Y_OFS) - y;
  91 }
  92 /*       pi/2
  93  * 0 --- * --- +-pi
  94  *      -pi/2
  95  */
  96 double angle_of(int dx, int dy)
  97 {
  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));
 102 }
 103 double weight_distance(int dx, int dy)
 104 {
 105         return pow((1 - 0.002), sqrt(dx*dx + dy*dy));
 106 }
 107
 108 #if 0
 109 /* Scan unmasked portion of image for blobs of given parity.
 110  * Consider their relative positions, producing angle in the average
 111  * blob direction:
 112  */
 113 double blob_scan(struct dpix *image, struct v4l2_format *fmt, int parity)
 114 {
 115         double avg_angle = 0;
 116         int avg_weight = 100; // tend to go straight ahead
 117
 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);
 125                                 continue;
 126                         }
 127                         assess *= parity;
 128                         if (assess < 0) {
 129                                 color_square(image, fmt, x, y, square_radius, square_radius, 255, 255);
 130                                 continue;
 131                         }
 132                         int dx, dy;
 133                         relative_coords(fmt, x, y, &dx, &dy);
 134                         double angle = angle_of(dx, dy);
 135                         double weight = assess * weight_distance(dx, dy);
 136
 137                         fprintf(stderr, "%d,%f blob @ %d,%d (%d,%d) angle %f\n", assess, weight, x, y, dx, dy, angle);
 138
 139                         avg_angle += (angle - avg_angle) * weight / avg_weight;
 140                         avg_weight += weight;
 141                 }
 142
 143         fprintf(stderr, "total angle %f [%d]\n", avg_angle, avg_weight);
 144         return avg_angle;
 145 }
 146 #endif
 147
 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)
 151 {
 152         *best_angle = NAN;
 153         *best_dist = 100000;
 154
 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) {
 160                                 continue;
 161                         }
 162                         assess *= parity;
 163                         if (assess < 0) {
 164                                 color_square(image, fmt, x, y, square_radius, square_radius, 128, 255);
 165                                 continue;
 166                         }
 167                         color_square(image, fmt, x, y, square_radius, square_radius, 128, 0);
 168                         int dx, dy;
 169                         relative_coords(fmt, x, y, &dx, &dy);
 170                         double angle = angle_of(dx, dy);
 171
 172                         fprintf(stderr, "%d>%d blob @ %d,%d (%d,%d) angle %f\n", assess, PUK_THRES, x, y, dx, dy, angle);
 173
 174                         double dist2 = dx * dx + dy * dy;
 175                         if (dist2 < *best_dist) {
 176                                 *best_angle = angle;
 177                                 *best_dist = dist2;
 178                         }
 179                 }
 180
 181         fprintf(stderr, "best blob angle %f dist %f\n", *best_angle, *best_dist);
 182         *best_dist = sqrt(*best_dist);
 183 }
 184
 185 int color_neighbor_blob_squares(struct dpix *image, struct v4l2_format *fmt, int parity, int kx, int ky, int base_radius, int dist)
 186 {
 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;
 194         int x = sx, y = sy;
 195         int i = 0;
 196         int taboo = 0;
 197
 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++; }
 202
 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;
 207
 208                 int assess = assess_square(image, fmt, x, y, square_radius, square_radius, 0) * parity;
 209                 if (assess < PUK_THRES)
 210                         continue;
 211                 // puk-like square near the corner is taboo!
 212                 color_square(image, fmt, x, y, square_radius, square_radius, 0, 0);
 213                 taboo++;
 214         }
 215         return taboo;
 216 }
 217
 218 double corner_angle(struct dpix *image, struct v4l2_format *fmt, int parity)
 219 {
 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);
 227                                 int dx, dy;
 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++);
 232                                 return angle;
 233                         }
 234                 }
 235         fprintf(stderr, "not seeing corner!\n");
 236         return NAN;
 237 }
 238
 239 bool pushing(struct dpix *image, struct v4l2_format *fmt, int parity)
 240 {
 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;
 246 }
 247
 248 bool in_corner(const struct dpix *image, struct v4l2_format *fmt, int parity)
 249 {
 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;
 253 }
 254
 255 double randomize_angle(double angle)
 256 {
 257 #define ANGLE_SPREAD (M_PI/5)
 258         return angle - ANGLE_SPREAD/2 + ((double)random() / RAND_MAX * ANGLE_SPREAD);
 259 }
 260
 261
 262 void process_image(void *p, int size /* 614400 */, struct v4l2_format *fmt)
 263 {
 264 #if 0
 265         static FILE *f;
 266         if (!f) f = fopen("stream.raw", "wb");
 267         fwrite(p,size,1,f);
 268 #endif
 269
 270         struct timeval tv;
 271         gettimeofday(&tv, NULL);
 272         static int framei;
 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);
 277         framei++;
 278
 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. */
 283                 goal_timer--;
 284                 printf("TICK\n");
 285                 goto next; fflush(stderr);
 286         }
 287
 288         /* Each two bytes are (Y, U, Y, V) representing two
 289          * adjecent pixels. */
 290         struct dpix *image = p;
 291
 292         /* Camera image:
 293          *
 294          *       RIGHT
 295          * FRONT       REAR
 296          *       LEFT
 297          */
 298
 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);
 303
 304         double angle = 0;
 305         bool backoff = false;
 306
 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)
 316                         corners_k = 0;
 317
 318                 for (int i = 0; i < FRAME_TEST; i++)
 319                         if (abs(corners[i] - corner) > 0.1)
 320                                 goto no_backoff;
 321                 angle = randomize_angle(M_PI);
 322                 backoff = 1;
 323                 goal_timer = 15;
 324                 fprintf(stderr, "Emergency backoff! Corner stays at %f\n", corner);
 325                 goto go;
 326 no_backoff:;
 327         }
 328
 329         if (are_in_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);
 336                 } else {
 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);
 340                 }
 341                 backoff = 1;
 342                 goal_timer = 25;
 343
 344         } else if (!isnan(corner) && are_pushing) {
 345                 /* Go to corner. */
 346                 fprintf(stderr, "Pushing to corner: %f\n", corner);
 347                 angle = corner;
 348                 goal_timer = 10;
 349
 350         } else {
 351                 /* Look for blobs. */
 352                 double dist;
 353                 nearest_blob_scan(image, fmt, PARITY, &angle, &dist);
 354                 if (!isnan(angle)) {
 355                         /* Cool! */
 356                         goal_timer = 10;
 357 #if 0
 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);
 367                                         goal_timer = 20;
 368                                 }
 369                         }
 370 #endif
 371
 372                 } else {
 373                         /* We are lost! Tilt around for a bit. */
 374                         angle = randomize_angle(M_PI);
 375                         goal_timer = 20;
 376                 }
 377         }
 378
 379 go:
 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)));
 384         fflush(stdout);
 385         fflush(stderr);
 386
 387 next:
 388         if (!debugout) debugout = fopen("out.raw", "wb");
 389         // comment out below for no debugging
 390         fwrite((char*) p, size, 1, debugout);
 391 }
 392
 393 #if 0
 394 int main(void)
 395 {
 396         struct v4l2_format fmt = { .fmt = { .pix = { .width = 640, .height = 480, .bytesperline = 1280 } } };
 397         char frame[614400];
 398         while (!feof(stdin)) {
 399                 int i = fread(frame, 614400, 1, stdin);
 400                 if (i < 1) break;
 401                 process_image(frame, 614400, &fmt);
 402         }
 403         return 0;
 404 }
 405 #endif