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 /* draw flow matrix as lines */
  20 static void draw_lines_from_matrix(CvMat *flow, int width, int height, int steps, int color, IplImage *image)
  21 {
  22         int flow_width = width / steps;
  23         int flow_height = height / steps;
  24         int i, j, x, y;
  25         const float *f;
  26
  27         for (i = 0, y = steps/2; i < flow_height && y < height; i++, y += steps) {
  28                 f = (const float *)(flow->data.ptr + flow->step * y);
  29                 for (j = 0, x = steps/2; j < flow_width && x < width; j++, x += steps) {
  30                         cvLine(image,
  31                                 cvPoint(cvRound(x), cvRound(y)),
  32                                 cvPoint(cvRound(f[2*x] + x), cvRound(f[2*x+1] + y)),
  33                                 CV_RGB(color,color,color),
  34                                 1, 8, 0);
  35                 }
  36         }
  37 }
  38
  39 /* take flow matrix to generate a map */
  40 static void copy_matrix_to_map(CvMat *flow, int width, int height, double *flow_map_x, double *flow_map_y)
  41 {
  42         int x, y;
  43         const float *f;
  44
  45         for (y = 0; y < height; y++) {
  46                 f = (const float *)(flow->data.ptr + flow->step * y);
  47                 for (x = 0; x < width; x++) {
  48                         *flow_map_x++ = *f++;
  49                         *flow_map_y++ = *f++;
  50                 }
  51         }
  52 }
  53
  54 /*
  55  * create flow maps
  56  *
  57  * img_prev_buffer = image buffer of previous image (or NULL)
  58  * img_next_buffer = image buffer of next image (or NULL)
  59  * img_buffer = current image buffer
  60  * width, height = dimensions of image buffer
  61  * steps = pixles between each feature = size of grid to track
  62  * win_size = flow algorithm window size
  63  * max_flow = maximum length of allowed flow vector (to avoid errors)
  64  * flow_map_x_prev = flow map for delta x of previous image (result is stored here)
  65  * flow_map_y_prev = flow map for delta y of previous image (result is stored here)
  66  * flow_map_x_next = flow map for delta x of next image (result is stored here)
  67  * flow_map_y_next = flow map for delta y of next image (result is stored here)
  68  *
  69  */
  70 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)
  71 {
  72         IplImage *image_prev = NULL, *image = NULL, *image_next = NULL, *image_preview = _image_preview;
  73         CvMat *flow;
  74         CvSize size;
  75         int i, j;
  76
  77         const float pyrScale = 0.5;
  78         const float levels = 3;
  79         const float winsize = win_size;
  80         const float iterations = 8;
  81         const float polyN = 5;
  82         const float polySigma = 1.2;
  83         const int flags = 0;
  84
  85         flow = cvCreateMat(height, width, CV_32FC2);
  86         if (!flow) {
  87                 fprintf(stderr, "failed to allocate opencv matrix\n");
  88                 return NULL;
  89         }
  90
  91         /* apply images to opencv image */
  92         size.width = width;
  93         size.height = height;
  94         if (img_prev_buffer) {
  95                 image_prev = cvCreateImage(size, IPL_DEPTH_32F, 1);
  96         }
  97         if (img_buffer) {
  98                 image = cvCreateImage(size, IPL_DEPTH_32F, 1);
  99         }
 100         if (img_next_buffer) {
 101                 image_next = cvCreateImage(size, IPL_DEPTH_32F, 1);
 102         }
 103
 104         if (image_prev) {
 105                 for (i = 0; i < height; i++) {
 106                         for (j = 0; j < width; j++) {
 107                                 CV_IMAGE_ELEM(image_prev, float, i, j) = img_prev_buffer[i*width+j] * 65535.0;
 108                         }
 109                 }
 110         }
 111         if (image) {
 112                 for (i = 0; i < height; i++) {
 113                         for (j = 0; j < width; j++) {
 114                                 CV_IMAGE_ELEM(image, float, i, j) = img_buffer[i*width+j] * 65535.0;
 115                                 if (image_preview)
 116                                         CV_IMAGE_ELEM(image_preview, uchar, i, j) = (img_buffer[i*width+j] / 2) * 255.0;
 117                         }
 118                 }
 119         }
 120         if (image_next) {
 121                 for (i = 0; i < height; i++) {
 122                         for (j = 0; j < width; j++) {
 123                                 CV_IMAGE_ELEM(image_next, float, i, j) = img_next_buffer[i*width+j] * 65535.0;
 124                         }
 125                 }
 126         }
 127
 128         /* optical flow and compose previous and next image with current image */
 129         if (image) {
 130                 /* calc flow */
 131                 if (image_prev) {
 132                         cvCalcOpticalFlowFarneback(image, image_prev, flow, pyrScale, levels, winsize, iterations, polyN, polySigma, flags);
 133                         if (image_preview && steps > 1)
 134                                 draw_lines_from_matrix(flow, width, height, steps, 255, image_preview);
 135                         if (flow_map_x_prev && flow_map_y_prev)
 136                                 copy_matrix_to_map(flow, width, height, flow_map_x_prev, flow_map_y_prev);
 137                 }
 138                 if (image_next) {
 139                         cvCalcOpticalFlowFarneback(image, image_next, flow, pyrScale, levels, winsize, iterations, polyN, polySigma, flags);
 140                         if (image_preview && steps > 1)
 141                                 draw_lines_from_matrix(flow, width, height, steps, 192, image_preview);
 142                         if (flow_map_x_next && flow_map_y_next)
 143                                 copy_matrix_to_map(flow, width, height, flow_map_x_next, flow_map_y_next);
 144                 }
 145         }
 146
 147 #if 0
 148 #include<opencv/highgui.h>
 149 cvNamedWindow( "Display window", 0 );// Create a window for display.
 150 cvShowImage( "Display window", image );                   // Show our image inside it.
 151 #endif
 152
 153         if (image)
 154                 cvReleaseImage(&image);
 155         if (image_prev)
 156                 cvReleaseImage(&image_prev);
 157         if (image_next)
 158                 cvReleaseImage(&image_next);
 159
 160         cvReleaseMat(&flow);
 161
 162         return image_preview;
 163 }
 164
 165 /*
 166  * use img_buffer and create view of optical flow between this image, previous image and next image.
 167  *
 168  * filename_prev = name of previous image
 169  * filename_next = name of next image
 170  * img_buffer = current image buffer
 171  * width, height = dimensions of image buffer
 172  * win_size = flow algorithm window size
 173  * steps = pixles between each feature to visualize vectors (1 to disable)
 174  * uv_scale = apply vector (pointing to next plane) by applying uv-color (0 to disable)
 175  *
 176  */
 177 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)
 178 {
 179         double *img_prev_buffer = NULL, *img_next_buffer = NULL, *img_buffer;
 180         double *flow_map_x_prev = NULL, *flow_map_y_prev = NULL, *flow_map_x_next = NULL, *flow_map_y_next = NULL;
 181         IplImage *image_preview = NULL;
 182         CvSize size;
 183         unsigned char c;
 184         double r, g, b;
 185         int w, h;
 186         int i, j;
 187         unsigned short *img;
 188
 189 #if 0
 190         cvInitMatHeader(&kernel, 3, 3, CV_64FC1, vals, 0);
 191 #endif
 192
 193 //printf("%s %s\n", filename_prev, filename_next);
 194
 195         /* convert current image to double */
 196         img_buffer = (double *)malloc(width*height*3*sizeof(double));
 197         if (!img_buffer) {
 198                 printf("%s:failed to allocate memory\n", __func__);
 199                 return;
 200         }
 201         img2array_short(_img_buffer, width, height, img_buffer, width, height);
 202         rgb2yuv(img_buffer, img_buffer, width, height);
 203
 204         /* read previous image */
 205         if (filename_prev) {
 206                 img = load_img(&w, &h, filename_prev, 0);
 207                 if (img) {
 208                         if (w == width && h == height) {
 209                                 img_prev_buffer = (double *)malloc(w*h*3*sizeof(double));
 210                                 if (img_prev_buffer) {
 211                                         img2array_short(img, w, h, img_prev_buffer, w, h);
 212                                         rgb2yuv(img_prev_buffer, img_prev_buffer, width, height);
 213                                 }
 214                         }
 215                         free(img);
 216                 }
 217         }
 218
 219         /* read next image */
 220         if (filename_next) {
 221                 img = load_img(&w, &h, filename_next, 0);
 222                 if (img) {
 223                         if (w == width && h == height) {
 224                                 img_next_buffer = (double *)malloc(w*h*3*sizeof(double));
 225                                 if (img_next_buffer) {
 226                                         img2array_short(img, w, h, img_next_buffer, w, h);
 227                                         rgb2yuv(img_next_buffer, img_next_buffer, width, height);
 228                                 }
 229                         }
 230                         free(img);
 231                 }
 232         }
 233
 234         /* apply images to opencv image */
 235         size.width = width;
 236         size.height = height;
 237         image_preview = cvCreateImage(size, IPL_DEPTH_8U, 1);
 238
 239         flow_map_x_prev = (double *)malloc(sizeof(double)*width*height);
 240         flow_map_y_prev = (double *)malloc(sizeof(double)*width*height);
 241         flow_map_x_next = (double *)malloc(sizeof(double)*width*height);
 242         flow_map_y_next = (double *)malloc(sizeof(double)*width*height);
 243
 244         if (image_preview) {
 245                 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);
 246
 247                 /* paint vectors from image_preview to img_buffer */
 248                 for (i = 0; i < height; i++) {
 249                         for (j = 0; j < width; j++) {
 250                                 c = CV_IMAGE_ELEM(image_preview, uchar, i, j);
 251                                 if (c > 224) {
 252                                         r = 1;
 253                                         g = 0;
 254                                         b = 0;
 255                                 } else if (c > 160) {
 256                                         r = 0;
 257                                         g = 1;
 258                                         b = 0;
 259                                 } else {
 260                                         if (!steps && uv_scale)
 261                                                 c += 64;
 262                                         if (img_next_buffer && uv_scale) {
 263                                                 yuv2rgb_pixle(
 264                                                         (double)c / 255.0,
 265                                                         flow_map_x_next[width * i + j] / uv_scale,
 266                                                         flow_map_y_next[width * i + j] / uv_scale,
 267                                                         &r, &g, &b);
 268                                         } else {
 269                                                 r = ((double)c) / 255.0;
 270                                                 g = ((double)c) / 255.0;
 271                                                 b = ((double)c) / 255.0;
 272                                         }
 273                                 }
 274                                 _img_buffer[(i*width+j)*3 + 0] = r * 65535.0;
 275                                 _img_buffer[(i*width+j)*3 + 1] = g * 65535.0;
 276                                 _img_buffer[(i*width+j)*3 + 2] = b * 65535.0;
 277                         }
 278                 }
 279         }
 280
 281         /* free stuff */
 282         if (img_buffer)
 283                 free(img_buffer);
 284         if (img_prev_buffer)
 285                 free(img_prev_buffer);
 286         if (img_next_buffer)
 287                 free(img_next_buffer);
 288         if (flow_map_x_prev)
 289                 free(flow_map_x_prev);
 290         if (flow_map_y_prev)
 291                 free(flow_map_y_prev);
 292         if (flow_map_x_next)
 293                 free(flow_map_x_next);
 294         if (flow_map_y_next)
 295                 free(flow_map_y_next);
 296         if (image_preview)
 297                 cvReleaseImage(&image_preview);
 298 }
 299
 300