diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..19800bb --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required (VERSION 2.8) +project (FastPatchOF) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -Wno-unknown-pragmas -Wall -std=c++11 -msse4") #-Wall +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -Wno-unknown-pragmas -Wall -msse4") #-Wall + +# For ghc machines +set(OpenCV_DIR "/afs/cs/academic/class/15418-s17/public/sw/opencv/build") +set(Eigen3_DIR "/afs/cs.cmu.edu/academic/class/15418-s17/public/sw/eigen/build") + +FIND_PACKAGE(OpenCV REQUIRED) +FIND_PACKAGE(Eigen3 REQUIRED) + +message(STATUS "OpenCV library status:") +message(STATUS " version: ${OpenCV_VERSION}") +message(STATUS " libraries: ${OpenCV_LIBRARIES}") +message(STATUS " include path: ${OpenCV_INCLUDE_DIRS}") + +# On ghc, include DIR from FIND_PACKAGE is set wrong +set(EIGEN3_INCLUDE_DIR "/afs/cs.cmu.edu/academic/class/15418-s17/public/sw/eigen/include/eigen3") + +message(STATUS "Eigen3 library status:") +message(STATUS " version: ${EIGEN3_VERSION_STRING}") +message(STATUS " include path: ${EIGEN3_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIRS}") + +include_directories(${EIGEN3_INCLUDE_DIR}) + +# # # UNCOMMENT THIS IF YOU WANT TO USE OPENMP PARALLELIZATION +# add_definitions(-DWITH_OPENMP=true) +# FIND_PACKAGE( OpenMP REQUIRED) +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +# set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") +# set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") +# # +# # ENABLE PARALLEL FLOW AGGREGATION, CAN LEAD TO DATA RACES, BUT IN PRACTICE HAS ONLY A WEAK NEGATIVE EFFECT ON THE RESULT, [affects only PatGridClass::AggregateFlowDense() ] +# # add_definitions(-DUSE_PARALLEL_ON_FLOWAGGR) + + +set(CODEFILES run_dense.cpp oflow.cpp patch.cpp patchgrid.cpp refine_variational.cpp FDF1.0.1/image.c FDF1.0.1/opticalflow_aux.c FDF1.0.1/solver.c) + +# GrayScale, Optical Flow +add_executable (run_OF_INT ${CODEFILES}) +set_target_properties (run_OF_INT PROPERTIES COMPILE_DEFINITIONS "SELECTMODE=1") +set_property(TARGET run_OF_INT APPEND PROPERTY COMPILE_DEFINITIONS "SELECTCHANNEL=1") # use grey-valued image +TARGET_LINK_LIBRARIES(run_OF_INT ${OpenCV_LIBS}) + +# RGB, Optical Flow +add_executable (run_OF_RGB ${CODEFILES}) +set_target_properties (run_OF_RGB PROPERTIES COMPILE_DEFINITIONS "SELECTMODE=1") +set_property(TARGET run_OF_RGB APPEND PROPERTY COMPILE_DEFINITIONS "SELECTCHANNEL=3") # use RGB image +TARGET_LINK_LIBRARIES(run_OF_RGB ${OpenCV_LIBS}) + +# GrayScale, Depth from Stereo +add_executable (run_DE_INT ${CODEFILES}) +set_target_properties (run_DE_INT PROPERTIES COMPILE_DEFINITIONS "SELECTMODE=2") +set_property(TARGET run_DE_INT APPEND PROPERTY COMPILE_DEFINITIONS "SELECTCHANNEL=1") +TARGET_LINK_LIBRARIES(run_DE_INT ${OpenCV_LIBS}) + +# RGB, Depth from Stereo +add_executable (run_DE_RGB ${CODEFILES}) +set_target_properties (run_DE_RGB PROPERTIES COMPILE_DEFINITIONS "SELECTMODE=2") +set_property(TARGET run_DE_RGB APPEND PROPERTY COMPILE_DEFINITIONS "SELECTCHANNEL=3") +TARGET_LINK_LIBRARIES(run_DE_RGB ${OpenCV_LIBS}) + diff --git a/src/FDF1.0.1/image.c b/src/FDF1.0.1/image.c new file mode 100644 index 0000000..460e274 --- /dev/null +++ b/src/FDF1.0.1/image.c @@ -0,0 +1,774 @@ +#include +#include +#include +#include +#include + +#include "image.h" + +#include +typedef __v4sf v4sf; + +/********** Create/Delete **********/ + +/* allocate a new image of size width x height */ +image_t *image_new(const int width, const int height){ + image_t *image = (image_t*) malloc(sizeof(image_t)); + if(image == NULL){ + fprintf(stderr, "Error: image_new() - not enough memory !\n"); + exit(1); + } + image->width = width; + image->height = height; + image->stride = ( (width+3) / 4 ) * 4; + image->c1 = (float*) memalign(16, image->stride*height*sizeof(float)); + if(image->c1== NULL){ + fprintf(stderr, "Error: image_new() - not enough memory !\n"); + exit(1); + } + return image; +} + +/* allocate a new image and copy the content from src */ +image_t *image_cpy(const image_t *src){ + image_t *dst = image_new(src->width, src->height); + memcpy(dst->c1, src->c1, src->stride*src->height*sizeof(float)); + return dst; +} + +/* set all pixels values to zeros */ +void image_erase(image_t *image){ + memset(image->c1, 0, image->stride*image->height*sizeof(float)); +} + + +/* multiply an image by a scalar */ +void image_mul_scalar(image_t *image, const float scalar){ + int i; + v4sf* imp = (v4sf*) image->c1; + const v4sf scalarp = {scalar,scalar,scalar,scalar}; + for( i=0 ; istride/4*image->height ; i++){ + (*imp) *= scalarp; + imp+=1; + } +} + +/* free memory of an image */ +void image_delete(image_t *image){ + if(image == NULL){ + //fprintf(stderr, "Warning: Delete image --> Ignore action (image not allocated)\n"); + }else{ + free(image->c1); + free(image); + } +} + + +/* allocate a new color image of size width x height */ +color_image_t *color_image_new(const int width, const int height){ + color_image_t *image = (color_image_t*) malloc(sizeof(color_image_t)); + if(image == NULL){ + fprintf(stderr, "Error: color_image_new() - not enough memory !\n"); + exit(1); + } + image->width = width; + image->height = height; + image->stride = ( (width+3) / 4 ) * 4; + image->c1 = (float*) memalign(16, 3*image->stride*height*sizeof(float)); + if(image->c1 == NULL){ + fprintf(stderr, "Error: color_image_new() - not enough memory !\n"); + exit(1); + } + image->c2 = image->c1+image->stride*height; + image->c3 = image->c2+image->stride*height; + return image; +} + +/* allocate a new color image and copy the content from src */ +color_image_t *color_image_cpy(const color_image_t *src){ + color_image_t *dst = color_image_new(src->width, src->height); + memcpy(dst->c1, src->c1, 3*src->stride*src->height*sizeof(float)); + return dst; +} + +/* set all pixels values to zeros */ +void color_image_erase(color_image_t *image){ + memset(image->c1, 0, 3*image->stride*image->height*sizeof(float)); +} + +/* free memory of a color image */ +void color_image_delete(color_image_t *image){ + if(image){ + free(image->c1); // c2 and c3 was allocated at the same moment + free(image); + } +} + +/* reallocate the memory of an image to fit the new width height */ +void resize_if_needed_newsize(image_t *im, const int w, const int h){ + if(im->width != w || im->height != h){ + im->width = w; + im->height = h; + im->stride = ((w+3)/4)*4; + float *data = (float *) memalign(16, im->stride*h*sizeof(float)); + if(data == NULL){ + fprintf(stderr, "Error: resize_if_needed_newsize() - not enough memory !\n"); + exit(1); + } + free(im->c1); + im->c1 = data; + } +} + + +/************ Resizing *********/ + +/* resize an image to a new size (assumes a difference only in width) */ +static void image_resize_horiz(image_t *dst, const image_t *src){ + const float real_scale = ((float) src->width-1) / ((float) dst->width-1); + int i; + for(i = 0; i < dst->height; i++){ + int j; + for(j = 0; j < dst->width; j++){ + const int x = floor((float) j * real_scale); + const float dx = j * real_scale - x; + if(x >= (src->width - 1)){ + dst->c1[i * dst->stride + j] = src->c1[i * src->stride + src->width - 1]; + }else{ + dst->c1[i * dst->stride + j] = + (1.0f - dx) * src->c1[i * src->stride + x ] + + ( dx) * src->c1[i * src->stride + x + 1]; + } + } + } +} + +/* resize a color image to a new size (assumes a difference only in width) */ +static void color_image_resize_horiz(color_image_t *dst, const color_image_t *src){ + const float real_scale = ((float) src->width-1) / ((float) dst->width-1); + int i; + for(i = 0; i < dst->height; i++){ + int j; + for(j = 0; j < dst->width; j++){ + const int x = floor((float) j * real_scale); + const float dx = j * real_scale - x; + if(x >= (src->width - 1)){ + dst->c1[i * dst->stride + j] = src->c1[i * src->stride + src->width - 1]; + dst->c2[i * dst->stride + j] = src->c2[i * src->stride + src->width - 1]; + dst->c3[i * dst->stride + j] = src->c3[i * src->stride + src->width - 1]; + }else{ + dst->c1[i * dst->stride + j] = + (1.0f - dx) * src->c1[i * src->stride + x ] + + ( dx) * src->c1[i * src->stride + x + 1]; + dst->c2[i * dst->stride + j] = + (1.0f - dx) * src->c2[i * src->stride + x ] + + ( dx) * src->c2[i * src->stride + x + 1]; + dst->c3[i * dst->stride + j] = + (1.0f - dx) * src->c3[i * src->stride + x ] + + ( dx) * src->c3[i * src->stride + x + 1]; + } + } + } +} + +/* resize an image to a new size (assumes a difference only in height) */ +static void image_resize_vert(image_t *dst, const image_t *src){ + const float real_scale = ((float) src->height-1) / ((float) dst->height-1); + int i; + for(i = 0; i < dst->width; i++){ + int j; + for(j = 0; j < dst->height; j++){ + const int y = floor((float) j * real_scale); + const float dy = j * real_scale - y; + if(y >= (src->height - 1)){ + dst->c1[j * dst->stride + i] = src->c1[i + (src->height - 1) * src->stride]; + }else{ + dst->c1[j * dst->stride + i] = + (1.0f - dy) * src->c1[i + (y ) * src->stride] + + ( dy) * src->c1[i + (y + 1) * src->stride]; + } + } + } +} + +/* resize a color image to a new size (assumes a difference only in height) */ +static void color_image_resize_vert(color_image_t *dst, const color_image_t *src){ + const float real_scale = ((float) src->height) / ((float) dst->height); + int i; + for(i = 0; i < dst->width; i++){ + int j; + for(j = 0; j < dst->height; j++){ + const int y = floor((float) j * real_scale); + const float dy = j * real_scale - y; + if(y >= (src->height - 1)){ + dst->c1[j * dst->stride + i] = src->c1[i + (src->height - 1) * src->stride]; + dst->c2[j * dst->stride + i] = src->c2[i + (src->height - 1) * src->stride]; + dst->c3[j * dst->stride + i] = src->c3[i + (src->height - 1) * src->stride]; + }else{ + dst->c1[j * dst->stride + i] = + (1.0f - dy) * src->c1[i + y * src->stride] + + ( dy) * src->c1[i + (y + 1) * src->stride]; + dst->c2[j * dst->stride + i] = + (1.0f - dy) * src->c2[i + y * src->stride] + + ( dy) * src->c2[i + (y + 1) * src->stride]; + dst->c3[j * dst->stride + i] = + (1.0f - dy) * src->c3[i + y * src->stride] + + ( dy) * src->c3[i + (y + 1) * src->stride]; + } + } + } +} + +/* return a resize version of the image with bilinear interpolation */ +image_t *image_resize_bilinear(const image_t *src, const float scale){ + const int width = src->width, height = src->height; + const int newwidth = (int) (1.5f + (width-1) / scale); // 0.5f for rounding instead of flooring, and the remaining comes from scale = (dst-1)/(src-1) + const int newheight = (int) (1.5f + (height-1) / scale); + image_t *dst = image_new(newwidth,newheight); + if(height*newwidth < width*newheight){ + image_t *tmp = image_new(newwidth,height); + image_resize_horiz(tmp,src); + image_resize_vert(dst,tmp); + image_delete(tmp); + }else{ + image_t *tmp = image_new(width,newheight); + image_resize_vert(tmp,src); + image_resize_horiz(dst,tmp); + image_delete(tmp); + } + return dst; +} + +/* resize an image with bilinear interpolation to fit the new weidht, height ; reallocation is done if necessary */ +void image_resize_bilinear_newsize(image_t *dst, const image_t *src, const int new_width, const int new_height){ + resize_if_needed_newsize(dst,new_width,new_height); + if(new_width < new_height){ + image_t *tmp = image_new(new_width,src->height); + image_resize_horiz(tmp,src); + image_resize_vert(dst,tmp); + image_delete(tmp); + }else{ + image_t *tmp = image_new(src->width,new_height); + image_resize_vert(tmp,src); + image_resize_horiz(dst,tmp); + image_delete(tmp); + } +} + +/* resize a color image with bilinear interpolation */ +color_image_t *color_image_resize_bilinear(const color_image_t *src, const float scale){ + const int width = src->width, height = src->height; + const int newwidth = (int) (1.5f + (width-1) / scale); // 0.5f for rounding instead of flooring, and the remaining comes from scale = (dst-1)/(src-1) + const int newheight = (int) (1.5f + (height-1) / scale); + color_image_t *dst = color_image_new(newwidth,newheight); + if(height*newwidth < width*newheight){ + color_image_t *tmp = color_image_new(newwidth,height); + color_image_resize_horiz(tmp,src); + color_image_resize_vert(dst,tmp); + color_image_delete(tmp); + }else{ + color_image_t *tmp = color_image_new(width,newheight); + color_image_resize_vert(tmp,src); + color_image_resize_horiz(dst,tmp); + color_image_delete(tmp); + } + return dst; +} + +/************ Convolution ******/ + +/* return half coefficient of a gaussian filter +Details: +- return a float* containing the coefficient from middle to border of the filter, so starting by 0, +- it so contains half of the coefficient. +- sigma is the standard deviation. +- filter_order is an output where the size of the output array is stored */ +float *gaussian_filter(const float sigma, int *filter_order){ + if(sigma == 0.0f){ + fprintf(stderr, "gaussian_filter() error: sigma is zeros\n"); + exit(1); + } + if(!filter_order){ + fprintf(stderr, "gaussian_filter() error: filter_order is null\n"); + exit(1); + } + // computer the filter order as 1 + 2* floor(3*sigma) + *filter_order = floor(3*sigma); + if ( *filter_order == 0 ) + *filter_order = 1; + // compute coefficients + float *data = (float*) malloc(sizeof(float) * (2*(*filter_order)+1)); + if(data == NULL ){ + fprintf(stderr, "gaussian_filter() error: not enough memory\n"); + exit(1); + } + const float alpha = 1.0f/(2.0f*sigma*sigma); + float sum = 0.0f; + int i; + for(i=-(*filter_order) ; i<=*filter_order ; i++){ + data[i+(*filter_order)] = exp(-i*i*alpha); + sum += data[i+(*filter_order)]; + } + for(i=-(*filter_order) ; i<=*filter_order ; i++){ + data[i+(*filter_order)] /= sum; + } + // fill the output + float *data2 = (float*) malloc(sizeof(float)*(*filter_order+1)); + if(data2 == NULL ){ + fprintf(stderr, "gaussian_filter() error: not enough memory\n"); + exit(1); + } + memcpy(data2, &data[*filter_order], sizeof(float)*(*filter_order)+sizeof(float)); + free(data); + return data2; +} + +/* given half of the coef, compute the full coefficients and the accumulated coefficients */ +static void convolve_extract_coeffs(const int order, const float *half_coeffs, float *coeffs, float *coeffs_accu, const int even){ + int i; + float accu = 0.0; + if(even){ + for(i = 0 ; i <= order; i++){ + coeffs[order - i] = coeffs[order + i] = half_coeffs[i]; + } + for(i = 0 ; i <= order; i++){ + accu += coeffs[i]; + coeffs_accu[2 * order - i] = coeffs_accu[i] = accu; + } + }else{ + for(i = 0; i <= order; i++){ + coeffs[order - i] = +half_coeffs[i]; + coeffs[order + i] = -half_coeffs[i]; + } + for(i = 0 ; i <= order; i++){ + accu += coeffs[i]; + coeffs_accu[i] = accu; + coeffs_accu[2 * order - i]= -accu; + } + } +} + +/* create a convolution structure with a given order, half_coeffs, symmetric or anti-symmetric according to even parameter */ +convolution_t *convolution_new(const int order, const float *half_coeffs, const int even){ + convolution_t *conv = (convolution_t *) malloc(sizeof(convolution_t)); + if(conv == NULL){ + fprintf(stderr, "Error: convolution_new() - not enough memory !\n"); + exit(1); + } + conv->order = order; + conv->coeffs = (float *) malloc((2 * order + 1) * sizeof(float)); + if(conv->coeffs == NULL){ + fprintf(stderr, "Error: convolution_new() - not enough memory !\n"); + free(conv); + exit(1); + } + conv->coeffs_accu = (float *) malloc((2 * order + 1) * sizeof(float)); + if(conv->coeffs_accu == NULL){ + fprintf(stderr, "Error: convolution_new() - not enough memory !\n"); + free(conv->coeffs); + free(conv); + exit(1); + } + convolve_extract_coeffs(order, half_coeffs, conv->coeffs,conv->coeffs_accu, even); + return conv; +} + +static void convolve_vert_fast_3(image_t *dst, const image_t *src, const convolution_t *conv){ + const int iterline = (src->stride>>2)+1; + const float *coeff = conv->coeffs; + //const float *coeff_accu = conv->coeffs_accu; + v4sf *srcp = (v4sf*) src->c1, *dstp = (v4sf*) dst->c1; + v4sf *srcp_p1 = (v4sf*) (src->c1+src->stride); + int i; + for(i=iterline ; --i ; ){ // first line + *dstp = (coeff[0]+coeff[1])*(*srcp) + coeff[2]*(*srcp_p1); + dstp+=1; srcp+=1; srcp_p1+=1; + } + v4sf* srcp_m1 = (v4sf*) src->c1; + for(i=src->height-1 ; --i ; ){ // others line + int j; + for(j=iterline ; --j ; ){ + *dstp = coeff[0]*(*srcp_m1) + coeff[1]*(*srcp) + coeff[2]*(*srcp_p1); + dstp+=1; srcp_m1+=1; srcp+=1; srcp_p1+=1; + } + } + for(i=iterline ; --i ; ){ // last line + *dstp = coeff[0]*(*srcp_m1) + (coeff[1]+coeff[2])*(*srcp); + dstp+=1; srcp_m1+=1; srcp+=1; + } +} + +static void convolve_vert_fast_5(image_t *dst, const image_t *src, const convolution_t *conv){ + const int iterline = (src->stride>>2)+1; + const float *coeff = conv->coeffs; + //const float *coeff_accu = conv->coeffs_accu; + v4sf *srcp = (v4sf*) src->c1, *dstp = (v4sf*) dst->c1; + v4sf *srcp_p1 = (v4sf*) (src->c1+src->stride); + v4sf *srcp_p2 = (v4sf*) (src->c1+2*src->stride); + int i; + for(i=iterline ; --i ; ){ // first line + *dstp = (coeff[0]+coeff[1]+coeff[2])*(*srcp) + coeff[3]*(*srcp_p1) + coeff[4]*(*srcp_p2); + dstp+=1; srcp+=1; srcp_p1+=1; srcp_p2+=1; + } + v4sf* srcp_m1 = (v4sf*) src->c1; + for(i=iterline ; --i ; ){ // second line + *dstp = (coeff[0]+coeff[1])*(*srcp_m1) + coeff[2]*(*srcp) + coeff[3]*(*srcp_p1) + coeff[4]*(*srcp_p2); + dstp+=1; srcp_m1+=1; srcp+=1; srcp_p1+=1; srcp_p2+=1; + } + v4sf* srcp_m2 = (v4sf*) src->c1; + for(i=src->height-3 ; --i ; ){ // others line + int j; + for(j=iterline ; --j ; ){ + *dstp = coeff[0]*(*srcp_m2) + coeff[1]*(*srcp_m1) + coeff[2]*(*srcp) + coeff[3]*(*srcp_p1) + coeff[4]*(*srcp_p2); + dstp+=1; srcp_m2+=1;srcp_m1+=1; srcp+=1; srcp_p1+=1; srcp_p2+=1; + } + } + for(i=iterline ; --i ; ){ // second to last line + *dstp = coeff[0]*(*srcp_m2) + coeff[1]*(*srcp_m1) + coeff[2]*(*srcp) + (coeff[3]+coeff[4])*(*srcp_p1); + dstp+=1; srcp_m2+=1;srcp_m1+=1; srcp+=1; srcp_p1+=1; + } + for(i=iterline ; --i ; ){ // last line + *dstp = coeff[0]*(*srcp_m2) + coeff[1]*(*srcp_m1) + (coeff[2]+coeff[3]+coeff[4])*(*srcp); + dstp+=1; srcp_m2+=1;srcp_m1+=1; srcp+=1; + } +} + +static void convolve_horiz_fast_3(image_t *dst, const image_t *src, const convolution_t *conv){ + const int stride_minus_1 = src->stride-1; + const int iterline = (src->stride>>2); + const float *coeff = conv->coeffs; + v4sf *srcp = (v4sf*) src->c1, *dstp = (v4sf*) dst->c1; + // create shifted version of src + float *src_p1 = (float*) malloc(sizeof(float)*src->stride), + *src_m1 = (float*) malloc(sizeof(float)*src->stride); + int j; + for(j=0;jheight;j++){ + int i; + float *srcptr = (float*) srcp; + const float right_coef = srcptr[src->width-1]; + for(i=src->width;istride;i++) + srcptr[i] = right_coef; + src_m1[0] = srcptr[0]; + memcpy(src_m1+1, srcptr , sizeof(float)*stride_minus_1); + src_p1[stride_minus_1] = right_coef; + memcpy(src_p1, srcptr+1, sizeof(float)*stride_minus_1); + v4sf *srcp_p1 = (v4sf*) src_p1, *srcp_m1 = (v4sf*) src_m1; + + for(i=0;istride-1; + const int stride_minus_2 = src->stride-2; + const int iterline = (src->stride>>2); + const float *coeff = conv->coeffs; + v4sf *srcp = (v4sf*) src->c1, *dstp = (v4sf*) dst->c1; + float *src_p1 = (float*) malloc(sizeof(float)*src->stride*4); + float *src_p2 = src_p1+src->stride; + float *src_m1 = src_p2+src->stride; + float *src_m2 = src_m1+src->stride; + int j; + for(j=0;jheight;j++){ + int i; + float *srcptr = (float*) srcp; + const float right_coef = srcptr[src->width-1]; + for(i=src->width;istride;i++) + srcptr[i] = right_coef; + src_m1[0] = srcptr[0]; + memcpy(src_m1+1, srcptr , sizeof(float)*stride_minus_1); + src_m2[0] = srcptr[0]; + src_m2[1] = srcptr[0]; + memcpy(src_m2+2, srcptr , sizeof(float)*stride_minus_2); + src_p1[stride_minus_1] = right_coef; + memcpy(src_p1, srcptr+1, sizeof(float)*stride_minus_1); + src_p2[stride_minus_1] = right_coef; + src_p2[stride_minus_2] = right_coef; + memcpy(src_p2, srcptr+2, sizeof(float)*stride_minus_2); + + v4sf *srcp_p1 = (v4sf*) src_p1, *srcp_p2 = (v4sf*) src_p2, *srcp_m1 = (v4sf*) src_m1, *srcp_m2 = (v4sf*) src_m2; + + for(i=0;iorder==1){ + convolve_horiz_fast_3(dest,src,conv); + return; + }else if(conv->order==2){ + convolve_horiz_fast_5(dest,src,conv); + return; + } + float *in = src->c1; + float * out = dest->c1; + int i, j, ii; + float *o = out; + int i0 = -conv->order; + int i1 = +conv->order; + float *coeff = conv->coeffs + conv->order; + float *coeff_accu = conv->coeffs_accu + conv->order; + for(j = 0; j < src->height; j++){ + const float *al = in + j * src->stride; + const float *f0 = coeff + i0; + float sum; + for(i = 0; i < -i0; i++){ + sum=coeff_accu[-i - 1] * al[0]; + for(ii = i1 + i; ii >= 0; ii--){ + sum += coeff[ii - i] * al[ii]; + } + *o++ = sum; + } + for(; i < src->width - i1; i++){ + sum = 0; + for(ii = i1 - i0; ii >= 0; ii--){ + sum += f0[ii] * al[ii]; + } + al++; + *o++ = sum; + } + for(; i < src->width; i++){ + sum = coeff_accu[src->width - i] * al[src->width - i0 - 1 - i]; + for(ii = src->width - i0 - 1 - i; ii >= 0; ii--){ + sum += f0[ii] * al[ii]; + } + al++; + *o++ = sum; + } + for(i = 0; i < src->stride - src->width; i++){ + o++; + } + } +} + +/* perform a vertical convolution of an image */ +void convolve_vert(image_t *dest, const image_t *src, const convolution_t *conv){ + if(conv->order==1){ + convolve_vert_fast_3(dest,src,conv); + return; + }else if(conv->order==2){ + convolve_vert_fast_5(dest,src,conv); + return; + } + float *in = src->c1; + float *out = dest->c1; + int i0 = -conv->order; + int i1 = +conv->order; + float *coeff = conv->coeffs + conv->order; + float *coeff_accu = conv->coeffs_accu + conv->order; + int i, j, ii; + float *o = out; + const float *alast = in + src->stride * (src->height - 1); + const float *f0 = coeff + i0; + for(i = 0; i < -i0; i++){ + float fa = coeff_accu[-i - 1]; + const float *al = in + i * src->stride; + for(j = 0; j < src->width; j++){ + float sum = fa * in[j]; + for(ii = -i; ii <= i1; ii++){ + sum += coeff[ii] * al[j + ii * src->stride]; + } + *o++ = sum; + } + for(j = 0; j < src->stride - src->width; j++) + { + o++; + } + } + for(; i < src->height - i1; i++){ + const float *al = in + (i + i0) * src->stride; + for(j = 0; j < src->width; j++){ + float sum = 0; + const float *al2 = al; + for(ii = 0; ii <= i1 - i0; ii++){ + sum += f0[ii] * al2[0]; + al2 += src->stride; + } + *o++ = sum; + al++; + } + for(j = 0; j < src->stride - src->width; j++){ + o++; + } + } + for(;i < src->height; i++){ + float fa = coeff_accu[src->height - i]; + const float *al = in + i * src->stride; + for(j = 0; j < src->width; j++){ + float sum = fa * alast[j]; + for(ii = i0; ii <= src->height - 1 - i; ii++){ + sum += coeff[ii] * al[j + ii * src->stride]; + } + *o++ = sum; + } + for(j = 0; j < src->stride - src->width; j++){ + o++; + } + } +} + +/* free memory of a convolution structure */ +void convolution_delete(convolution_t *conv){ + if(conv) + { + free(conv->coeffs); + free(conv->coeffs_accu); + free(conv); + } +} + +/* perform horizontal and/or vertical convolution to a color image */ +void color_image_convolve_hv(color_image_t *dst, const color_image_t *src, const convolution_t *horiz_conv, const convolution_t *vert_conv){ + const int width = src->width, height = src->height, stride = src->stride; + // separate channels of images + image_t src_red = {width,height,stride,src->c1}, src_green = {width,height,stride,src->c2}, src_blue = {width,height,stride,src->c3}, + dst_red = {width,height,stride,dst->c1}, dst_green = {width,height,stride,dst->c2}, dst_blue = {width,height,stride,dst->c3}; + // horizontal and vertical + if(horiz_conv != NULL && vert_conv != NULL){ + float *tmp_data = malloc(sizeof(float)*stride*height); + if(tmp_data == NULL){ + fprintf(stderr,"error color_image_convolve_hv(): not enough memory\n"); + exit(1); + } + image_t tmp = {width,height,stride,tmp_data}; + // perform convolution for each channel + convolve_horiz(&tmp,&src_red,horiz_conv); + convolve_vert(&dst_red,&tmp,vert_conv); + convolve_horiz(&tmp,&src_green,horiz_conv); + convolve_vert(&dst_green,&tmp,vert_conv); + convolve_horiz(&tmp,&src_blue,horiz_conv); + convolve_vert(&dst_blue,&tmp,vert_conv); + free(tmp_data); + }else if(horiz_conv != NULL && vert_conv == NULL){ // only horizontal + convolve_horiz(&dst_red,&src_red,horiz_conv); + convolve_horiz(&dst_green,&src_green,horiz_conv); + convolve_horiz(&dst_blue,&src_blue,horiz_conv); + }else if(vert_conv != NULL && horiz_conv == NULL){ // only vertical + convolve_vert(&dst_red,&src_red,vert_conv); + convolve_vert(&dst_green,&src_green,vert_conv); + convolve_vert(&dst_blue,&src_blue,vert_conv); + } +} + +/* perform horizontal and/or vertical convolution to a single band image*/ +void image_convolve_hv(image_t *dst, const image_t *src, const convolution_t *horiz_conv, const convolution_t *vert_conv) +{ + const int width = src->width, height = src->height, stride = src->stride; + // separate channels of images + image_t src_red = {width,height,stride,src->c1}, + dst_red = {width,height,stride,dst->c1}; + // horizontal and vertical + if(horiz_conv != NULL && vert_conv != NULL){ + float *tmp_data = malloc(sizeof(float)*stride*height); + if(tmp_data == NULL){ + fprintf(stderr,"error image_convolve_hv(): not enough memory\n"); + exit(1); + } + image_t tmp = {width,height,stride,tmp_data}; + // perform convolution for each channel + convolve_horiz(&tmp,&src_red,horiz_conv); + convolve_vert(&dst_red,&tmp,vert_conv); + free(tmp_data); + }else if(horiz_conv != NULL && vert_conv == NULL){ // only horizontal + convolve_horiz(&dst_red,&src_red,horiz_conv); + }else if(vert_conv != NULL && horiz_conv == NULL){ // only vertical + convolve_vert(&dst_red,&src_red,vert_conv); + } +} + +/************ Pyramid **********/ + +/* create new color image pyramid structures */ +static color_image_pyramid_t* color_image_pyramid_new(){ + color_image_pyramid_t* pyr = (color_image_pyramid_t*) malloc(sizeof(color_image_pyramid_t)); + if(pyr == NULL){ + fprintf(stderr,"Error in color_image_pyramid_new(): not enough memory\n"); + exit(1); + } + pyr->min_size = -1; + pyr->scale_factor = -1.0f; + pyr->size = -1; + pyr->images = NULL; + return pyr; +} + +/* set the size of the color image pyramid structures (reallocate the array of pointers to images) */ +static void color_image_pyramid_set_size(color_image_pyramid_t* pyr, const int size){ + if(size<0){ + fprintf(stderr,"Error in color_image_pyramid_set_size(): size is negative\n"); + exit(1); + } + if(pyr->images == NULL){ + pyr->images = (color_image_t**) malloc(sizeof(color_image_t*)*size); + }else{ + pyr->images = (color_image_t**) realloc(pyr->images,sizeof(color_image_t*)*size); + } + if(pyr->images == NULL){ + fprintf(stderr,"Error in color_image_pyramid_set_size(): not enough memory\n"); + exit(1); + } + pyr->size = size; +} + +/* create a pyramid of color images using a given scale factor, stopping when one dimension reach min_size and with applying a gaussian smoothing of standard deviation spyr (no smoothing if 0) */ +color_image_pyramid_t *color_image_pyramid_create(const color_image_t *src, const float scale_factor, const int min_size, const float spyr){ + const int nb_max_scale = 1000; + // allocate structure + color_image_pyramid_t *pyramid = color_image_pyramid_new(); + pyramid->min_size = min_size; + pyramid->scale_factor = scale_factor; + convolution_t *conv = NULL; + if(spyr>0.0f){ + int fsize; + float *filter_coef = gaussian_filter(spyr, &fsize); + conv = convolution_new(fsize, filter_coef, 1); + free(filter_coef); + } + color_image_pyramid_set_size(pyramid, nb_max_scale); + pyramid->images[0] = color_image_cpy(src); + int i; + for( i=1 ; iimages[i-1]->width, oldheight = pyramid->images[i-1]->height; + const int newwidth = (int) (1.5f + (oldwidth-1) / scale_factor); + const int newheight = (int) (1.5f + (oldheight-1) / scale_factor); + if( newwidth <= min_size || newheight <= min_size){ + color_image_pyramid_set_size(pyramid, i); + break; + } + if(spyr>0.0f){ + color_image_t* tmp = color_image_new(oldwidth, oldheight); + color_image_convolve_hv(tmp,pyramid->images[i-1], conv, conv); + pyramid->images[i]= color_image_resize_bilinear(tmp, scale_factor); + color_image_delete(tmp); + }else{ + pyramid->images[i] = color_image_resize_bilinear(pyramid->images[i-1], scale_factor); + } + } + if(spyr>0.0f){ + convolution_delete(conv); + } + return pyramid; +} + +/* delete the structure of a pyramid of color images and all the color images in it*/ +void color_image_pyramid_delete(color_image_pyramid_t *pyr){ + if(pyr==NULL){ + return; + } + int i; + for(i=0 ; isize ; i++){ + color_image_delete(pyr->images[i]); + } + free(pyr->images); + free(pyr); +} diff --git a/src/FDF1.0.1/image.h b/src/FDF1.0.1/image.h new file mode 100644 index 0000000..5363292 --- /dev/null +++ b/src/FDF1.0.1/image.h @@ -0,0 +1,136 @@ +#ifndef __IMAGE_H_ +#define __IMAGE_H_ + +#include + +#define MIN_TA(a, b) ((a) < (b) ? (a) : (b)) +#define MAX_TA(a, b) ((a) > (b) ? (a) : (b)) +#define MINMAX_TA(a,b) MIN_TA( MAX_TA(a,0) , b-1 ) + +#ifdef __cplusplus +extern "C" { +#endif + + +/********** STRUCTURES *********/ + +/* structure for 1-channel image */ +typedef struct image_s +{ + int width; /* Width of the image */ + int height; /* Height of the image */ + int stride; /* Width of the memory (width + paddind such that it is a multiple of 4) */ + float *c1; /* Image data, aligned */ +} image_t; + +/* structure for 3-channels image stored with one layer per color, it assumes that c2 = c1+width*height and c3 = c2+width*height. */ +typedef struct color_image_s +{ + int width; /* Width of the image */ + int height; /* Height of the image */ + int stride; /* Width of the memory (width + paddind such that it is a multiple of 4) */ + float *c1; /* Color 1, aligned */ + float *c2; /* Color 2, consecutive to c1*/ + float *c3; /* Color 3, consecutive to c2 */ +} color_image_t; + +/* structure for color image pyramid */ +typedef struct color_image_pyramid_s +{ + float scale_factor; /* difference of scale between two levels */ + int min_size; /* minimum size for width or height at the coarsest level */ + int size; /* number of levels in the pyramid */ + color_image_t **images; /* list of images with images[0] the original one, images[size-1] the finest one */ +} color_image_pyramid_t; + +/* structure for convolutions */ +typedef struct convolution_s +{ + int order; /* Order of the convolution */ + float *coeffs; /* Coefficients */ + float *coeffs_accu; /* Accumulated coefficients */ +} convolution_t; + +/********** Create/Delete **********/ + +/* allocate a new image of size width x height */ +image_t *image_new(const int width, const int height); + +/* allocate a new image and copy the content from src */ +image_t *image_cpy(const image_t *src); + +/* set all pixels values to zeros */ +void image_erase(image_t *image); + +/* free memory of an image */ +void image_delete(image_t *image); + +/* multiply an image by a scalar */ +void image_mul_scalar(image_t *image, const float scalar); + +/* allocate a new color image of size width x height */ +color_image_t *color_image_new(const int width, const int height); + +/* allocate a new color image and copy the content from src */ +color_image_t *color_image_cpy(const color_image_t *src); + +/* set all pixels values to zeros */ +void color_image_erase(color_image_t *image); + +/* free memory of a color image */ +void color_image_delete(color_image_t *image); + +/* reallocate the memory of an image to fit the new width height */ +void resize_if_needed_newsize(image_t *im, const int w, const int h); + +/************ Resizing *********/ + +/* resize an image with bilinear interpolation */ +image_t *image_resize_bilinear(const image_t *src, const float scale); + +/* resize an image with bilinear interpolation to fit the new weidht, height ; reallocation is done if necessary */ +void image_resize_bilinear_newsize(image_t *dst, const image_t *src, const int new_width, const int new_height); + +/* resize a color image with bilinear interpolation */ +color_image_t *color_image_resize_bilinear(const color_image_t *src, const float scale); + +/************ Convolution ******/ + +/* return half coefficient of a gaussian filter */ +float *gaussian_filter(const float sigma, int *fSize); + +/* create a convolution structure with a given order, half_coeffs, symmetric or anti-symmetric according to even parameter */ +convolution_t *convolution_new(int order, const float *half_coeffs, const int even); + +/* perform an horizontal convolution of an image */ +void convolve_horiz(image_t *dest, const image_t *src, const convolution_t *conv); + +/* perform a vertical convolution of an image */ +void convolve_vert(image_t *dest, const image_t *src, const convolution_t *conv); + +/* free memory of a convolution structure */ +void convolution_delete(convolution_t *conv); + +/* perform horizontal and/or vertical convolution to a color image */ +void color_image_convolve_hv(color_image_t *dst, const color_image_t *src, const convolution_t *horiz_conv, const convolution_t *vert_conv); + +/* perform horizontal and/or vertical convolution to a single band image */ +void image_convolve_hv(image_t *dst, const image_t *src, const convolution_t *horiz_conv, const convolution_t *vert_conv); + + +/************ Pyramid **********/ + +/* create a pyramid of color images using a given scale factor, stopping when one dimension reach min_size and with applying a gaussian smoothing of standard deviation spyr (no smoothing if 0) */ +color_image_pyramid_t *color_image_pyramid_create(const color_image_t *src, const float scale_factor, const int min_size, const float spyr); + +/* delete the structure of a pyramid of color images */ +void color_image_pyramid_delete(color_image_pyramid_t *pyr); + +#ifdef __cplusplus +} +#endif + + +#endif + + diff --git a/src/FDF1.0.1/opticalflow_aux.c b/src/FDF1.0.1/opticalflow_aux.c new file mode 100644 index 0000000..957a946 --- /dev/null +++ b/src/FDF1.0.1/opticalflow_aux.c @@ -0,0 +1,629 @@ +#include +#include +#include +#include +#include "opticalflow_aux.h" + +#include +typedef __v4sf v4sf; + +#define datanorm 0.1f*0.1f//0.01f // square of the normalization factor +#define epsilon_color (0.001f*0.001f)//0.000001f +#define epsilon_grad (0.001f*0.001f)//0.000001f +#define epsilon_desc (0.001f*0.001f)//0.000001f +#define epsilon_smooth (0.001f*0.001f)//0.000001f + +/* warp a color image according to a flow. src is the input image, wx and wy, the input flow. dst is the warped image and mask contains 0 or 1 if the pixels goes outside/inside image boundaries */ +#if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // use single band image_delete +void image_warp(image_t *dst, image_t *mask, const image_t *src, const image_t *wx, const image_t *wy) +#else +void image_warp(color_image_t *dst, image_t *mask, const color_image_t *src, const image_t *wx, const image_t *wy) +#endif +{ + int i, j, offset, incr_line = mask->stride-mask->width, x, y, x1, x2, y1, y2; + float xx, yy, dx, dy; + for(j=0,offset=0 ; jheight ; j++) + { + for(i=0 ; iwidth ; i++,offset++) + { + xx = i+wx->c1[offset]; + yy = j+wy->c1[offset]; + x = floor(xx); + y = floor(yy); + dx = xx-x; + dy = yy-y; + mask->c1[offset] = (xx>=0 && xx<=src->width-1 && yy>=0 && yy<=src->height-1); + x1 = MINMAX_TA(x,src->width); + x2 = MINMAX_TA(x+1,src->width); + y1 = MINMAX_TA(y,src->height); + y2 = MINMAX_TA(y+1,src->height); + dst->c1[offset] = + src->c1[y1*src->stride+x1]*(1.0f-dx)*(1.0f-dy) + + src->c1[y1*src->stride+x2]*dx*(1.0f-dy) + + src->c1[y2*src->stride+x1]*(1.0f-dx)*dy + + src->c1[y2*src->stride+x2]*dx*dy; + #if (SELECTCHANNEL==3) + dst->c2[offset] = + src->c2[y1*src->stride+x1]*(1.0f-dx)*(1.0f-dy) + + src->c2[y1*src->stride+x2]*dx*(1.0f-dy) + + src->c2[y2*src->stride+x1]*(1.0f-dx)*dy + + src->c2[y2*src->stride+x2]*dx*dy; + dst->c3[offset] = + src->c3[y1*src->stride+x1]*(1.0f-dx)*(1.0f-dy) + + src->c3[y1*src->stride+x2]*dx*(1.0f-dy) + + src->c3[y2*src->stride+x1]*(1.0f-dx)*dy + + src->c3[y2*src->stride+x2]*dx*dy; + #endif + } + offset += incr_line; + } +} + + +/* compute image first and second order spatio-temporal derivatives of a color image */ +#if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // use single band image_delete +void get_derivatives(const image_t *im1, const image_t *im2, const convolution_t *deriv, + image_t *dx, image_t *dy, image_t *dt, + image_t *dxx, image_t *dxy, image_t *dyy, image_t *dxt, image_t *dyt) +#else +void get_derivatives(const color_image_t *im1, const color_image_t *im2, const convolution_t *deriv, + color_image_t *dx, color_image_t *dy, color_image_t *dt, + color_image_t *dxx, color_image_t *dxy, color_image_t *dyy, color_image_t *dxt, color_image_t *dyt) +#endif +{ + // derivatives are computed on the mean of the first image and the warped second image +#if (SELECTCHANNEL==1 | SELECTCHANNEL==2) + image_t *tmp_im2 = image_new(im2->width,im2->height); + v4sf *tmp_im2p = (v4sf*) tmp_im2->c1, *dtp = (v4sf*) dt->c1, *im1p = (v4sf*) im1->c1, *im2p = (v4sf*) im2->c1; + const v4sf half = {0.5f,0.5f,0.5f,0.5f}; + int i=0; + for(i=0 ; iheight*im1->stride/4 ; i++){ + *tmp_im2p = half * ( (*im2p) + (*im1p) ); + *dtp = (*im2p)-(*im1p); + dtp+=1; im1p+=1; im2p+=1; tmp_im2p+=1; + } + // compute all other derivatives + image_convolve_hv(dx, tmp_im2, deriv, NULL); + image_convolve_hv(dy, tmp_im2, NULL, deriv); + image_convolve_hv(dxx, dx, deriv, NULL); + image_convolve_hv(dxy, dx, NULL, deriv); + image_convolve_hv(dyy, dy, NULL, deriv); + image_convolve_hv(dxt, dt, deriv, NULL); + image_convolve_hv(dyt, dt, NULL, deriv); + // free memory + image_delete(tmp_im2); +#else + color_image_t *tmp_im2 = color_image_new(im2->width,im2->height); + v4sf *tmp_im2p = (v4sf*) tmp_im2->c1, *dtp = (v4sf*) dt->c1, *im1p = (v4sf*) im1->c1, *im2p = (v4sf*) im2->c1; + const v4sf half = {0.5f,0.5f,0.5f,0.5f}; + int i=0; + for(i=0 ; i<3*im1->height*im1->stride/4 ; i++){ + *tmp_im2p = half * ( (*im2p) + (*im1p) ); + *dtp = (*im2p)-(*im1p); + dtp+=1; im1p+=1; im2p+=1; tmp_im2p+=1; + } + // compute all other derivatives + color_image_convolve_hv(dx, tmp_im2, deriv, NULL); + color_image_convolve_hv(dy, tmp_im2, NULL, deriv); + color_image_convolve_hv(dxx, dx, deriv, NULL); + color_image_convolve_hv(dxy, dx, NULL, deriv); + color_image_convolve_hv(dyy, dy, NULL, deriv); + color_image_convolve_hv(dxt, dt, deriv, NULL); + color_image_convolve_hv(dyt, dt, NULL, deriv); + // free memory + color_image_delete(tmp_im2); +#endif +} + + +/* compute the smoothness term */ +/* It is represented as two images, the first one for horizontal smoothness, the second for vertical + in dst_horiz, the pixel i,j represents the smoothness weight between pixel i,j and i,j+1 + in dst_vert, the pixel i,j represents the smoothness weight between pixel i,j and i+1,j */ +void compute_smoothness(image_t *dst_horiz, image_t *dst_vert, const image_t *uu, const image_t *vv, const convolution_t *deriv_flow, const float quarter_alpha){ + const int width = uu->width, height = vv->height, stride = uu->stride; + int j; + image_t *ux = image_new(width,height), *vx = image_new(width,height), *uy = image_new(width,height), *vy = image_new(width,height), *smoothness = image_new(width,height); + // compute derivatives [-0.5 0 0.5] + convolve_horiz(ux, uu, deriv_flow); + convolve_horiz(vx, vv, deriv_flow); + convolve_vert(uy, uu, deriv_flow); + convolve_vert(vy, vv, deriv_flow); + // compute smoothness + v4sf *uxp = (v4sf*) ux->c1, *vxp = (v4sf*) vx->c1, *uyp = (v4sf*) uy->c1, *vyp = (v4sf*) vy->c1, *sp = (v4sf*) smoothness->c1; + const v4sf qa = {quarter_alpha,quarter_alpha,quarter_alpha,quarter_alpha}; + const v4sf epsmooth = {epsilon_smooth,epsilon_smooth,epsilon_smooth,epsilon_smooth}; + for(j=0 ; j< height*stride/4 ; j++){ + *sp = qa / __builtin_ia32_sqrtps( (*uxp)*(*uxp) + (*uyp)*(*uyp) + (*vxp)*(*vxp) + (*vyp)*(*vyp) + epsmooth ); + sp+=1;uxp+=1; uyp+=1; vxp+=1; vyp+=1; + } + image_delete(ux); image_delete(uy); image_delete(vx); image_delete(vy); + // compute dst_horiz + v4sf *dsthp = (v4sf*) dst_horiz->c1; sp = (v4sf*) smoothness->c1; + float *sp_shift = (float*) memalign(16, stride*sizeof(float)); // aligned shifted copy of the current line + for(j=0;jc1[j*stride+width-1], 0, sizeof(float)*(stride-width+1)); + } + free(sp_shift); + // compute dst_vert + v4sf *dstvp = (v4sf*) dst_vert->c1, *sp_bottom = (v4sf*) (smoothness->c1+stride); sp = (v4sf*) smoothness->c1; + for(j=0 ; j<(height-1)*stride/4 ; j++){ + *dstvp = (*sp) + (*sp_bottom); + dstvp+=1; sp+=1; sp_bottom+=1; + } + memset( &dst_vert->c1[(height-1)*stride], 0, sizeof(float)*stride); + image_delete(smoothness); +} + + + + + +/* sub the laplacian (smoothness term) to the right-hand term */ +void sub_laplacian(image_t *dst, const image_t *src, const image_t *weight_horiz, const image_t *weight_vert){ + int j; + const int offsetline = src->stride-src->width; + float *src_ptr = src->c1, *dst_ptr = dst->c1, *weight_horiz_ptr = weight_horiz->c1; + // horizontal filtering + for(j=src->height+1;--j;){ // faster than for(j=0;jheight;j++) + int i; + for(i=src->width;--i;){ + const float tmp = (*weight_horiz_ptr)*((*(src_ptr+1))-(*src_ptr)); + *dst_ptr += tmp; + *(dst_ptr+1) -= tmp; + dst_ptr++; + src_ptr++; + weight_horiz_ptr++; + } + dst_ptr += offsetline+1; + src_ptr += offsetline+1; + weight_horiz_ptr += offsetline+1; + } + + v4sf *wvp = (v4sf*) weight_vert->c1, *srcp = (v4sf*) src->c1, *srcp_s = (v4sf*) (src->c1+src->stride), *dstp = (v4sf*) dst->c1, *dstp_s = (v4sf*) (dst->c1+src->stride); + for(j=1+(src->height-1)*src->stride/4 ; --j ;){ + const v4sf tmp = (*wvp) * ((*srcp_s)-(*srcp)); + *dstp += tmp; + *dstp_s -= tmp; + wvp+=1; srcp+=1; srcp_s+=1; dstp+=1; dstp_s+=1; + } +} + +/* compute the dataterm and the matching term + a11 a12 a22 represents the 2x2 diagonal matrix, b1 and b2 the right hand side + other (color) images are input */ +void compute_data_and_match(image_t *a11, image_t *a12, image_t *a22, image_t *b1, image_t *b2, image_t *mask, image_t *wx, image_t *wy, image_t *du, image_t *dv, image_t *uu, image_t *vv, color_image_t *Ix, color_image_t *Iy, color_image_t *Iz, color_image_t *Ixx, color_image_t *Ixy, color_image_t *Iyy, color_image_t *Ixz, color_image_t *Iyz, image_t *desc_weight, image_t *desc_flow_x, image_t *desc_flow_y, const float half_delta_over3, const float half_beta, const float half_gamma_over3){ + + const v4sf dnorm = {datanorm, datanorm, datanorm, datanorm}; + const v4sf hdover3 = {half_delta_over3, half_delta_over3, half_delta_over3, half_delta_over3}; + const v4sf epscolor = {epsilon_color, epsilon_color, epsilon_color, epsilon_color}; + const v4sf hgover3 = {half_gamma_over3, half_gamma_over3, half_gamma_over3, half_gamma_over3}; + const v4sf epsgrad = {epsilon_grad, epsilon_grad, epsilon_grad, epsilon_grad}; + const v4sf hbeta = {half_beta,half_beta,half_beta,half_beta}; + const v4sf epsdesc = {epsilon_desc,epsilon_desc,epsilon_desc,epsilon_desc}; + + v4sf *dup = (v4sf*) du->c1, *dvp = (v4sf*) dv->c1, + *maskp = (v4sf*) mask->c1, + *a11p = (v4sf*) a11->c1, *a12p = (v4sf*) a12->c1, *a22p = (v4sf*) a22->c1, + *b1p = (v4sf*) b1->c1, *b2p = (v4sf*) b2->c1, + *ix1p=(v4sf*)Ix->c1, *iy1p=(v4sf*)Iy->c1, *iz1p=(v4sf*)Iz->c1, *ixx1p=(v4sf*)Ixx->c1, *ixy1p=(v4sf*)Ixy->c1, *iyy1p=(v4sf*)Iyy->c1, *ixz1p=(v4sf*)Ixz->c1, *iyz1p=(v4sf*) Iyz->c1, + *ix2p=(v4sf*)Ix->c2, *iy2p=(v4sf*)Iy->c2, *iz2p=(v4sf*)Iz->c2, *ixx2p=(v4sf*)Ixx->c2, *ixy2p=(v4sf*)Ixy->c2, *iyy2p=(v4sf*)Iyy->c2, *ixz2p=(v4sf*)Ixz->c2, *iyz2p=(v4sf*) Iyz->c2, + *ix3p=(v4sf*)Ix->c3, *iy3p=(v4sf*)Iy->c3, *iz3p=(v4sf*)Iz->c3, *ixx3p=(v4sf*)Ixx->c3, *ixy3p=(v4sf*)Ixy->c3, *iyy3p=(v4sf*)Iyy->c3, *ixz3p=(v4sf*)Ixz->c3, *iyz3p=(v4sf*) Iyz->c3, + *uup = (v4sf*) uu->c1, *vvp = (v4sf*)vv->c1, *wxp = (v4sf*)wx->c1, *wyp = (v4sf*)wy->c1, + *descflowxp = (v4sf*)desc_flow_x->c1, *descflowyp = (v4sf*)desc_flow_y->c1, *descweightp = (v4sf*)desc_weight->c1; + + memset(a11->c1, 0, sizeof(float)*uu->height*uu->stride); + memset(a12->c1, 0, sizeof(float)*uu->height*uu->stride); + memset(a22->c1, 0, sizeof(float)*uu->height*uu->stride); + memset(b1->c1 , 0, sizeof(float)*uu->height*uu->stride); + memset(b2->c1 , 0, sizeof(float)*uu->height*uu->stride); + + int i; + for(i = 0 ; iheight*uu->stride/4 ; i++){ + v4sf tmp, tmp2, tmp3, tmp4, tmp5, tmp6, n1, n2, n3, n4, n5, n6; + // dpsi color + if(half_delta_over3){ + tmp = *iz1p + (*ix1p)*(*dup) + (*iy1p)*(*dvp); + n1 = (*ix1p) * (*ix1p) + (*iy1p) * (*iy1p) + dnorm; + tmp2 = *iz2p + (*ix2p)*(*dup) + (*iy2p)*(*dvp); + n2 = (*ix2p) * (*ix2p) + (*iy2p) * (*iy2p) + dnorm; + tmp3 = *iz3p + (*ix3p)*(*dup) + (*iy3p)*(*dvp); + n3 = (*ix3p) * (*ix3p) + (*iy3p) * (*iy3p) + dnorm; + tmp = (*maskp) * hdover3 / __builtin_ia32_sqrtps(tmp*tmp/n1 + tmp2*tmp2/n2 + tmp3*tmp3/n3 + epscolor); + tmp3 = tmp/n3; tmp2 = tmp/n2; tmp /= n1; + *a11p += tmp * (*ix1p) * (*ix1p); + *a12p += tmp * (*ix1p) * (*iy1p); + *a22p += tmp * (*iy1p) * (*iy1p); + *b1p -= tmp * (*iz1p) * (*ix1p); + *b2p -= tmp * (*iz1p) * (*iy1p); + *a11p += tmp2 * (*ix2p) * (*ix2p); + *a12p += tmp2 * (*ix2p) * (*iy2p); + *a22p += tmp2 * (*iy2p) * (*iy2p); + *b1p -= tmp2 * (*iz2p) * (*ix2p); + *b2p -= tmp2 * (*iz2p) * (*iy2p); + *a11p += tmp3 * (*ix3p) * (*ix3p); + *a12p += tmp3 * (*ix3p) * (*iy3p); + *a22p += tmp3 * (*iy3p) * (*iy3p); + *b1p -= tmp3 * (*iz3p) * (*ix3p); + *b2p -= tmp3 * (*iz3p) * (*iy3p); + } + // dpsi gradient + n1 = (*ixx1p) * (*ixx1p) + (*ixy1p) * (*ixy1p) + dnorm; + n2 = (*iyy1p) * (*iyy1p) + (*ixy1p) * (*ixy1p) + dnorm; + tmp = *ixz1p + (*ixx1p) * (*dup) + (*ixy1p) * (*dvp); + tmp2 = *iyz1p + (*ixy1p) * (*dup) + (*iyy1p) * (*dvp); + n3 = (*ixx2p) * (*ixx2p) + (*ixy2p) * (*ixy2p) + dnorm; + n4 = (*iyy2p) * (*iyy2p) + (*ixy2p) * (*ixy2p) + dnorm; + tmp3 = *ixz2p + (*ixx2p) * (*dup) + (*ixy2p) * (*dvp); + tmp4 = *iyz2p + (*ixy2p) * (*dup) + (*iyy2p) * (*dvp); + n5 = (*ixx3p) * (*ixx3p) + (*ixy3p) * (*ixy3p) + dnorm; + n6 = (*iyy3p) * (*iyy3p) + (*ixy3p) * (*ixy3p) + dnorm; + tmp5 = *ixz3p + (*ixx3p) * (*dup) + (*ixy3p) * (*dvp); + tmp6 = *iyz3p + (*ixy3p) * (*dup) + (*iyy3p) * (*dvp); + tmp = (*maskp) * hgover3 / __builtin_ia32_sqrtps(tmp*tmp/n1 + tmp2*tmp2/n2 + tmp3*tmp3/n3 + tmp4*tmp4/n4 + tmp5*tmp5/n5 + tmp6*tmp6/n6 + epsgrad); + tmp6 = tmp/n6; tmp5 = tmp/n5; tmp4 = tmp/n4; tmp3 = tmp/n3; tmp2 = tmp/n2; tmp /= n1; + *a11p += tmp *(*ixx1p)*(*ixx1p) + tmp2*(*ixy1p)*(*ixy1p); + *a12p += tmp *(*ixx1p)*(*ixy1p) + tmp2*(*ixy1p)*(*iyy1p); + *a22p += tmp2*(*iyy1p)*(*iyy1p) + tmp *(*ixy1p)*(*ixy1p); + *b1p -= tmp *(*ixx1p)*(*ixz1p) + tmp2*(*ixy1p)*(*iyz1p); + *b2p -= tmp2*(*iyy1p)*(*iyz1p) + tmp *(*ixy1p)*(*ixz1p); + *a11p += tmp3*(*ixx2p)*(*ixx2p) + tmp4*(*ixy2p)*(*ixy2p); + *a12p += tmp3*(*ixx2p)*(*ixy2p) + tmp4*(*ixy2p)*(*iyy2p); + *a22p += tmp4*(*iyy2p)*(*iyy2p) + tmp3*(*ixy2p)*(*ixy2p); + *b1p -= tmp3*(*ixx2p)*(*ixz2p) + tmp4*(*ixy2p)*(*iyz2p); + *b2p -= tmp4*(*iyy2p)*(*iyz2p) + tmp3*(*ixy2p)*(*ixz2p); + *a11p += tmp5*(*ixx3p)*(*ixx3p) + tmp6*(*ixy3p)*(*ixy3p); + *a12p += tmp5*(*ixx3p)*(*ixy3p) + tmp6*(*ixy3p)*(*iyy3p); + *a22p += tmp6*(*iyy3p)*(*iyy3p) + tmp5*(*ixy3p)*(*ixy3p); + *b1p -= tmp5*(*ixx3p)*(*ixz3p) + tmp6*(*ixy3p)*(*iyz3p); + *b2p -= tmp6*(*iyy3p)*(*iyz3p) + tmp5*(*ixy3p)*(*ixz3p); + if(half_beta){ // dpsi_match + tmp = *uup - (*descflowxp); + tmp2 = *vvp - (*descflowyp); + tmp = hbeta*(*descweightp)/__builtin_ia32_sqrtps(tmp*tmp+tmp2*tmp2+epsdesc); + *a11p += tmp; + *a22p += tmp; + *b1p -= tmp*((*wxp)-(*descflowxp)); + *b2p -= tmp*((*wyp)-(*descflowyp)); + } + dup+=1; dvp+=1; maskp+=1; a11p+=1; a12p+=1; a22p+=1; b1p+=1; b2p+=1; + ix1p+=1; iy1p+=1; iz1p+=1; ixx1p+=1; ixy1p+=1; iyy1p+=1; ixz1p+=1; iyz1p+=1; + ix2p+=1; iy2p+=1; iz2p+=1; ixx2p+=1; ixy2p+=1; iyy2p+=1; ixz2p+=1; iyz2p+=1; + ix3p+=1; iy3p+=1; iz3p+=1; ixx3p+=1; ixy3p+=1; iyy3p+=1; ixz3p+=1; iyz3p+=1; + uup+=1;vvp+=1;wxp+=1; wyp+=1;descflowxp+=1;descflowyp+=1;descweightp+=1; + } +} + +/* compute the dataterm // REMOVED MATCHING TERM + a11 a12 a22 represents the 2x2 diagonal matrix, b1 and b2 the right hand side + other (color) images are input */ +#if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // use single band image_delete +void compute_data(image_t *a11, image_t *a12, image_t *a22, image_t *b1, image_t *b2, image_t *mask, image_t *wx, image_t *wy, image_t *du, image_t *dv, image_t *uu, image_t *vv, image_t *Ix, image_t *Iy, image_t *Iz, image_t *Ixx, image_t *Ixy, image_t *Iyy, image_t *Ixz, image_t *Iyz, const float half_delta_over3, const float half_beta, const float half_gamma_over3) +#else +void compute_data(image_t *a11, image_t *a12, image_t *a22, image_t *b1, image_t *b2, image_t *mask, image_t *wx, image_t *wy, image_t *du, image_t *dv, image_t *uu, image_t *vv, color_image_t *Ix, color_image_t *Iy, color_image_t *Iz, color_image_t *Ixx, color_image_t *Ixy, color_image_t *Iyy, color_image_t *Ixz, color_image_t *Iyz, const float half_delta_over3, const float half_beta, const float half_gamma_over3) +#endif +{ + const v4sf dnorm = {datanorm, datanorm, datanorm, datanorm}; + const v4sf hdover3 = {half_delta_over3, half_delta_over3, half_delta_over3, half_delta_over3}; + const v4sf epscolor = {epsilon_color, epsilon_color, epsilon_color, epsilon_color}; + const v4sf hgover3 = {half_gamma_over3, half_gamma_over3, half_gamma_over3, half_gamma_over3}; + const v4sf epsgrad = {epsilon_grad, epsilon_grad, epsilon_grad, epsilon_grad}; + //const v4sf hbeta = {half_beta,half_beta,half_beta,half_beta}; + //const v4sf epsdesc = {epsilon_desc,epsilon_desc,epsilon_desc,epsilon_desc}; + + v4sf *dup = (v4sf*) du->c1, *dvp = (v4sf*) dv->c1, + *maskp = (v4sf*) mask->c1, + *a11p = (v4sf*) a11->c1, *a12p = (v4sf*) a12->c1, *a22p = (v4sf*) a22->c1, + *b1p = (v4sf*) b1->c1, *b2p = (v4sf*) b2->c1, + *ix1p=(v4sf*)Ix->c1, *iy1p=(v4sf*)Iy->c1, *iz1p=(v4sf*)Iz->c1, *ixx1p=(v4sf*)Ixx->c1, *ixy1p=(v4sf*)Ixy->c1, *iyy1p=(v4sf*)Iyy->c1, *ixz1p=(v4sf*)Ixz->c1, *iyz1p=(v4sf*) Iyz->c1, + #if (SELECTCHANNEL==3) + *ix2p=(v4sf*)Ix->c2, *iy2p=(v4sf*)Iy->c2, *iz2p=(v4sf*)Iz->c2, *ixx2p=(v4sf*)Ixx->c2, *ixy2p=(v4sf*)Ixy->c2, *iyy2p=(v4sf*)Iyy->c2, *ixz2p=(v4sf*)Ixz->c2, *iyz2p=(v4sf*) Iyz->c2, + *ix3p=(v4sf*)Ix->c3, *iy3p=(v4sf*)Iy->c3, *iz3p=(v4sf*)Iz->c3, *ixx3p=(v4sf*)Ixx->c3, *ixy3p=(v4sf*)Ixy->c3, *iyy3p=(v4sf*)Iyy->c3, *ixz3p=(v4sf*)Ixz->c3, *iyz3p=(v4sf*) Iyz->c3, + #endif + *uup = (v4sf*) uu->c1, *vvp = (v4sf*)vv->c1, *wxp = (v4sf*)wx->c1, *wyp = (v4sf*)wy->c1; + + + memset(a11->c1, 0, sizeof(float)*uu->height*uu->stride); + memset(a12->c1, 0, sizeof(float)*uu->height*uu->stride); + memset(a22->c1, 0, sizeof(float)*uu->height*uu->stride); + memset(b1->c1 , 0, sizeof(float)*uu->height*uu->stride); + memset(b2->c1 , 0, sizeof(float)*uu->height*uu->stride); + + int i; + for(i = 0 ; iheight*uu->stride/4 ; i++){ + v4sf tmp, tmp2, n1, n2; + #if (SELECTCHANNEL==3) + v4sf tmp3, tmp4, tmp5, tmp6, n3, n4, n5, n6; + #endif + // dpsi color + if(half_delta_over3){ + tmp = *iz1p + (*ix1p)*(*dup) + (*iy1p)*(*dvp); + n1 = (*ix1p) * (*ix1p) + (*iy1p) * (*iy1p) + dnorm; + #if (SELECTCHANNEL==3) + tmp2 = *iz2p + (*ix2p)*(*dup) + (*iy2p)*(*dvp); + n2 = (*ix2p) * (*ix2p) + (*iy2p) * (*iy2p) + dnorm; + tmp3 = *iz3p + (*ix3p)*(*dup) + (*iy3p)*(*dvp); + n3 = (*ix3p) * (*ix3p) + (*iy3p) * (*iy3p) + dnorm; + tmp = (*maskp) * hdover3 / __builtin_ia32_sqrtps(tmp*tmp/n1 + tmp2*tmp2/n2 + tmp3*tmp3/n3 + epscolor); + tmp3 = tmp/n3; tmp2 = tmp/n2; tmp /= n1; + #else + tmp = (*maskp) * hdover3 / __builtin_ia32_sqrtps(3 * tmp*tmp/n1 + epscolor); + tmp /= n1; + #endif + *a11p += tmp * (*ix1p) * (*ix1p); + *a12p += tmp * (*ix1p) * (*iy1p); + *a22p += tmp * (*iy1p) * (*iy1p); + *b1p -= tmp * (*iz1p) * (*ix1p); + *b2p -= tmp * (*iz1p) * (*iy1p); + #if (SELECTCHANNEL==3) + *a11p += tmp2 * (*ix2p) * (*ix2p); + *a12p += tmp2 * (*ix2p) * (*iy2p); + *a22p += tmp2 * (*iy2p) * (*iy2p); + *b1p -= tmp2 * (*iz2p) * (*ix2p); + *b2p -= tmp2 * (*iz2p) * (*iy2p); + *a11p += tmp3 * (*ix3p) * (*ix3p); + *a12p += tmp3 * (*ix3p) * (*iy3p); + *a22p += tmp3 * (*iy3p) * (*iy3p); + *b1p -= tmp3 * (*iz3p) * (*ix3p); + *b2p -= tmp3 * (*iz3p) * (*iy3p); + #endif + } + + // dpsi gradient + n1 = (*ixx1p) * (*ixx1p) + (*ixy1p) * (*ixy1p) + dnorm; + n2 = (*iyy1p) * (*iyy1p) + (*ixy1p) * (*ixy1p) + dnorm; + tmp = *ixz1p + (*ixx1p) * (*dup) + (*ixy1p) * (*dvp); + tmp2 = *iyz1p + (*ixy1p) * (*dup) + (*iyy1p) * (*dvp); + #if (SELECTCHANNEL==3) + n3 = (*ixx2p) * (*ixx2p) + (*ixy2p) * (*ixy2p) + dnorm; + n4 = (*iyy2p) * (*iyy2p) + (*ixy2p) * (*ixy2p) + dnorm; + tmp3 = *ixz2p + (*ixx2p) * (*dup) + (*ixy2p) * (*dvp); + tmp4 = *iyz2p + (*ixy2p) * (*dup) + (*iyy2p) * (*dvp); + n5 = (*ixx3p) * (*ixx3p) + (*ixy3p) * (*ixy3p) + dnorm; + n6 = (*iyy3p) * (*iyy3p) + (*ixy3p) * (*ixy3p) + dnorm; + tmp5 = *ixz3p + (*ixx3p) * (*dup) + (*ixy3p) * (*dvp); + tmp6 = *iyz3p + (*ixy3p) * (*dup) + (*iyy3p) * (*dvp); + tmp = (*maskp) * hgover3 / __builtin_ia32_sqrtps(tmp*tmp/n1 + tmp2*tmp2/n2 + tmp3*tmp3/n3 + tmp4*tmp4/n4 + tmp5*tmp5/n5 + tmp6*tmp6/n6 + epsgrad); + tmp6 = tmp/n6; tmp5 = tmp/n5; tmp4 = tmp/n4; tmp3 = tmp/n3; tmp2 = tmp/n2; tmp /= n1; + #else + tmp = (*maskp) * hgover3 / __builtin_ia32_sqrtps(3* tmp*tmp/n1 + 3* tmp2*tmp2/n2 + epsgrad); + tmp2 = tmp/n2; tmp /= n1; + #endif + *a11p += tmp *(*ixx1p)*(*ixx1p) + tmp2*(*ixy1p)*(*ixy1p); + *a12p += tmp *(*ixx1p)*(*ixy1p) + tmp2*(*ixy1p)*(*iyy1p); + *a22p += tmp2*(*iyy1p)*(*iyy1p) + tmp *(*ixy1p)*(*ixy1p); + *b1p -= tmp *(*ixx1p)*(*ixz1p) + tmp2*(*ixy1p)*(*iyz1p); + *b2p -= tmp2*(*iyy1p)*(*iyz1p) + tmp *(*ixy1p)*(*ixz1p); + #if (SELECTCHANNEL==3) + *a11p += tmp3*(*ixx2p)*(*ixx2p) + tmp4*(*ixy2p)*(*ixy2p); + *a12p += tmp3*(*ixx2p)*(*ixy2p) + tmp4*(*ixy2p)*(*iyy2p); + *a22p += tmp4*(*iyy2p)*(*iyy2p) + tmp3*(*ixy2p)*(*ixy2p); + *b1p -= tmp3*(*ixx2p)*(*ixz2p) + tmp4*(*ixy2p)*(*iyz2p); + *b2p -= tmp4*(*iyy2p)*(*iyz2p) + tmp3*(*ixy2p)*(*ixz2p); + *a11p += tmp5*(*ixx3p)*(*ixx3p) + tmp6*(*ixy3p)*(*ixy3p); + *a12p += tmp5*(*ixx3p)*(*ixy3p) + tmp6*(*ixy3p)*(*iyy3p); + *a22p += tmp6*(*iyy3p)*(*iyy3p) + tmp5*(*ixy3p)*(*ixy3p); + *b1p -= tmp5*(*ixx3p)*(*ixz3p) + tmp6*(*ixy3p)*(*iyz3p); + *b2p -= tmp6*(*iyy3p)*(*iyz3p) + tmp5*(*ixy3p)*(*ixz3p); + #endif + + + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // multiply system to make smoothing parameters same for RGB and single-channel image + *a11p *= 3; + *a12p *= 3; + *a22p *= 3; + *b1p *= 3; + *b2p *= 3; + + #endif + + dup+=1; dvp+=1; maskp+=1; a11p+=1; a12p+=1; a22p+=1; b1p+=1; b2p+=1; + ix1p+=1; iy1p+=1; iz1p+=1; ixx1p+=1; ixy1p+=1; iyy1p+=1; ixz1p+=1; iyz1p+=1; + #if (SELECTCHANNEL==3) + ix2p+=1; iy2p+=1; iz2p+=1; ixx2p+=1; ixy2p+=1; iyy2p+=1; ixz2p+=1; iyz2p+=1; + ix3p+=1; iy3p+=1; iz3p+=1; ixx3p+=1; ixy3p+=1; iyy3p+=1; ixz3p+=1; iyz3p+=1; + #endif + uup+=1;vvp+=1;wxp+=1; wyp+=1; + + } +} + + + +/* compute the dataterm // REMOVED MATCHING TERM + a11 a12 a22 represents the 2x2 diagonal matrix, b1 and b2 the right hand side + other (color) images are input */ +#if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // use single band image_delete +void compute_data_DE(image_t *a11, image_t *b1, image_t *mask, image_t *wx, image_t *du, image_t *uu, image_t *Ix, image_t *Iy, image_t *Iz, image_t *Ixx, image_t *Ixy, image_t *Iyy, image_t *Ixz, image_t *Iyz, const float half_delta_over3, const float half_beta, const float half_gamma_over3) +#else +void compute_data_DE(image_t *a11, image_t *b1, image_t *mask, image_t *wx, image_t *du, image_t *uu, color_image_t *Ix, color_image_t *Iy, color_image_t *Iz, color_image_t *Ixx, color_image_t *Ixy, color_image_t *Iyy, color_image_t *Ixz, color_image_t *Iyz, const float half_delta_over3, const float half_beta, const float half_gamma_over3) +#endif +{ + const v4sf dnorm = {datanorm, datanorm, datanorm, datanorm}; + const v4sf hdover3 = {half_delta_over3, half_delta_over3, half_delta_over3, half_delta_over3}; + const v4sf epscolor = {epsilon_color, epsilon_color, epsilon_color, epsilon_color}; + const v4sf hgover3 = {half_gamma_over3, half_gamma_over3, half_gamma_over3, half_gamma_over3}; + const v4sf epsgrad = {epsilon_grad, epsilon_grad, epsilon_grad, epsilon_grad}; + //const v4sf hbeta = {half_beta,half_beta,half_beta,half_beta}; + //const v4sf epsdesc = {epsilon_desc,epsilon_desc,epsilon_desc,epsilon_desc}; + + v4sf *dup = (v4sf*) du->c1, + *maskp = (v4sf*) mask->c1, + *a11p = (v4sf*) a11->c1, + *b1p = (v4sf*) b1->c1, + *ix1p=(v4sf*)Ix->c1, *iy1p=(v4sf*)Iy->c1, *iz1p=(v4sf*)Iz->c1, *ixx1p=(v4sf*)Ixx->c1, *ixy1p=(v4sf*)Ixy->c1, *iyy1p=(v4sf*)Iyy->c1, *ixz1p=(v4sf*)Ixz->c1, *iyz1p=(v4sf*) Iyz->c1, + #if (SELECTCHANNEL==3) + *ix2p=(v4sf*)Ix->c2, *iy2p=(v4sf*)Iy->c2, *iz2p=(v4sf*)Iz->c2, *ixx2p=(v4sf*)Ixx->c2, *ixy2p=(v4sf*)Ixy->c2, *iyy2p=(v4sf*)Iyy->c2, *ixz2p=(v4sf*)Ixz->c2, *iyz2p=(v4sf*) Iyz->c2, + *ix3p=(v4sf*)Ix->c3, *iy3p=(v4sf*)Iy->c3, *iz3p=(v4sf*)Iz->c3, *ixx3p=(v4sf*)Ixx->c3, *ixy3p=(v4sf*)Ixy->c3, *iyy3p=(v4sf*)Iyy->c3, *ixz3p=(v4sf*)Ixz->c3, *iyz3p=(v4sf*) Iyz->c3, + #endif + *uup = (v4sf*) uu->c1, *wxp = (v4sf*)wx->c1; + + + memset(a11->c1, 0, sizeof(float)*uu->height*uu->stride); + memset(b1->c1 , 0, sizeof(float)*uu->height*uu->stride); + + int i; + for(i = 0 ; iheight*uu->stride/4 ; i++){ + v4sf tmp, tmp2, n1, n2; + #if (SELECTCHANNEL==3) + v4sf tmp3, tmp4, tmp5, tmp6, n3, n4, n5, n6; + #endif + // dpsi color + if(half_delta_over3){ + tmp = *iz1p + (*ix1p)*(*dup); + n1 = (*ix1p) * (*ix1p) + (*iy1p) * (*iy1p) + dnorm; + #if (SELECTCHANNEL==3) + tmp2 = *iz2p + (*ix2p)*(*dup); + n2 = (*ix2p) * (*ix2p) + (*iy2p) * (*iy2p) + dnorm; + tmp3 = *iz3p + (*ix3p)*(*dup); + n3 = (*ix3p) * (*ix3p) + (*iy3p) * (*iy3p) + dnorm; + tmp = (*maskp) * hdover3 / __builtin_ia32_sqrtps(tmp*tmp/n1 + tmp2*tmp2/n2 + tmp3*tmp3/n3 + epscolor); + tmp3 = tmp/n3; tmp2 = tmp/n2; tmp /= n1; + #else + tmp = (*maskp) * hdover3 / __builtin_ia32_sqrtps(3 * tmp*tmp/n1 + epscolor); + tmp /= n1; + #endif + *a11p += tmp * (*ix1p) * (*ix1p); + *b1p -= tmp * (*iz1p) * (*ix1p); + #if (SELECTCHANNEL==3) + *a11p += tmp2 * (*ix2p) * (*ix2p); + *b1p -= tmp2 * (*iz2p) * (*ix2p); + *a11p += tmp3 * (*ix3p) * (*ix3p); + *b1p -= tmp3 * (*iz3p) * (*ix3p); + #endif + } + // dpsi gradient + n1 = (*ixx1p) * (*ixx1p) + (*ixy1p) * (*ixy1p) + dnorm; + n2 = (*iyy1p) * (*iyy1p) + (*ixy1p) * (*ixy1p) + dnorm; + tmp = *ixz1p + (*ixx1p) * (*dup); + tmp2 = *iyz1p + (*ixy1p) * (*dup); + #if (SELECTCHANNEL==3) + n3 = (*ixx2p) * (*ixx2p) + (*ixy2p) * (*ixy2p) + dnorm; + n4 = (*iyy2p) * (*iyy2p) + (*ixy2p) * (*ixy2p) + dnorm; + tmp3 = *ixz2p + (*ixx2p) * (*dup); + tmp4 = *iyz2p + (*ixy2p) * (*dup); + n5 = (*ixx3p) * (*ixx3p) + (*ixy3p) * (*ixy3p) + dnorm; + n6 = (*iyy3p) * (*iyy3p) + (*ixy3p) * (*ixy3p) + dnorm; + tmp5 = *ixz3p + (*ixx3p) * (*dup); + tmp6 = *iyz3p + (*ixy3p) * (*dup); + tmp = (*maskp) * hgover3 / __builtin_ia32_sqrtps(tmp*tmp/n1 + tmp2*tmp2/n2 + tmp3*tmp3/n3 + tmp4*tmp4/n4 + tmp5*tmp5/n5 + tmp6*tmp6/n6 + epsgrad); + tmp6 = tmp/n6; tmp5 = tmp/n5; tmp4 = tmp/n4; tmp3 = tmp/n3; tmp2 = tmp/n2; tmp /= n1; + #else + tmp = (*maskp) * hgover3 / __builtin_ia32_sqrtps(3* tmp*tmp/n1 + 3* tmp2*tmp2/n2 + epsgrad); + tmp2 = tmp/n2; tmp /= n1; + #endif + *a11p += tmp *(*ixx1p)*(*ixx1p) + tmp2*(*ixy1p)*(*ixy1p); + *b1p -= tmp *(*ixx1p)*(*ixz1p) + tmp2*(*ixy1p)*(*iyz1p); + #if (SELECTCHANNEL==3) + *a11p += tmp3*(*ixx2p)*(*ixx2p) + tmp4*(*ixy2p)*(*ixy2p); + *b1p -= tmp3*(*ixx2p)*(*ixz2p) + tmp4*(*ixy2p)*(*iyz2p); + *a11p += tmp5*(*ixx3p)*(*ixx3p) + tmp6*(*ixy3p)*(*ixy3p); + *b1p -= tmp5*(*ixx3p)*(*ixz3p) + tmp6*(*ixy3p)*(*iyz3p); + #endif + + + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // multiply system to make smoothing parameters same for RGB and single-channel image + *a11p *= 3; + *b1p *= 3; + #endif + + dup+=1; maskp+=1; a11p+=1; b1p+=1; + ix1p+=1; iy1p+=1; iz1p+=1; ixx1p+=1; ixy1p+=1; iyy1p+=1; ixz1p+=1; iyz1p+=1; + #if (SELECTCHANNEL==3) + ix2p+=1; iy2p+=1; iz2p+=1; ixx2p+=1; ixy2p+=1; iyy2p+=1; ixz2p+=1; iyz2p+=1; + ix3p+=1; iy3p+=1; iz3p+=1; ixx3p+=1; ixy3p+=1; iyy3p+=1; ixz3p+=1; iyz3p+=1; + #endif + uup+=1; wxp+=1; + + } +} + + + +/* resize the descriptors to the new size using a weighted mean */ +void descflow_resize(image_t *dst_flow_x, image_t *dst_flow_y, image_t *dst_weight, const image_t *src_flow_x, const image_t *src_flow_y, const image_t *src_weight){ + const int src_width = src_flow_x->width, src_height = src_flow_x->height, src_stride = src_flow_x->stride, + dst_width = dst_flow_x->width, dst_height = dst_flow_x->height, dst_stride = dst_flow_x->stride; + const float scale_x = ((float)dst_width-1)/((float)src_width-1), scale_y = ((float)dst_height-1)/((float)src_height-1); + image_erase(dst_flow_x); image_erase(dst_flow_y); image_erase(dst_weight); + int j; + for( j=0 ; jc1[j*src_stride+i]; + if( weight<0.0000000001f ) continue; + const float xx = ((float)i)*scale_x; + const float xxf = floor(xx); + const float dx = xx-xxf; + const int x1 = MINMAX_TA( (int) xxf , dst_width); + const int x2 = MINMAX_TA( (int) xxf+1 , dst_width); + float weightxy, newweight; + if( dx ){ + if( dy ){ + weightxy = weight*dx*dy; + newweight = dst_weight->c1[y2*dst_stride+x2] + weightxy; + dst_flow_x->c1[y2*dst_stride+x2] = (dst_flow_x->c1[y2*dst_stride+x2]*dst_weight->c1[y2*dst_stride+x2] + src_flow_x->c1[j*src_stride+i]*weightxy*scale_x)/newweight; + dst_flow_y->c1[y2*dst_stride+x2] = (dst_flow_y->c1[y2*dst_stride+x2]*dst_weight->c1[y2*dst_stride+x2] + src_flow_y->c1[j*src_stride+i]*weightxy*scale_y)/newweight; + dst_weight->c1[y2*dst_stride+x2] = newweight; + } + weightxy = weight*dx*(1.0f-dy); + newweight = dst_weight->c1[y1*dst_stride+x2] + weightxy; + dst_flow_x->c1[y1*dst_stride+x2] = (dst_flow_x->c1[y1*dst_stride+x2]*dst_weight->c1[y1*dst_stride+x2] + src_flow_x->c1[j*src_stride+i]*weightxy*scale_x)/newweight; + dst_flow_y->c1[y1*dst_stride+x2] = (dst_flow_y->c1[y1*dst_stride+x2]*dst_weight->c1[y1*dst_stride+x2] + src_flow_y->c1[j*src_stride+i]*weightxy*scale_y)/newweight; + dst_weight->c1[y1*dst_stride+x2] = newweight; + } + if( dy ) { + weightxy = weight*(1.0f-dx)*dy; + newweight = dst_weight->c1[y2*dst_stride+x1] + weightxy; + dst_flow_x->c1[y2*dst_stride+x1] = (dst_flow_x->c1[y2*dst_stride+x1]*dst_weight->c1[y2*dst_stride+x1] + src_flow_x->c1[j*src_stride+i]*weightxy*scale_x)/newweight; + dst_flow_y->c1[y2*dst_stride+x1] = (dst_flow_y->c1[y2*dst_stride+x1]*dst_weight->c1[y2*dst_stride+x1] + src_flow_y->c1[j*src_stride+i]*weightxy*scale_y)/newweight; + dst_weight->c1[y2*dst_stride+x1] = newweight; + } + weightxy = weight*(1.0f-dx)*(1.0f-dy); + newweight = dst_weight->c1[y1*dst_stride+x1] + weightxy; + dst_flow_x->c1[y1*dst_stride+x1] = (dst_flow_x->c1[y1*dst_stride+x1]*dst_weight->c1[y1*dst_stride+x1] + src_flow_x->c1[j*src_stride+i]*weightxy*scale_x)/newweight; + dst_flow_y->c1[y1*dst_stride+x1] = (dst_flow_y->c1[y1*dst_stride+x1]*dst_weight->c1[y1*dst_stride+x1] + src_flow_y->c1[j*src_stride+i]*weightxy*scale_y)/newweight; + dst_weight->c1[y1*dst_stride+x1] = newweight; + } + } +} + +/* resize the descriptors to the new size using a nearest neighbor method while keeping the descriptor with the higher weight at the end */ +void descflow_resize_nn(image_t *dst_flow_x, image_t *dst_flow_y, image_t *dst_weight, const image_t *src_flow_x, const image_t *src_flow_y, const image_t *src_weight){ + const int src_width = src_flow_x->width, src_height = src_flow_x->height, src_stride = src_flow_x->stride, + dst_width = dst_flow_x->width, dst_height = dst_flow_x->height, dst_stride = dst_flow_x->stride; + const float scale_x = ((float)dst_width-1)/((float)src_width-1), scale_y = ((float)dst_height-1)/((float)src_height-1); + image_erase(dst_flow_x); image_erase(dst_flow_y); image_erase(dst_weight); + int j; + for( j=0 ; jc1[j*src_stride+i]; + if( !weight ) + continue; + const float xx = ((float)i)*scale_x; + const int x = (int) 0.5f+xx; // equivalent to round(xx) + if( dst_weight->c1[y*dst_stride+x] < weight ){ + dst_weight->c1[y*dst_stride+x] = weight; + dst_flow_x->c1[y*dst_stride+x] = src_flow_x->c1[j*src_stride+i]*scale_x; + dst_flow_y->c1[y*dst_stride+x] = src_flow_y->c1[j*src_stride+i]*scale_y; + } + } + } +} diff --git a/src/FDF1.0.1/opticalflow_aux.h b/src/FDF1.0.1/opticalflow_aux.h new file mode 100644 index 0000000..063ac23 --- /dev/null +++ b/src/FDF1.0.1/opticalflow_aux.h @@ -0,0 +1,68 @@ +#ifndef __OPTICALFLOW_AUX_ +#define __OPTICALFLOW_AUX_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#include "image.h" + +#if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // use single band image +/* warp a color image according to a flow. src is the input image, wx and wy, the input flow. dst is the warped image and mask contains 0 or 1 if the pixels goes outside/inside image boundaries */ +void image_warp(image_t *dst, image_t *mask, const image_t *src, const image_t *wx, const image_t *wy); + +/* compute image first and second order spatio-temporal derivatives of a color image */ +void get_derivatives(const image_t *im1, const image_t *im2, const convolution_t *deriv, image_t *dx, image_t *dy, image_t *dt, image_t *dxx, image_t *dxy, image_t *dyy, image_t *dxt, image_t *dyt); + +#else // use RGB image_new +/* warp a color image according to a flow. src is the input image, wx and wy, the input flow. dst is the warped image and mask contains 0 or 1 if the pixels goes outside/inside image boundaries */ +void image_warp(color_image_t *dst, image_t *mask, const color_image_t *src, const image_t *wx, const image_t *wy); + +/* compute image first and second order spatio-temporal derivatives of a color image */ +void get_derivatives(const color_image_t *im1, const color_image_t *im2, const convolution_t *deriv, color_image_t *dx, color_image_t *dy, color_image_t *dt, color_image_t *dxx, color_image_t *dxy, color_image_t *dyy, color_image_t *dxt, color_image_t *dyt); + +#endif + + +/* compute the smoothness term */ +void compute_smoothness(image_t *dst_horiz, image_t *dst_vert, const image_t *uu, const image_t *vv, const convolution_t *deriv_flow, const float quarter_alpha); +// void compute_smoothness_SF(image_t *dst_horiz, image_t *dst_vert, const image_t *xx1, const image_t *xx2, const image_t *yy, const image_t *xx3, const convolution_t *deriv_flow, const float quarter_alpha); + +/* sub the laplacian (smoothness term) to the right-hand term */ +void sub_laplacian(image_t *dst, const image_t *src, const image_t *weight_horiz, const image_t *weight_vert); + +/* compute the dataterm and the matching term + a11 a12 a22 represents the 2x2 diagonal matrix, b1 and b2 the right hand side + other (color) images are input */ +void compute_data_and_match(image_t *a11, image_t *a12, image_t *a22, image_t *b1, image_t *b2, image_t *mask, image_t *wx, image_t *wy, image_t *du, image_t *dv, image_t *uu, image_t *vv, color_image_t *Ix, color_image_t *Iy, color_image_t *Iz, color_image_t *Ixx, color_image_t *Ixy, color_image_t *Iyy, color_image_t *Ixz, color_image_t *Iyz, image_t *desc_weight, image_t *desc_flow_x, image_t *desc_flow_y, const float half_delta_over3, const float half_beta, const float half_gamma_over3); + +/* compute the dataterm and ... REMOVED THE MATCHING TERM + a11 a12 a22 represents the 2x2 diagonal matrix, b1 and b2 the right hand side + other (color) images are input */ +#if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // use single band image_delete +void compute_data(image_t *a11, image_t *a12, image_t *a22, image_t *b1, image_t *b2, image_t *mask, image_t *wx, image_t *wy, image_t *du, image_t *dv, image_t *uu, image_t *vv, image_t *Ix, image_t *Iy, image_t *Iz, image_t *Ixx, image_t *Ixy, image_t *Iyy, image_t *Ixz, image_t *Iyz, const float half_delta_over3, const float half_beta, const float half_gamma_over3); +#else +void compute_data(image_t *a11, image_t *a12, image_t *a22, image_t *b1, image_t *b2, image_t *mask, image_t *wx, image_t *wy, image_t *du, image_t *dv, image_t *uu, image_t *vv, color_image_t *Ix, color_image_t *Iy, color_image_t *Iz, color_image_t *Ixx, color_image_t *Ixy, color_image_t *Iyy, color_image_t *Ixz, color_image_t *Iyz, const float half_delta_over3, const float half_beta, const float half_gamma_over3); +#endif + +#if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // use single band image_delete +void compute_data_DE(image_t *a11, image_t *b1, image_t *mask, image_t *wx, image_t *du, image_t *uu, image_t *Ix, image_t *Iy, image_t *Iz, image_t *Ixx, image_t *Ixy, image_t *Iyy, image_t *Ixz, image_t *Iyz, const float half_delta_over3, const float half_beta, const float half_gamma_over3); +#else +void compute_data_DE(image_t *a11, image_t *b1, image_t *mask, image_t *wx, image_t *du, image_t *uu, color_image_t *Ix, color_image_t *Iy, color_image_t *Iz, color_image_t *Ixx, color_image_t *Ixy, color_image_t *Iyy, color_image_t *Ixz, color_image_t *Iyz, const float half_delta_over3, const float half_beta, const float half_gamma_over3); +#endif + + +/* resize the descriptors to the new size using a weighted mean */ +void descflow_resize(image_t *dst_flow_x, image_t *dst_flow_y, image_t *dst_weight, const image_t *src_flow_x, const image_t *src_flow_y, const image_t *src_weight); + +/* resize the descriptors to the new size using a nearest neighbor method while keeping the descriptor with the higher weight at the end */ +void descflow_resize_nn(image_t *dst_flow_x, image_t *dst_flow_y, image_t *dst_weight, const image_t *src_flow_x, const image_t *src_flow_y, const image_t *src_weight); + +#ifdef __cplusplus +} +#endif + + +#endif diff --git a/src/FDF1.0.1/solver.c b/src/FDF1.0.1/solver.c new file mode 100644 index 0000000..a4402e9 --- /dev/null +++ b/src/FDF1.0.1/solver.c @@ -0,0 +1,469 @@ +#include +#include +#include +#include +#include + +#include + +#include "image.h" +#include "solver.h" + +#include +typedef __v4sf v4sf; + +//THIS IS A SLOW VERSION BUT READABLE +//Perform n iterations of the sor_coupled algorithm +//du and dv are used as initial guesses +//The system form is the same as in opticalflow.c +void sor_coupled_slow_but_readable(image_t *du, image_t *dv, image_t *a11, image_t *a12, image_t *a22, const image_t *b1, const image_t *b2, const image_t *dpsis_horiz, const image_t *dpsis_vert, const int iterations, const float omega) +{ + int i,j,iter; + for(iter = 0 ; iterheight ; j++) + { + float sigma_u,sigma_v,sum_dpsis,A11,A22,A12,B1,B2;//,det; + for(i=0 ; iwidth ; i++) + { + sigma_u = 0.0f; + sigma_v = 0.0f; + sum_dpsis = 0.0f; + if(j>0) + { + sigma_u -= dpsis_vert->c1[(j-1)*du->stride+i]*du->c1[(j-1)*du->stride+i]; + sigma_v -= dpsis_vert->c1[(j-1)*du->stride+i]*dv->c1[(j-1)*du->stride+i]; + sum_dpsis += dpsis_vert->c1[(j-1)*du->stride+i]; + } + if(i>0) + { + sigma_u -= dpsis_horiz->c1[j*du->stride+i-1]*du->c1[j*du->stride+i-1]; + sigma_v -= dpsis_horiz->c1[j*du->stride+i-1]*dv->c1[j*du->stride+i-1]; + sum_dpsis += dpsis_horiz->c1[j*du->stride+i-1]; + } + if(jheight-1) + { + sigma_u -= dpsis_vert->c1[j*du->stride+i]*du->c1[(j+1)*du->stride+i]; + sigma_v -= dpsis_vert->c1[j*du->stride+i]*dv->c1[(j+1)*du->stride+i]; + sum_dpsis += dpsis_vert->c1[j*du->stride+i]; + } + if(iwidth-1) + { + sigma_u -= dpsis_horiz->c1[j*du->stride+i]*du->c1[j*du->stride+i+1]; + sigma_v -= dpsis_horiz->c1[j*du->stride+i]*dv->c1[j*du->stride+i+1]; + sum_dpsis += dpsis_horiz->c1[j*du->stride+i]; + } + A11 = a11->c1[j*du->stride+i]+sum_dpsis; + A12 = a12->c1[j*du->stride+i]; + A22 = a22->c1[j*du->stride+i]+sum_dpsis; + //det = A11*A22-A12*A12; + B1 = b1->c1[j*du->stride+i]-sigma_u; + B2 = b2->c1[j*du->stride+i]-sigma_v; +// du->c1[j*du->stride+i] = (1.0f-omega)*du->c1[j*du->stride+i] +omega*( A22*B1-A12*B2)/det; +// dv->c1[j*du->stride+i] = (1.0f-omega)*dv->c1[j*du->stride+i] +omega*(-A12*B1+A11*B2)/det; + du->c1[j*du->stride+i] = (1.0f-omega)*du->c1[j*du->stride+i] + omega/A11 *(B1 - A12* dv->c1[j*du->stride+i] ); + dv->c1[j*du->stride+i] = (1.0f-omega)*dv->c1[j*du->stride+i] + omega/A22 *(B2 - A12* du->c1[j*du->stride+i] ); + + + } + } + } +} + + // THIS IS A FASTER VERSION BUT UNREADABLE, ONLY OPTICAL FLOW WITHOUT OPENMP PARALLELIZATION + // the first iteration is separated from the other to compute the inverse of the 2x2 block diagonal + // each iteration is split in two first line / middle lines / last line, and the left block is computed separately on each line +void sor_coupled(image_t *du, image_t *dv, image_t *a11, image_t *a12, image_t *a22, const image_t *b1, const image_t *b2, const image_t *dpsis_horiz, const image_t *dpsis_vert, const int iterations, const float omega){ + //sor_coupled_slow(du,dv,a11,a12,a22,b1,b2,dpsis_horiz,dpsis_vert,iterations,omega); return; printf("test\n"); + + if(du->width<2 || du->height<2 || iterations < 1){ + sor_coupled_slow_but_readable(du,dv,a11,a12,a22,b1,b2,dpsis_horiz,dpsis_vert,iterations,omega); + return; + } + + const int stride = du->stride, width = du->width; + const int iterheight = du->height-1, iterline = (stride)/4, width_minus_1_sizeoffloat = sizeof(float)*(width-1); + int j,iter,i,k; + float *floatarray = (float*) memalign(16, stride*sizeof(float)*3); + if(floatarray==NULL){ + fprintf(stderr, "error in sor_coupled(): not enough memory\n"); + exit(1); + } + float *f1 = floatarray; + float *f2 = f1+stride; + float *f3 = f2+stride; + f1[0] = 0.0f; + memset(&f1[width], 0, sizeof(float)*(stride-width)); + memset(&f2[width-1], 0, sizeof(float)*(stride-width+1)); + memset(&f3[width-1], 0, sizeof(float)*(stride-width+1)); + + { // first iteration + v4sf *a11p = (v4sf*) a11->c1, *a12p = (v4sf*) a12->c1, *a22p = (v4sf*) a22->c1, *b1p = (v4sf*) b1->c1, *b2p = (v4sf*) b2->c1, *hp = (v4sf*) dpsis_horiz->c1, *vp = (v4sf*) dpsis_vert->c1; + float *du_ptr = du->c1, *dv_ptr = dv->c1; + v4sf *dub = (v4sf*) (du_ptr+stride), *dvb = (v4sf*) (dv_ptr+stride); + + { // first iteration - first line + + memcpy(f1+1, ((float*) hp), width_minus_1_sizeoffloat); + memcpy(f2, du_ptr+1, width_minus_1_sizeoffloat); + memcpy(f3, dv_ptr+1, width_minus_1_sizeoffloat); + v4sf* hpl = (v4sf*) f1, *dur = (v4sf*) f2, *dvr = (v4sf*) f3; + + { // left block + // reverse 2x2 diagonal block + const v4sf dpsis = (*hpl) + (*hp) + (*vp); + const v4sf A11 = (*a22p)+dpsis, A22 = (*a11p)+dpsis; + const v4sf det = A11*A22 - (*a12p)*(*a12p); + *a11p = A11/det; + *a22p = A22/det; + *a12p /= -det; + // do one iteration + const v4sf s1 = (*hp)*(*dur) + (*vp)*(*dub) + (*b1p); + const v4sf s2 = (*hp)*(*dvr) + (*vp)*(*dvb) + (*b2p); + du_ptr[0] += omega*( a11p[0][0]*s1[0] + a12p[0][0]*s2[0] - du_ptr[0] ); + dv_ptr[0] += omega*( a12p[0][0]*s1[0] + a22p[0][0]*s2[0] - dv_ptr[0] ); + for(k=1;k<4;k++){ + const float B1 = hpl[0][k]*du_ptr[k-1] + s1[k]; + const float B2 = hpl[0][k]*dv_ptr[k-1] + s2[k]; + du_ptr[k] += omega*( a11p[0][k]*B1 + a12p[0][k]*B2 - du_ptr[k] ); + dv_ptr[k] += omega*( a12p[0][k]*B1 + a22p[0][k]*B2 - dv_ptr[k] ); + } + // increment pointer + hpl+=1; hp+=1; vp+=1; a11p+=1; a12p+=1; a22p+=1; + dur+=1; dvr+=1; dub+=1; dvb +=1; b1p+=1; b2p+=1; + du_ptr += 4; dv_ptr += 4; + } + for(i=iterline;--i;){ + // reverse 2x2 diagonal block + const v4sf dpsis = (*hpl) + (*hp) + (*vp); + const v4sf A11 = (*a22p)+dpsis, A22 = (*a11p)+dpsis; + const v4sf det = A11*A22 - (*a12p)*(*a12p); + *a11p = A11/det; + *a22p = A22/det; + *a12p /= -det; + // do one iteration + const v4sf s1 = (*hp)*(*dur) + (*vp)*(*dub) + (*b1p); + const v4sf s2 = (*hp)*(*dvr) + (*vp)*(*dvb) + (*b2p); + for(k=0;k<4;k++){ + const float B1 = hpl[0][k]*du_ptr[k-1] + s1[k]; + const float B2 = hpl[0][k]*dv_ptr[k-1] + s2[k]; + du_ptr[k] += omega*( a11p[0][k]*B1 + a12p[0][k]*B2 - du_ptr[k] ); + dv_ptr[k] += omega*( a12p[0][k]*B1 + a22p[0][k]*B2 - dv_ptr[k] ); + } + // increment pointer + hpl+=1; hp+=1; vp+=1; a11p+=1; a12p+=1; a22p+=1; + dur+=1; dvr+=1; dub+=1; dvb +=1; b1p+=1; b2p+=1; + du_ptr += 4; dv_ptr += 4; + } + + } + + v4sf *vpt = (v4sf*) dpsis_vert->c1; + v4sf *dut = (v4sf*) du->c1, *dvt = (v4sf*) dv->c1; + + for(j=iterheight;--j;){ // first iteration - middle lines + memcpy(f1+1, ((float*) hp), width_minus_1_sizeoffloat); + memcpy(f2, du_ptr+1, width_minus_1_sizeoffloat); + memcpy(f3, dv_ptr+1, width_minus_1_sizeoffloat); + v4sf* hpl = (v4sf*) f1, *dur = (v4sf*) f2, *dvr = (v4sf*) f3; + + { // left block + // reverse 2x2 diagonal block + const v4sf dpsis = (*hpl) + (*hp) + (*vpt) + (*vp); + const v4sf A11 = (*a22p)+dpsis, A22 = (*a11p)+dpsis; + const v4sf det = A11*A22 - (*a12p)*(*a12p); + *a11p = A11/det; + *a22p = A22/det; + *a12p /= -det; + // do one iteration + const v4sf s1 = (*hp)*(*dur) + (*vpt)*(*dut) + (*vp)*(*dub) + (*b1p); + const v4sf s2 = (*hp)*(*dvr) + (*vpt)*(*dvt) + (*vp)*(*dvb) + (*b2p); + du_ptr[0] += omega*( a11p[0][0]*s1[0] + a12p[0][0]*s2[0] - du_ptr[0] ); + dv_ptr[0] += omega*( a12p[0][0]*s1[0] + a22p[0][0]*s2[0] - dv_ptr[0] ); + for(k=1;k<4;k++){ + const float B1 = hpl[0][k]*du_ptr[k-1] + s1[k]; + const float B2 = hpl[0][k]*dv_ptr[k-1] + s2[k]; + du_ptr[k] += omega*( a11p[0][k]*B1 + a12p[0][k]*B2 - du_ptr[k] ); + dv_ptr[k] += omega*( a12p[0][k]*B1 + a22p[0][k]*B2 - dv_ptr[k] ); + } + // increment pointer + hpl+=1; hp+=1; vpt+=1; vp+=1; a11p+=1; a12p+=1; a22p+=1; + dur+=1; dvr+=1; dut+=1; dvt+=1; dub+=1; dvb +=1; b1p+=1; b2p+=1; + du_ptr += 4; dv_ptr += 4; + } + for(i=iterline;--i;){ + // reverse 2x2 diagonal block + const v4sf dpsis = (*hpl) + (*hp) + (*vpt) + (*vp); + const v4sf A11 = (*a22p)+dpsis, A22 = (*a11p)+dpsis; + const v4sf det = A11*A22 - (*a12p)*(*a12p); + *a11p = A11/det; + *a22p = A22/det; + *a12p /= -det; + // do one iteration + const v4sf s1 = (*hp)*(*dur) + (*vpt)*(*dut) + (*vp)*(*dub) + (*b1p); + const v4sf s2 = (*hp)*(*dvr) + (*vpt)*(*dvt) + (*vp)*(*dvb) + (*b2p); + for(k=0;k<4;k++){ + const float B1 = hpl[0][k]*du_ptr[k-1] + s1[k]; + const float B2 = hpl[0][k]*dv_ptr[k-1] + s2[k]; + du_ptr[k] += omega*( a11p[0][k]*B1 + a12p[0][k]*B2 - du_ptr[k] ); + dv_ptr[k] += omega*( a12p[0][k]*B1 + a22p[0][k]*B2 - dv_ptr[k] ); + } + // increment pointer + hpl+=1; hp+=1; vpt+=1; vp+=1; a11p+=1; a12p+=1; a22p+=1; + dur+=1; dvr+=1; dut+=1; dvt+=1; dub+=1; dvb +=1; b1p+=1; b2p+=1; + du_ptr += 4; dv_ptr += 4; + } + + } + + { // first iteration - last line + memcpy(f1+1, ((float*) hp), width_minus_1_sizeoffloat); + memcpy(f2, du_ptr+1, width_minus_1_sizeoffloat); + memcpy(f3, dv_ptr+1, width_minus_1_sizeoffloat); + v4sf* hpl = (v4sf*) f1, *dur = (v4sf*) f2, *dvr = (v4sf*) f3; + + { // left block + // reverse 2x2 diagonal block + const v4sf dpsis = (*hpl) + (*hp) + (*vpt); + const v4sf A11 = (*a22p)+dpsis, A22 = (*a11p)+dpsis; + const v4sf det = A11*A22 - (*a12p)*(*a12p); + *a11p = A11/det; + *a22p = A22/det; + *a12p /= -det; + // do one iteration + const v4sf s1 = (*hp)*(*dur) + (*vpt)*(*dut) + (*b1p); + const v4sf s2 = (*hp)*(*dvr) + (*vpt)*(*dvt) + (*b2p); + du_ptr[0] += omega*( a11p[0][0]*s1[0] + a12p[0][0]*s2[0] - du_ptr[0] ); + dv_ptr[0] += omega*( a12p[0][0]*s1[0] + a22p[0][0]*s2[0] - dv_ptr[0] ); + for(k=1;k<4;k++){ + const float B1 = hpl[0][k]*du_ptr[k-1] + s1[k]; + const float B2 = hpl[0][k]*dv_ptr[k-1] + s2[k]; + du_ptr[k] += omega*( a11p[0][k]*B1 + a12p[0][k]*B2 - du_ptr[k] ); + dv_ptr[k] += omega*( a12p[0][k]*B1 + a22p[0][k]*B2 - dv_ptr[k] ); + } + // increment pointer + hpl+=1; hp+=1; vpt+=1; a11p+=1; a12p+=1; a22p+=1; + dur+=1; dvr+=1; dut+=1; dvt+=1; b1p+=1; b2p+=1; + du_ptr += 4; dv_ptr += 4; + } + for(i=iterline;--i;){ + // reverse 2x2 diagonal block + const v4sf dpsis = (*hpl) + (*hp) + (*vpt); + const v4sf A11 = (*a22p)+dpsis, A22 = (*a11p)+dpsis; + const v4sf det = A11*A22 - (*a12p)*(*a12p); + *a11p = A11/det; + *a22p = A22/det; + *a12p /= -det; + // do one iteration + const v4sf s1 = (*hp)*(*dur) + (*vpt)*(*dut) + (*b1p); + const v4sf s2 = (*hp)*(*dvr) + (*vpt)*(*dvt) + (*b2p); + for(k=0;k<4;k++){ + const float B1 = hpl[0][k]*du_ptr[k-1] + s1[k]; + const float B2 = hpl[0][k]*dv_ptr[k-1] + s2[k]; + du_ptr[k] += omega*( a11p[0][k]*B1 + a12p[0][k]*B2 - du_ptr[k] ); + dv_ptr[k] += omega*( a12p[0][k]*B1 + a22p[0][k]*B2 - dv_ptr[k] ); + } + // increment pointer + hpl+=1; hp+=1; vpt+=1; a11p+=1; a12p+=1; a22p+=1; + dur+=1; dvr+=1; dut+=1; dvt+=1; b1p+=1; b2p+=1; + du_ptr += 4; dv_ptr += 4; + } + + } + } + + + + for(iter=iterations;--iter;) // other iterations + { + v4sf *a11p = (v4sf*) a11->c1, *a12p = (v4sf*) a12->c1, *a22p = (v4sf*) a22->c1, *b1p = (v4sf*) b1->c1, *b2p = (v4sf*) b2->c1, *hp = (v4sf*) dpsis_horiz->c1, *vp = (v4sf*) dpsis_vert->c1; + float *du_ptr = du->c1, *dv_ptr = dv->c1; + v4sf *dub = (v4sf*) (du_ptr+stride), *dvb = (v4sf*) (dv_ptr+stride); + + { // other iteration - first line + + memcpy(f1+1, ((float*) hp), width_minus_1_sizeoffloat); + memcpy(f2, du_ptr+1, width_minus_1_sizeoffloat); + memcpy(f3, dv_ptr+1, width_minus_1_sizeoffloat); + v4sf* hpl = (v4sf*) f1, *dur = (v4sf*) f2, *dvr = (v4sf*) f3; + + { // left block + // do one iteration + const v4sf s1 = (*hp)*(*dur) + (*vp)*(*dub) + (*b1p); + const v4sf s2 = (*hp)*(*dvr) + (*vp)*(*dvb) + (*b2p); + du_ptr[0] += omega*( a11p[0][0]*s1[0] + a12p[0][0]*s2[0] - du_ptr[0] ); + dv_ptr[0] += omega*( a12p[0][0]*s1[0] + a22p[0][0]*s2[0] - dv_ptr[0] ); + for(k=1;k<4;k++){ + const float B1 = hpl[0][k]*du_ptr[k-1] + s1[k]; + const float B2 = hpl[0][k]*dv_ptr[k-1] + s2[k]; + du_ptr[k] += omega*( a11p[0][k]*B1 + a12p[0][k]*B2 - du_ptr[k] ); + dv_ptr[k] += omega*( a12p[0][k]*B1 + a22p[0][k]*B2 - dv_ptr[k] ); + } + // increment pointer + hpl+=1; hp+=1; vp+=1; a11p+=1; a12p+=1; a22p+=1; + dur+=1; dvr+=1; dub+=1; dvb +=1; b1p+=1; b2p+=1; + du_ptr += 4; dv_ptr += 4; + } + for(i=iterline;--i;){ + // do one iteration + const v4sf s1 = (*hp)*(*dur) + (*vp)*(*dub) + (*b1p); + const v4sf s2 = (*hp)*(*dvr) + (*vp)*(*dvb) + (*b2p); + for(k=0;k<4;k++){ + const float B1 = hpl[0][k]*du_ptr[k-1] + s1[k]; + const float B2 = hpl[0][k]*dv_ptr[k-1] + s2[k]; + du_ptr[k] += omega*( a11p[0][k]*B1 + a12p[0][k]*B2 - du_ptr[k] ); + dv_ptr[k] += omega*( a12p[0][k]*B1 + a22p[0][k]*B2 - dv_ptr[k] ); + } + // increment pointer + hpl+=1; hp+=1; vp+=1; a11p+=1; a12p+=1; a22p+=1; + dur+=1; dvr+=1; dub+=1; dvb +=1; b1p+=1; b2p+=1; + du_ptr += 4; dv_ptr += 4; + } + + } + + v4sf *vpt = (v4sf*) dpsis_vert->c1; + v4sf *dut = (v4sf*) du->c1, *dvt = (v4sf*) dv->c1; + + for(j=iterheight;--j;) // other iteration - middle lines + { + memcpy(f1+1, ((float*) hp), width_minus_1_sizeoffloat); + memcpy(f2, du_ptr+1, width_minus_1_sizeoffloat); + memcpy(f3, dv_ptr+1, width_minus_1_sizeoffloat); + v4sf* hpl = (v4sf*) f1, *dur = (v4sf*) f2, *dvr = (v4sf*) f3; + + { // left block + // do one iteration + const v4sf s1 = (*hp)*(*dur) + (*vpt)*(*dut) + (*vp)*(*dub) + (*b1p); + const v4sf s2 = (*hp)*(*dvr) + (*vpt)*(*dvt) + (*vp)*(*dvb) + (*b2p); + du_ptr[0] += omega*( a11p[0][0]*s1[0] + a12p[0][0]*s2[0] - du_ptr[0] ); + dv_ptr[0] += omega*( a12p[0][0]*s1[0] + a22p[0][0]*s2[0] - dv_ptr[0] ); + for(k=1;k<4;k++) + { + const float B1 = hpl[0][k]*du_ptr[k-1] + s1[k]; + const float B2 = hpl[0][k]*dv_ptr[k-1] + s2[k]; + du_ptr[k] += omega*( a11p[0][k]*B1 + a12p[0][k]*B2 - du_ptr[k] ); + dv_ptr[k] += omega*( a12p[0][k]*B1 + a22p[0][k]*B2 - dv_ptr[k] ); + } + // increment pointer + hpl+=1; hp+=1; vpt+=1; vp+=1; a11p+=1; a12p+=1; a22p+=1; + dur+=1; dvr+=1; dut+=1; dvt+=1; dub+=1; dvb +=1; b1p+=1; b2p+=1; + du_ptr += 4; dv_ptr += 4; + } + + for(i=iterline; --i;) + { + // do one iteration + const v4sf s1 = (*hp)*(*dur) + (*vpt)*(*dut) + (*vp)*(*dub) + (*b1p); + const v4sf s2 = (*hp)*(*dvr) + (*vpt)*(*dvt) + (*vp)*(*dvb) + (*b2p); + for(k=0;k<4;k++) + { + const float B1 = hpl[0][k]*du_ptr[k-1] + s1[k]; + const float B2 = hpl[0][k]*dv_ptr[k-1] + s2[k]; + du_ptr[k] += omega*( a11p[0][k]*B1 + a12p[0][k]*B2 - du_ptr[k] ); + dv_ptr[k] += omega*( a12p[0][k]*B1 + a22p[0][k]*B2 - dv_ptr[k] ); + } + // increment pointer + hpl+=1; hp+=1; vpt+=1; vp+=1; a11p+=1; a12p+=1; a22p+=1; + dur+=1; dvr+=1; dut+=1; dvt+=1; dub+=1; dvb +=1; b1p+=1; b2p+=1; + du_ptr += 4; dv_ptr += 4; + } + + } + + { // other iteration - last line + memcpy(f1+1, ((float*) hp), width_minus_1_sizeoffloat); + memcpy(f2, du_ptr+1, width_minus_1_sizeoffloat); + memcpy(f3, dv_ptr+1, width_minus_1_sizeoffloat); + v4sf* hpl = (v4sf*) f1, *dur = (v4sf*) f2, *dvr = (v4sf*) f3; + + { // left block + // do one iteration + const v4sf s1 = (*hp)*(*dur) + (*vpt)*(*dut) + (*b1p); + const v4sf s2 = (*hp)*(*dvr) + (*vpt)*(*dvt) + (*b2p); + du_ptr[0] += omega*( a11p[0][0]*s1[0] + a12p[0][0]*s2[0] - du_ptr[0] ); + dv_ptr[0] += omega*( a12p[0][0]*s1[0] + a22p[0][0]*s2[0] - dv_ptr[0] ); + for(k=1;k<4;k++){ + const float B1 = hpl[0][k]*du_ptr[k-1] + s1[k]; + const float B2 = hpl[0][k]*dv_ptr[k-1] + s2[k]; + du_ptr[k] += omega*( a11p[0][k]*B1 + a12p[0][k]*B2 - du_ptr[k] ); + dv_ptr[k] += omega*( a12p[0][k]*B1 + a22p[0][k]*B2 - dv_ptr[k] ); + } + // increment pointer + hpl+=1; hp+=1; vpt+=1; a11p+=1; a12p+=1; a22p+=1; + dur+=1; dvr+=1; dut+=1; dvt+=1; b1p+=1; b2p+=1; + du_ptr += 4; dv_ptr += 4; + } + for(i=iterline;--i;){ + // do one iteration + const v4sf s1 = (*hp)*(*dur) + (*vpt)*(*dut) + (*b1p); + const v4sf s2 = (*hp)*(*dvr) + (*vpt)*(*dvt) + (*b2p); + for(k=0;k<4;k++){ + const float B1 = hpl[0][k]*du_ptr[k-1] + s1[k]; + const float B2 = hpl[0][k]*dv_ptr[k-1] + s2[k]; + du_ptr[k] += omega*( a11p[0][k]*B1 + a12p[0][k]*B2 - du_ptr[k] ); + dv_ptr[k] += omega*( a12p[0][k]*B1 + a22p[0][k]*B2 - dv_ptr[k] ); + } + // increment pointer + hpl+=1; hp+=1; vpt+=1; a11p+=1; a12p+=1; a22p+=1; + dur+=1; dvr+=1; dut+=1; dvt+=1; b1p+=1; b2p+=1; + du_ptr += 4; dv_ptr += 4; + } + + } + } + + + + free(floatarray); + +} + + +//THIS IS A SLOW VERSION BUT READABLE +//Perform n iterations of the sor_coupled algorithm +//du is used as initial guesses +//The system form is the same as in opticalflow.c +void sor_coupled_slow_but_readable_DE(image_t *du, const image_t *a11, const image_t *b1, const image_t *dpsis_horiz, const image_t *dpsis_vert, const int iterations, const float omega) +{ + int i,j,iter; + for(iter = 0 ; iterheight ; j++) + { + float sigma_u,sum_dpsis,A11,B1; + for(i=0 ; iwidth ; i++){ + sigma_u = 0.0f; + sum_dpsis = 0.0f; + if(j>0) + { + sigma_u -= dpsis_vert->c1[(j-1)*du->stride+i]*du->c1[(j-1)*du->stride+i]; + sum_dpsis += dpsis_vert->c1[(j-1)*du->stride+i]; + } + if(i>0) + { + sigma_u -= dpsis_horiz->c1[j*du->stride+i-1]*du->c1[j*du->stride+i-1]; + sum_dpsis += dpsis_horiz->c1[j*du->stride+i-1]; + } + if(jheight-1) + { + sigma_u -= dpsis_vert->c1[j*du->stride+i]*du->c1[(j+1)*du->stride+i]; + sum_dpsis += dpsis_vert->c1[j*du->stride+i]; + } + if(iwidth-1) + { + sigma_u -= dpsis_horiz->c1[j*du->stride+i]*du->c1[j*du->stride+i+1]; + sum_dpsis += dpsis_horiz->c1[j*du->stride+i]; + } + A11 = a11->c1[j*du->stride+i]+sum_dpsis; + B1 = b1->c1[j*du->stride+i]-sigma_u; + du->c1[j*du->stride+i] = (1.0f-omega)*du->c1[j*du->stride+i] +omega*( B1/A11 ); + } + } + } +} + + + diff --git a/src/FDF1.0.1/solver.h b/src/FDF1.0.1/solver.h new file mode 100644 index 0000000..6064349 --- /dev/null +++ b/src/FDF1.0.1/solver.h @@ -0,0 +1,19 @@ +#include +#include + +#include "image.h" + +#ifdef __cplusplus +extern "C" { +#endif + +// Perform n iterations of the sor_coupled algorithm for a system of the form as described in opticalflow.c +void sor_coupled(image_t *du, image_t *dv, image_t *a11, image_t *a12, image_t *a22, const image_t *b1, const image_t *b2, const image_t *dpsis_horiz, const image_t *dpsis_vert, const int iterations, const float omega); + +void sor_coupled_slow_but_readable(image_t *du, image_t *dv, image_t *a11, image_t *a12, image_t *a22, const image_t *b1, const image_t *b2, const image_t *dpsis_horiz, const image_t *dpsis_vert, const int iterations, const float omega); + +void sor_coupled_slow_but_readable_DE(image_t *du, const image_t *a11, const image_t *b1, const image_t *dpsis_horiz, const image_t *dpsis_vert, const int iterations, const float omega); + +#ifdef __cplusplus +} +#endif diff --git a/src/LICENSE b/src/LICENSE new file mode 100644 index 0000000..9cecc1d --- /dev/null +++ b/src/LICENSE @@ -0,0 +1,674 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + {one line to give the program's name and a brief idea of what it does.} + Copyright (C) {year} {name of author} + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + {project} Copyright (C) {year} {fullname} + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/src/README.md b/src/README.md new file mode 100644 index 0000000..4fae1d6 --- /dev/null +++ b/src/README.md @@ -0,0 +1,147 @@ +# Fast Optical Flow using Dense Inverse Search (DIS) # + +Our code is released only for scientific or personal use. +Please contact us for commercial use. + +If used this work, please cite: + +`@inproceedings{kroegerECCV2016, + Author = {Till Kroeger and Radu Timofte and Dengxin Dai and Luc Van Gool}, + Title = {Fast Optical Flow using Dense Inverse Search}, + Booktitle = {Proceedings of the European Conference on Computer Vision ({ECCV})}, + Year = {2016}} ` + +Is you use the variational refinement, please additionally cite: + +` @inproceedings{weinzaepfelICCV2013, + TITLE = {{DeepFlow: Large displacement optical flow with deep matching}}, + AUTHOR = {Weinzaepfel, Philippe and Revaud, J{\'e}r{\^o}me and Harchaoui, Zaid and Schmid, Cordelia}, + BOOKTITLE = {{ICCV 2013 - IEEE International Conference on Computer Vision}}, + YEAR = {2013}} ` + + + + +## Compiling ## + +The program was only tested under a 64-bit Linux distribution. +SSE instructions from built-in X86 functions for GNU GCC were used. + +The following will build four binaries: +Two for optical flow (`run_OF_*`) and two for depth from stereo (`run_DE_*`). +For each problem, a fast variant operating on intensity images (`run_*_INT`) and +a slower variant operating on RGB images (`run_*_RGB`) is provided. + +``` +mkdir build +cd build +cmake ../ +make -j +``` + +The code depends on Eigen3 and OpenCV. However, OpenCV is only used for image loading, +scaling and gradient computation (`run_dense.cpp`). It can easily be replaced by other libraries. + + + + +## Usage ## +The interface for all four binaries (`run_*_*`) is the same. + +VARIANT 1 (Uses operating point 2 of the paper, automatically selects coarsest scale): + +` ./run_*_* image1.png image2.png outputfile ` + + +VARIANT 2 (Manually select operating point X=1-4, automatically selects coarsest scale): + +` ./run_*_* image1.png image2.png outputfile X ` + + +VARIANT 3 (Set all parameters explicitly): + +` ./run_*_* image1.png image2.png outputfile p1 p2 p3 p4 p5 p6 p7 p8 p9 p10 p11 p12 p13 p14 p15 p16 p17 p18 p19 p20` + +Example for variant 3 using operating point 2 of the paper: + +` ./run_OF_INT in1.png int2.png out.flo 5 3 12 12 0.05 0.95 0 8 0.40 0 1 0 1 10 10 5 1 3 1.6 2 ` + + + +Parameters: +``` +1. Coarsest scale (here: 5) +2. Finest scale (here: 3) +3/4. Min./Max. iterations (here: 12) +5./6./7. Early stopping parameters +8. Patch size (here: 8) +9. Patch overlap (here: 0.4) +10.Use forward-backward consistency (here: 0/no) +11.Mean-normalize patches (here: 1/yes) +12.Cost function (here: 0/L2) Alternatives: 1/L1, 2/Huber, 10/NCC +13.Use TV refinement (here: 1/yes) +14./15./16. TV parameters alpha,gamma,delta (here 10,10,5) +17. Number of TV outer iterations (here: 1) +18. Number of TV solver iterations (here: 3) +19. TV SOR value (here: 1.6) +20. Verbosity (here: 2) Alternatives: 0/no output, 1/only flow runtime, 2/total runtime +``` + + +The optical flow output is saves as .flo file. +(http://sintel.is.tue.mpg.de/downloads) + +The interface for depth from stereo is exactly the same. The output is saves as pfm file. +(http://vision.middlebury.edu/stereo/code/) + + +NOTES: +1. For better quality, increase the number iterations (param 3/4), use finer scales (param. 2), higher patch overlap (param. 9), more outer TV iterations (param. 17) +2. L1/Huber cost functions (param. 12) provide better results, but require more iterations (param. 3/4) + + + +## Bugs and extensions ## + +If you find bugs, etc., please feel free to contact me. +Contact details are available on my webpage. +http://www.vision.ee.ethz.ch/~kroegert/ + + + +## History ## + +July 2016 v1.0.0 - Initial Release +August 2016 v1.0.1 - Minor Bugfix: Error in L1 and Huber error norm computation. + + + + +## LICENCE CONDITIONS ## + +GPLv3: http://gplv3.fsf.org/ + +All programs in this collection are free software: +you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . + + + + + + + + + + + diff --git a/src/oflow.cpp b/src/oflow.cpp new file mode 100644 index 0000000..7fa552d --- /dev/null +++ b/src/oflow.cpp @@ -0,0 +1,393 @@ + + +#include +#include +#include + +#include + +#include +#include +#include + +// #include // needed for verbosity >= 3, DISVISUAL +// #include // needed for verbosity >= 3, DISVISUAL +// #include // needed for verbosity >= 3, DISVISUAL + +#include // timeof day +#include + +#include "oflow.h" +#include "patchgrid.h" +#include "refine_variational.h" + + +using std::cout; +using std::endl; +using std::vector; + +namespace OFC +{ + + OFClass::OFClass(const float ** im_ao_in, const float ** im_ao_dx_in, const float ** im_ao_dy_in, // expects #sc_f_in pointers to float arrays for images and gradients. + // E.g. im_ao[sc_f_in] will be used as coarsest coarsest, im_ao[sc_l_in] as finest scale + // im_ao[ (sc_l_in-1) : 0 ] can be left as nullptr pointers + // IMPORTANT assumption: mod(width,2^sc_f_in)==0 AND mod(height,2^sc_f_in)==0, + const float ** im_bo_in, const float ** im_bo_dx_in, const float ** im_bo_dy_in, + const int imgpadding_in, + float * outflow, + const float * initflow, + const int width_in, const int height_in, + const int sc_f_in, const int sc_l_in, + const int max_iter_in, const int min_iter_in, + const float dp_thresh_in, + const float dr_thresh_in, + const float res_thresh_in, + const int p_samp_s_in, + const float patove_in, + const bool usefbcon_in, + const int costfct_in, + const int noc_in, + const int patnorm_in, + const bool usetvref_in, + const float tv_alpha_in, + const float tv_gamma_in, + const float tv_delta_in, + const int tv_innerit_in, + const int tv_solverit_in, + const float tv_sor_in, + const int verbosity_in) + : im_ao(im_ao_in), im_ao_dx(im_ao_dx_in), im_ao_dy(im_ao_dy_in), + im_bo(im_bo_in), im_bo_dx(im_bo_dx_in), im_bo_dy(im_bo_dy_in) +{ + + + #ifdef WITH_OPENMP + if (verbosity_in>1) + cout << "OPENMP is ON - used in pconst, pinit, potim"; + #ifdef USE_PARALLEL_ON_FLOWAGGR + if (verbosity_in>1) + cout << ", cflow "; + #endif + if (verbosity_in>1) cout << endl; + #endif //DWITH_OPENMP + + // Parse optimization parameters + #if (SELECTMODE==1) + op.nop = 2; + #else + op.nop = 1; + #endif + op.p_samp_s = p_samp_s_in; // patch has even border length, center pixel is at (p_samp_s/2, p_samp_s/2) (ZERO INDEXED!) + op.outlierthresh = (float)op.p_samp_s/2; + op.patove = patove_in; + op.sc_f = sc_f_in; + op.sc_l = sc_l_in; + op.max_iter = max_iter_in; + op.min_iter = min_iter_in; + op.dp_thresh = dp_thresh_in*dp_thresh_in; // saves the square to compare with squared L2-norm (saves sqrt operation) + op.dr_thresh = dr_thresh_in; + op.res_thresh = res_thresh_in; + op.steps = std::max(1, (int)floor(op.p_samp_s*(1-op.patove))); + op.novals = noc_in * (p_samp_s_in)*(p_samp_s_in); + op.usefbcon = usefbcon_in; + op.costfct = costfct_in; + op.noc = noc_in; + op.patnorm = patnorm_in; + op.verbosity = verbosity_in; + op.noscales = op.sc_f-op.sc_l+1; + op.usetvref = usetvref_in; + op.tv_alpha = tv_alpha_in; + op.tv_gamma = tv_gamma_in; + op.tv_delta = tv_delta_in; + op.tv_innerit = tv_innerit_in; + op.tv_solverit = tv_solverit_in; + op.tv_sor = tv_sor_in; + op.normoutlier_tmpbsq = (v4sf) {op.normoutlier*op.normoutlier, op.normoutlier*op.normoutlier, op.normoutlier*op.normoutlier, op.normoutlier*op.normoutlier}; + op.normoutlier_tmp2bsq = __builtin_ia32_mulps(op.normoutlier_tmpbsq, op.twos); + op.normoutlier_tmp4bsq = __builtin_ia32_mulps(op.normoutlier_tmpbsq, op.fours); + + + // Variables for algorithm timings + struct timeval tv_start_all, tv_end_all, tv_start_all_global, tv_end_all_global; + if (op.verbosity>0) + gettimeofday(&tv_start_all_global, nullptr); + + // ... per each scale + double tt_patconstr[op.noscales], tt_patinit[op.noscales], tt_patoptim[op.noscales], tt_compflow[op.noscales], tt_tvopt[op.noscales], tt_all[op.noscales]; + for (int sl=op.sc_f; sl>=op.sc_l; --sl) + { + tt_patconstr[sl-op.sc_l]=0; + tt_patinit[sl-op.sc_l]=0; + tt_patoptim[sl-op.sc_l]=0; + tt_compflow[sl-op.sc_l]=0; + tt_tvopt[sl-op.sc_l]=0; + tt_all[sl-op.sc_l]=0; + } + + if (op.verbosity>1) gettimeofday(&tv_start_all, nullptr); + + + // Create grids on each scale + vector grid_fw(op.noscales); + vector grid_bw(op.noscales); // grid for backward OF computation, only needed if 'usefbcon' is set to 1. + vector flow_fw(op.noscales); + vector flow_bw(op.noscales); + cpl.resize(op.noscales); + cpr.resize(op.noscales); + for (int sl=op.sc_f; sl>=op.sc_l; --sl) + { + int i = sl-op.sc_l; + + float sc_fct = pow(2,-sl); // scaling factor at current scale + cpl[i].sc_fct = sc_fct; + cpl[i].height = height_in * sc_fct; + cpl[i].width = width_in * sc_fct; + cpl[i].imgpadding = imgpadding_in; + cpl[i].tmp_lb = -(float)op.p_samp_s/2; + cpl[i].tmp_ubw = (float) (cpl[i].width +op.p_samp_s/2-2); + cpl[i].tmp_ubh = (float) (cpl[i].height+op.p_samp_s/2-2); + cpl[i].tmp_w = cpl[i].width + 2*imgpadding_in; + cpl[i].tmp_h = cpl[i].height+ 2*imgpadding_in; + cpl[i].curr_lv = sl; + cpl[i].camlr = 0; + + + cpr[i] = cpl[i]; + cpr[i].camlr = 1; + + flow_fw[i] = new float[op.nop * cpl[i].width * cpl[i].height]; + grid_fw[i] = new OFC::PatGridClass(&(cpl[i]), &(cpr[i]), &op); + + if (op.usefbcon) // for merging forward and backward flow + { + flow_bw[i] = new float[op.nop * cpr[i].width * cpr[i].height]; + grid_bw[i] = new OFC::PatGridClass(&(cpr[i]), &(cpl[i]), &op); + + // Make grids known to each other, necessary for AggregateFlowDense(); + grid_fw[i]->SetComplGrid( grid_bw[i] ); + grid_bw[i]->SetComplGrid( grid_fw[i] ); + } + } + + + // Timing, Grid memory allocation + if (op.verbosity>1) + { + gettimeofday(&tv_end_all, nullptr); + double tt_gridconst = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f; + printf("TIME (Grid Memo. Alloc. ) (ms): %3g\n", tt_gridconst); + } + + + // *** Main loop; Operate over scales, coarse-to-fine + for (int sl=op.sc_f; sl>=op.sc_l; --sl) + { + int ii = sl-op.sc_l; + + if (op.verbosity>1) gettimeofday(&tv_start_all, nullptr); + + // Initialize grid (Step 1 in Algorithm 1 of paper) + grid_fw[ii]-> InitializeGrid(im_ao[sl], im_ao_dx[sl], im_ao_dy[sl]); + grid_fw[ii]-> SetTargetImage(im_bo[sl], im_bo_dx[sl], im_bo_dy[sl]); + if (op.usefbcon) + { + grid_bw[ii]->InitializeGrid(im_bo[sl], im_bo_dx[sl], im_bo_dy[sl]); + grid_bw[ii]->SetTargetImage(im_ao[sl], im_ao_dx[sl], im_ao_dy[sl]); + } + + // Timing, Grid construction + if (op.verbosity>1) + { + gettimeofday(&tv_end_all, nullptr); + tt_patconstr[ii] = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f; + tt_all[ii] += tt_patconstr[ii]; + gettimeofday(&tv_start_all, nullptr); + } + + // Initialization from previous scale, or to zero at first iteration. (Step 2 in Algorithm 1 of paper) + if (sl < op.sc_f) + { + grid_fw[ii]->InitializeFromCoarserOF(flow_fw[ii+1]); // initialize from flow at previous coarser scale + + // Initialize backward flow + if (op.usefbcon) + grid_bw[ii]->InitializeFromCoarserOF(flow_bw[ii+1]); + } + else if (sl == op.sc_f && initflow != nullptr) // initialization given input flow + { + grid_fw[ii]->InitializeFromCoarserOF(initflow); // initialize from flow at coarser scale + } + + // Timing, Grid initialization + if (op.verbosity>1) + { + gettimeofday(&tv_end_all, nullptr); + tt_patinit[ii] = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f; + tt_all[ii] += tt_patinit[ii]; + gettimeofday(&tv_start_all, nullptr); + } + + + // Dense Inverse Search. (Step 3 in Algorithm 1 of paper) + grid_fw[ii]->Optimize(); + if (op.usefbcon) + grid_bw[ii]->Optimize(); + +// if (op.verbosity==4) // needed for verbosity >= 3, DISVISUAL +// { +// grid_fw[ii]->OptimizeAndVisualize(pow(2, sl)); +// if (op.usefbcon) +// grid_bw[ii]->Optimize(); +// } +// else +// { +// grid_fw[ii]->Optimize(); +// if (op.usefbcon) +// grid_bw[ii]->Optimize(); +// } + + + // Timing, DIS + if (op.verbosity>1) + { + gettimeofday(&tv_end_all, nullptr); + tt_patoptim[ii] = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f; + tt_all[ii] += tt_patoptim[ii]; + + gettimeofday(&tv_start_all, nullptr); + } + + + // Densification. (Step 4 in Algorithm 1 of paper) + float *tmp_ptr = flow_fw[ii]; + if (sl == op.sc_l) + tmp_ptr = outflow; + + grid_fw[ii]->AggregateFlowDense(tmp_ptr); + + if (op.usefbcon && sl > op.sc_l ) // skip at last scale, backward flow no longer needed + grid_bw[ii]->AggregateFlowDense(flow_bw[ii]); + + + // Timing, Densification + if (op.verbosity>1) + { + gettimeofday(&tv_end_all, nullptr); + tt_compflow[ii] = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f; + tt_all[ii] += tt_compflow[ii]; + + gettimeofday(&tv_start_all, nullptr); + } + + + // Variational refinement, (Step 5 in Algorithm 1 of paper) + if (op.usetvref) + { + OFC::VarRefClass varref_fw(im_ao[sl], im_ao_dx[sl], im_ao_dy[sl], + im_bo[sl], im_bo_dx[sl], im_bo_dy[sl] + ,&(cpl[ii]), &(cpr[ii]), &op, tmp_ptr); + + if (op.usefbcon && sl > op.sc_l ) // skip at last scale, backward flow no longer needed + OFC::VarRefClass varref_bw(im_bo[sl], im_bo_dx[sl], im_bo_dy[sl], + im_ao[sl], im_ao_dx[sl], im_ao_dy[sl] + ,&(cpr[ii]), &(cpl[ii]), &op, flow_bw[ii]); + } + + // Timing, Variational Refinement + if (op.verbosity>1) + { + gettimeofday(&tv_end_all, nullptr); + tt_tvopt[ii] = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f; + tt_all[ii] += tt_tvopt[ii]; + printf("TIME (Sc: %i, #p:%6i, pconst, pinit, poptim, cflow, tvopt, total): %8.2f %8.2f %8.2f %8.2f %8.2f -> %8.2f ms.\n", sl, grid_fw[ii]->GetNoPatches(), tt_patconstr[ii], tt_patinit[ii], tt_patoptim[ii], tt_compflow[ii], tt_tvopt[ii], tt_all[ii]); + } + + +// if (op.verbosity==3) // Display displacement result of this scale // needed for verbosity >= 3, DISVISUAL +// { +// // Display Grid on current scale +// float sc_fct_tmp = pow(2, sl); // upscale factor +// +// cv::Mat src(cpl[ii].height+2*cpl[ii].imgpadding, cpl[ii].width+2*cpl[ii].imgpadding, CV_32FC1, (void*) im_ao[sl]); +// cv::Mat img_ao_mat = src(cv::Rect(cpl[ii].imgpadding, cpl[ii].imgpadding, cpl[ii].width, cpl[ii].height)); +// +// cv::Mat outimg; +// img_ao_mat.convertTo(outimg, CV_8UC1); +// cv::cvtColor(outimg, outimg, CV_GRAY2RGB); +// cv::resize(outimg, outimg, cv::Size(), sc_fct_tmp, sc_fct_tmp, cv::INTER_NEAREST); +// for (int i = 0; i < grid_fw[ii]->GetNoPatches() ; ++i) +// DisplayDrawPatchBoundary(outimg, grid_fw[ii]->GetRefPatchPos(i), sc_fct_tmp); +// +// for (int i = 0; i < grid_fw[ii]->GetNoPatches(); ++i) +// { +// // Show displacement vector +// const Eigen::Vector2f pt_ref = grid_fw[ii]->GetRefPatchPos(i); +// const Eigen::Vector2f pt_ret = grid_fw[ii]->GetQuePatchPos(i); +// +// Eigen::Vector2f pta, ptb; +// cv::line(outimg, cv::Point( (pt_ref[0]+.5)*sc_fct_tmp, (pt_ref[1]+.5)*sc_fct_tmp ), cv::Point( (pt_ret[0]+.5)*sc_fct_tmp, (pt_ret[1]+.5)*sc_fct_tmp ), cv::Scalar(0,255,0), 2); +// } +// cv::namedWindow( "Img_ao", cv::WINDOW_AUTOSIZE ); +// cv::imshow( "Img_ao", outimg); +// +// cv::waitKey(0); +// } + + } + + // Clean up + for (int sl=op.sc_f; sl>=op.sc_l; --sl) + { + + delete[] flow_fw[sl-op.sc_l]; + delete grid_fw[sl-op.sc_l]; + + if (op.usefbcon) + { + delete[] flow_bw[sl-op.sc_l]; + delete grid_bw[sl-op.sc_l]; + } + } + + + // Timing, total algorithm run-time + if (op.verbosity>0) + { + gettimeofday(&tv_end_all_global, nullptr); + double tt = (tv_end_all_global.tv_sec-tv_start_all_global.tv_sec)*1000.0f + (tv_end_all_global.tv_usec-tv_start_all_global.tv_usec)/1000.0f; + printf("TIME (O.Flow Run-Time ) (ms): %3g\n", tt); + } + + +} + +// // needed for verbosity >= 3, DISVISUAL +// void OFClass::DisplayDrawPatchBoundary(cv::Mat img, const Eigen::Vector2f pt, const float sc) +// { +// cv::line(img, cv::Point( (pt[0]+.5)*sc, (pt[1]+.5)*sc ), cv::Point( (pt[0]+.5)*sc, (pt[1]+.5)*sc ), cv::Scalar(0,0,255), 4); +// +// float lb = -op.p_samp_s/2; +// float ub = op.p_samp_s/2-1; +// +// cv::line(img, cv::Point( ((pt[0]+lb)+.5)*sc, ((pt[1]+lb)+.5)*sc ), cv::Point( ((pt[0]+ub)+.5)*sc, ((pt[1]+lb)+.5)*sc ), cv::Scalar(0,0,255), 1); +// cv::line(img, cv::Point( ((pt[0]+ub)+.5)*sc, ((pt[1]+lb)+.5)*sc ), cv::Point( ((pt[0]+ub)+.5)*sc, ((pt[1]+ub)+.5)*sc ), cv::Scalar(0,0,255), 1); +// cv::line(img, cv::Point( ((pt[0]+ub)+.5)*sc, ((pt[1]+ub)+.5)*sc ), cv::Point( ((pt[0]+lb)+.5)*sc, ((pt[1]+ub)+.5)*sc ), cv::Scalar(0,0,255), 1); +// cv::line(img, cv::Point( ((pt[0]+lb)+.5)*sc, ((pt[1]+ub)+.5)*sc ), cv::Point( ((pt[0]+lb)+.5)*sc, ((pt[1]+lb)+.5)*sc ), cv::Scalar(0,0,255), 1); +// } + +} + + + + + + + + + + + + + + diff --git a/src/oflow.h b/src/oflow.h new file mode 100644 index 0000000..964c59d --- /dev/null +++ b/src/oflow.h @@ -0,0 +1,130 @@ + +// Class implements main flow computation loop over all scales + +#ifndef OFC_HEADER +#define OFC_HEADER + +using std::cout; +using std::endl; + +namespace OFC +{ + +typedef __v4sf v4sf; + + +typedef struct +{ + int width; // image width, does not include '2*imgpadding', but includes original padding to ensure integer divisible image width and height + int height; // image height, does not include '2*imgpadding', but includes original padding to ensure integer divisible image width and height + int imgpadding; // image padding in pixels at all sides, images padded with replicated border, gradients padded with zero, ADD THIS ONLY WHEN ADDRESSING THE IMAGE OR GRADIENT + float tmp_lb; // lower bound for valid image region, pre-compute for image padding to avoid border check + float tmp_ubw; // upper width bound for valid image region, pre-compute for image padding to avoid border check + float tmp_ubh; // upper height bound for valid image region, pre-compute for image padding to avoid border check + int tmp_w; // width + 2*imgpadding + int tmp_h; // height + 2*imgpadding + float sc_fct; // scaling factor at current scale + int curr_lv; // current level + int camlr; // 0: left camera, 1: right camera, used only for depth, to restrict sideways patch motion +} camparam ; + +typedef struct +{ + // Explicitly set parameters: + int sc_f; // first (coarsest) scale + int sc_l; // last (finest) scale + int p_samp_s; // patch size (edge length in pixels) + int max_iter; // max. iterations on one scale + int min_iter; // min. iterations on one scale + float dp_thresh; // minimum rate of change of delta_p before descending one level, e.g. .1 : change scales when norm(delta_p_last)/norm(delta_p_init) < .1 + float dr_thresh; // minimum rate of change of residual within 3-iterations-window before descending one level, e.g. .8 : res_new/res_old > * .8, SET HIGH (1e10) TO DISABLE + float res_thresh; // if (mean absolute) residual falls below this threshold, terminate iterations on current scale, IGNORES MIN_ITER , SET TO LOW (1e-10) TO DISABLE + int patnorm; // Use patch mean-normalization + int verbosity; // Verbosity, 0: plot nothing, 1: final internal timing 2: complete iteration timing, (UNCOMMENTED -> 3: Display flow scales, 4: Display flow scale iterations) + bool usefbcon; // use forward-backward flow merging + int costfct; // Cost function: 0: L2-Norm, 1: L1-Norm, 2: PseudoHuber-Norm + bool usetvref; // TV parameters + float tv_alpha; + float tv_gamma; + float tv_delta; + int tv_innerit; + int tv_solverit; + float tv_sor; // Successive-over-relaxation weight + + // Automatically set parameters / fixed parameters + int nop; // number of parameters per pixel, 1 for depth, 2 for optical flow, 4 for scene flow + float patove; // point/line padding to all sides (px) + float outlierthresh; // displacement threshold (in px) before a patch is flagged as outlier + int steps; // horizontal and vertical distance (in px) between patch centers + int novals; // number of points in patch (=p_samp_s*p_samp_s) + int noc; // number of channels in image and gradients + int noscales; // total number of scales + float minerrval = 2.0f; // 1/max(this, error) for pixel averaging weight + float normoutlier = 5.0f; // norm error threshold for huber norm + + // Helper variables + v4sf zero = (v4sf) {0.0f, 0.0f, 0.0f, 0.0f}; + v4sf negzero = (v4sf) {-0.0f, -0.0f, -0.0f, -0.0f}; + v4sf half = (v4sf) {0.5f, 0.5f, 0.5f, 0.5f}; + v4sf ones = (v4sf) {1.0f, 1.0f, 1.0f, 1.0f}; + v4sf twos = (v4sf) {2.0f, 2.0f, 2.0f, 2.0f}; + v4sf fours = (v4sf) {4.0f, 4.0f, 4.0f, 4.0f}; + v4sf normoutlier_tmpbsq; + v4sf normoutlier_tmp2bsq; + v4sf normoutlier_tmp4bsq; + +} optparam; + + + +class OFClass +{ + +public: + OFClass(const float ** im_ao_in, const float ** im_ao_dx_in, const float ** im_ao_dy_in, // expects #sc_f_in pointers to float arrays for images and gradients. + // E.g. im_ao[sc_f_in] will be used as coarsest coarsest, im_ao[sc_l_in] as finest scale + // im_ao[ (sc_l_in-1) : 0 ] can be left as nullptr pointers + // IMPORTANT assumption: mod(width,2^sc_f_in)==0 AND mod(height,2^sc_f_in)==0, + const float ** im_bo_in, const float ** im_bo_dx_in, const float ** im_bo_dy_in, + const int imgpadding_in, + float * outflow, // Output-flow: has to be of size to fit the last computed OF scale [width / 2^(last scale) , height / 2^(last scale)] , 1 channel depth / 2 for OF + const float * initflow, // Initialization-flow: has to be of size to fit the first computed OF scale [width / 2^(first scale+1), height / 2^(first scale+1)], 1 channel depth / 2 for OF + const int width_in, const int height_in, + const int sc_f_in, const int sc_l_in, + const int max_iter_in, const int min_iter_in, + const float dp_thresh_in, + const float dr_thresh_in, + const float res_thresh_in, + const int padval_in, + const float patove_in, + const bool usefbcon_in, + const int costfct_in, + const int noc_in, + const int patnorm_in, + const bool usetvref_in, + const float tv_alpha_in, + const float tv_gamma_in, + const float tv_delta_in, + const int tv_innerit_in, + const int tv_solverit_in, + const float tv_sor_in, + const int verbosity_in); + +private: + + // needed for verbosity >= 3, DISVISUAL + //void DisplayDrawPatchBoundary(cv::Mat img, const Eigen::Vector2f pt, const float sc); + + const float ** im_ao, ** im_ao_dx, ** im_ao_dy; + const float ** im_bo, ** im_bo_dx, ** im_bo_dy; + + optparam op; // Struct for pptimization parameters + std::vector cpl, cpr; // Struct (for each scale) for camera/image parameter +}; + + +} + +#endif /* OFC_HEADER */ + + diff --git a/src/patch.cpp b/src/patch.cpp new file mode 100644 index 0000000..4241011 --- /dev/null +++ b/src/patch.cpp @@ -0,0 +1,407 @@ + + +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include "patch.h" + +using std::cout; +using std::endl; +using std::vector; + +namespace OFC +{ + + typedef __v4sf v4sf; + + PatClass::PatClass( + const camparam* cpt_in, + const camparam* cpo_in, + const optparam* op_in, + const int patchid_in) + : + cpt(cpt_in), + cpo(cpo_in), + op(op_in), + patchid(patchid_in) +{ + pc = new patchstate; + CreateStatusStruct(pc); + + tmp.resize(op->novals,1); + dxx_tmp.resize(op->novals,1); + dyy_tmp.resize(op->novals,1); +} + +void PatClass::CreateStatusStruct(patchstate * psin) +{ + // get reference / template patch + psin->pdiff.resize(op->novals,1); + psin->pweight.resize(op->novals,1); +} + +PatClass::~PatClass() +{ + delete pc; +} + +void PatClass::InitializePatch(Eigen::Map * im_ao_in, Eigen::Map * im_ao_dx_in, Eigen::Map * im_ao_dy_in, const Eigen::Vector2f pt_ref_in) +{ + im_ao = im_ao_in; + im_ao_dx = im_ao_dx_in; + im_ao_dy = im_ao_dy_in; + + pt_ref = pt_ref_in; + ResetPatch(); + + getPatchStaticNNGrad(im_ao->data(), im_ao_dx->data(), im_ao_dy->data(), &pt_ref, &tmp, &dxx_tmp, &dyy_tmp); + + ComputeHessian(); +} + +void PatClass::ComputeHessian() +{ + #if (SELECTMODE==1) + pc->Hes(0,0) = (dxx_tmp.array() * dxx_tmp.array()).sum(); + pc->Hes(0,1) = (dxx_tmp.array() * dyy_tmp.array()).sum(); + pc->Hes(1,1) = (dyy_tmp.array() * dyy_tmp.array()).sum(); + pc->Hes(1,0) = pc->Hes(0,1); + if (pc->Hes.determinant()==0) + { + pc->Hes(0,0)+=1e-10; + pc->Hes(1,1)+=1e-10; + } + #else + pc->Hes(0,0) = (dxx_tmp.array() * dxx_tmp.array()).sum(); + if (pc->Hes.sum()==0) + pc->Hes(0,0)+=1e-10; + #endif +} + +void PatClass::SetTargetImage(Eigen::Map * im_bo_in, Eigen::Map * im_bo_dx_in, Eigen::Map * im_bo_dy_in) +{ + im_bo = im_bo_in; + im_bo_dx = im_bo_dx_in; + im_bo_dy = im_bo_dy_in; + + ResetPatch(); +} + +void PatClass::ResetPatch() +{ + pc->hasconverged=0; + pc->hasoptstarted=0; + + pc->pt_st = pt_ref; + pc->pt_iter = pt_ref; + + pc->p_in.setZero(); + pc->p_iter.setZero(); + pc->delta_p.setZero(); + + pc->delta_p_sqnorm = 1e-10; + pc->delta_p_sqnorm_init = 1e-10; + pc->mares = 1e20; + pc->mares_old = 1e20; + pc->cnt=0; + pc->invalid = false; +} + +#if (SELECTMODE==1) +void PatClass::OptimizeStart(const Eigen::Vector2f p_in_arg) +#else +void PatClass::OptimizeStart(const Eigen::Matrix p_in_arg) +#endif +{ + pc->p_in = p_in_arg; + pc->p_iter = p_in_arg; + + // convert from input parameters to 2D query location(s) for patches + paramtopt(); + + // save starting location, only needed for outlier check + pc->pt_st = pc->pt_iter; + + //Check if initial position is already invalid + if (pc->pt_iter[0] < cpt->tmp_lb || pc->pt_iter[1] < cpt->tmp_lb || // check if patch left valid image region + pc->pt_iter[0] > cpt->tmp_ubw || pc->pt_iter[1] > cpt->tmp_ubh) + { + pc->hasconverged=1; + pc->pdiff = tmp; + pc->hasoptstarted=1; + } + else + { + pc->cnt=0; // reset iteration counter + pc->delta_p_sqnorm = 1e-10; + pc->delta_p_sqnorm_init = 1e-10; // set to arbitrary low value, s.t. that loop condition is definitely true on first iteration + pc->mares = 1e5; // mean absolute residual + pc->mares_old = 1e20; // for rate of change, keep mares from last iteration in here. Set high so that loop condition is definitely true on first iteration + pc->hasconverged=0; + + OptimizeComputeErrImg(); + + pc->hasoptstarted=1; + pc->invalid = false; + } +} + +#if (SELECTMODE==1) +void PatClass::OptimizeIter(const Eigen::Vector2f p_in_arg, const bool untilconv) +#else +void PatClass::OptimizeIter(const Eigen::Matrix p_in_arg, const bool untilconv) +#endif +{ + if (!pc->hasoptstarted) + { + ResetPatch(); + OptimizeStart(p_in_arg); + } + int oldcnt=pc->cnt; + + // optimize patch until convergence, or do only one iteration if DIS visualization is used + while ( ! (pc->hasconverged || (untilconv == false && (pc->cnt > oldcnt))) ) + { + pc->cnt++; + + // Projection onto sd_images + #if (SELECTMODE==1) + pc->delta_p[0] = (dxx_tmp.array() * pc->pdiff.array()).sum(); + pc->delta_p[1] = (dyy_tmp.array() * pc->pdiff.array()).sum(); + #else + pc->delta_p[0] = (dxx_tmp.array() * pc->pdiff.array()).sum(); + #endif + + pc->delta_p = pc->Hes.llt().solve(pc->delta_p); // solve linear system + + pc->p_iter -= pc->delta_p; // update flow vector + + #if (SELECTMODE==2) // if stereo depth + if (cpt->camlr==0) + pc->p_iter[0] = std::min(pc->p_iter[0],0.0f); // disparity in t can only be negative (in right image) + else + pc->p_iter[0] = std::max(pc->p_iter[0],0.0f); // ... positive (in left image) + #endif + + // compute patch locations based on new parameter vector + paramtopt(); + + // check if patch(es) moved too far from starting location, if yes, stop iteration and reset to starting location + if ((pc->pt_st - pc->pt_iter).norm() > op->outlierthresh // check if query patch moved more than >padval from starting location -> most likely outlier + || + pc->pt_iter[0] < cpt->tmp_lb || pc->pt_iter[1] < cpt->tmp_lb || // check patch left valid image region + pc->pt_iter[0] > cpt->tmp_ubw || pc->pt_iter[1] > cpt->tmp_ubh) + { + pc->p_iter = pc->p_in; // reset + paramtopt(); + pc->hasconverged=1; + pc->hasoptstarted=1; + } + + OptimizeComputeErrImg(); + } +} + +inline void PatClass::paramtopt() +{ + #if (SELECTMODE==1) + pc->pt_iter = pt_ref + pc->p_iter; // for optical flow the point displacement and the parameter vector are equivalent + #else + pc->pt_iter[0] = pt_ref[0] + pc->p_iter[0]; + #endif +} + +void PatClass::LossComputeErrorImage(Eigen::Matrix* patdest, Eigen::Matrix* wdest, const Eigen::Matrix* patin, const Eigen::Matrix* tmpin) +{ + v4sf * pd = (v4sf*) patdest->data(), + * pa = (v4sf*) patin->data(), + * te = (v4sf*) tmpin->data(), + * pw = (v4sf*) wdest->data(); + + if (op->costfct==0) // L2 cost function + { + for (int i=op->novals/4; i--; ++pd, ++pa, ++te, ++pw) + { + (*pd) = (*pa)-(*te); // difference image + (*pw) = __builtin_ia32_andnps(op->negzero, (*pd) ); + } + } + else if (op->costfct==1) // L1 cost function + { + for (int i=op->novals/4; i--; ++pd, ++pa, ++te, ++pw) + { + (*pd) = (*pa)-(*te); // difference image + (*pd) = __builtin_ia32_orps( __builtin_ia32_andps(op->negzero, (*pd) ) , __builtin_ia32_sqrtps (__builtin_ia32_andnps(op->negzero, (*pd) )) ); // sign(pdiff) * sqrt(abs(pdiff)) + (*pw) = __builtin_ia32_andnps(op->negzero, (*pd) ); + } + } + else if (op->costfct==2) // Pseudo Huber cost function + { + for (int i=op->novals/4; i--; ++pd, ++pa, ++te, ++pw) + { + (*pd) = (*pa)-(*te); // difference image + (*pd) = __builtin_ia32_orps(__builtin_ia32_andps(op->negzero, (*pd) ), + __builtin_ia32_sqrtps ( + __builtin_ia32_mulps( // PSEUDO HUBER NORM + __builtin_ia32_sqrtps (op->ones + __builtin_ia32_divps(__builtin_ia32_mulps((*pd),(*pd)) , op->normoutlier_tmpbsq)) - op->ones, // PSEUDO HUBER NORM + op->normoutlier_tmp2bsq) // PSEUDO HUBER NORM + ) + ); // sign(pdiff) * sqrt( 2*b^2*( sqrt(1+abs(pdiff)^2/b^2)+1) )) // <- looks like this without SSE instruction + (*pw) = __builtin_ia32_andnps(op->negzero, (*pd) ); + } + } +} + +void PatClass::OptimizeComputeErrImg() +{ + getPatchStaticBil(im_bo->data(), &(pc->pt_iter), &(pc->pdiff)); + + // Get photometric patch error + LossComputeErrorImage(&pc->pdiff, &pc->pweight, &pc->pdiff, &tmp); + + // Compute step norm + pc->delta_p_sqnorm = pc->delta_p.squaredNorm(); + if (pc->cnt==1) + pc->delta_p_sqnorm_init = pc->delta_p_sqnorm; + + // Check early termination criterions + pc->mares_old = pc->mares; + pc->mares = pc->pweight.lpNorm<1>() / (op->novals); + if ( ! ((pc->cnt < op->max_iter) & (pc->mares > op->res_thresh) & + ((pc->cnt < op->min_iter) | (pc->delta_p_sqnorm / pc->delta_p_sqnorm_init >= op->dp_thresh)) & + ((pc->cnt < op->min_iter) | (pc->mares / pc->mares_old <= op->dr_thresh))) ) + pc->hasconverged=1; + +} + +// Extract patch on integer position, and gradients, No Bilinear interpolation +void PatClass::getPatchStaticNNGrad(const float* img, const float* img_dx, const float* img_dy, + const Eigen::Vector2f* mid_in, + Eigen::Matrix* tmp_in_e, + Eigen::Matrix* tmp_dx_in_e, + Eigen::Matrix* tmp_dy_in_e) +{ + float *tmp_in = tmp_in_e->data(); + float *tmp_dx_in = tmp_dx_in_e->data(); + float *tmp_dy_in = tmp_dy_in_e->data(); + + Eigen::Vector2i pos; + Eigen::Vector2i pos_it; + + pos[0] = round((*mid_in)[0]) + cpt->imgpadding; + pos[1] = round((*mid_in)[1]) + cpt->imgpadding; + + int posxx = 0; + + int lb = -op->p_samp_s/2; + int ub = op->p_samp_s/2-1; + + for (int j=lb; j <= ub; ++j) + { + for (int i=lb; i <= ub; ++i, ++posxx) + { + pos_it[0] = pos[0]+i; + pos_it[1] = pos[1]+j; + int idx = pos_it[0] + pos_it[1] * cpt->tmp_w; + + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // Single channel + tmp_in[posxx] = img[idx]; + tmp_dx_in[posxx] = img_dx[idx]; + tmp_dy_in[posxx] = img_dy[idx]; + #else // 3 RGB channels + idx *= 3; + tmp_in[posxx] = img[idx]; tmp_dx_in[posxx] = img_dx[idx]; tmp_dy_in[posxx] = img_dy[idx]; ++posxx; ++idx; + tmp_in[posxx] = img[idx]; tmp_dx_in[posxx] = img_dx[idx]; tmp_dy_in[posxx] = img_dy[idx]; ++posxx; ++idx; + tmp_in[posxx] = img[idx]; tmp_dx_in[posxx] = img_dx[idx]; tmp_dy_in[posxx] = img_dy[idx]; + #endif + } + } + + // PATCH NORMALIZATION + if (op->patnorm>0) // Subtract Mean + tmp_in_e->array() -= (tmp_in_e->sum() / op->novals); +} + +// Extract patch on float position with bilinear interpolation, no gradients. +void PatClass::getPatchStaticBil(const float* img, const Eigen::Vector2f* mid_in, Eigen::Matrix* tmp_in_e) +{ + float *tmp_in = tmp_in_e->data(); + + Eigen::Vector2f resid; + Eigen::Vector4f we; // bilinear weight vector + Eigen::Vector4i pos; + Eigen::Vector2i pos_it; + + // Compute the bilinear weight vector, for patch without orientation/scale change -> weight vector is constant for all pixels + pos[0] = ceil((*mid_in)[0]+.00001f); // ensure rounding up to natural numbers + pos[1] = ceil((*mid_in)[1]+.00001f); + pos[2] = floor((*mid_in)[0]); + pos[3] = floor((*mid_in)[1]); + + resid[0] = (*mid_in)[0] - (float)pos[2]; + resid[1] = (*mid_in)[1] - (float)pos[3]; + we[0] = resid[0]*resid[1]; + we[1] = (1-resid[0])*resid[1]; + we[2] = resid[0]*(1-resid[1]); + we[3] = (1-resid[0])*(1-resid[1]); + + pos[0] += cpt->imgpadding; + pos[1] += cpt->imgpadding; + + float * tmp_it = tmp_in; + const float * img_a, * img_b, * img_c, * img_d, *img_e; + + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // 1 channel image + img_e = img + pos[0]-op->p_samp_s/2; + #else // 3-channel RGB image + img_e = img + (pos[0]-op->p_samp_s/2)*3; + #endif + + int lb = -op->p_samp_s/2; + int ub = op->p_samp_s/2-1; + + for (pos_it[1]=pos[1]+lb; pos_it[1] <= pos[1]+ub; ++pos_it[1]) + { + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // 1 channel image + img_a = img_e + pos_it[1] * cpt->tmp_w; + img_c = img_e + (pos_it[1]-1) * cpt->tmp_w; + img_b = img_a-1; + img_d = img_c-1; + #else // 3-channel RGB image + img_a = img_e + pos_it[1] * cpt->tmp_w * 3; + img_c = img_e + (pos_it[1]-1) * cpt->tmp_w * 3; + img_b = img_a-3; + img_d = img_c-3; + #endif + + + for (pos_it[0]=pos[0]+lb; pos_it[0] <= pos[0]+ub; ++pos_it[0], + ++tmp_it,++img_a,++img_b,++img_c,++img_d) + { + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // Single channel + (*tmp_it) = we[0] * (*img_a) + we[1] * (*img_b) + we[2] * (*img_c) + we[3] * (*img_d); + #else // 3-channel RGB image + (*tmp_it) = we[0] * (*img_a) + we[1] * (*img_b) + we[2] * (*img_c) + we[3] * (*img_d); ++tmp_it; ++img_a; ++img_b; ++img_c; ++img_d; + (*tmp_it) = we[0] * (*img_a) + we[1] * (*img_b) + we[2] * (*img_c) + we[3] * (*img_d); ++tmp_it; ++img_a; ++img_b; ++img_c; ++img_d; + (*tmp_it) = we[0] * (*img_a) + we[1] * (*img_b) + we[2] * (*img_c) + we[3] * (*img_d); + #endif + } + } + // PATCH NORMALIZATION + if (op->patnorm>0) // Subtract Mean + tmp_in_e->array() -= (tmp_in_e->sum() / op->novals); +} + + +} + + diff --git a/src/patch.h b/src/patch.h new file mode 100644 index 0000000..fed74d1 --- /dev/null +++ b/src/patch.h @@ -0,0 +1,131 @@ + +// Class implements step (3.) in Algorithm 1 of the paper: +// It finds the displacement of one patch from reference/template image to the closest-matching patch in target image via gradient descent. + + +#ifndef PAT_HEADER +#define PAT_HEADER + + //#include // needed for verbosity >= 3, DISVISUAL + //#include // needed for verbosity >= 3, DISVISUAL + //#include // needed for verbosity >= 3, DISVISUAL + +#include "oflow.h" // For camera intrinsic and opt. parameter struct + +namespace OFC +{ + + +typedef struct +{ + bool hasconverged; + bool hasoptstarted; + + // reference/template patch + Eigen::Matrix pdiff; // image error to reference image + Eigen::Matrix pweight; // absolute error image + + #if (SELECTMODE==1) // Optical Flow + Eigen::Matrix Hes; // Hessian for optimization + Eigen::Vector2f p_in, p_iter, delta_p; // point position, displacement to starting position, iteration update + #else // Depth from Stereo + Eigen::Matrix Hes; // Hessian for optimization + Eigen::Matrix p_in, p_iter, delta_p; // point position, displacement to starting position, iteration update + #endif + + // start positions, current point position, patch norm + Eigen::Matrix normtmp; + Eigen::Vector2f pt_iter; + Eigen::Vector2f pt_st; + + float delta_p_sqnorm = 1e-10; + float delta_p_sqnorm_init = 1e-10; + float mares = 1e20; // mares: Mean Absolute RESidual + float mares_old = 1e20; + int cnt=0; + bool invalid=false; +} patchstate; + + + +class PatClass +{ + +public: + PatClass(const camparam* cpt_in, + const camparam* cpo_in, + const optparam* op_in, + const int patchid_in); + + ~PatClass(); + + void InitializePatch(Eigen::Map * im_ao_in, Eigen::Map * im_ao_dx_in, Eigen::Map * im_ao_dy_in, const Eigen::Vector2f pt_ref_in); + void SetTargetImage(Eigen::Map * im_bo_in, Eigen::Map * im_bo_dx_in, Eigen::Map * im_bo_dy_in); + + #if (SELECTMODE==1) // Optical Flow + void OptimizeIter(const Eigen::Vector2f p_in_arg, const bool untilconv); + #else // Depth from Stereo + void OptimizeIter(const Eigen::Matrix p_in_arg, const bool untilconv); + #endif + + inline const bool isConverged() const { return pc->hasconverged; } + inline const bool hasOptStarted() const { return pc->hasoptstarted; } + inline const Eigen::Vector2f GetPointPos() const { return pc->pt_iter; } // get current iteration patch position (in this frame's opposite camera for OF, Depth) + inline const bool IsValid() const { return (!pc->invalid) ; } + inline const float * GetpWeightPtr() const {return (float*) pc->pweight.data(); } // Return data pointer to image error patch, used in efficient indexing for densification in patchgrid class + + #if (SELECTMODE==1) // Optical Flow + inline const Eigen::Vector2f* GetParam() const { return &(pc->p_iter); } // get current iteration parameters + #else // Depth from Stereo + inline const Eigen::Matrix* GetParam() const { return &(pc->p_iter); } // get current iteration parameters + #endif + + #if (SELECTMODE==1) // Optical Flow + inline const Eigen::Vector2f* GetParamStart() const { return &(pc->p_in); } + #else // Depth from Stereo + inline const Eigen::Matrix* GetParamStart() const { return &(pc->p_in); } + #endif + +private: + + #if (SELECTMODE==1) // Optical Flow + void OptimizeStart(const Eigen::Vector2f p_in_arg); + #else // Depth from Stereo + void OptimizeStart(const Eigen::Matrix p_in_arg); + #endif + + void OptimizeComputeErrImg(); + void paramtopt(); + void ResetPatch(); + void ComputeHessian(); + void CreateStatusStruct(patchstate * psin); + void LossComputeErrorImage(Eigen::Matrix* patdest, Eigen::Matrix* wdest, const Eigen::Matrix* patin, const Eigen::Matrix* tmpin); + + // Extract patch on integer position, and gradients, No Bilinear interpolation + void getPatchStaticNNGrad (const float* img, const float* img_dx, const float* img_dy, const Eigen::Vector2f* mid_in, Eigen::Matrix* tmp_in, Eigen::Matrix* tmp_dx_in, Eigen::Matrix* tmp_dy_in); + // Extract patch on float position with bilinear interpolation, no gradients. + void getPatchStaticBil(const float* img, const Eigen::Vector2f* mid_in, Eigen::Matrix* tmp_in_e); + + Eigen::Vector2f pt_ref; // reference point location + Eigen::Matrix tmp; + Eigen::Matrix dxx_tmp; // x derivative, doubles as steepest descent image for OF, Depth, SF + Eigen::Matrix dyy_tmp; // y derivative, doubles as steepest descent image for OF, SF + + Eigen::Map * im_ao, * im_ao_dx, * im_ao_dy; + Eigen::Map * im_bo, * im_bo_dx, * im_bo_dy; + + const camparam* cpt; + const camparam* cpo; + const optparam* op; + const int patchid; + + patchstate * pc = nullptr; // current patch state + +}; + + +} + +#endif /* PAT_HEADER */ + + diff --git a/src/patchgrid.cpp b/src/patchgrid.cpp new file mode 100644 index 0000000..edb12ce --- /dev/null +++ b/src/patchgrid.cpp @@ -0,0 +1,401 @@ + +// #include // needed for verbosity >= 3, DISVISUAL +// #include // needed for verbosity >= 3, DISVISUAL +// #include // needed for verbosity >= 3, DISVISUAL + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include "patch.h" +#include "patchgrid.h" + + +using std::cout; +using std::endl; +using std::vector; + + +namespace OFC +{ + + PatGridClass::PatGridClass( + const camparam* cpt_in, + const camparam* cpo_in, + const optparam* op_in) + : + cpt(cpt_in), + cpo(cpo_in), + op(op_in) + { + + // Generate grid on current scale + steps = op->steps; + nopw = ceil( (float)cpt->width / (float)steps ); + noph = ceil( (float)cpt->height / (float)steps ); + const int offsetw = floor((cpt->width - (nopw-1)*steps)/2); + const int offseth = floor((cpt->height - (noph-1)*steps)/2); + + nopatches = nopw*noph; + pt_ref.resize(nopatches); + p_init.resize(nopatches); + pat.reserve(nopatches); + + im_ao_eg = new Eigen::Map(nullptr,cpt->height,cpt->width); + im_ao_dx_eg = new Eigen::Map(nullptr,cpt->height,cpt->width); + im_ao_dy_eg = new Eigen::Map(nullptr,cpt->height,cpt->width); + + im_bo_eg = new Eigen::Map(nullptr,cpt->height,cpt->width); + im_bo_dx_eg = new Eigen::Map(nullptr,cpt->height,cpt->width); + im_bo_dy_eg = new Eigen::Map(nullptr,cpt->height,cpt->width); + + int patchid=0; + for (int x = 0; x < nopw; ++x) + { + for (int y = 0; y < noph; ++y) + { + int i = x*noph + y; + + pt_ref[i][0] = x * steps + offsetw; + pt_ref[i][1] = y * steps + offseth; + p_init[i].setZero(); + + pat.push_back(new OFC::PatClass(cpt, cpo, op, patchid)); + patchid++; + } + } +} + +PatGridClass::~PatGridClass() +{ + delete im_ao_eg; + delete im_ao_dx_eg; + delete im_ao_dy_eg; + + delete im_bo_eg; + delete im_bo_dx_eg; + delete im_bo_dy_eg; + + for (int i=0; i< nopatches; ++i) + delete pat[i]; +} + +void PatGridClass::SetComplGrid(PatGridClass *cg_in) +{ + cg = cg_in; +} + + +void PatGridClass::InitializeGrid(const float * im_ao_in, const float * im_ao_dx_in, const float * im_ao_dy_in) +{ + im_ao = im_ao_in; + im_ao_dx = im_ao_dx_in; + im_ao_dy = im_ao_dy_in; + + new (im_ao_eg) Eigen::Map(im_ao,cpt->height,cpt->width); // new placement operator + new (im_ao_dx_eg) Eigen::Map(im_ao_dx,cpt->height,cpt->width); + new (im_ao_dy_eg) Eigen::Map(im_ao_dy,cpt->height,cpt->width); + + + #pragma omp parallel for schedule(static) + for (int i = 0; i < nopatches; ++i) + { + pat[i]->InitializePatch(im_ao_eg, im_ao_dx_eg, im_ao_dy_eg, pt_ref[i]); + p_init[i].setZero(); + } + +} + +void PatGridClass::SetTargetImage(const float * im_bo_in, const float * im_bo_dx_in, const float * im_bo_dy_in) +{ + im_bo = im_bo_in; + im_bo_dx = im_bo_dx_in; + im_bo_dy = im_bo_dy_in; + + new (im_bo_eg) Eigen::Map(im_bo,cpt->height,cpt->width); // new placement operator + new (im_bo_dx_eg) Eigen::Map(im_bo_dx,cpt->height,cpt->width); // new placement operator + new (im_bo_dy_eg) Eigen::Map(im_bo_dy,cpt->height,cpt->width); // new placement operator + + #pragma omp parallel for schedule(static) + for (int i = 0; i < nopatches; ++i) + pat[i]->SetTargetImage(im_bo_eg, im_bo_dx_eg, im_bo_dy_eg); + +} + +void PatGridClass::Optimize() +{ + #pragma omp parallel for schedule(dynamic,10) + for (int i = 0; i < nopatches; ++i) + { + pat[i]->OptimizeIter(p_init[i], true); // optimize until convergence + } +} + +// void PatGridClass::OptimizeAndVisualize(const float sc_fct_tmp) // needed for verbosity >= 3, DISVISUAL +// { +// bool allconverged=0; +// int cnt = 0; +// while (!allconverged) +// { +// cnt++; +// +// allconverged=1; +// +// for (int i = 0; i < nopatches; ++i) +// { +// if (pat[i]->isConverged()==0) +// { +// pat[i]->OptimizeIter(p_init[i], false); // optimize, only one iterations +// allconverged=0; +// } +// } +// +// +// // Display original image +// const cv::Mat src(cpt->height+2*cpt->imgpadding, cpt->width+2*cpt->imgpadding, CV_32FC1, (void*) im_ao); +// cv::Mat img_ao_mat = src(cv::Rect(cpt->imgpadding,cpt->imgpadding,cpt->width,cpt->height)); +// cv::Mat outimg; +// img_ao_mat.convertTo(outimg, CV_8UC1); +// cv::cvtColor(outimg, outimg, CV_GRAY2RGB); +// cv::resize(outimg, outimg, cv::Size(), sc_fct_tmp, sc_fct_tmp, cv::INTER_NEAREST); +// +// for (int i = 0; i < nopatches; ++i) +// { +// // Show displacement vector +// const Eigen::Vector2f pt_ret = pat[i]->GetPointPos(); +// +// Eigen::Vector2f pta, ptb; +// +// cv::line(outimg, cv::Point( (pt_ref[i][0]+.5)*sc_fct_tmp, (pt_ref[i][1]+.5)*sc_fct_tmp ), cv::Point( (pt_ret[0]+.5)*sc_fct_tmp, (pt_ret[1]+.5)*sc_fct_tmp ), cv::Scalar(255*pat[i]->isConverged() ,255*(!pat[i]->isConverged()),0), 2); +// +// cv::line(outimg, cv::Point( (cpt->cx+.5)*sc_fct_tmp, (cpt->cy+.5)*sc_fct_tmp ), cv::Point( (cpt->cx+.5)*sc_fct_tmp, (cpt->cy+.5)*sc_fct_tmp ), cv::Scalar(0,0, 255), 2); +// +// } +// +// char str[200]; +// sprintf(str,"Iter: %i",cnt); +// cv::putText(outimg, str, cv::Point2f(20,20), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0,0,255,255), 2); +// +// cv::namedWindow( "Img_iter", cv::WINDOW_AUTOSIZE ); +// cv::imshow( "Img_iter", outimg); +// +// cv::waitKey(500); +// } +// } + +void PatGridClass::InitializeFromCoarserOF(const float * flow_prev) +{ + #pragma omp parallel for schedule(dynamic,10) + for (int ip = 0; ip < nopatches; ++ip) + { + int x = floor(pt_ref[ip][0] / 2); // better, but slower: use bil. interpolation here + int y = floor(pt_ref[ip][1] / 2); + int i = y*(cpt->width/2) + x; + + #if (SELECTMODE==1) + p_init[ip](0) = flow_prev[2*i ]*2; + p_init[ip](1) = flow_prev[2*i+1]*2; + #else + p_init[ip](0) = flow_prev[ i ]*2; + #endif + } +} + +void PatGridClass::AggregateFlowDense(float *flowout) const +{ + float* we = new float[cpt->width * cpt->height]; + + memset(flowout, 0, sizeof(float) * (op->nop * cpt->width * cpt->height) ); + memset(we, 0, sizeof(float) * ( cpt->width * cpt->height) ); + + #ifdef USE_PARALLEL_ON_FLOWAGGR // Using this enables OpenMP on flow aggregation. This can lead to race conditions. Experimentally we found that the result degrades only marginally. However, for our experiments we did not enable this. + #pragma omp parallel for schedule(static) + #endif + for (int ip = 0; ip < nopatches; ++ip) + { + + if (pat[ip]->IsValid()) + { + #if (SELECTMODE==1) + const Eigen::Vector2f* fl = pat[ip]->GetParam(); // flow displacement of this patch + Eigen::Vector2f flnew; + #else + const Eigen::Matrix* fl = pat[ip]->GetParam(); // horz. displacement of this patch + Eigen::Matrix flnew; + #endif + + const float * pweight = pat[ip]->GetpWeightPtr(); // use image error as weight + + int lb = -op->p_samp_s/2; + int ub = op->p_samp_s/2-1; + + for (int y = lb; y <= ub; ++y) + { + for (int x = lb; x <= ub; ++x, ++pweight) + { + int yt = (y + pt_ref[ip][1]); + int xt = (x + pt_ref[ip][0]); + + if (xt >= 0 && yt >= 0 && xt < cpt->width && yt < cpt->height) + { + + int i = yt*cpt->width + xt; + + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // single channel/gradient image + float absw = 1.0f / (float)(std::max(op->minerrval ,*pweight)); + #else // RGB image + float absw = (float)(std::max(op->minerrval ,*pweight)); ++pweight; + absw+= (float)(std::max(op->minerrval ,*pweight)); ++pweight; + absw+= (float)(std::max(op->minerrval ,*pweight)); + absw = 1.0f / absw; + #endif + + flnew = (*fl) * absw; + we[i] += absw; + + #if (SELECTMODE==1) + flowout[2*i] += flnew[0]; + flowout[2*i+1] += flnew[1]; + #else + flowout[i] += flnew[0]; + #endif + } + } + } + } + } + + // if complementary (forward-backward merging) is given, integrate negative backward flow as well + if (cg) + { + Eigen::Vector4f wbil; // bilinear weight vector + Eigen::Vector4i pos; + + #ifdef USE_PARALLEL_ON_FLOWAGGR + #pragma omp parallel for schedule(static) + #endif + for (int ip = 0; ip < cg->nopatches; ++ip) + { + if (cg->pat[ip]->IsValid()) + { + #if (SELECTMODE==1) + const Eigen::Vector2f* fl = (cg->pat[ip]->GetParam()); // flow displacement of this patch + Eigen::Vector2f flnew; + #else + const Eigen::Matrix* fl = (cg->pat[ip]->GetParam()); // horz. displacement of this patch + Eigen::Matrix flnew; + #endif + + const Eigen::Vector2f rppos = cg->pat[ip]->GetPointPos(); // get patch position after optimization + const float * pweight = cg->pat[ip]->GetpWeightPtr(); // use image error as weight + + Eigen::Vector2f resid; + + // compute bilinear weight vector + pos[0] = ceil(rppos[0] +.00001); // make sure they are rounded up to natural number + pos[1] = ceil(rppos[1] +.00001); // make sure they are rounded up to natural number + pos[2] = floor(rppos[0]); + pos[3] = floor(rppos[1]); + + resid[0] = rppos[0] - pos[2]; + resid[1] = rppos[1] - pos[3]; + wbil[0] = resid[0]*resid[1]; + wbil[1] = (1-resid[0])*resid[1]; + wbil[2] = resid[0]*(1-resid[1]); + wbil[3] = (1-resid[0])*(1-resid[1]); + + int lb = -op->p_samp_s/2; + int ub = op->p_samp_s/2-1; + + + for (int y = lb; y <= ub; ++y) + { + for (int x = lb; x <= ub; ++x, ++pweight) + { + + int yt = y + pos[1]; + int xt = x + pos[0]; + if (xt >= 1 && yt >= 1 && xt < (cpt->width-1) && yt < (cpt->height-1)) + { + + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // single channel/gradient image + float absw = 1.0f / (float)(std::max(op->minerrval ,*pweight)); + #else // RGB + float absw = (float)(std::max(op->minerrval ,*pweight)); ++pweight; + absw+= (float)(std::max(op->minerrval ,*pweight)); ++pweight; + absw+= (float)(std::max(op->minerrval ,*pweight)); + absw = 1.0f / absw; + #endif + + + flnew = (*fl) * absw; + + int idxcc = xt + yt *cpt->width; + int idxfc = (xt-1) + yt *cpt->width; + int idxcf = xt + (yt-1)*cpt->width; + int idxff = (xt-1) + (yt-1)*cpt->width; + + we[idxcc] += wbil[0] * absw; + we[idxfc] += wbil[1] * absw; + we[idxcf] += wbil[2] * absw; + we[idxff] += wbil[3] * absw; + + #if (SELECTMODE==1) + flowout[2*idxcc ] -= wbil[0] * flnew[0]; // use reversed flow + flowout[2*idxcc+1] -= wbil[0] * flnew[1]; + + flowout[2*idxfc ] -= wbil[1] * flnew[0]; + flowout[2*idxfc+1] -= wbil[1] * flnew[1]; + + flowout[2*idxcf ] -= wbil[2] * flnew[0]; + flowout[2*idxcf+1] -= wbil[2] * flnew[1]; + + flowout[2*idxff ] -= wbil[3] * flnew[0]; + flowout[2*idxff+1] -= wbil[3] * flnew[1]; + #else + flowout[idxcc] -= wbil[0] * flnew[0]; // simple averaging of inverse horizontal displacement + flowout[idxfc] -= wbil[1] * flnew[0]; + flowout[idxcf] -= wbil[2] * flnew[0]; + flowout[idxff] -= wbil[3] * flnew[0]; + #endif + } + } + } + } + } + } + + #pragma omp parallel for schedule(static, 100) + // normalize each pixel by dividing displacement by aggregated weights from all patches + for (int yi = 0; yi < cpt->height; ++yi) + { + for (int xi = 0; xi < cpt->width; ++xi) + { + int i = yi*cpt->width + xi; + if (we[i]>0) + { + #if (SELECTMODE==1) + flowout[2*i ] /= we[i]; + flowout[2*i+1] /= we[i]; + #else + flowout[i] /= we[i]; + #endif + } + } + } + + delete[] we; +} + +} + + diff --git a/src/patchgrid.h b/src/patchgrid.h new file mode 100644 index 0000000..8aa1e50 --- /dev/null +++ b/src/patchgrid.h @@ -0,0 +1,79 @@ + +// Class implements step (3.) in Algorithm 1 of the paper: +// I holds all patch objects on a specific scale, and organizes 1) the dense inverse search, 2) densification + +#ifndef PATGRID_HEADER +#define PATGRID_HEADER + +#include "patch.h" +#include "oflow.h" // For camera intrinsic and opt. parameter struct + + +namespace OFC +{ + +class PatGridClass +{ + +public: + PatGridClass(const camparam* cpt_in, + const camparam* cpo_in, + const optparam* op_in); + + ~PatGridClass(); + + void InitializeGrid(const float * im_ao_in, const float * im_ao_dx_in, const float * im_ao_dy_in); + void SetTargetImage(const float * im_bo_in, const float * im_bo_dx_in, const float * im_bo_dy_in); + void InitializeFromCoarserOF(const float * flow_prev); + + void AggregateFlowDense(float *flowout) const; + + // Optimizes grid to convergence of each patch + void Optimize(); + //Optimize each patch in grid for one iteration, visualize displacement vector, repeat + //void OptimizeAndVisualize(const float sc_fct_tmp); // needed for verbosity >= 3, DISVISUAL + + void SetComplGrid(PatGridClass *cg_in); + + inline const int GetNoPatches() const { return nopatches; } + inline const int GetNoph() const { return noph; } + inline const int GetNopw() const { return nopw; } + + inline const Eigen::Vector2f GetRefPatchPos(int i) const { return pt_ref[i]; } // Get reference patch position + inline const Eigen::Vector2f GetQuePatchPos(int i) const { return pat[i]->GetPointPos(); } // Get target/query patch position + inline const Eigen::Vector2f GetQuePatchDis(int i) const { return pt_ref[i]-pat[i]->GetPointPos(); } // Get query patch displacement from reference patch + +private: + + const float * im_ao, * im_ao_dx, * im_ao_dy; + const float * im_bo, * im_bo_dx, * im_bo_dy; + + Eigen::Map * im_ao_eg, * im_ao_dx_eg, * im_ao_dy_eg; + Eigen::Map * im_bo_eg, * im_bo_dx_eg, * im_bo_dy_eg; + + const camparam* cpt; + const camparam* cpo; + const optparam* op; + + int steps; + int nopw; + int noph; + int nopatches; + + std::vector pat; // Patch Objects + std::vector pt_ref; // Midpoints for reference patches + #if (SELECTMODE==1) + std::vector p_init; // starting parameters for query patches, use only 1 for depth, 2 for OF, all 4 for scene flow + #else + std::vector> p_init; // starting parameters for query patches, use only 1 for depth, 2 for OF, all 4 for scene flow + #endif + + const PatGridClass * cg=nullptr; +}; + + +} + +#endif /* PATGRID_HEADER */ + + diff --git a/src/refine_variational.cpp b/src/refine_variational.cpp new file mode 100644 index 0000000..8ab62bc --- /dev/null +++ b/src/refine_variational.cpp @@ -0,0 +1,344 @@ + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include "refine_variational.h" + +using std::cout; +using std::endl; +using std::vector; + + +namespace OFC +{ + + VarRefClass::VarRefClass(const float * im_ao_in, const float * im_ao_dx_in, const float * im_ao_dy_in, + const float * im_bo_in, const float * im_bo_dx_in, const float * im_bo_dy_in, + const camparam* cpt_in,const camparam* cpo_in,const optparam* op_in, float *flowout) + : cpt(cpt_in), cpo(cpo_in), op(op_in) +{ + + // initialize parameters + tvparams.alpha = op->tv_alpha; + tvparams.beta = 0.0f; // for matching term, not needed for us + tvparams.gamma = op->tv_gamma; + tvparams.delta = op->tv_delta; + tvparams.n_inner_iteration = op->tv_innerit * (cpt->curr_lv+1); + tvparams.n_solver_iteration = op->tv_solverit;//5; + tvparams.sor_omega = op->tv_sor; + + tvparams.tmp_quarter_alpha = 0.25f*tvparams.alpha; + tvparams.tmp_half_gamma_over3 = tvparams.gamma*0.5f/3.0f; + tvparams.tmp_half_delta_over3 = tvparams.delta*0.5f/3.0f; + tvparams.tmp_half_beta = tvparams.beta*0.5f; + + float deriv_filter[3] = {0.0f, -8.0f/12.0f, 1.0f/12.0f}; + deriv = convolution_new(2, deriv_filter, 0); + float deriv_filter_flow[2] = {0.0f, -0.5f}; + deriv_flow = convolution_new(1, deriv_filter_flow, 0); + + // copy flow initialization into FV structs + #if (SELECTMODE==1) + static int noparam = 2; // Optical flow + #else + static int noparam = 1; // Only horizontal displacements for stereo depth + #endif + std::vector flow_sep(noparam); + + for (int i = 0; i < noparam; ++i ) + flow_sep[i] = image_new(cpt->width,cpt->height); + + for (int iy = 0; iy < cpt->height; ++iy) + for (int ix = 0; ix < cpt->width; ++ix) + { + int i = iy * cpt->width + ix; + int is = iy * flow_sep[0]->stride + ix; + for (int j = 0; j < noparam; ++j) + flow_sep[j]->c1[is] = flowout[i*noparam + j]; + } + + // copy image data into FV structs + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) + image_t * im_ao, *im_bo; + im_ao = image_new(cpt->width,cpt->height); + im_bo = image_new(cpt->width,cpt->height); + #else + color_image_t * im_ao, *im_bo; + im_ao = color_image_new(cpt->width,cpt->height); + im_bo = color_image_new(cpt->width,cpt->height); + #endif + + copyimage(im_ao_in, im_ao); + copyimage(im_bo_in, im_bo); + + // Call solver + #if (SELECTMODE==1) + RefLevelOF(flow_sep[0], flow_sep[1], im_ao, im_bo); + #else + RefLevelDE(flow_sep[0], im_ao, im_bo); + #endif + + // Copy flow result back + for (int iy = 0; iy < cpt->height; ++iy) + for (int ix = 0; ix < cpt->width; ++ix) + { + int i = iy * cpt->width + ix; + int is = iy * flow_sep[0]->stride + ix; + for (int j = 0; j < noparam; ++j) + flowout[i*noparam + j] = flow_sep[j]->c1[is]; + } + + // free FV structs + for (int i = 0; i < noparam; ++i ) + image_delete(flow_sep[i]); + + convolution_delete(deriv); + convolution_delete(deriv_flow); + + + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) + image_delete(im_ao); + image_delete(im_bo); + #else + color_image_delete(im_ao); + color_image_delete(im_bo); + #endif +} + + +#if (SELECTCHANNEL==1 | SELECTCHANNEL==2) +void VarRefClass::copyimage(const float* img, image_t * img_t) +#else +void VarRefClass::copyimage(const float* img, color_image_t * img_t) +#endif +{ + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) + const float * img_st = img + (cpt->tmp_w + 1 ) * (cpt->imgpadding); // remove image padding, start at first valid pixel + #else + const float * img_st = img + 3 * (cpt->tmp_w + 1 ) * (cpt->imgpadding); + #endif + + for (int yi = 0; yi < cpt->height; ++yi) + { + for (int xi = 0; xi < cpt->width; ++xi, ++img_st) + { + int i = yi*img_t->stride+ xi; + + img_t->c1[i] = (*img_st); + #if (SELECTCHANNEL==3) + ++img_st; img_t->c2[i] = (*img_st); + ++img_st; img_t->c3[i] = (*img_st); + #endif + } + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) + img_st += 2 * cpt->imgpadding; + #else + img_st += 3 * 2 * cpt->imgpadding; + #endif + } +} + + +#if (SELECTCHANNEL==1 | SELECTCHANNEL==2) +void VarRefClass::RefLevelOF(image_t *wx, image_t *wy, const image_t *im1, const image_t *im2) +#else +void VarRefClass::RefLevelOF(image_t *wx, image_t *wy, const color_image_t *im1, const color_image_t *im2) +#endif +{ + int i_inner_iteration; + int width = wx->width; + int height = wx->height; + int stride = wx->stride; + + + image_t *du = image_new(width,height), *dv = image_new(width,height), // the flow increment + *mask = image_new(width,height), // mask containing 0 if a point goes outside image boundary, 1 otherwise + *smooth_horiz = image_new(width,height), *smooth_vert = image_new(width,height), // horiz: (i,j) contains the diffusivity coeff. from (i,j) to (i+1,j) + *uu = image_new(width,height), *vv = image_new(width,height), // flow plus flow increment + *a11 = image_new(width,height), *a12 = image_new(width,height), *a22 = image_new(width,height), // system matrix A of Ax=b for each pixel + *b1 = image_new(width,height), *b2 = image_new(width,height); // system matrix b of Ax=b for each pixel + + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // use single band image + image_t *w_im2 = image_new(width,height), // warped second image + *Ix = image_new(width,height), *Iy = image_new(width,height), *Iz = image_new(width,height), // first order derivatives + *Ixx = image_new(width,height), *Ixy = image_new(width,height), *Iyy = image_new(width,height), *Ixz = image_new(width,height), *Iyz = image_new(width,height); // second order derivatives + #else // use RGB image + color_image_t *w_im2 = color_image_new(width,height), // warped second image + *Ix = color_image_new(width,height), *Iy = color_image_new(width,height), *Iz = color_image_new(width,height), // first order derivatives + *Ixx = color_image_new(width,height), *Ixy = color_image_new(width,height), *Iyy = color_image_new(width,height), *Ixz = color_image_new(width,height), *Iyz = color_image_new(width,height); // second order derivatives + #endif + + // warp second image + image_warp(w_im2, mask, im2, wx, wy); + // compute derivatives + get_derivatives(im1, w_im2, deriv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz); + // erase du and dv + image_erase(du); + image_erase(dv); + // initialize uu and vv + memcpy(uu->c1,wx->c1,wx->stride*wx->height*sizeof(float)); + memcpy(vv->c1,wy->c1,wy->stride*wy->height*sizeof(float)); + // inner fixed point iterations + for(i_inner_iteration = 0 ; i_inner_iteration < tvparams.n_inner_iteration ; i_inner_iteration++) + { + // compute robust function and system + compute_smoothness(smooth_horiz, smooth_vert, uu, vv, deriv_flow, tvparams.tmp_quarter_alpha ); + //compute_data_and_match(a11, a12, a22, b1, b2, mask, wx, wy, du, dv, uu, vv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz, desc_weight, desc_flow_x, desc_flow_y, tvparams.tmp_half_delta_over3, tvparams.tmp_half_beta, tvparams.tmp_half_gamma_over3); + compute_data(a11, a12, a22, b1, b2, mask, wx, wy, du, dv, uu, vv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz, tvparams.tmp_half_delta_over3, tvparams.tmp_half_beta, tvparams.tmp_half_gamma_over3); + sub_laplacian(b1, wx, smooth_horiz, smooth_vert); + sub_laplacian(b2, wy, smooth_horiz, smooth_vert); + + // solve system + #ifdef WITH_OPENMP + sor_coupled_slow_but_readable(du, dv, a11, a12, a22, b1, b2, smooth_horiz, smooth_vert, tvparams.n_solver_iteration, tvparams.sor_omega); // slower but parallelized + #else + sor_coupled(du, dv, a11, a12, a22, b1, b2, smooth_horiz, smooth_vert, tvparams.n_solver_iteration, tvparams.sor_omega); + #endif + + // update flow plus flow increment + int i; + v4sf *uup = (v4sf*) uu->c1, *vvp = (v4sf*) vv->c1, *wxp = (v4sf*) wx->c1, *wyp = (v4sf*) wy->c1, *dup = (v4sf*) du->c1, *dvp = (v4sf*) dv->c1; + for( i=0 ; ic1,uu->c1,uu->stride*uu->height*sizeof(float)); + memcpy(wy->c1,vv->c1,vv->stride*vv->height*sizeof(float)); + + // free memory + image_delete(du); image_delete(dv); + image_delete(mask); + image_delete(smooth_horiz); image_delete(smooth_vert); + image_delete(uu); image_delete(vv); + image_delete(a11); image_delete(a12); image_delete(a22); + image_delete(b1); image_delete(b2); + + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // use single band image + image_delete(w_im2); + image_delete(Ix); image_delete(Iy); image_delete(Iz); + image_delete(Ixx); image_delete(Ixy); image_delete(Iyy); image_delete(Ixz); image_delete(Iyz); + #else + color_image_delete(w_im2); + color_image_delete(Ix); color_image_delete(Iy); color_image_delete(Iz); + color_image_delete(Ixx); color_image_delete(Ixy); color_image_delete(Iyy); color_image_delete(Ixz); color_image_delete(Iyz); + #endif + +} + + +#if (SELECTCHANNEL==1 | SELECTCHANNEL==2) +void VarRefClass::RefLevelDE(image_t *wx, const image_t *im1, const image_t *im2) +#else +void VarRefClass::RefLevelDE(image_t *wx, const color_image_t *im1, const color_image_t *im2) +#endif +{ + int i_inner_iteration; + int width = wx->width; + int height = wx->height; + int stride = wx->stride; + + image_t *du = image_new(width,height), *wy_dummy = image_new(width,height), // the flow increment + *mask = image_new(width,height), // mask containing 0 if a point goes outside image boundary, 1 otherwise + *smooth_horiz = image_new(width,height), *smooth_vert = image_new(width,height), // horiz: (i,j) contains the diffusivity coeff. from (i,j) to (i+1,j) + *uu = image_new(width,height), // flow plus flow increment + *a11 = image_new(width,height), // system matrix A of Ax=b for each pixel + *b1 = image_new(width,height); // system matrix b of Ax=b for each pixel + + image_erase(wy_dummy); + + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // use single band image + image_t *w_im2 = image_new(width,height), // warped second image + *Ix = image_new(width,height), *Iy = image_new(width,height), *Iz = image_new(width,height), // first order derivatives + *Ixx = image_new(width,height), *Ixy = image_new(width,height), *Iyy = image_new(width,height), *Ixz = image_new(width,height), *Iyz = image_new(width,height); // second order derivatives + #else // use RGB image + color_image_t *w_im2 = color_image_new(width,height), // warped second image + *Ix = color_image_new(width,height), *Iy = color_image_new(width,height), *Iz = color_image_new(width,height), // first order derivatives + *Ixx = color_image_new(width,height), *Ixy = color_image_new(width,height), *Iyy = color_image_new(width,height), *Ixz = color_image_new(width,height), *Iyz = color_image_new(width,height); // second order derivatives + #endif + + // warp second image + image_warp(w_im2, mask, im2, wx, wy_dummy); + // compute derivatives + get_derivatives(im1, w_im2, deriv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz); + // erase du and dv + image_erase(du); + + // initialize uu and vv + memcpy(uu->c1,wx->c1,wx->stride*wx->height*sizeof(float)); + + // inner fixed point iterations + for(i_inner_iteration = 0 ; i_inner_iteration < tvparams.n_inner_iteration ; i_inner_iteration++) + { + // compute robust function and system + compute_smoothness(smooth_horiz, smooth_vert, uu, wy_dummy, deriv_flow, tvparams.tmp_quarter_alpha ); + compute_data_DE(a11, b1, mask, wx, du, uu, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz, tvparams.tmp_half_delta_over3, tvparams.tmp_half_beta, tvparams.tmp_half_gamma_over3); + sub_laplacian(b1, wx, smooth_horiz, smooth_vert); + + // solve system + sor_coupled_slow_but_readable_DE(du, a11, b1, smooth_horiz, smooth_vert, tvparams.n_solver_iteration, tvparams.sor_omega); + + // update flow plus flow increment + int i; + v4sf *uup = (v4sf*) uu->c1, *wxp = (v4sf*) wx->c1, *dup = (v4sf*) du->c1; + + if(cpt->camlr==0) // check if right or left camera, needed to truncate values above/below zero + { + for( i=0 ; izero); + uup+=1; wxp+=1; dup+=1; + } + } + else + { + for( i=0 ; izero); + uup+=1; wxp+=1; dup+=1; + } + } + } + // add flow increment to current flow + memcpy(wx->c1,uu->c1,uu->stride*uu->height*sizeof(float)); + + // free memory + image_delete(du); image_delete(wy_dummy); + image_delete(mask); + image_delete(smooth_horiz); image_delete(smooth_vert); + image_delete(uu); + image_delete(a11); + image_delete(b1); + + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) + image_delete(w_im2); + image_delete(Ix); image_delete(Iy); image_delete(Iz); + image_delete(Ixx); image_delete(Ixy); image_delete(Iyy); image_delete(Ixz); image_delete(Iyz); + #else + color_image_delete(w_im2); + color_image_delete(Ix); color_image_delete(Iy); color_image_delete(Iz); + color_image_delete(Ixx); color_image_delete(Ixy); color_image_delete(Iyy); color_image_delete(Ixz); color_image_delete(Iyz); + #endif +} + + +VarRefClass::~VarRefClass() +{ + +} + +} \ No newline at end of file diff --git a/src/refine_variational.h b/src/refine_variational.h new file mode 100644 index 0000000..b988a40 --- /dev/null +++ b/src/refine_variational.h @@ -0,0 +1,69 @@ +#ifndef VARREF_HEADER +#define VARREF_HEADER + +#include "FDF1.0.1/image.h" +#include "FDF1.0.1/opticalflow_aux.h" +#include "FDF1.0.1/solver.h" + +#include "oflow.h" + +namespace OFC +{ + +typedef __v4sf v4sf; + +typedef struct +{ + float alpha; // smoothness weight + float beta; // matching weight + float gamma; // gradient constancy assumption weight + float delta; // color constancy assumption weight + int n_inner_iteration; // number of inner fixed point iterations + int n_solver_iteration; // number of solver iterations + float sor_omega; // omega parameter of sor method + + float tmp_quarter_alpha; + float tmp_half_gamma_over3; + float tmp_half_delta_over3; + float tmp_half_beta; + +} TVparams; + + +class VarRefClass +{ + +public: + VarRefClass(const float * im_ao_in, const float * im_ao_dx_in, const float * im_ao_dy_in, // expects #sc_f_in pointers to float arrays for images and gradients. + const float * im_bo_in, const float * im_bo_dx_in, const float * im_bo_dy_in, + const camparam* cpt_in, const camparam* cpo_in,const optparam* op_in, float *flowout); + ~VarRefClass(); + +private: + + convolution_t *deriv, *deriv_flow; + + + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // Intensity image, or gradient image + void copyimage(const float* img, image_t * img_t); + void RefLevelOF(image_t *wx, image_t *wy, const image_t *im1, const image_t *im2); + void RefLevelDE(image_t *wx, const image_t *im1, const image_t *im2); + #else // 3-Color RGB image + void copyimage(const float* img, color_image_t * img_t); + void RefLevelOF(image_t *wx, image_t *wy, const color_image_t *im1, const color_image_t *im2); + void RefLevelDE(image_t *wx, const color_image_t *im1, const color_image_t *im2); + #endif + + TVparams tvparams; + + const camparam* cpt; + const camparam* cpo; + const optparam* op; + +}; + +} + +#endif /* VARREF_HEADER */ + + diff --git a/src/run_dense.cpp b/src/run_dense.cpp new file mode 100644 index 0000000..83917c6 --- /dev/null +++ b/src/run_dense.cpp @@ -0,0 +1,436 @@ + +#include +#include +#include + +#include +#include +#include + +#include "oflow.h" + + +using namespace std; + +// Save a Depth/OF/SF as .flo file +void SaveFlowFile(cv::Mat& img, const char* filename) +{ + cv::Size szt = img.size(); + int width = szt.width, height = szt.height; + int nc = img.channels(); + float tmp[nc]; + + FILE *stream = fopen(filename, "wb"); + if (stream == 0) + cout << "WriteFile: could not open file" << endl; + + // write the header + fprintf(stream, "PIEH"); + if ((int)fwrite(&width, sizeof(int), 1, stream) != 1 || + (int)fwrite(&height, sizeof(int), 1, stream) != 1) + cout << "WriteFile: problem writing header" << endl; + + for (int y = 0; y < height; y++) + { + for (int x = 0; x < width; x++) + { + if (nc==1) // depth + tmp[0] = img.at(y,x); + else if (nc==2) // Optical Flow + { + tmp[0] = img.at(y,x)[0]; + tmp[1] = img.at(y,x)[1]; + } + else if (nc==4) // Scene Flow + { + tmp[0] = img.at(y,x)[0]; + tmp[1] = img.at(y,x)[1]; + tmp[2] = img.at(y,x)[2]; + tmp[3] = img.at(y,x)[3]; + } + + if ((int)fwrite(tmp, sizeof(float), nc, stream) != nc) + cout << "WriteFile: problem writing data" << endl; + } + } + fclose(stream); +} + +// Save a depth as .pfm file +void SavePFMFile(cv::Mat& img, const char* filename) +{ + cv::Size szt = img.size(); + + FILE *stream = fopen(filename, "wb"); + if (stream == 0) + cout << "WriteFile: could not open file" << endl; + + // write the header + fprintf(stream, "Pf\n%d %d\n%f\n", szt.width, szt.height, (float)-1.0f); + + for (int y = szt.height-1; y >= 0 ; --y) + { + for (int x = 0; x < szt.width; ++x) + { + float tmp = -img.at(y,x); + if ((int)fwrite(&tmp, sizeof(float), 1, stream) != 1) + cout << "WriteFile: problem writing data" << endl; + } + } + fclose(stream); +} + +// Read a depth/OF/SF as file +void ReadFlowFile(cv::Mat& img, const char* filename) +{ + FILE *stream = fopen(filename, "rb"); + if (stream == 0) + cout << "ReadFile: could not open %s" << endl; + + int width, height; + float tag; + int nc = img.channels(); + float tmp[nc]; + + if ((int)fread(&tag, sizeof(float), 1, stream) != 1 || + (int)fread(&width, sizeof(int), 1, stream) != 1 || + (int)fread(&height, sizeof(int), 1, stream) != 1) + cout << "ReadFile: problem reading file %s" << endl; + + for (int y = 0; y < height; y++) + { + for (int x = 0; x < width; x++) + { + if ((int)fread(tmp, sizeof(float), nc, stream) != nc) + cout << "ReadFile(%s): file is too short" << endl; + + if (nc==1) // depth + img.at(y,x) = tmp[0]; + else if (nc==2) // Optical Flow + { + img.at(y,x)[0] = tmp[0]; + img.at(y,x)[1] = tmp[1]; + } + else if (nc==4) // Scene Flow + { + img.at(y,x)[0] = tmp[0]; + img.at(y,x)[1] = tmp[1]; + img.at(y,x)[2] = tmp[2]; + img.at(y,x)[3] = tmp[3]; + } + } + } + + if (fgetc(stream) != EOF) + cout << "ReadFile(%s): file is too long" << endl; + + fclose(stream); +} + +void ConstructImgPyramide(const cv::Mat & img_ao_fmat, cv::Mat * img_ao_fmat_pyr, cv::Mat * img_ao_dx_fmat_pyr, cv::Mat * img_ao_dy_fmat_pyr, const float ** img_ao_pyr, const float ** img_ao_dx_pyr, const float ** img_ao_dy_pyr, const int lv_f, const int lv_l, const int rpyrtype, const bool getgrad, const int imgpadding, const int padw, const int padh) +{ + for (int i=0; i<=lv_f; ++i) // Construct image and gradient pyramides + { + if (i==0) // At finest scale: copy directly, for all other: downscale previous scale by .5 + { + #if (SELECTCHANNEL==1 | SELECTCHANNEL==3) // use RGB or intensity image directly + img_ao_fmat_pyr[i] = img_ao_fmat.clone(); + #elif (SELECTCHANNEL==2) // use gradient magnitude image as input + cv::Mat dx,dy,dx2,dy2,dmag; + cv::Sobel( img_ao_fmat, dx, CV_32F, 1, 0, 1, 1, 0, cv::BORDER_DEFAULT ); + cv::Sobel( img_ao_fmat, dy, CV_32F, 0, 1, 1, 1, 0, cv::BORDER_DEFAULT ); + dx2 = dx.mul(dx); + dy2 = dy.mul(dy); + dmag = dx2+dy2; + cv::sqrt(dmag,dmag); + img_ao_fmat_pyr[i] = dmag.clone(); + #endif + } + else + cv::resize(img_ao_fmat_pyr[i-1], img_ao_fmat_pyr[i], cv::Size(), .5, .5, cv::INTER_LINEAR); + + img_ao_fmat_pyr[i].convertTo(img_ao_fmat_pyr[i], rpyrtype); + + if ( getgrad ) + { + cv::Sobel( img_ao_fmat_pyr[i], img_ao_dx_fmat_pyr[i], CV_32F, 1, 0, 1, 1, 0, cv::BORDER_DEFAULT ); + cv::Sobel( img_ao_fmat_pyr[i], img_ao_dy_fmat_pyr[i], CV_32F, 0, 1, 1, 1, 0, cv::BORDER_DEFAULT ); + img_ao_dx_fmat_pyr[i].convertTo(img_ao_dx_fmat_pyr[i], CV_32F); + img_ao_dy_fmat_pyr[i].convertTo(img_ao_dy_fmat_pyr[i], CV_32F); + } + } + + // pad images + for (int i=0; i<=lv_f; ++i) // Construct image and gradient pyramides + { + copyMakeBorder(img_ao_fmat_pyr[i],img_ao_fmat_pyr[i],imgpadding,imgpadding,imgpadding,imgpadding,cv::BORDER_REPLICATE); // Replicate border for image padding + img_ao_pyr[i] = (float*)img_ao_fmat_pyr[i].data; + + if ( getgrad ) + { + copyMakeBorder(img_ao_dx_fmat_pyr[i],img_ao_dx_fmat_pyr[i],imgpadding,imgpadding,imgpadding,imgpadding,cv::BORDER_CONSTANT , 0); // Zero padding for gradients + copyMakeBorder(img_ao_dy_fmat_pyr[i],img_ao_dy_fmat_pyr[i],imgpadding,imgpadding,imgpadding,imgpadding,cv::BORDER_CONSTANT , 0); + + img_ao_dx_pyr[i] = (float*)img_ao_dx_fmat_pyr[i].data; + img_ao_dy_pyr[i] = (float*)img_ao_dy_fmat_pyr[i].data; + } + } +} + +int AutoFirstScaleSelect(int imgwidth, int fratio, int patchsize) +{ + return std::max(0,(int)std::floor(log2((2.0f*(float)imgwidth) / ((float)fratio * (float)patchsize)))); +} + +int main( int argc, char** argv ) +{ + struct timeval tv_start_all, tv_end_all; + gettimeofday(&tv_start_all, NULL); + + + + // *** Parse and load input images + char *imgfile_ao = argv[1]; + char *imgfile_bo = argv[2]; + char *outfile = argv[3]; + + cv::Mat img_ao_mat, img_bo_mat, img_tmp; + int rpyrtype, nochannels, incoltype; + #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // use Intensity or Gradient image + incoltype = CV_LOAD_IMAGE_GRAYSCALE; + rpyrtype = CV_32FC1; + nochannels = 1; + #elif (SELECTCHANNEL==3) // use RGB image + incoltype = CV_LOAD_IMAGE_COLOR; + rpyrtype = CV_32FC3; + nochannels = 3; + #endif + img_ao_mat = cv::imread(imgfile_ao, incoltype); // Read the file + img_bo_mat = cv::imread(imgfile_bo, incoltype); // Read the file + cv::Mat img_ao_fmat, img_bo_fmat; + cv::Size sz = img_ao_mat.size(); + int width_org = sz.width; // unpadded original image size + int height_org = sz.height; // unpadded original image size + + + + + // *** Parse rest of parameters, See oflow.h for definitions. + int lv_f, lv_l, maxiter, miniter, patchsz, patnorm, costfct, tv_innerit, tv_solverit, verbosity; + float mindprate, mindrrate, minimgerr, poverl, tv_alpha, tv_gamma, tv_delta, tv_sor; + bool usefbcon, usetvref; + //bool hasinfile; // initialization flow file + //char *infile = nullptr; + + if (argc<=5) // Use operation point X, set scales automatically + { + mindprate = 0.05; mindrrate = 0.95; minimgerr = 0.0; + usefbcon = 0; patnorm = 1; costfct = 0; + tv_alpha = 10.0; tv_gamma = 10.0; tv_delta = 5.0; + tv_innerit = 1; tv_solverit = 3; tv_sor = 1.6; + verbosity = 2; // Default: Plot detailed timings + + int fratio = 5; // For automatic selection of coarsest scale: 1/fratio * width = maximum expected motion magnitude in image. Set lower to restrict search space. + + int sel_oppoint = 2; // Default operating point + if (argc==5) // Use provided operating point + sel_oppoint=atoi(argv[4]); + + switch (sel_oppoint) + { + case 1: + patchsz = 8; poverl = 0.3; + lv_f = AutoFirstScaleSelect(width_org, fratio, patchsz); + lv_l = std::max(lv_f-2,0); maxiter = 16; miniter = 16; + usetvref = 0; + break; + case 3: + patchsz = 12; poverl = 0.75; + lv_f = AutoFirstScaleSelect(width_org, fratio, patchsz); + lv_l = std::max(lv_f-4,0); maxiter = 16; miniter = 16; + usetvref = 1; + break; + case 4: + patchsz = 12; poverl = 0.75; + lv_f = AutoFirstScaleSelect(width_org, fratio, patchsz); + lv_l = std::max(lv_f-5,0); maxiter = 128; miniter = 128; + usetvref = 1; + break; + case 2: + default: + patchsz = 8; poverl = 0.4; + lv_f = AutoFirstScaleSelect(width_org, fratio, patchsz); + lv_l = std::max(lv_f-2,0); maxiter = 12; miniter = 12; + usetvref = 1; + break; + + } + } + else // Parse explicitly provided parameters + { + int acnt = 4; // Argument counter + lv_f = atoi(argv[acnt++]); + lv_l = atoi(argv[acnt++]); + maxiter = atoi(argv[acnt++]); + miniter = atoi(argv[acnt++]); + mindprate = atof(argv[acnt++]); + mindrrate = atof(argv[acnt++]); + minimgerr = atof(argv[acnt++]); + patchsz = atoi(argv[acnt++]); + poverl = atof(argv[acnt++]); + usefbcon = atoi(argv[acnt++]); + patnorm = atoi(argv[acnt++]); + costfct = atoi(argv[acnt++]); + usetvref = atoi(argv[acnt++]); + tv_alpha = atof(argv[acnt++]); + tv_gamma = atof(argv[acnt++]); + tv_delta = atof(argv[acnt++]); + tv_innerit = atoi(argv[acnt++]); + tv_solverit = atoi(argv[acnt++]); + tv_sor = atof(argv[acnt++]); + verbosity = atoi(argv[acnt++]); + //hasinfile = (bool)atoi(argv[acnt++]); // initialization flow file + //if (hasinfile) infile = argv[acnt++]; + } + + + + // *** Pad image such that width and height are restless divisible on all scales (except last) + int padw=0, padh=0; + int scfct = pow(2,lv_f); // enforce restless division by this number on coarsest scale + //if (hasinfile) scfct = pow(2,lv_f+1); // if initialization file is given, make sure that size is restless divisible by 2^(lv_f+1) ! + int div = sz.width % scfct; + if (div>0) padw = scfct - div; + div = sz.height % scfct; + if (div>0) padh = scfct - div; + if (padh>0 || padw>0) + { + copyMakeBorder(img_ao_mat,img_ao_mat,floor((float)padh/2.0f),ceil((float)padh/2.0f),floor((float)padw/2.0f),ceil((float)padw/2.0f),cv::BORDER_REPLICATE); + copyMakeBorder(img_bo_mat,img_bo_mat,floor((float)padh/2.0f),ceil((float)padh/2.0f),floor((float)padw/2.0f),ceil((float)padw/2.0f),cv::BORDER_REPLICATE); + } + sz = img_ao_mat.size(); // padded image size, ensures divisibility by 2 on all scales (except last) + + // Timing, image loading + if (verbosity > 1) + { + gettimeofday(&tv_end_all, NULL); + double tt = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f; + printf("TIME (Image loading ) (ms): %3g\n", tt); + gettimeofday(&tv_start_all, NULL); + } + + + + + // *** Generate scale pyramides + img_ao_mat.convertTo(img_ao_fmat, CV_32F); // convert to float + img_bo_mat.convertTo(img_bo_fmat, CV_32F); + + const float* img_ao_pyr[lv_f+1]; + const float* img_bo_pyr[lv_f+1]; + const float* img_ao_dx_pyr[lv_f+1]; + const float* img_ao_dy_pyr[lv_f+1]; + const float* img_bo_dx_pyr[lv_f+1]; + const float* img_bo_dy_pyr[lv_f+1]; + + cv::Mat img_ao_fmat_pyr[lv_f+1]; + cv::Mat img_bo_fmat_pyr[lv_f+1]; + cv::Mat img_ao_dx_fmat_pyr[lv_f+1]; + cv::Mat img_ao_dy_fmat_pyr[lv_f+1]; + cv::Mat img_bo_dx_fmat_pyr[lv_f+1]; + cv::Mat img_bo_dy_fmat_pyr[lv_f+1]; + + ConstructImgPyramide(img_ao_fmat, img_ao_fmat_pyr, img_ao_dx_fmat_pyr, img_ao_dy_fmat_pyr, img_ao_pyr, img_ao_dx_pyr, img_ao_dy_pyr, lv_f, lv_l, rpyrtype, 1, patchsz, padw, padh); + ConstructImgPyramide(img_bo_fmat, img_bo_fmat_pyr, img_bo_dx_fmat_pyr, img_bo_dy_fmat_pyr, img_bo_pyr, img_bo_dx_pyr, img_bo_dy_pyr, lv_f, lv_l, rpyrtype, 1, patchsz, padw, padh); + + // Timing, image gradients and pyramid + if (verbosity > 1) + { + gettimeofday(&tv_end_all, NULL); + double tt = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f; + printf("TIME (Pyramide+Gradients) (ms): %3g\n", tt); + } + + +// // Read Initial Truth flow (if available) +// float * initptr = nullptr; +// cv::Mat flowinit; +// if (hasinfile) +// { +// #if (SELECTMODE==1) +// flowinit.create(height_org, width_org, CV_32FC2); +// #else +// flowinit.create(height_org, width_org, CV_32FC1); +// #endif +// +// ReadFlowFile(flowinit, infile); +// +// // padding to ensure divisibility by 2 +// if (padh>0 || padw>0) +// copyMakeBorder(flowinit,flowinit,floor((float)padh/2.0f),ceil((float)padh/2.0f),floor((float)padw/2.0f),ceil((float)padw/2.0f),cv::BORDER_REPLICATE); +// +// // resizing to coarsest scale - 1, since the array is upsampled at .5 in the code +// float sc_fct = pow(2,-lv_f-1); +// flowinit *= sc_fct; +// cv::resize(flowinit, flowinit, cv::Size(), sc_fct, sc_fct , cv::INTER_AREA); +// +// initptr = (float*)flowinit.data; +// } + + + + + // *** Run main optical flow / depth algorithm + float sc_fct = pow(2,lv_l); + #if (SELECTMODE==1) + cv::Mat flowout(sz.height / sc_fct , sz.width / sc_fct, CV_32FC2); // Optical Flow + #else + cv::Mat flowout(sz.height / sc_fct , sz.width / sc_fct, CV_32FC1); // Depth + #endif + + OFC::OFClass ofc(img_ao_pyr, img_ao_dx_pyr, img_ao_dy_pyr, + img_bo_pyr, img_bo_dx_pyr, img_bo_dy_pyr, + patchsz, // extra image padding to avoid border violation check + (float*)flowout.data, // pointer to n-band output float array + nullptr, // pointer to n-band input float array of size of first (coarsest) scale, pass as nullptr to disable + sz.width, sz.height, + lv_f, lv_l, maxiter, miniter, mindprate, mindrrate, minimgerr, patchsz, poverl, + usefbcon, costfct, nochannels, patnorm, + usetvref, tv_alpha, tv_gamma, tv_delta, tv_innerit, tv_solverit, tv_sor, + verbosity); + + if (verbosity > 1) gettimeofday(&tv_start_all, NULL); + + + + // *** Resize to original scale, if not run to finest level + if (lv_l != 0) + { + flowout *= sc_fct; + cv::resize(flowout, flowout, cv::Size(), sc_fct, sc_fct , cv::INTER_LINEAR); + } + + // If image was padded, remove padding before saving to file + flowout = flowout(cv::Rect((int)floor((float)padw/2.0f),(int)floor((float)padh/2.0f),width_org,height_org)); + + // Save Result Image + #if (SELECTMODE==1) + SaveFlowFile(flowout, outfile); + #else + SavePFMFile(flowout, outfile); + #endif + + if (verbosity > 1) + { + gettimeofday(&tv_end_all, NULL); + double tt = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f; + printf("TIME (Saving flow file ) (ms): %3g\n", tt); + } + + return 0; +} + + + + +