src/opticalflow.cpp

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