##### https://doi.org/10.5201/ipol.2016.172
Tip revision: edce3fb
robust_expo_methods.h
``````// This program is free software: you can use, modify and/or redistribute it
// under the terms of the simplified BSD License. You should have received a
// copy of this license along this program. If not, see
//
// Copyright (C) 2012, Javier Sánchez Pérez <jsanchez@dis.ulpgc.es>
// Copyright (C) 2014, 2015 Nelson Monzón López <nmonzon@ctim.es>

#ifndef ROBUST_EXPO_METHODS_H
#define ROBUST_EXPO_METHODS_H

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <omp.h>
#include <vector>
#include <iostream>
using namespace std;

#include "zoom.h"
#include "bicubic_interpolation.h"
#include "generic_tensor.h"
#include "smoothness.h"

#define MAXITER 300
#define SOR_PARAMETER 1.9
#define GAUSSIAN_SIGMA 0.8

/**
*
* Compute the coefficients of the robust functional (data term)
*
**/
void psi_data (
const float *I1,	//first image
const float *I2,	//second image
const float *I2x,	//gradient of the second image
const float *I2y,	//gradient of the second image
const float *du,	//motion increment
const float *dv,	//motion increment
float *psip,		//output coefficients
const int nx,		//image width
const int ny,		//image height
const int nz 		//image channels
)
{
const int size = nx * ny;

//compute 1/(sqrt(Sum(I2-I1+I2x*du+I2y*dv)²+e²) in each pixel
#pragma omp parallel for
for (int i = 0; i < size; i++)
{
float dI2 = 0;
for (int k = 0; k < nz; k++){

int   real_index = i*nz + k;
const float dI  = I2[real_index] + I2x[real_index] * du[i] + I2y[real_index] * dv[i] - I1[real_index];

dI2 += dI * dI;
}

psip[i] = (1. / sqrt (dI2 + EPSILON * EPSILON));

}

}

/**
*
* Compute the coefficients of the robust functional (gradient term)
*
**/

const float *I1x,	//gradient of the first image
const float *I1y,		//gradient of the first image
const float *I2x,		//gradient of the second image
const float *I2y,		//gradient of the second image
const float *I2xx,	//second derivatives of the second image
const float *I2xy,	//second derivatives of the second image
const float *I2yy,	//second derivatives of the second image
const float *du,		//motion increment
const float *dv,		//motion increment
float *psip,		//output coefficients
const int nx,		//image width
const int ny,		//image height
const int nz		// nº image channels
)
{
const int size = nx * ny;

//compute 1/(sqrt(|DI2-DI1=HI2*(du,dv)|²+e²) in each pixel
#pragma omp parallel for
for (int i = 0; i < size; i++)
{
float dI2 = 0;

for (int k = 0; k < nz; k++){

int   real_index = i*nz + k;

const float dIx = I2x[real_index] + I2xx[real_index] * du[i] + I2xy[real_index] * dv[i] - I1x[real_index];
const float dIy = I2y[real_index] + I2xy[real_index] * du[i] + I2yy[real_index] * dv[i] - I1y[real_index];

dI2 += dIx * dIx + dIy * dIy;
}

psip[i] = (1. / sqrt (dI2 + EPSILON * EPSILON));

}
}

/**
*
*  SOR iteration in one position
*
*/
inline float sor_iteration(
const float *Au,   //constant part of the numerator of u
const float *Av,   //constant part of the numerator of vPriscila Estévez
const float *Du,   //denominator of u
const float *Dv,   //denominator of v
const float *D,    //constant part of the numerator
float       *du,   //x component of the motion increment
float       *dv,   //y component of the motion increment
const float alpha, //alpha smoothness parameter
const float *psi1, //coefficients of the divergence
const float *psi2,
const float *psi3,
const float *psi4,
const int   i,     //current row
const int   i0,    //previous row
const int   i1,    //following row
const int   j,     //current column
const int   j0,    //previous column
const int   j1,    //following column
const int   nx     //number of columns
)
{
//set the SOR extrapolation parameter
const float w = SOR_PARAMETER;

//calculate the position in the array
const int k = i * nx + j;

//compute the divergence part of the numerator
const float div_du = psi1[k] * du[k+j1] + psi2[k] * du[k-j0] +
psi3[k] * du[k+i1] + psi4[k] * du[k-i0];

const float div_dv = psi1[k] * dv[k+j1] + psi2[k] * dv[k-j0] +
psi3[k] * dv[k+i1] + psi4[k] * dv[k-i0];

const float duk = du[k];
const float dvk = dv[k];

//update the motion increment
du[k] = (1.-w) * du[k] + w * (Au[k] - D[k] * dv[k] + alpha * div_du) / Du[k];
dv[k] = (1.-w) * dv[k] + w * (Av[k] - D[k] * du[k] + alpha * div_dv) / Dv[k];

//return the covergence error in this position
return (du[k] - duk) * (du[k] - duk) + (dv[k] - dvk) * (dv[k] - dvk);
}

