Ejemplo n.º 1
0
//--------------------------------------------------------------------------------------------------------
// 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();
		}
	}

}
Ejemplo n.º 2
0
//--------------------------------------------------------------------------------------------------------
// 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);
		}
	}

}
Ejemplo n.º 3
0
//--------------------------------------------------------------------------------------------------------
// 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;
}
Ejemplo n.º 4
0
//--------------------------------------------------------------------------------------------------------
// 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;
}