//--------------------------------------------------------------------------------------------------------
// function to compute optical flow field using two fixed point iterations
// Input arguments:
// Im1, Im2: frame 1 and frame 2
// warpIm2: the warped frame 2 according to the current flow field u and v
// u,v: the current flow field, NOTICE that they are also output arguments
//
//--------------------------------------------------------------------------------------------------------
void OpticalFlow::SmoothFlowSOR(const DImage &Im1, const DImage &Im2, DImage &warpIm2, DImage &u, DImage &v,
double alpha, int nOuterFPIterations, int nInnerFPIterations, int nSORIterations)
{
// DImage mask,imdx,imdy,imdt;
DImage imdx,imdy,imdt;
int imWidth,imHeight,nChannels,nPixels;
imWidth=Im1.width();
imHeight=Im1.height();
nChannels=Im1.nchannels();
nPixels=imWidth*imHeight;
DImage du(imWidth,imHeight),dv(imWidth,imHeight);
DImage uu(imWidth,imHeight),vv(imWidth,imHeight);
DImage ux(imWidth,imHeight),uy(imWidth,imHeight);
DImage vx(imWidth,imHeight),vy(imWidth,imHeight);
DImage Phi_1st(imWidth,imHeight);
DImage Psi_1st(imWidth,imHeight,nChannels);
DImage imdxy,imdx2,imdy2,imdtdx,imdtdy;
DImage ImDxy,ImDx2,ImDy2,ImDtDx,ImDtDy;
DImage foo1,foo2;
double varepsilon_phi=pow(0.001,2);
double varepsilon_psi=pow(0.001,2);
//--------------------------------------------------------------------------
// the outer fixed point iteration
//--------------------------------------------------------------------------
for(int count=0;count<nOuterFPIterations;count++)
{
// compute the gradient
getDxs(imdx,imdy,imdt,Im1,warpIm2);
// generate the mask to set the weight of the pxiels moving outside of the image boundary to be zero
// genInImageMask(mask,u,v);
// set the derivative of the flow field to be zero
du.reset();
dv.reset();
//--------------------------------------------------------------------------
// the inner fixed point iteration
//--------------------------------------------------------------------------
for(int hh=0;hh<nInnerFPIterations;hh++)
{
// compute the derivatives of the current flow field
if(hh==0)
{
uu.copyData(u);
vv.copyData(v);
}
else
{
uu.Add(u,du);
vv.Add(v,dv);
}
uu.dx(ux);
uu.dy(uy);
vv.dx(vx);
vv.dy(vy);
// compute the weight of phi
Phi_1st.reset();
_FlowPrecision* phiData=Phi_1st.data();
double temp;
const _FlowPrecision *uxData,*uyData,*vxData,*vyData;
uxData=ux.data();
uyData=uy.data();
vxData=vx.data();
vyData=vy.data();
for(int i=0;i<nPixels;i++)
{
temp=uxData[i]*uxData[i]+uyData[i]*uyData[i]+vxData[i]*vxData[i]+vyData[i]*vyData[i];
phiData[i] = 0.5/sqrt(temp+varepsilon_phi);
}
// compute the nonlinear term of psi
Psi_1st.reset();
_FlowPrecision* psiData=Psi_1st.data();
const _FlowPrecision *imdxData,*imdyData,*imdtData;
const _FlowPrecision *duData,*dvData;
imdxData=imdx.data();
imdyData=imdy.data();
imdtData=imdt.data();
duData=du.data();
dvData=dv.data();
if(nChannels==1)
for(int i=0;i<nPixels;i++)
{
temp=imdtData[i]+imdxData[i]*duData[i]+imdyData[i]*dvData[i];
temp *= temp;
psiData[i]=1/(2*sqrt(temp+varepsilon_psi));
}
else
for(int i=0;i<nPixels;i++)
for(int k=0;k<nChannels;k++)
{
int offset=i*nChannels+k;
temp=imdtData[offset]+imdxData[offset]*duData[i]+imdyData[offset]*dvData[i];
temp *= temp;
psiData[offset]=1/(2*sqrt(temp+varepsilon_psi));
}
// prepare the components of the large linear system
ImDxy.Multiply(Psi_1st,imdx,imdy);
ImDx2.Multiply(Psi_1st,imdx,imdx);
ImDy2.Multiply(Psi_1st,imdy,imdy);
ImDtDx.Multiply(Psi_1st,imdx,imdt);
ImDtDy.Multiply(Psi_1st,imdy,imdt);
if(nChannels>1)
{
ImDxy.collapse(imdxy);
ImDx2.collapse(imdx2);
ImDy2.collapse(imdy2);
ImDtDx.collapse(imdtdx);
ImDtDy.collapse(imdtdy);
}
else
{
imdxy.copyData(ImDxy);
imdx2.copyData(ImDx2);
imdy2.copyData(ImDy2);
imdtdx.copyData(ImDtDx);
imdtdy.copyData(ImDtDy);
}
// laplacian filtering of the current flow field
Laplacian(foo1,u,Phi_1st);
Laplacian(foo2,v,Phi_1st);
for(int i=0;i<nPixels;i++)
{
imdtdx.data()[i] = -imdtdx.data()[i]-alpha*foo1.data()[i];
imdtdy.data()[i] = -imdtdy.data()[i]-alpha*foo2.data()[i];
}
// here we start SOR
// set omega
double omega = 1.8;
du.reset();
dv.reset();
for(int k = 0; k<nSORIterations; k++)
for(int i = 0; i<imHeight; i++)
for(int j = 0; j<imWidth; j++)
{
int offset = i * imWidth+j;
double sigma1 = 0, sigma2 = 0, coeff = 0;
double _weight;
if(j>0)
{
_weight = phiData[offset-1];
sigma1 += _weight*du.data()[offset-1];
sigma2 += _weight*dv.data()[offset-1];
coeff += _weight;
}
if(j<imWidth-1)
{
_weight = phiData[offset];
sigma1 += _weight*du.data()[offset+1];
sigma2 += _weight*dv.data()[offset+1];
coeff += _weight;
}
if(i>0)
{
_weight = phiData[offset-imWidth];
sigma1 += _weight*du.data()[offset-imWidth];
sigma2 += _weight*dv.data()[offset-imWidth];
coeff += _weight;
}
if(i<imHeight-1)
{
_weight = phiData[offset];
sigma1 += _weight*du.data()[offset+imWidth];
sigma2 += _weight*dv.data()[offset+imWidth];
coeff += _weight;
}
sigma1 *= -alpha;
sigma2 *= -alpha;
coeff *= alpha;
// compute du
sigma1 += imdxy.data()[offset]*dv.data()[offset];
du.data()[offset] = (1-omega)*du.data()[offset] + omega/(imdx2.data()[offset] + /*alpha*0.05*/ + coeff)*(imdtdx.data()[offset] - sigma1);
// compute dv
sigma2 += imdxy.data()[offset]*du.data()[offset];
dv.data()[offset] = (1-omega)*dv.data()[offset] + omega/(imdy2.data()[offset] + /*alpha*0.05*/ + coeff)*(imdtdy.data()[offset] - sigma2);
}
}
u.Add(du);
v.Add(dv);
if(interpolation == Bilinear){
warpFL(warpIm2,Im1,Im2,u,v);
}
else
{
Im2.warpImageBicubicRef(Im1,warpIm2,u,v);
warpIm2.threshold();
}
}
}
//--------------------------------------------------------------------------------------------------------
// function to compute optical flow field using two fixed point iterations
// Input arguments:
// Im1, Im2: frame 1 and frame 2
// warpIm2: the warped frame 2 according to the current flow field u and v
// u,v: the current flow field, NOTICE that they are also output arguments
//
//--------------------------------------------------------------------------------------------------------
void OpticalFlow::SmoothFlowSOR(const DImage &Im1, const DImage &Im2, DImage &warpIm2, DImage &u, DImage &v,
double alpha, int nOuterFPIterations, int nInnerFPIterations, int nSORIterations)
{
DImage mask,imdx,imdy,imdt;
int imWidth,imHeight,nChannels,nPixels;
imWidth=Im1.width();
imHeight=Im1.height();
nChannels=Im1.nchannels();
nPixels=imWidth*imHeight;
DImage du(imWidth,imHeight),dv(imWidth,imHeight);
DImage uu(imWidth,imHeight),vv(imWidth,imHeight);
DImage ux(imWidth,imHeight),uy(imWidth,imHeight);
DImage vx(imWidth,imHeight),vy(imWidth,imHeight);
DImage Phi_1st(imWidth,imHeight);
DImage Psi_1st(imWidth,imHeight,nChannels);
DImage imdxy,imdx2,imdy2,imdtdx,imdtdy;
DImage ImDxy,ImDx2,ImDy2,ImDtDx,ImDtDy;
DImage foo1,foo2;
double prob1,prob2,prob11,prob22;
double varepsilon_phi=pow(0.001,2);
double varepsilon_psi=pow(0.001,2);
//--------------------------------------------------------------------------
// the outer fixed point iteration
//--------------------------------------------------------------------------
for(int count=0;count<nOuterFPIterations;count++)
{
// compute the gradient
getDxs(imdx,imdy,imdt,Im1,warpIm2);
// generate the mask to set the weight of the pxiels moving outside of the image boundary to be zero
genInImageMask(mask,u,v);
// set the derivative of the flow field to be zero
du.reset();
dv.reset();
//--------------------------------------------------------------------------
// the inner fixed point iteration
//--------------------------------------------------------------------------
for(int hh=0;hh<nInnerFPIterations;hh++)
{
// compute the derivatives of the current flow field
if(hh==0)
{
uu.copyData(u);
vv.copyData(v);
}
else
{
uu.Add(u,du);
vv.Add(v,dv);
}
uu.dx(ux);
uu.dy(uy);
vv.dx(vx);
vv.dy(vy);
// compute the weight of phi
Phi_1st.reset();
_FlowPrecision* phiData=Phi_1st.data();
double temp;
const _FlowPrecision *uxData,*uyData,*vxData,*vyData;
uxData=ux.data();
uyData=uy.data();
vxData=vx.data();
vyData=vy.data();
double power_alpha = 0.5;
for(int i=0;i<nPixels;i++)
{
temp=uxData[i]*uxData[i]+uyData[i]*uyData[i]+vxData[i]*vxData[i]+vyData[i]*vyData[i];
//phiData[i]=power_alpha*pow(temp+varepsilon_phi,power_alpha-1);
phiData[i] = 0.5/sqrt(temp+varepsilon_phi);
//phiData[i] = 1/(power_alpha+temp);
}
// compute the nonlinear term of psi
Psi_1st.reset();
_FlowPrecision* psiData=Psi_1st.data();
const _FlowPrecision *imdxData,*imdyData,*imdtData;
const _FlowPrecision *duData,*dvData;
imdxData=imdx.data();
imdyData=imdy.data();
imdtData=imdt.data();
duData=du.data();
dvData=dv.data();
double _a = 10000, _b = 0.1;
if(nChannels==1)
for(int i=0;i<nPixels;i++)
{
temp=imdtData[i]+imdxData[i]*duData[i]+imdyData[i]*dvData[i];
//if(temp*temp<0.04)
// psiData[i]=1/(2*sqrt(temp*temp+varepsilon_psi));
//psiData[i] = _a*_b/(1+_a*temp*temp);
// the following code is for log Gaussian mixture probability model
temp *= temp;
switch(noiseModel)
{
case GMixture:
prob1 = GMPara.Gaussian(temp,0,0)*GMPara.alpha[0];
prob2 = GMPara.Gaussian(temp,1,0)*(1-GMPara.alpha[0]);
prob11 = prob1/(2*GMPara.sigma_square[0]);
prob22 = prob2/(2*GMPara.beta_square[0]);
psiData[i] = (prob11+prob22)/(prob1+prob2);
break;
case Lap:
if(LapPara[0]<1E-20)
continue;
//psiData[i]=1/(2*sqrt(temp+varepsilon_psi)*LapPara[0]);
psiData[i]=1/(2*sqrt(temp+varepsilon_psi));
break;
}
}
else
for(int i=0;i<nPixels;i++)
for(int k=0;k<nChannels;k++)
{
int offset=i*nChannels+k;
temp=imdtData[offset]+imdxData[offset]*duData[i]+imdyData[offset]*dvData[i];
//if(temp*temp<0.04)
// psiData[offset]=1/(2*sqrt(temp*temp+varepsilon_psi));
//psiData[offset] = _a*_b/(1+_a*temp*temp);
temp *= temp;
switch(noiseModel)
{
case GMixture:
prob1 = GMPara.Gaussian(temp,0,k)*GMPara.alpha[k];
prob2 = GMPara.Gaussian(temp,1,k)*(1-GMPara.alpha[k]);
prob11 = prob1/(2*GMPara.sigma_square[k]);
prob22 = prob2/(2*GMPara.beta_square[k]);
psiData[offset] = (prob11+prob22)/(prob1+prob2);
break;
case Lap:
if(LapPara[k]<1E-20)
continue;
//psiData[offset]=1/(2*sqrt(temp+varepsilon_psi)*LapPara[k]);
psiData[offset]=1/(2*sqrt(temp+varepsilon_psi));
break;
}
}
// prepare the components of the large linear system
ImDxy.Multiply(Psi_1st,imdx,imdy);
ImDx2.Multiply(Psi_1st,imdx,imdx);
ImDy2.Multiply(Psi_1st,imdy,imdy);
ImDtDx.Multiply(Psi_1st,imdx,imdt);
ImDtDy.Multiply(Psi_1st,imdy,imdt);
if(nChannels>1)
{
ImDxy.collapse(imdxy);
ImDx2.collapse(imdx2);
ImDy2.collapse(imdy2);
ImDtDx.collapse(imdtdx);
ImDtDy.collapse(imdtdy);
}
else
{
imdxy.copyData(ImDxy);
imdx2.copyData(ImDx2);
imdy2.copyData(ImDy2);
imdtdx.copyData(ImDtDx);
imdtdy.copyData(ImDtDy);
}
// laplacian filtering of the current flow field
Laplacian(foo1,u,Phi_1st);
Laplacian(foo2,v,Phi_1st);
for(int i=0;i<nPixels;i++)
{
imdtdx.data()[i] = -imdtdx.data()[i]-alpha*foo1.data()[i];
imdtdy.data()[i] = -imdtdy.data()[i]-alpha*foo2.data()[i];
}
// here we start SOR
// set omega
double omega = 1.8;
du.reset();
dv.reset();
for(int k = 0; k<nSORIterations; k++)
for(int i = 0; i<imHeight; i++)
for(int j = 0; j<imWidth; j++)
{
int offset = i * imWidth+j;
double sigma1 = 0, sigma2 = 0, coeff = 0;
double _weight;
if(j>0)
{
_weight = phiData[offset-1];
sigma1 += _weight*du.data()[offset-1];
sigma2 += _weight*dv.data()[offset-1];
coeff += _weight;
}
if(j<imWidth-1)
{
_weight = phiData[offset];
sigma1 += _weight*du.data()[offset+1];
sigma2 += _weight*dv.data()[offset+1];
coeff += _weight;
}
if(i>0)
{
_weight = phiData[offset-imWidth];
sigma1 += _weight*du.data()[offset-imWidth];
sigma2 += _weight*dv.data()[offset-imWidth];
coeff += _weight;
}
if(i<imHeight-1)
{
_weight = phiData[offset];
sigma1 += _weight*du.data()[offset+imWidth];
sigma2 += _weight*dv.data()[offset+imWidth];
coeff += _weight;
}
sigma1 *= -alpha;
sigma2 *= -alpha;
coeff *= alpha;
// compute du
sigma1 += imdxy.data()[offset]*dv.data()[offset];
du.data()[offset] = (1-omega)*du.data()[offset] + omega/(imdx2.data()[offset] + alpha*0.05 + coeff)*(imdtdx.data()[offset] - sigma1);
// compute dv
sigma2 += imdxy.data()[offset]*du.data()[offset];
dv.data()[offset] = (1-omega)*dv.data()[offset] + omega/(imdy2.data()[offset] + alpha*0.05 + coeff)*(imdtdy.data()[offset] - sigma2);
}
}
u.Add(du);
v.Add(dv);
if(interpolation == Bilinear)
warpFL(warpIm2,Im1,Im2,u,v);
else
{
Im2.warpImageBicubicRef(Im1,warpIm2,u,v);
warpIm2.threshold();
}
//Im2.warpImageBicubicRef(Im1,warpIm2,BicubicCoeff,u,v);
// estimate noise level
switch(noiseModel)
{
case GMixture:
estGaussianMixture(Im1,warpIm2,GMPara);
break;
case Lap:
estLaplacianNoise(Im1,warpIm2,LapPara);
}
}
}
//--------------------------------------------------------------------------------------------------------
// function to compute optical flow field using two fixed point iterations
// Input arguments:
// Im1, Im2: frame 1 and frame 2
// warpIm2: the warped frame 2 according to the current flow field u and v
// u,v: the current flow field, NOTICE that they are also output arguments
//
//--------------------------------------------------------------------------------------------------------
void OpticalFlow::SmoothFlowPDE(const DImage &Im1, const DImage &Im2, DImage &warpIm2, DImage &u, DImage &v,
double alpha, int nOuterFPIterations, int nInnerFPIterations, int nCGIterations)
{
DImage mask,imdx,imdy,imdt;
int imWidth,imHeight,nChannels,nPixels;
imWidth=Im1.width();
imHeight=Im1.height();
nChannels=Im1.nchannels();
nPixels=imWidth*imHeight;
DImage du(imWidth,imHeight),dv(imWidth,imHeight);
DImage uu(imWidth,imHeight),vv(imWidth,imHeight);
DImage ux(imWidth,imHeight),uy(imWidth,imHeight);
DImage vx(imWidth,imHeight),vy(imWidth,imHeight);
DImage Phi_1st(imWidth,imHeight);
DImage Psi_1st(imWidth,imHeight,nChannels);
DImage imdxy,imdx2,imdy2,imdtdx,imdtdy;
DImage ImDxy,ImDx2,ImDy2,ImDtDx,ImDtDy;
DImage A11,A12,A22,b1,b2;
DImage foo1,foo2;
// compute bicubic interpolation coeff
//DImage BicubicCoeff;
//Im2.warpImageBicubicCoeff(BicubicCoeff);
double prob1,prob2,prob11,prob22;
// variables for conjugate gradient
DImage r1,r2,p1,p2,q1,q2;
double* rou;
rou=new double[nCGIterations];
double varepsilon_phi=pow(0.001,2);
double varepsilon_psi=pow(0.001,2);
//--------------------------------------------------------------------------
// the outer fixed point iteration
//--------------------------------------------------------------------------
for(int count=0;count<nOuterFPIterations;count++)
{
// compute the gradient
getDxs(imdx,imdy,imdt,Im1,warpIm2);
// generate the mask to set the weight of the pxiels moving outside of the image boundary to be zero
genInImageMask(mask,u,v);
// set the derivative of the flow field to be zero
du.reset();
dv.reset();
//--------------------------------------------------------------------------
// the inner fixed point iteration
//--------------------------------------------------------------------------
for(int hh=0;hh<nInnerFPIterations;hh++)
{
// compute the derivatives of the current flow field
if(hh==0)
{
uu.copyData(u);
vv.copyData(v);
}
else
{
uu.Add(u,du);
vv.Add(v,dv);
}
uu.dx(ux);
uu.dy(uy);
vv.dx(vx);
vv.dy(vy);
// compute the weight of phi
Phi_1st.reset();
_FlowPrecision* phiData=Phi_1st.data();
_FlowPrecision temp;
const _FlowPrecision *uxData,*uyData,*vxData,*vyData;
uxData=ux.data();
uyData=uy.data();
vxData=vx.data();
vyData=vy.data();
double power_alpha = 0.5;
for(int i=0;i<nPixels;i++)
{
temp=uxData[i]*uxData[i]+uyData[i]*uyData[i]+vxData[i]*vxData[i]+vyData[i]*vyData[i];
//phiData[i]=power_alpha*pow(temp+varepsilon_phi,power_alpha-1);
phiData[i] = 0.5/sqrt(temp+varepsilon_phi);
//phiData[i] = 1/(power_alpha+temp);
}
// compute the nonlinear term of psi
Psi_1st.reset();
_FlowPrecision* psiData=Psi_1st.data();
const _FlowPrecision *imdxData,*imdyData,*imdtData;
const _FlowPrecision *duData,*dvData;
imdxData=imdx.data();
imdyData=imdy.data();
imdtData=imdt.data();
duData=du.data();
dvData=dv.data();
double _a = 10000, _b = 0.1;
if(nChannels==1)
for(int i=0;i<nPixels;i++)
{
temp=imdtData[i]+imdxData[i]*duData[i]+imdyData[i]*dvData[i];
//if(temp*temp<0.04)
// psiData[i]=1/(2*sqrt(temp*temp+varepsilon_psi));
//psiData[i] = _a*_b/(1+_a*temp*temp);
// the following code is for log Gaussian mixture probability model
temp *= temp;
switch(noiseModel)
{
case GMixture:
prob1 = GMPara.Gaussian(temp,0,0)*GMPara.alpha[0];
prob2 = GMPara.Gaussian(temp,1,0)*(1-GMPara.alpha[0]);
prob11 = prob1/(2*GMPara.sigma_square[0]);
prob22 = prob2/(2*GMPara.beta_square[0]);
psiData[i] = (prob11+prob22)/(prob1+prob2);
break;
case Lap:
if(LapPara[0]<1E-20)
continue;
psiData[i]=1/(2*sqrt(temp+varepsilon_psi)*LapPara[0]);
break;
}
}
else
for(int i=0;i<nPixels;i++)
for(int k=0;k<nChannels;k++)
{
int offset=i*nChannels+k;
temp=imdtData[offset]+imdxData[offset]*duData[i]+imdyData[offset]*dvData[i];
//if(temp*temp<0.04)
// psiData[offset]=1/(2*sqrt(temp*temp+varepsilon_psi));
//psiData[offset] = _a*_b/(1+_a*temp*temp);
temp *= temp;
switch(noiseModel)
{
case GMixture:
prob1 = GMPara.Gaussian(temp,0,k)*GMPara.alpha[k];
prob2 = GMPara.Gaussian(temp,1,k)*(1-GMPara.alpha[k]);
prob11 = prob1/(2*GMPara.sigma_square[k]);
prob22 = prob2/(2*GMPara.beta_square[k]);
psiData[offset] = (prob11+prob22)/(prob1+prob2);
break;
case Lap:
if(LapPara[k]<1E-20)
continue;
psiData[offset]=1/(2*sqrt(temp+varepsilon_psi)*LapPara[k]);
break;
}
}
// prepare the components of the large linear system
ImDxy.Multiply(Psi_1st,imdx,imdy);
ImDx2.Multiply(Psi_1st,imdx,imdx);
ImDy2.Multiply(Psi_1st,imdy,imdy);
ImDtDx.Multiply(Psi_1st,imdx,imdt);
ImDtDy.Multiply(Psi_1st,imdy,imdt);
if(nChannels>1)
{
ImDxy.collapse(imdxy);
ImDx2.collapse(imdx2);
ImDy2.collapse(imdy2);
ImDtDx.collapse(imdtdx);
ImDtDy.collapse(imdtdy);
}
else
{
imdxy.copyData(ImDxy);
imdx2.copyData(ImDx2);
imdy2.copyData(ImDy2);
imdtdx.copyData(ImDtDx);
imdtdy.copyData(ImDtDy);
}
// filtering
//imdx2.smoothing(A11,3);
//imdxy.smoothing(A12,3);
//imdy2.smoothing(A22,3);
A11.copyData(imdx2);
A12.copyData(imdxy);
A22.copyData(imdy2);
// add epsilon to A11 and A22
A11.Add(alpha*0.5);
A22.Add(alpha*0.5);
// form b
//imdtdx.smoothing(b1,3);
//imdtdy.smoothing(b2,3);
b1.copyData(imdtdx);
b2.copyData(imdtdy);
// laplacian filtering of the current flow field
Laplacian(foo1,u,Phi_1st);
Laplacian(foo2,v,Phi_1st);
_FlowPrecision *b1Data,*b2Data;
const _FlowPrecision *foo1Data,*foo2Data;
b1Data=b1.data();
b2Data=b2.data();
foo1Data=foo1.data();
foo2Data=foo2.data();
for(int i=0;i<nPixels;i++)
{
b1Data[i]=-b1Data[i]-alpha*foo1Data[i];
b2Data[i]=-b2Data[i]-alpha*foo2Data[i];
}
// for debug only, displaying the matrix coefficients
//A11.imwrite("A11.bmp",ImageIO::normalized);
//A12.imwrite("A12.bmp",ImageIO::normalized);
//A22.imwrite("A22.bmp",ImageIO::normalized);
//b1.imwrite("b1.bmp",ImageIO::normalized);
//b2.imwrite("b2.bmp",ImageIO::normalized);
//-----------------------------------------------------------------------
// conjugate gradient algorithm
//-----------------------------------------------------------------------
r1.copyData(b1);
r2.copyData(b2);
du.reset();
dv.reset();
for(int k=0;k<nCGIterations;k++)
{
rou[k]=r1.norm2()+r2.norm2();
//cout<<rou[k]<<endl;
if(rou[k]<1E-10)
break;
if(k==0)
{
p1.copyData(r1);
p2.copyData(r2);
}
else
{
double ratio=rou[k]/rou[k-1];
p1.Add(r1,p1,ratio);
p2.Add(r2,p2,ratio);
}
// go through the large linear system
foo1.Multiply(A11,p1);
foo2.Multiply(A12,p2);
q1.Add(foo1,foo2);
Laplacian(foo1,p1,Phi_1st);
q1.Add(foo1,alpha);
foo1.Multiply(A12,p1);
foo2.Multiply(A22,p2);
q2.Add(foo1,foo2);
Laplacian(foo2,p2,Phi_1st);
q2.Add(foo2,alpha);
double beta;
beta=rou[k]/(p1.innerproduct(q1)+p2.innerproduct(q2));
du.Add(p1,beta);
dv.Add(p2,beta);
r1.Add(q1,-beta);
r2.Add(q2,-beta);
}
//-----------------------------------------------------------------------
// end of conjugate gradient algorithm
//-----------------------------------------------------------------------
}// end of inner fixed point iteration
// the following procedure is merely for debugging
//cout<<"du "<<du.norm2()<<" dv "<<dv.norm2()<<endl;
// update the flow field
u.Add(du,1);
v.Add(dv,1);
if(interpolation == Bilinear)
warpFL(warpIm2,Im1,Im2,u,v);
else
{
Im2.warpImageBicubicRef(Im1,warpIm2,u,v);
warpIm2.threshold();
}
//Im2.warpImageBicubicRef(Im1,warpIm2,BicubicCoeff,u,v);
// estimate noise level
switch(noiseModel)
{
case GMixture:
estGaussianMixture(Im1,warpIm2,GMPara);
break;
case Lap:
estLaplacianNoise(Im1,warpIm2,LapPara);
}
}// end of outer fixed point iteration
delete rou;
}
//--------------------------------------------------------------------------------------------------------
// function to compute optical flow field using two fixed point iterations
// Input arguments:
// Im1, Im2: frame 1 and frame 2
// warpIm2: the warped frame 2 according to the current flow field u and v
// u,v: the current flow field, NOTICE that they are also output arguments
//
//--------------------------------------------------------------------------------------------------------
void OpticalFlow::SmoothFlowPDE(const DImage &Im1, const DImage &Im2, DImage &warpIm2, DImage &u, DImage &v,
double alpha, int nOuterFPIterations, int nInnerFPIterations, int nCGIterations)
{
DImage mask,imdx,imdy,imdt;
int imWidth,imHeight,nChannels,nPixels;
imWidth=Im1.width();
imHeight=Im1.height();
nChannels=Im1.nchannels();
nPixels=imWidth*imHeight;
DImage du(imWidth,imHeight),dv(imWidth,imHeight);
DImage uu(imWidth,imHeight),vv(imWidth,imHeight);
DImage ux(imWidth,imHeight),uy(imWidth,imHeight);
DImage vx(imWidth,imHeight),vy(imWidth,imHeight);
DImage Phi_1st(imWidth,imHeight);
DImage Psi_1st(imWidth,imHeight,nChannels);
DImage imdxy,imdx2,imdy2,imdtdx,imdtdy;
DImage ImDxy,ImDx2,ImDy2,ImDtDx,ImDtDy;
DImage A11,A12,A22,b1,b2;
DImage foo1,foo2;
// variables for conjugate gradient
DImage r1,r2,p1,p2,q1,q2;
double* rou;
rou=new double[nCGIterations];
double varepsilon_phi=pow(0.001,2);
double varepsilon_psi=pow(0.001,2);
//--------------------------------------------------------------------------
// the outer fixed point iteration
//--------------------------------------------------------------------------
for(int count=0; count<nOuterFPIterations; count++)
{
// compute the gradient
getDxs(imdx,imdy,imdt,Im1,warpIm2);
// generate the mask to set the weight of the pxiels moving outside of the image boundary to be zero
genInImageMask(mask,vx,vy);
// set the derivative of the flow field to be zero
du.reset();
dv.reset();
//--------------------------------------------------------------------------
// the inner fixed point iteration
//--------------------------------------------------------------------------
for(int hh=0; hh<nInnerFPIterations; hh++)
{
// compute the derivatives of the current flow field
if(hh==0)
{
uu.copyData(u);
vv.copyData(v);
}
else
{
uu.Add(u,du);
vv.Add(v,dv);
}
uu.dx(ux);
uu.dy(uy);
vv.dx(vx);
vv.dy(vy);
// compute the weight of phi
Phi_1st.reset();
double* phiData=Phi_1st.data();
double temp;
const double *uxData,*uyData,*vxData,*vyData;
uxData=ux.data();
uyData=uy.data();
vxData=vx.data();
vyData=vy.data();
for(int i=0; i<nPixels; i++)
{
temp=uxData[i]*uxData[i]+uyData[i]*uyData[i]+vxData[i]*vxData[i]+vyData[i]*vyData[i];
phiData[i]=1/(2*sqrt(temp+varepsilon_phi));
}
// compute the nonlinear term of psi
Psi_1st.reset();
double* psiData=Psi_1st.data();
const double *imdxData,*imdyData,*imdtData;
const double *duData,*dvData;
imdxData=imdx.data();
imdyData=imdy.data();
imdtData=imdt.data();
duData=du.data();
dvData=dv.data();
double _a = 10000, _b = 0.1;
if(nChannels==1)
{
for(int i=0; i<nPixels; i++)
{
temp=imdtData[i]+imdxData[i]*duData[i]+imdyData[i]*dvData[i];
//if(temp*temp<0.04)
psiData[i]=1/(2*sqrt(temp*temp+varepsilon_psi));
//psiData[i] = _a*_b/(1+_a*temp*temp);
}
}
else
{
for(int i=0; i<nPixels; i++)
for(int k=0; k<nChannels; k++)
{
int offset=i*nChannels+k;
temp=imdtData[offset]+imdxData[offset]*duData[i]+imdyData[offset]*dvData[i];
//if(temp*temp<0.04)
psiData[offset]=1/(2*sqrt(temp*temp+varepsilon_psi));
//psiData[offset] = _a*_b/(1+_a*temp*temp);
}
}
// prepare the components of the large linear system
ImDxy.Multiply(Psi_1st,imdx,imdy);
ImDx2.Multiply(Psi_1st,imdx,imdx);
ImDy2.Multiply(Psi_1st,imdy,imdy);
ImDtDx.Multiply(Psi_1st,imdx,imdt);
ImDtDy.Multiply(Psi_1st,imdy,imdt);
if(nChannels>1)
{
ImDxy.collapse(imdxy);
ImDx2.collapse(imdx2);
ImDy2.collapse(imdy2);
ImDtDx.collapse(imdtdx);
ImDtDy.collapse(imdtdy);
}
else
{
imdxy.copyData(ImDxy);
imdx2.copyData(ImDx2);
imdy2.copyData(ImDy2);
imdtdx.copyData(ImDtDx);
imdtdy.copyData(ImDtDy);
}
// filtering
imdx2.smoothing(A11,3);
imdxy.smoothing(A12,3);
imdy2.smoothing(A22,3);
// add epsilon to A11 and A22
A11.Add(alpha*0.1);
A22.Add(alpha*0.1);
// form b
imdtdx.smoothing(b1,3);
imdtdy.smoothing(b2,3);
// laplacian filtering of the current flow field
Laplacian(foo1,u,Phi_1st);
Laplacian(foo2,v,Phi_1st);
double *b1Data,*b2Data;
const double *foo1Data,*foo2Data;
b1Data=b1.data();
b2Data=b2.data();
foo1Data=foo1.data();
foo2Data=foo2.data();
for(int i=0; i<nPixels; i++)
{
b1Data[i]=-b1Data[i]-alpha*foo1Data[i];
b2Data[i]=-b2Data[i]-alpha*foo2Data[i];
}
// for debug only, displaying the matrix coefficients
//A11.imwrite("A11.bmp",ImageIO::normalized);
//A12.imwrite("A12.bmp",ImageIO::normalized);
//A22.imwrite("A22.bmp",ImageIO::normalized);
//b1.imwrite("b1.bmp",ImageIO::normalized);
//b2.imwrite("b2.bmp",ImageIO::normalized);
//-----------------------------------------------------------------------
// conjugate gradient algorithm
//-----------------------------------------------------------------------
r1.copyData(b1);
r2.copyData(b2);
du.reset();
dv.reset();
for(int k=0; k<nCGIterations; k++)
{
rou[k]=r1.norm2()+r2.norm2();
//cout<<rou[k]<<endl;
if(rou[k]<1E-10)
break;
if(k==0)
{
p1.copyData(r1);
p2.copyData(r2);
}
else
{
double ratio=rou[k]/rou[k-1];
p1.Add(r1,p1,ratio);
p2.Add(r2,p2,ratio);
}
// go through the large linear system
foo1.Multiply(A11,p1);
foo2.Multiply(A12,p2);
q1.Add(foo1,foo2);
Laplacian(foo1,p1,Phi_1st);
q1.Add(foo1,alpha);
foo1.Multiply(A12,p1);
foo2.Multiply(A22,p2);
q2.Add(foo1,foo2);
Laplacian(foo2,p2,Phi_1st);
q2.Add(foo2,alpha);
double beta;
beta=rou[k]/(p1.innerproduct(q1)+p2.innerproduct(q2));
du.Add(p1,beta);
dv.Add(p2,beta);
r1.Add(q1,-beta);
r2.Add(q2,-beta);
}
//-----------------------------------------------------------------------
// end of conjugate gradient algorithm
//-----------------------------------------------------------------------
}// end of inner fixed point iteration
// the following procedure is merely for debugging
//cout<<"du "<<du.norm2()<<" dv "<<dv.norm2()<<endl;
// update the flow field
u.Add(du,1);
v.Add(dv,1);
warpFL(warpIm2,Im1,Im2,u,v);
}// end of outer fixed point iteration
delete rou;
}