/**
*
* Compute the optic flow with the matrix
*
**/
void robust_expo_methods(
const float *I1,          //first image
const float *I2,          //second image
float *u, 		  //x component of the optical flow
float *v, 		  //y component of the optical flow
const int    nx,          //image width
const int    ny,          //image height
const int    nz,          // number of color channels in the image
const int    method_type, // choose the diffusion strategy
const float  alpha,       // smoothness parameter
const float  gamma,       // gradient term parameter
const float  lambda,      // coefficient parameter for the decreasing function (if needed)
const float  TOL,         // stopping criterion threshold
const int    inner_iter,  // number of inner iterations
const int    outer_iter,  // number of outer iterations
const bool   verbose      // switch on messages
)
{
const int size_flow = nx * ny;
const int size      = size_flow * nz;

//allocate memory
float *du    = new float[size_flow];
float *dv    = new float[size_flow];

float *ux    = new float[size_flow];
float *uy    = new float[size_flow];
float *vx    = new float[size_flow];
float *vy    = new float[size_flow];

float *I1x   = new float[size];
float *I1y   = new float[size];
float *I2x   = new float[size];
float *I2y   = new float[size];
float *I2w   = new float[size];
float *I2wx  = new float[size];
float *I2wy  = new float[size];
float *I2xx  = new float[size];
float *I2yy  = new float[size];
float *I2xy  = new float[size];
float *I2wxx = new float[size];
float *I2wyy = new float[size];
float *I2wxy = new float[size];

float *div_u = new float[size_flow];
float *div_v = new float[size_flow];
float *div_d = new float[size_flow];

float *Au    = new float[size_flow];
float *Av    = new float[size_flow];
float *Du    = new float[size_flow];
float *Dv    = new float[size_flow];
float *D     = new float[size_flow];
float *expo  = new float[size_flow];

float *psid  = new float[size_flow];
float *psig  = new float[size_flow];
float *psis  = new float[size_flow];
float *psi1  = new float[size_flow];
float *psi2  = new float[size_flow];
float *psi3  = new float[size_flow];
float *psi4  = new float[size_flow];

//compute the gradient of the images
gradient(I1, I1x, I1y, nx, ny, nz);
gradient(I2, I2x, I2y, nx, ny, nz);

//compute second order derivatives
Dxx(I2, I2xx, nx, ny, nz);
Dyy(I2, I2yy, nx, ny, nz);
Dxy(I2, I2xy, nx, ny, nz);

//compute the smoothness_coefficients including the robust function Phi for the smoothness term
exponential_calculation (I1x, I1y, size_flow, size, nz, alpha, lambda, method_type, expo);

//outer iterations loop
for(int no = 0; no < outer_iter; no++){

// Warp the second image and its derivatives
bicubic_interpolation (I2, u, v, I2w, nx, ny, nz, true);
bicubic_interpolation (I2x, u, v, I2wx, nx, ny, nz, true);
bicubic_interpolation (I2y, u, v, I2wy, nx, ny, nz, true);
bicubic_interpolation (I2xx, u, v, I2wxx, nx, ny, nz, true);
bicubic_interpolation (I2xy, u, v, I2wxy, nx, ny, nz, true);
bicubic_interpolation (I2yy, u, v, I2wyy, nx, ny, nz, true);

gradient (u, ux, uy, nx, ny, 1);
gradient (v, vx, vy, nx, ny, 1);

//compute robust function Phi for the smoothness term
psi_smooth(ux, uy, vx, vy, expo, size_flow, psis);

//compute coefficients of Phi functions in divergence
psi_divergence (psi1, psi2, psi3, psi4, psis, nx, ny);

//Calculate the divergence
divergence (u, psi1, psi2, psi3, psi4, nx, ny, div_u);
divergence (v, psi1, psi2, psi3, psi4, nx, ny, div_v);

#pragma omp parallel for
for(int i = 0; i < size_flow; i++){

//compute the coefficents of dw[i] in the smoothness term using gradient exponent
div_d[i] = alpha * (psi1[i] + psi2[i] + psi3[i] + psi4[i]);

//initialize the motion increment
du[i] = dv[i] = 0;

}

//inner iterations loop
for(int ni = 0; ni < inner_iter; ni++){

//compute robust function Psi for the data and gradient terms
psi_data (I1, I2w, I2wx, I2wy, du, dv, psid, nx, ny, nz);
psi_gradient (I1x, I1y, I2wx, I2wy, I2wxx, I2wxy, I2wyy, du, dv, psig, nx, ny, nz);

//store constant parts of the numerical scheme (equation (11))
float BNu, dif, BNv, BDu, BDv, dx, dy, GNu, GNv, GDu, GDv, DI_Gradient, DI_Data;

int index_flow = 0;

for(int index_image = 0; index_image < size; index_image += nz){

BNu = BNv = BDu = BDv = dx = dy = GNu = GNv = GDu = GDv = DI_Gradient = DI_Data = 0;

for(int index_multichannel = 0; index_multichannel < nz; index_multichannel++){

const int real_index = index_image + index_multichannel;

//brightness constancy term
dif  = I2w[real_index] - I1[real_index];
BNu += dif * I2wx[real_index];
BNv += dif * I2wy[real_index];
BDu += I2wx[real_index] * I2wx[real_index];
BDv += I2wy[real_index] * I2wy[real_index];
DI_Data += (I2wy [real_index] * I2wx [real_index]);

dx   = (I2wx[real_index] - I1x[real_index]);
dy   = (I2wy[real_index] - I1y[real_index]);
GNu += (dx * I2wxx[real_index] + dy * I2wxy[real_index]);
GNv += (dx * I2wxy[real_index] + dy * I2wyy[real_index]);
GDu += (I2wxx[real_index] * I2wxx[real_index] + I2wxy[real_index] * I2wxy[real_index]);
GDv += (I2wyy[real_index] * I2wyy[real_index] + I2wxy[real_index] * I2wxy[real_index]);
DI_Gradient  += (I2wxx[real_index] + I2wyy[real_index]) * I2wxy[real_index];

}

const float g = gamma * psig[index_flow];

BNu  = -psid[index_flow] * BNu;
BNv  = -psid[index_flow] * BNv;
BDu  =  psid[index_flow] * BDu;
BDv  =  psid[index_flow] * BDv;
GNu  = -g * GNu;
GNv  = -g * GNv;
GDu  =  g * GDu;
GDv  =  g * GDv;

Au[index_flow] = BNu + GNu + alpha * div_u[index_flow];
Av[index_flow] = BNv + GNv + alpha * div_v[index_flow];
Du[index_flow] = BDu + GDu + div_d[index_flow];
Dv[index_flow] = BDv + GDv + div_d[index_flow];
D [index_flow] = psid[index_flow] * DI_Data + g * DI_Gradient;

index_flow++;

} // end image loop

//sor iterations loop
float error = 1000;
int nsor = 0;

while( error > TOL && nsor < MAXITER){

error = 0;
nsor++;

//#pragma omp parallel for reduction(+:error)
//update the motion increment in the center of the images
for(int i = 1; i < ny-1; i++){
for(int j = 1; j < nx-1; j++)

error += sor_iteration(
Au, Av, Du, Dv, D, du, dv, alpha,
psi1, psi2, psi3, psi4,
i, nx, nx, j, 1, 1, nx
);
}

//update the motion increment in the first and last rows
for(int j = 1; j < nx-1; j++){

error += sor_iteration(
Au, Av, Du, Dv, D, du, dv, alpha,
psi1, psi2, psi3, psi4,
0, 0, nx, j, 1, 1, nx
);

error += sor_iteration(
Au, Av, Du, Dv, D, du, dv, alpha,
psi1, psi2, psi3, psi4,
ny-1, nx, 0, j, 1, 1, nx
);
}

//update the motion increment in the first and last columns
for(int i = 1; i < ny-1; i++){

error += sor_iteration(
Au, Av, Du, Dv, D, du, dv, alpha,
psi1, psi2, psi3, psi4,
i, nx, nx, 0, 0, 1, nx
);

error += sor_iteration(
Au, Av, Du, Dv, D, du, dv, alpha,
psi1, psi2, psi3, psi4,
i, nx, nx, nx-1, 1, 0, nx
);
}

//process the top-left corner (0,0)
error += sor_iteration(
Au, Av, Du, Dv, D, du, dv, alpha,
psi1, psi2, psi3, psi4,
0, 0, nx, 0, 0, 1, nx
);

//process the top-right corner (0,nx-1)
error += sor_iteration(
Au, Av, Du, Dv, D, du, dv, alpha,
psi1, psi2, psi3, psi4,
0, 0, nx, nx-1, 1, 0, nx
);

//process the bottom-left corner (ny-1,0)
error += sor_iteration(
Au, Av, Du, Dv, D, du, dv, alpha,
psi1, psi2, psi3, psi4,
ny-1, nx, 0, 0, 0, 1, nx
);

//process the bottom-right corner (ny-1,nx-1)
error += sor_iteration(
Au, Av, Du, Dv, D, du, dv, alpha,
psi1, psi2, psi3, psi4,
ny-1, nx, 0, nx-1, 1, 0, nx
);

error = sqrt(error / size);

}
if(verbose)
std::cout << "Iterations: " << nsor << " Error: " << error << std::endl;

}

//update the flow with the estimated motion increment
for(int i = 0; i < size_flow; i++){
u[i] += du[i];
v[i] += dv[i];
}

}
//delete allocated memory
delete []du;
delete []dv;

delete []ux;
delete []uy;
delete []vx;
delete []vy;

delete []I1x;
delete []I1y;
delete []I2x;
delete []I2y;
delete []I2w;
delete []I2wx;
delete []I2wy;
delete []I2xx;
delete []I2yy;
delete []I2xy;
delete []I2wxx;
delete []I2wyy;
delete []I2wxy;

delete []div_u;
delete []div_v;
delete []div_d;

delete []Au;
delete []Av;
delete []Du;
delete []Dv;
delete []D;

delete []expo;

delete []psid;
delete []psig;
delete []psis;
delete []psi1;
delete []psi2;
delete []psi3;
delete []psi4;

}

