src/opticalflow.c

   1 #include <stdio.h>
   2 #include "img.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 double *img_prev_buffer, const double *img_next_buffer, const double *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                                 CV_IMAGE_ELEM(image_prev, uchar, i, j) = img_prev_buffer[i*width+j] * 255.0;
 173                         }
 174                 }
 175         }
 176         if (image) {
 177                 for (i = 0; i < height; i++) {
 178                         for (j = 0; j < width; j++) {
 179                                 CV_IMAGE_ELEM(image, uchar, i, j) = img_buffer[i*width+j] * 255.0;
 180                                 if (image_preview)
 181                                         CV_IMAGE_ELEM(image_preview, uchar, i, j) = (img_buffer[i*width+j] / 2) * 255.0;
 182                         }
 183                 }
 184         }
 185         if (image_next) {
 186                 for (i = 0; i < height; i++) {
 187                         for (j = 0; j < width; j++) {
 188                                 CV_IMAGE_ELEM(image_next, uchar, i, j) = img_next_buffer[i*width+j] * 255.0;
 189                         }
 190                 }
 191         }
 192
 193         /* optical flow and compose previous and next image with current image */
 194         if (image) {
 195                 /* calc flow */
 196                 if (image_prev) {
 197                         cvCalcOpticalFlowFarneback(image, image_prev, flow, pyrScale, levels, winsize, iterations, polyN, polySigma, flags);
 198                         if (image_preview && steps > 1)
 199                                 draw_lines_from_matrix(flow, width, height, steps, 255, image_preview);
 200                         if (flow_map_x_prev && flow_map_y_prev)
 201                                 copy_matrix_to_map(flow, width, height, flow_map_x_prev, flow_map_y_prev);
 202                 }
 203                 if (image_next) {
 204                         cvCalcOpticalFlowFarneback(image, image_next, flow, pyrScale, levels, winsize, iterations, polyN, polySigma, flags);
 205                         if (image_preview && steps > 1)
 206                                 draw_lines_from_matrix(flow, width, height, steps, 192, image_preview);
 207                         if (flow_map_x_next && flow_map_y_next)
 208                                 copy_matrix_to_map(flow, width, height, flow_map_x_next, flow_map_y_next);
 209                 }
 210         }
 211
 212 #if 0
 213 #include<opencv/highgui.h>
 214 cvNamedWindow( "Display window", 0 );// Create a window for display.
 215 cvShowImage( "Display window", image );                   // Show our image inside it.
 216 #endif
 217
 218         if (image)
 219                 cvReleaseImage(&image);
 220         if (image_prev)
 221                 cvReleaseImage(&image_prev);
 222         if (image_next)
 223                 cvReleaseImage(&image_next);
 224
 225         cvReleaseMat(&flow);
 226
 227         return image_preview;
 228 }
 229
 230 /*
 231  * use img_buffer and create view of optical flow between this image, previous image and next image.
 232  *
 233  * filename_prev = name of previous image
 234  * filename_next = name of next image
 235  * img_buffer = current image buffer
 236  * width, height = dimensions of image buffer
 237  * win_size = flow algorithm window size
 238  * steps = pixles between each feature to visualize vectors (1 to disable)
 239  * uv_scale = apply vector (pointing to next plane) by applying uv-color (0 to disable)
 240  *
 241  */
 242 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)
 243 {
 244         double *img_prev_buffer = NULL, *img_next_buffer = NULL, *img_buffer;
 245         double *flow_map_x_prev = NULL, *flow_map_y_prev = NULL, *flow_map_x_next = NULL, *flow_map_y_next = NULL;
 246         IplImage *image_preview = NULL;
 247         CvSize size;
 248         unsigned char c;
 249         double r, g, b;
 250         int w, h;
 251         int i, j;
 252         unsigned short *img;
 253
 254 #if 0
 255         cvInitMatHeader(&kernel, 3, 3, CV_64FC1, vals, 0);
 256 #endif
 257
 258 //printf("%s %s\n", filename_prev, filename_next);
 259
 260         /* convert current image to double */
 261         img_buffer = (double *)malloc(width*height*3*sizeof(double));
 262         if (!img_buffer) {
 263                 printf("%s:failed to allocate memory\n", __func__);
 264                 return;
 265         }
 266         img2array_short(_img_buffer, width, height, img_buffer, width, height);
 267         rgb2yuv(img_buffer, img_buffer, width, height);
 268
 269         /* read previous image */
 270         if (filename_prev) {
 271                 img = load_img(&w, &h, filename_prev, 0);
 272                 if (img) {
 273                         if (w == width && h == height) {
 274                                 img_prev_buffer = (double *)malloc(w*h*3*sizeof(double));
 275                                 if (img_prev_buffer) {
 276                                         img2array_short(img, w, h, img_prev_buffer, w, h);
 277                                         rgb2yuv(img_prev_buffer, img_prev_buffer, width, height);
 278                                 }
 279                         }
 280                         free(img);
 281                 }
 282         }
 283
 284         /* read next image */
 285         if (filename_next) {
 286                 img = load_img(&w, &h, filename_next, 0);
 287                 if (img) {
 288                         if (w == width && h == height) {
 289                                 img_next_buffer = (double *)malloc(w*h*3*sizeof(double));
 290                                 if (img_next_buffer) {
 291                                         img2array_short(img, w, h, img_next_buffer, w, h);
 292                                         rgb2yuv(img_next_buffer, img_next_buffer, width, height);
 293                                 }
 294                         }
 295                         free(img);
 296                 }
 297         }
 298
 299         /* apply images to opencv image */
 300         size.width = width;
 301         size.height = height;
 302         image_preview = cvCreateImage(size, IPL_DEPTH_8U, 1);
 303
 304         flow_map_x_prev = (double *)malloc(sizeof(double)*width*height);
 305         flow_map_y_prev = (double *)malloc(sizeof(double)*width*height);
 306         flow_map_x_next = (double *)malloc(sizeof(double)*width*height);
 307         flow_map_y_next = (double *)malloc(sizeof(double)*width*height);
 308
 309         if (image_preview) {
 310                 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);
 311
 312                 /* paint vectors from image_preview to img_buffer */
 313                 for (i = 0; i < height; i++) {
 314                         for (j = 0; j < width; j++) {
 315                                 c = CV_IMAGE_ELEM(image_preview, uchar, i, j);
 316                                 if (c > 224) {
 317                                         r = 1;
 318                                         g = 0;
 319                                         b = 0;
 320                                 } else if (c > 160) {
 321                                         r = 0;
 322                                         g = 1;
 323                                         b = 0;
 324                                 } else {
 325                                         if (!steps && uv_scale)
 326                                                 c += 64;
 327                                         if (img_next_buffer && uv_scale) {
 328                                                 yuv2rgb_pixle(
 329                                                         (double)c / 255.0,
 330                                                         flow_map_x_next[width * i + j] / uv_scale,
 331                                                         flow_map_y_next[width * i + j] / uv_scale,
 332                                                         &r, &g, &b);
 333                                         } else {
 334                                                 r = ((double)c) / 255.0;
 335                                                 g = ((double)c) / 255.0;
 336                                                 b = ((double)c) / 255.0;
 337                                         }
 338                                 }
 339                                 _img_buffer[(i*width+j)*3 + 0] = r * 65535.0;
 340                                 _img_buffer[(i*width+j)*3 + 1] = g * 65535.0;
 341                                 _img_buffer[(i*width+j)*3 + 2] = b * 65535.0;
 342                         }
 343                 }
 344         }
 345
 346         /* free stuff */
 347         if (img_buffer)
 348                 free(img_buffer);
 349         if (img_prev_buffer)
 350                 free(img_prev_buffer);
 351         if (img_next_buffer)
 352                 free(img_next_buffer);
 353         if (flow_map_x_prev)
 354                 free(flow_map_x_prev);
 355         if (flow_map_y_prev)
 356                 free(flow_map_y_prev);
 357         if (flow_map_x_next)
 358                 free(flow_map_x_next);
 359         if (flow_map_y_next)
 360                 free(flow_map_y_next);
 361         if (image_preview)
 362                 cvReleaseImage(&image_preview);
 363 }
 364
 365