/** * * Horn & Schunck method for optical flow estimation at a single scale * */ void horn_schunck_optical_flow( const float *I1, // source image const float *I2, // target image float *u, // x component of optical flow float *v, // y component of optical flow const int nx, // image width const int ny, // image height const float alpha, // smoothing parameter const int warps, // number of warpings per scale const float TOL, // stopping criterion threshold const int maxiter, // maximum number of iterations const bool verbose // switch on messages ) { if (verbose) fprintf(stderr, "Single-scale Horn-Schunck of a %dx%d " "image\n\ta=%g nw=%d eps=%g mi=%d v=%d\n", nx, ny, alpha, warps, TOL, maxiter, verbose); const int size = nx * ny; const float alpha2 = alpha * alpha; //allocate memory int sf = sizeof(float); float *I2x = xmalloc(size * sf); // x derivative of I2 float *I2y = xmalloc(size * sf); // y derivative of I2 float *I2w = xmalloc(size * sf); // warping of I2 float *I2wx = xmalloc(size * sf); // warping of I2x float *I2wy = xmalloc(size * sf); // warping of I2y float *Au = xmalloc(size * sf); // constant part of numerator of u float *Av = xmalloc(size * sf); // constant part of numerator of v float *Du = xmalloc(size * sf); // denominator of u float *Dv = xmalloc(size * sf); // denominator of v float *D = xmalloc(size * sf); // common numerator of u and v // compute the gradient of the second image gradient(I2, I2x, I2y, nx, ny); // iterative approximation to the Taylor expansions for(int n = 0; n < warps; n++) { if(verbose) fprintf(stderr, "Warping %d:", n); // warp the second image and its derivatives bicubic_interpolation_warp(I2, u, v, I2w, nx, ny, true); bicubic_interpolation_warp(I2x, u, v, I2wx, nx, ny, true); bicubic_interpolation_warp(I2y, u, v, I2wy, nx, ny, true); // store the constant parts of the system for(int i = 0; i < size; i++) { const float I2wl = I2wx[i] * u[i] + I2wy[i] * v[i]; const float dif = I1[i] - I2w[i] + I2wl; Au[i] = dif * I2wx[i]; Av[i] = dif * I2wy[i]; Du[i] = I2wx[i] * I2wx[i] + alpha2; Dv[i] = I2wy[i] * I2wy[i] + alpha2; D[i] = I2wx[i] * I2wy[i]; } int niter = 0; float error = 1000; // iterations of the SOR numerical scheme while(error > TOL && niter < maxiter) { niter++; error = 0; //process the central part of the optical flow #pragma omp parallel for reduction(+:error) for(int i = 1; i < ny-1; i++) for(int j = 1; j < nx-1; j++) { const int k = i * nx + j; error += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-nx-1, k-nx+1, k+nx-1, k+nx+1, k-nx, k-1, k+nx, k+1 ); } // process the first and last rows for(int j = 1; j < nx-1; j++) { // first row int k = j; error += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-1, k+1, k+nx-1, k+nx+1, k, k-1, k+nx, k+1 ); // last row k = (ny-1) * nx + j; error += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-nx-1, k-nx+1, k-1, k+1, k-nx, k-1, k, k+1 ); } // process the first and last columns for(int i = 1; i < ny-1; i++) { // first column int k = i * nx; error += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-nx, k-nx+1, k+nx, k+nx+1, k-nx, k, k+nx, k+1 ); // last column k = (i+1) * nx - 1; error += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-nx-1, k-nx, k+nx-1, k+nx, k-nx, k-1, k+nx, k ); } // process the corners // up-left corner error += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, 0, 0, 1, nx, nx+1, 0, 0, nx, 1 ); // up-right corner int k = nx - 1; error += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-1, k, k+nx-1, k+nx, k, k-1, k+nx, k ); // bottom-left corner k = (ny-1) * nx; error += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-nx, k-nx+1,k, k+1, k-nx, k, k, k+1 ); // bottom-right corner k = ny * nx - 1; error += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-1, k, k-nx-1, k-nx, k-nx, k-1, k, k ); error = sqrt(error / size); } if(verbose) fprintf(stderr, "Iterations %d (%g)\n", niter, error); } // free the allocated memory free(I2x); free(I2y); free(I2w); free(I2wx); free(I2wy); free(Au); free(Av); free(Du); free(Dv); free(D); }
/** * * Horn & Schunck method for optical flow estimation at a single scale * */ static void horn_schunck_optical_flow( const float *I1, // source image const float *I2, // target image float *u, // x component of optical flow float *v, // y component of optical flow const int nx, // image width const int ny, // image height const float alpha, // smoothing parameter const int nwarps, // number of warpings per scale const float epsilon, // stopping criterion threshold const int nmaxiter, // maximum number of iterations const bool verbose // switch on messages ) { if (verbose) fprintf(stderr, "Single-scale Horn-Schunck of a %dx%d " "image\n\ta=%g nw=%d eps=%g mi=%d v=%d\n", nx, ny, alpha, nwarps, epsilon, nmaxiter, verbose); const int npixels = nx * ny; const float alpha2 = alpha * alpha; //allocate memory int sf = sizeof(float); float *I2x = xmalloc(npixels * sf); // x derivative of I2 float *I2y = xmalloc(npixels * sf); // y derivative of I2 float *I2w = xmalloc(npixels * sf); // warping of I2 float *I2wx = xmalloc(npixels * sf); // warping of I2x float *I2wy = xmalloc(npixels * sf); // warping of I2y float *Au = xmalloc(npixels * sf); // constant part of numerator of u float *Av = xmalloc(npixels * sf); // constant part of numerator of v float *Du = xmalloc(npixels * sf); // denominator of u float *Dv = xmalloc(npixels * sf); // denominator of v float *D = xmalloc(npixels * sf); // common numerator of u and v // compute the gradient of the second image compute_gradient_using_centered_differences(I2, I2x, I2y, nx, ny); // iterative approximation to the Taylor expansions for (int n = 0; n < nwarps; n++) { if(verbose) fprintf(stderr, "Warping %d:", n); // warp the second image and its derivatives bicubic_interpolation_warp(I2, u, v, I2w, nx, ny, true); bicubic_interpolation_warp(I2x, u, v, I2wx, nx, ny, true); bicubic_interpolation_warp(I2y, u, v, I2wy, nx, ny, true); // store the constant parts of the system // (including the starting values of (u,v) at this warp, // denoted by u^n and v^n in the pseudocode of the article) for(int i = 0; i < npixels; i++) { const float I2wl = I2wx[i] * u[i] + I2wy[i] * v[i]; const float dif = I1[i] - I2w[i] + I2wl; Au[i] = dif * I2wx[i]; Av[i] = dif * I2wy[i]; Du[i] = I2wx[i] * I2wx[i] + alpha2; Dv[i] = I2wy[i] * I2wy[i] + alpha2; D[i] = I2wx[i] * I2wy[i]; } // counter for the loop below (named "r" on article pseudocode) int niter = 0; // this variable starts with a dummy value to enter the loop float stopping_criterion = epsilon + 1; // iterations of the SOR numerical scheme while (stopping_criterion > epsilon && niter < nmaxiter) { niter++; stopping_criterion = 0; //process the central part of the optical flow #ifdef _OPENMP #pragma omp parallel for reduction(+:stopping_criterion) #endif for(int i = 1; i < ny-1; i++) for(int j = 1; j < nx-1; j++) { const int k = i * nx + j; stopping_criterion += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-nx-1, k-nx+1, k+nx-1, k+nx+1, k-nx, k-1, k+nx, k+1 ); } // process the first and last rows for(int j = 1; j < nx-1; j++) { // first row int k = j; stopping_criterion += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-1, k+1, k+nx-1, k+nx+1, k, k-1, k+nx, k+1 ); // last row k = (ny-1) * nx + j; stopping_criterion += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-nx-1, k-nx+1, k-1, k+1, k-nx, k-1, k, k+1 ); } // process the first and last columns for(int i = 1; i < ny-1; i++) { // first column int k = i * nx; stopping_criterion += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-nx, k-nx+1, k+nx, k+nx+1, k-nx, k, k+nx, k+1 ); // last column k = (i+1) * nx - 1; stopping_criterion += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-nx-1, k-nx, k+nx-1, k+nx, k-nx, k-1, k+nx, k ); } // process the corners // up-left corner stopping_criterion += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, 0, 0, 1, nx, nx+1, 0, 0, nx, 1 ); // up-right corner int k = nx - 1; stopping_criterion += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-1, k, k+nx-1, k+nx, k, k-1, k+nx, k ); // bottom-left corner k = (ny-1) * nx; stopping_criterion += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-nx, k-nx+1,k, k+1, k-nx, k, k, k+1 ); // bottom-right corner k = ny * nx - 1; stopping_criterion += sor_iteration( Au, Av, Du, Dv, D, u, v, alpha2, k, k-1, k, k-nx-1, k-nx, k-nx, k-1, k, k ); stopping_criterion = sqrt(stopping_criterion / npixels); } if(verbose) fprintf(stderr, "Iterations %d (%g)\n", niter, stopping_criterion); } // free the allocated memory free(I2x); free(I2y); free(I2w); free(I2wx); free(I2wy); free(Au); free(Av); free(Du); free(Dv); free(D); }
/** * calcMSCLG_OF * * Main Multiscale CLG-optical flow (CLG-OF) computation function. * * Parameters: * * image1 Pointer to the first image (the "previous" time frame). * image2 Pointer to the second image (the "current" time frame). * uOut Pointer to the horizontal component of the CLG-OF solution. * vOut Pointer to the vertical component of the CLG-OF solution. * nRows Number of image rows (same for the CLG-OF vector field). * nCols Number of image columns (same for the CLG-OF vector field). * iterations Number of iterations for iterative solution. * alpha Global smoothing coefficient of the CLG-OF. * rho Local spatio-temporal smoothing coefficient of the CLG-OF. * sigma Standard deviation for an optional gaussian smoothing kernel, * applied to the input images prior to the CLG-OF computation. * wFactor SOR relaxation factor, between 0 and 2. * nScales Total number of scales (at least 1). * scaleFactor Downsampling factor, between 0.5 and 1.0. * coupledMode Iteration type. 1->Pointwise-Coupled Gauss-Seidel, 0->SOR. * verbose Display/hide messages to the stdout. * */ int calcMSCLG_OF(double* image1, double* image2, double* uOut, double* vOut, int nRows, int nCols, int iterations, double alpha, double rho, double sigma, double wFactor, int nScales, const double scaleFactor, int coupledMode, int verbose) { if (verbose) { printf("calcMSCLG_OF\n"); printf(" parameters: nScales=%d, scaleFactor=%f\n", nScales, scaleFactor); } // Variables declaration. int s, i; double *image2warped; int size = nCols * nRows; double *I1s[nScales]; double *I2s[nScales]; double *us[nScales]; double *vs[nScales]; int nxx[nScales]; int nyy[nScales]; I1s[0] = image1; I2s[0] = image2; // Pre-smoothing the finest scale images. int filterSize = (int) 2*(DEFAULT_GAUSSIAN_WINDOW_SIZE * sigma) + 1; if (filterSize >= MIN_GAUSSIAN_SIZE) { printf(" apply gaussian smoothing for each frame\n"); gaussian(I1s[0], nCols, nRows, sigma); gaussian(I2s[0], nCols, nRows, sigma); } else { printf(" no gaussian smoothing was applied to input frames\n"); } us[0] = uOut; vs[0] = vOut; nxx[0] = nCols; nyy[0] = nRows; // Create the scales. for (s=1; s<nScales; s++) { if (verbose) printf(" scales %d init\n", s); zoom_size(nxx[s-1], nyy[s-1], nxx+s, nyy+s, scaleFactor); const int sizes = nxx[s] * nyy[s]; I1s[s] = (double *) xmalloc(sizes * sizeof(double)); I2s[s] = (double *) xmalloc(sizes * sizeof(double)); us[s] = (double *) xmalloc(sizes * sizeof(double)); vs[s] = (double *) xmalloc(sizes * sizeof(double)); // Compute the zoom from the previous finer scale. zoom_out(I1s[s-1], I1s[s], nxx[s-1], nyy[s-1], scaleFactor); zoom_out(I2s[s-1], I2s[s], nxx[s-1], nyy[s-1], scaleFactor); } // Initialize the OF arrays. for (i=0; i < nxx[nScales-1] * nyy[nScales-1]; i++) { us[nScales-1][i] = 0.0; vs[nScales-1][i] = 0.0; } // Pyramidal approximation to the OF. for (s=nScales-1; s>=0; s--) { if (verbose) printf("Scale: %d %dx%d, nScales=%d\n", s, nxx[s], nyy[s], nScales); image2warped = (double *) xmalloc(nxx[s] * nyy[s] * sizeof(double)); // Warp the second image before computing the derivatives. bicubic_interpolation_warp(I2s[s], us[s], vs[s], image2warped, nxx[s], nyy[s], false); // Compute the optical flow. calcCLG_OF(I1s[s], image2warped, us[s], vs[s], nyy[s], nxx[s], iterations, alpha, rho, wFactor, verbose, coupledMode); free(image2warped); // If this was the last scale, finish now. if (!s) break; // Otherwise, upsample the optical flow. // Zoom the OF for the next finer scale. zoom_in(us[s], us[s-1], nxx[s], nyy[s], nxx[s-1], nyy[s-1]); zoom_in(vs[s], vs[s-1], nxx[s], nyy[s], nxx[s-1], nyy[s-1]); // Scale the OF with the appropriate zoom factor. for (i=0; i < nxx[s-1] * nyy[s-1]; i++) { us[s-1][i] *= 1.0 / scaleFactor; vs[s-1][i] *= 1.0 / scaleFactor; } } // Free the allocated memory. for (i=1; i<nScales; i++) { if (verbose) printf("Free Scale: %d %dx%d\n", s, nxx[s], nyy[s]); free(I1s[i]); free(I2s[i]); free(us[i]); free(vs[i]); } if (verbose) printf("calcMSCLG_OF: done\n"); return 1; }
/** * * Function to compute the optical flow in one scale * **/ void Dual_TVL1_optic_flow( float *I0, // source image float *I1, // target image float *u1, // x component of the optical flow float *u2, // y component of the optical flow const int nx, // image width const int ny, // image height const float tau, // time step const float lambda, // weight parameter for the data term const float theta, // weight parameter for (u - v)² const int warps, // number of warpings per scale const float epsilon, // tolerance for numerical convergence const bool verbose // enable/disable the verbose mode ) { const int size = nx * ny; const float l_t = lambda * theta; size_t sf = sizeof(float); float *I1x = malloc(size*sf); float *I1y = xmalloc(size*sf); float *I1w = xmalloc(size*sf); float *I1wx = xmalloc(size*sf); float *I1wy = xmalloc(size*sf); float *rho_c = xmalloc(size*sf); float *v1 = xmalloc(size*sf); float *v2 = xmalloc(size*sf); float *p11 = xmalloc(size*sf); float *p12 = xmalloc(size*sf); float *p21 = xmalloc(size*sf); float *p22 = xmalloc(size*sf); float *div = xmalloc(size*sf); float *grad = xmalloc(size*sf); float *div_p1 = xmalloc(size*sf); float *div_p2 = xmalloc(size*sf); float *u1x = xmalloc(size*sf); float *u1y = xmalloc(size*sf); float *u2x = xmalloc(size*sf); float *u2y = xmalloc(size*sf); centered_gradient(I1, I1x, I1y, nx, ny); // initialization of p for (int i = 0; i < size; i++) { p11[i] = p12[i] = 0.0; p21[i] = p22[i] = 0.0; } for (int warpings = 0; warpings < warps; warpings++) { // compute the warping of the target image and its derivatives bicubic_interpolation_warp(I1, u1, u2, I1w, nx, ny, true); bicubic_interpolation_warp(I1x, u1, u2, I1wx, nx, ny, true); bicubic_interpolation_warp(I1y, u1, u2, I1wy, nx, ny, true); #pragma omp parallel for for (int i = 0; i < size; i++) { const float Ix2 = I1wx[i] * I1wx[i]; const float Iy2 = I1wy[i] * I1wy[i]; // store the |Grad(I1)|^2 grad[i] = (Ix2 + Iy2); // compute the constant part of the rho function rho_c[i] = (I1w[i] - I1wx[i] * u1[i] - I1wy[i] * u2[i] - I0[i]); } int n = 0; float error = INFINITY; while (error > epsilon * epsilon && n < MAX_ITERATIONS) { n++; // estimate the values of the variable (v1, v2) // (thresholding opterator TH) #pragma omp parallel for for (int i = 0; i < size; i++) { const float rho = rho_c[i] + (I1wx[i] * u1[i] + I1wy[i] * u2[i]); float d1, d2; if (rho < - l_t * grad[i]) { d1 = l_t * I1wx[i]; d2 = l_t * I1wy[i]; } else { if (rho > l_t * grad[i]) { d1 = -l_t * I1wx[i]; d2 = -l_t * I1wy[i]; } else { if (grad[i] < GRAD_IS_ZERO) d1 = d2 = 0; else { float fi = -rho/grad[i]; d1 = fi * I1wx[i]; d2 = fi * I1wy[i]; } } } v1[i] = u1[i] + d1; v2[i] = u2[i] + d2; } // compute the divergence of the dual variable (p1, p2) divergence(p11, p12, div_p1, nx ,ny); divergence(p21, p22, div_p2, nx ,ny); // estimate the values of the optical flow (u1, u2) error = 0.0; #pragma omp parallel for reduction(+:error) for (int i = 0; i < size; i++) { const float u1k = u1[i]; const float u2k = u2[i]; u1[i] = v1[i] + theta * div_p1[i]; u2[i] = v2[i] + theta * div_p2[i]; error += (u1[i] - u1k) * (u1[i] - u1k) + (u2[i] - u2k) * (u2[i] - u2k); } error /= size; // compute the gradient of the optical flow (Du1, Du2) forward_gradient(u1, u1x, u1y, nx ,ny); forward_gradient(u2, u2x, u2y, nx ,ny); // estimate the values of the dual variable (p1, p2) #pragma omp parallel for for (int i = 0; i < size; i++) { const float taut = tau / theta; const float g1 = hypot(u1x[i], u1y[i]); const float g2 = hypot(u2x[i], u2y[i]); const float ng1 = 1.0 + taut * g1; const float ng2 = 1.0 + taut * g2; p11[i] = (p11[i] + taut * u1x[i]) / ng1; p12[i] = (p12[i] + taut * u1y[i]) / ng1; p21[i] = (p21[i] + taut * u2x[i]) / ng2; p22[i] = (p22[i] + taut * u2y[i]) / ng2; } } if (verbose) fprintf(stderr, "Warping: %d, " "Iterations: %d, " "Error: %f\n", warpings, n, error); } // delete allocated memory free(I1x); free(I1y); free(I1w); free(I1wx); free(I1wy); free(rho_c); free(v1); free(v2); free(p11); free(p12); free(p21); free(p22); free(div); free(grad); free(div_p1); free(div_p2); free(u1x); free(u1y); free(u2x); free(u2y); }