/**
*
* Function to normalize the images between 0 and 255
*
**/
void image_normalization (
const float *I1,	//input image 1
const float *I2,	//input image 2
float *I1n,	//normalized output image 1
float *I2n,	//normalized output image 2
int size,		//size of the image
int nz		//number of color channels in the images
)
{

float max, min;

// Initial min and max values of the images
for(int index_multichannel = 0; index_multichannel < nz; index_multichannel++){

if (I1[index_multichannel] > I2[index_multichannel])
max = I1[index_multichannel];
else
max = I2[index_multichannel];

if (I1[index_multichannel] < I2[index_multichannel])
min = I1[index_multichannel];
else
min = I2[index_multichannel];

//compute the max and min values of the images
for (int i = nz; i < size; i+=nz){

int real_index = i + index_multichannel;

if (I1[real_index] > max)
max = I1[real_index];

if (I1[real_index] < min)
min = I1[real_index];

if (I2[real_index] > max)
max = I2[real_index];

if (I2[real_index] < min)
min = I2[real_index];
}

//compute the global max and min
float den = max - min;

if (den > 0){
//normalize the images between 0 and 255
#pragma omp parallel for
for (int index_image = 0; index_image < size; index_image +=nz)  {

int real_index = index_image + index_multichannel;

I1n[real_index] = 255.0 * (I1[real_index] - min) / den;
I2n[real_index] = 255.0 * (I2[real_index] - min) / den;
}
}
else{
//copy the original data
#pragma omp parallel for
for (int index_image = 0; index_image < size; index_image += nz)  {

int real_index = index_image + index_multichannel;

I1n[real_index] = I1[real_index];
I2n[real_index] = I2[real_index];

}
}
}
}

