src/opticalflow.c

   1 #include <stdio.h>
   2 #include "ppm.h"
   3 #include "opticalflow.h"
   4 #include "yuv.h"
   5 #include <opencv2/core/core_c.h>
   6 #include <opencv2/imgproc/imgproc_c.h>
   7 #include <opencv2/video/tracking.hpp>
   8 #include "dir_seperator.h"
   9
  10 int flow_enable = 0, flow_window, flow_view_vector, flow_view_uv;
  11
  12 void flow_default(void)
  13 {
  14         flow_window = 15;
  15         flow_view_vector = 16;
  16         flow_view_uv = 0; /* disabled */
  17 }
  18
  19 /* save flow setting */
  20 void save_flow(const char *filename)
  21 {
  22         char name[256];
  23         FILE *fp;
  24
  25         sprintf(name, "%s_opticalflow", filename);
  26         fp = fopen(name, "w");
  27         if (!fp) {
  28                 printf("failed to save flow setting '%s'\n", name);
  29                 return;
  30         }
  31         fprintf(fp, "flow_enable %d\n", flow_enable);
  32         fprintf(fp, "flow_window %d\n", flow_window);
  33         fprintf(fp, "flow_view_vector %d\n", flow_view_vector);
  34         fprintf(fp, "flow_view_uv %d\n", flow_view_uv);
  35         fclose(fp);
  36 }
  37
  38 /* load flow setting */
  39 int load_flow(const char *filename)
  40 {
  41         char name[256];
  42         char line[256],*p;
  43         FILE *fp;
  44         int i;
  45
  46         sprintf(name, "%s_opticalflow", filename);
  47         fp = fopen(name, "r");
  48         if (!fp) {
  49                 printf("flow setting '%s' does not (yet) exists\n", name);
  50                 return -1;
  51         }
  52         for (i = 0; ; i++) {
  53                 if (!fgets(line, sizeof(line), fp))
  54                         break;
  55                 line[sizeof(line)-1] = '\0';
  56                 if (line[0]) line[strlen(line)-1] = '\0';
  57                 if (line[0] && line[strlen(line)-1] == '\r') line[strlen(line)-1] = '\0';
  58
  59                 if (!strncmp(line, "flow_enable ", 12)) {
  60                         p = line + 12;
  61                         sscanf(p, "%d", &flow_enable);
  62                         continue;
  63                 }
  64                 if (!strncmp(line, "flow_window ", 12)) {
  65                         p = line + 12;
  66                         sscanf(p, "%d", &flow_window);
  67                         continue;
  68                 }
  69                 if (!strncmp(line, "flow_view_vector ", 17)) {
  70                         p = line + 17;
  71                         sscanf(p, "%d", &flow_view_vector);
  72                         continue;
  73                 }
  74                 if (!strncmp(line, "flow_view_uv ", 13)) {
  75                         p = line + 13;
  76                         sscanf(p, "%d", &flow_view_uv);
  77                         continue;
  78                 }
  79         }
  80         fclose(fp);
  81         return 0;
  82 }
  83
  84 /* draw flow matrix as lines */
  85 static void draw_lines_from_matrix(CvMat *flow, int width, int height, int steps, int color, IplImage *image)
  86 {
  87         int flow_width = width / steps;
  88         int flow_height = height / steps;
  89         int i, j, x, y;
  90         const float *f;
  91
  92         for (i = 0, y = steps/2; i < flow_height && y < height; i++, y += steps) {
  93                 f = (const float *)(flow->data.ptr + flow->step * y);
  94                 for (j = 0, x = steps/2; j < flow_width && x < width; j++, x += steps) {
  95                         cvLine(image,
  96                                 cvPoint(cvRound(x), cvRound(y)),
  97                                 cvPoint(cvRound(f[2*x] + x), cvRound(f[2*x+1] + y)),
  98                                 CV_RGB(color,color,color),
  99                                 1, 8, 0);
 100                 }
 101         }
 102 }
 103
 104 /* take flow matrix to generate a map */
 105 static void copy_matrix_to_map(CvMat *flow, int width, int height, double *flow_map_x, double *flow_map_y)
 106 {
 107         int x, y;
 108         const float *f;
 109
 110         for (y = 0; y < height; y++) {
 111                 f = (const float *)(flow->data.ptr + flow->step * y);
 112                 for (x = 0; x < width; x++) {
 113                         *flow_map_x++ = *f++;
 114                         *flow_map_y++ = *f++;
 115                 }
 116         }
 117 }
 118
 119 /*
 120  * create flow maps
 121  *
 122  * img_prev_buffer = image buffer of previous image (or NULL)
 123  * img_next_buffer = image buffer of next image (or NULL)
 124  * img_buffer = current image buffer
 125  * width, height = dimensions of image buffer
 126  * steps = pixles between each feature = size of grid to track
 127  * win_size = flow algorithm window size
 128  * max_flow = maximum length of allowed flow vector (to avoid errors)
 129  * flow_map_x_prev = flow map for delta x of previous image (result is stored here)
 130  * flow_map_y_prev = flow map for delta y of previous image (result is stored here)
 131  * flow_map_x_next = flow map for delta x of next image (result is stored here)
 132  * flow_map_y_next = flow map for delta y of next image (result is stored here)
 133  *
 134  */
 135 void *create_flow_maps(const unsigned short *img_prev_buffer, const unsigned short *img_next_buffer, unsigned short *img_buffer, int width, int height, int win_size, int steps, double *flow_map_x_prev, double *flow_map_y_prev, double *flow_map_x_next, double *flow_map_y_next, void *_image_preview)
 136 {
 137         IplImage *image_prev = NULL, *image = NULL, *image_next = NULL, *image_preview = _image_preview;
 138         CvMat *flow;
 139         CvSize size;
 140         int i, j;
 141
 142         const float pyrScale = 0.5;
 143         const float levels = 3;
 144         const float winsize = win_size;
 145         const float iterations = 8;
 146         const float polyN = 5;
 147         const float polySigma = 1.2;
 148         const int flags = 0;
 149
 150         flow = cvCreateMat(height, width, CV_32FC2);
 151         if (!flow) {
 152                 fprintf(stderr, "failed to allocate opencv matrix\n");
 153                 return NULL;
 154         }
 155
 156         /* apply images to opencv image */
 157         size.width = width;
 158         size.height = height;
 159         if (img_prev_buffer) {
 160                 image_prev = cvCreateImage(size, IPL_DEPTH_8U, 1);
 161         }
 162         if (img_buffer) {
 163                 image = cvCreateImage(size, IPL_DEPTH_8U, 1);
 164         }
 165         if (img_next_buffer) {
 166                 image_next = cvCreateImage(size, IPL_DEPTH_8U, 1);
 167         }
 168
 169         if (image_prev) {
 170                 for (i = 0; i < height; i++) {
 171                         for (j = 0; j < width; j++) {
 172                                 /* copy green channel */
 173                                 CV_IMAGE_ELEM(image_prev, uchar, i, j) = img_prev_buffer[(i*width+j)*3 + 1] >> 8;
 174                         }
 175                 }
 176         }
 177         if (image) {
 178                 for (i = 0; i < height; i++) {
 179                         for (j = 0; j < width; j++) {
 180                                 /* copy green channel */
 181                                 CV_IMAGE_ELEM(image, uchar, i, j) = img_buffer[(i*width+j)*3 + 1] >> 8;
 182                                 if (image_preview)
 183                                         CV_IMAGE_ELEM(image_preview, uchar, i, j) = (img_buffer[(i*width+j)*3 + 1] / 2) >> 8;
 184                         }
 185                 }
 186         }
 187         if (image_next) {
 188                 for (i = 0; i < height; i++) {
 189                         for (j = 0; j < width; j++) {
 190                                 /* copy green channel */
 191                                 CV_IMAGE_ELEM(image_next, uchar, i, j) = img_next_buffer[(i*width+j)*3 + 1] >> 8;
 192                         }
 193                 }
 194         }
 195
 196         /* optical flow and compose previous and next image with current image */
 197         if (image) {
 198                 /* calc flow */
 199                 if (image_prev) {
 200                         cvCalcOpticalFlowFarneback(image, image_prev, flow, pyrScale, levels, winsize, iterations, polyN, polySigma, flags);
 201                         if (image_preview && steps > 1)
 202                                 draw_lines_from_matrix(flow, width, height, steps, 255, image_preview);
 203                         if (flow_map_x_prev && flow_map_y_prev)
 204                                 copy_matrix_to_map(flow, width, height, flow_map_x_prev, flow_map_y_prev);
 205                 }
 206                 if (image_next) {
 207                         cvCalcOpticalFlowFarneback(image, image_next, flow, pyrScale, levels, winsize, iterations, polyN, polySigma, flags);
 208                         if (image_preview && steps > 1)
 209                                 draw_lines_from_matrix(flow, width, height, steps, 192, image_preview);
 210                         if (flow_map_x_next && flow_map_y_next)
 211                                 copy_matrix_to_map(flow, width, height, flow_map_x_next, flow_map_y_next);
 212                 }
 213         }
 214
 215         if (image)
 216                 cvReleaseImage(&image);
 217         if (image_prev)
 218                 cvReleaseImage(&image_prev);
 219         if (image_next)
 220                 cvReleaseImage(&image_next);
 221
 222         cvReleaseMat(&flow);
 223
 224         return image_preview;
 225 }
 226
 227 /*
 228  * use img_buffer and create view of optical flow between this image, previous image and next image.
 229  *
 230  * filename_prev = name of previous image
 231  * filename_next = name of next image
 232  * img_buffer = current image buffer
 233  * width, height = dimensions of image buffer
 234  * win_size = flow algorithm window size
 235  * steps = pixles between each feature to visualize vectors (1 to disable)
 236  * uv_scale = apply vector (pointing to next plane) by applying uv-color (0 to disable)
 237  *
 238  */
 239 void create_flow_view(const char *filename_prev, const char *filename_next, unsigned short *img_buffer, int width, int height, int win_size, int steps, int uv_scale)
 240 {
 241         unsigned short *img_prev_buffer = NULL, *img_next_buffer = NULL;
 242         double *flow_map_x_prev = NULL, *flow_map_y_prev = NULL, *flow_map_x_next = NULL, *flow_map_y_next = NULL;
 243         IplImage *image_preview = NULL;
 244         CvSize size;
 245         unsigned char c;
 246         double r, g, b;
 247         int w, h, rc;
 248         int i, j;
 249
 250 #if 0
 251         cvInitMatHeader(&kernel, 3, 3, CV_64FC1, vals, 0);
 252 #endif
 253
 254 //printf("%s %s\n", filename_prev, filename_next);
 255         /* read previous image */
 256         if (filename_prev) {
 257                 rc = load_img(-1, &img_prev_buffer, &w, &h, filename_prev, 0);
 258                 if (rc == 0) {
 259                         if (w != width || h != height) {
 260                                 rc = -1;
 261                                 free(img_prev_buffer);
 262                                 img_prev_buffer = NULL;
 263                         }
 264                 }
 265         }
 266
 267         /* read next image */
 268         if (filename_next) {
 269                 rc = load_img(-1, &img_next_buffer, &w, &h, filename_next, 0);
 270                 if (rc == 0) {
 271                         if (w != width || h != height) {
 272                                 rc = -1;
 273                                 free(img_next_buffer);
 274                                 img_next_buffer = NULL;
 275                         }
 276                 }
 277         }
 278
 279         /* apply images to opencv image */
 280         size.width = width;
 281         size.height = height;
 282         if (img_buffer) {
 283                 image_preview = cvCreateImage(size, IPL_DEPTH_8U, 1);
 284         }
 285
 286         flow_map_x_prev = (double *)malloc(sizeof(double)*width*height);
 287         flow_map_y_prev = (double *)malloc(sizeof(double)*width*height);
 288         flow_map_x_next = (double *)malloc(sizeof(double)*width*height);
 289         flow_map_y_next = (double *)malloc(sizeof(double)*width*height);
 290
 291         if (image_preview)
 292                 image_preview = create_flow_maps(img_prev_buffer, img_next_buffer, img_buffer, width, height, win_size, steps, flow_map_x_prev, flow_map_y_prev, flow_map_x_next, flow_map_y_next, image_preview);
 293
 294         if (img_buffer && image_preview) {
 295                 /* paint vectors from image_preview to img_buffer */
 296                 for (i = 0; i < height; i++) {
 297                         for (j = 0; j < width; j++) {
 298                                 c = CV_IMAGE_ELEM(image_preview, uchar, i, j);
 299                                 if (c > 224) {
 300                                         r = 1;
 301                                         g = 0;
 302                                         b = 0;
 303                                 } else if (c > 160) {
 304                                         r = 0;
 305                                         g = 1;
 306                                         b = 0;
 307                                 } else {
 308                                         if (!steps && uv_scale)
 309                                                 c += 64;
 310                                         if (img_next_buffer && uv_scale) {
 311                                                 yuv2rgb_pixle(
 312                                                         (double)c / 255.0,
 313                                                         flow_map_x_next[width * i + j] / uv_scale,
 314                                                         flow_map_y_next[width * i + j] / uv_scale,
 315                                                         &r, &g, &b);
 316                                         } else {
 317                                                 r = (double)c / 255.0;
 318                                                 g = (double)c / 255.0;
 319                                                 b = (double)c / 255.0;
 320                                         }
 321                                 }
 322                                 img_buffer[(i*width+j)*3 + 0] = r * 65535.0;
 323                                 img_buffer[(i*width+j)*3 + 1] = g * 65535.0;
 324                                 img_buffer[(i*width+j)*3 + 2] = b * 65535.0;
 325                         }
 326                 }
 327         }
 328
 329         /* free stuff */
 330         if (img_prev_buffer)
 331                 free(img_prev_buffer);
 332         if (img_next_buffer)
 333                 free(img_next_buffer);
 334         if (flow_map_x_prev)
 335                 free(flow_map_x_prev);
 336         if (flow_map_y_prev)
 337                 free(flow_map_y_prev);
 338         if (flow_map_x_next)
 339                 free(flow_map_x_next);
 340         if (flow_map_y_next)
 341                 free(flow_map_y_next);
 342         if (image_preview)
 343                 cvReleaseImage(&image_preview);
 344 }
 345
 346