/**
*
* 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);
}