/**
*
*  Multiscale approach for computing the optical flow
*
**/
void robust_expo_methods(
const float *I1,          // first image
const float *I2,          // second image
float *u, 		      // x component of the optical flow
float *v, 		      // y component of the optical flow
const int    nxx,         // image width
const int    nyy,         // image height
const int    nzz,	      // number of color channels in image
const int    method_type, // choose the diffusion strategy
const float  alpha,       // smoothness parameter
const float  gamma,       // gradient term parameter
const float  lambda,      // coefficient parameter for the decreasing function (if needed)
const int    nscales,     // number of scales
const float  nu,          // downsampling factor
const float  TOL,         // stopping criterion threshold
const int    inner_iter,  // number of inner iterations
const int    outer_iter,  // number of outer iterations
const bool   verbose      // switch on messages
)
{
int size = nxx * nyy * nzz;

std::vector<float *> I1s(nscales);
std::vector<float *> I2s(nscales);
std::vector<float *> us (nscales);
std::vector<float *> vs (nscales);

std::vector<int> nx(nscales);
std::vector<int> ny(nscales);

I1s[0] = new float[size];
I2s[0] = new float[size];

//normalize the input images between 0 and 255
image_normalization(I1, I2, I1s[0], I2s[0], size, nzz);

//presmoothing the finest scale images
gaussian(I1s[0], nxx, nyy, nzz, GAUSSIAN_SIGMA);
gaussian(I2s[0], nxx, nyy, nzz, GAUSSIAN_SIGMA);

us [0] = u;
vs [0] = v;
nx [0] = nxx;
ny [0] = nyy;

//create the scales
for(int s = 1; s < nscales; s++){

zoom_size(nx[s-1], ny[s-1], nx[s], ny[s], nu);
const int sizes_flow = nx[s] * ny[s];
const int sizes = sizes_flow * nzz;

I1s[s] = new float[sizes];
I2s[s] = new float[sizes];
us[s]  = new float[sizes_flow];
vs[s]  = new float[sizes_flow];

//compute the zoom from the previous scale
zoom_out(I1s[s-1], I1s[s], nx[s-1], ny[s-1], nzz, nu);
zoom_out(I2s[s-1], I2s[s], nx[s-1], ny[s-1], nzz, nu);
}

//initialization of the optical flow at the coarsest scale
for(int i = 0; i < nx[nscales-1] * ny[nscales-1]; i++)
us[nscales-1][i] = vs[nscales-1][i] = 0.0;

// readapt alpha for multichannel information
int alpha_adapted_for_nchannels = alpha * nzz;

//pyramidal approach for computing the optical flow
for(int s = nscales-1; s >= 0; s--){

if(verbose)
std::cout << "Scale: " << s << std::endl;

robust_expo_methods(
I1s[s], I2s[s], us[s], vs[s], nx[s], ny[s], nzz,
);

//if it is not the finer scale, then upsample the optical flow and adapt it conveniently
if(s){
zoom_in_flow (us[s], us[s - 1], nx[s], ny[s], nx[s - 1], ny[s - 1]);
zoom_in_flow (vs[s], vs[s - 1], nx[s], ny[s], nx[s - 1], ny[s - 1]);

for(int i = 0; i < nx[s-1] * ny[s-1]; i++){
us[s-1][i] *= 1.0 / nu;
vs[s-1][i] *= 1.0 / nu;
}
}
}

//delete allocated memory
delete []I1s[0];
delete []I2s[0];

for(int i = 1; i < nscales; i++){
delete []I1s[i];
delete []I2s[i];
delete []us [i];
delete []vs [i];
}
}

#endif
``````