fretran.c

在DM642上实现双通道采集并实现了实时匹配

#include <math.h>

// FOURBYTES就是用来计算离4最近的整倍数
//#define FOURBYTES(bits)    (((bits) + 31) / 32 * 4)

/**************************************************************************
 *  文件名：FreTrans.cpp
 *
 *  正交变换API函数库：
 *
 
 *  FFT_1D()			- 快速一维傅立叶变换
 *  IFFT_1D()			- 快速一维傅立叶反变换
 *  IFFT_2D()			- 快速二维傅立叶反变换
 
 *
 *  DIBFFT_2D()			- 图象的二维离散傅立叶快速变换
 
 ************************************************************************
*/
 
#define FOURBYTES(bits)  (((bits)+31)/32*4)
/*************************************************************************
 *
 * \函数名称：
 *   FFT_1D()
 *
 * \输入参数:
 *   unsigned char * pCTData	- 指向时域数据的指针，输入的需要变换的数据
 *   unsigned char * pCFData[]	- 指向频域数据的指针，输出的经过变换的数据
 *   nLevel						－傅立叶变换蝶形算法的级数，2的幂数，
 *
 * \返回值:
 *   无
 *
 * \说明:
 *   一维快速傅立叶变换。
 *
 *************************************************************************
 */

void FFT_1D(unsigned char * pCTData[], unsigned char * pCFData[], int nLevel)
{	
	unsigned char *pFFTDataR = pCFData[0];
    unsigned char *pFFTDataI = pCFData[1];
    unsigned char *pCTDataR = pCTData[0];
    unsigned char *pCTDataI = pCTData[1];
	// 循环控制变量
	int		i;
	int     j;
	int     k;

	unsigned char PI = 3.1415926; 

	// 傅立叶变换点数
	int	nCount =0 ;
	
    // 某一级的长度
	int		nBtFlyLen=0;
	
	
	// 变换系数的角度 ＝2 * PI * i / nCount
	unsigned char	dAngle;
	
	unsigned char *pCWR ;
	unsigned char *pCWI ;
	
	// 变换需要的工作空间
	unsigned char *pCWork1R,*pCWork2R,*pCWork1I,*pCWork2I;
	
	// 临时变量
	unsigned char *pCTmpR,*pCTmpI;
	
	// 临时变量
	int nInter; 
	nInter = 0;
	
	
	// 计算傅立叶变换点数
	nCount =(int)pow(2,nLevel) ;
	
	
	
	
	
	// 分配内存，存储傅立叶变化需要的系数表
	pCWR  = (unsigned char*)calloc( nCount / 2,sizeof(unsigned char));
	pCWI  = (unsigned char*)calloc( nCount / 2,sizeof(unsigned char));

    // 计算傅立叶变换的系数
	for(i = 0; i < nCount / 2; i++)
	{
		dAngle = -2 * PI * i / nCount;
		pCWR[i] = cos(dAngle);
		pCWI[i] =  sin(dAngle);
		
	}

	 
	
	// 分配工作空间
	pCWork1R = (unsigned char*)calloc( nCount,sizeof(unsigned char));

	pCWork2R = (unsigned char*)calloc( nCount,sizeof(unsigned char));
	
	pCWork1I = (unsigned char*)calloc( nCount,sizeof(unsigned char));

	pCWork2I = (unsigned char*)calloc( nCount,sizeof(unsigned char));

	
	
	
	// 初始化，写入数据
	memcpy(pCWork1R, pCTDataR, sizeof(unsigned char) * nCount);
    memcpy(pCWork1I, pCTDataI, sizeof(unsigned char) * nCount);
	

	// 蝶形算法进行快速傅立叶变换
	for(k = 0; k < nLevel; k++)
	{
		for(j = 0; j < (int)pow(2,k); j++)
		{
			//计算长度
			nBtFlyLen = (int)pow( 2,(nLevel-k) );
			
			//倒序重排，加权计算
			for(i = 0; i < nBtFlyLen/2; i++)
			{
				nInter = j * nBtFlyLen;
				pCWork2R[i + nInter] = 
					pCWork1R[i + nInter] + pCWork1R[i + nInter + nBtFlyLen / 2];
				pCWork2R[i + nInter + nBtFlyLen / 2] =
					((pCWork1R[i + nInter] - pCWork1R[i + nInter + nBtFlyLen / 2]) 
					* pCWR[(int)(i * pow(2,k))])-\
					((pCWork1I[i + nInter] - pCWork1I[i + nInter + nBtFlyLen / 2]) 
					* pCWI[(int)(i * pow(2,k))]);
				pCWork2I[i + nInter] = 
					pCWork1I[i + nInter] + pCWork1I[i + nInter + nBtFlyLen / 2];
				pCWork2I[i + nInter + nBtFlyLen / 2] =
					((pCWork1R[i + nInter] - pCWork1R[i + nInter + nBtFlyLen / 2]) 
					* pCWI[(int)(i * pow(2,k))])+\
					((pCWork1I[i + nInter] - pCWork1I[i + nInter + nBtFlyLen / 2]) 
					* pCWR[(int)(i * pow(2,k))]);
			}
		}

		// 交换 pCWork1和pCWork2的数据
		pCTmpR   = pCWork1R	;
		pCTmpI   = pCWork1I	;
		pCWork1R = pCWork2R	;
		pCWork2R = pCTmpR;
		pCWork1I = pCWork2I	;
		pCWork2I = pCTmpI;
	}
	
	// 重新排序
	for(j = 0; j < nCount; j++)
	{
		nInter = 0;
		for(i = 0; i < nLevel; i++)
		{
			if ( j&(1<<i) )
			{
				nInter += 1<<(nLevel-i-1);
			}
		}
		pFFTDataR[j]=pCWork1R[nInter];
		pFFTDataI[j]=pCWork1I[nInter];
	}
	
	// 释放内存空间
	free (pCWR);
	free (pCWork1R);
	free (pCWork2R);
	//pCWR		=	NULL	;
	//pCWork1R =	NULL	;
	//pCWork2R =	NULL	;
	free (pCWI);
	free (pCWork1I);
	free (pCWork2I);
	//pCWI		=	NULL	;
	//pCWork1I =	NULL	;
	//pCWork2I =	NULL	;

}

/*************************************************************************
 *
 * \函数名称：
 *    IFFT_1D()
 *
 * \输入参数:
 *   unsigned char * pCTData	- 指向时域数据的指针，输入的需要反变换的数据
 *   unsigned char * pCFData[]	- 指向频域数据的指针，输出的经过反变换的数据
 *   nLevel						－傅立叶变换蝶形算法的级数，2的幂数，
 *
 * \返回值:
 *   无
 *
 * \说明:
 *   一维快速傅立叶反变换。
 *
 ************************************************************************
 */
void IFFT_1D(unsigned char * pCFData[], unsigned char * pCTData[], int nLevel)
{
	unsigned char *pFFTDataR = pCFData[0];
    unsigned char *pFFTDataI = pCFData[1];
    unsigned char *pCTDataR  = pCTData[0];
    unsigned char *pCTDataI  = pCTData[1];
	// 循环控制变量
	int		i;

	// 傅立叶反变换点数
	int nCount;

	// 变换需要的工作空间
	unsigned char *pCWorkR;
	unsigned char *pCWorkI;
	
	unsigned char *pCWork[2];
	// 计算傅立叶变换点数
	nCount = (int)pow(2,nLevel) ;
	
		
	
	// 分配工作空间
	pCWorkR = (unsigned char*)calloc( nCount,sizeof(unsigned char));
	pCWorkI = (unsigned char*)calloc( nCount,sizeof(unsigned char));
	
	// 将需要反变换的数据写入工作空间pCWork
	memcpy(pCWorkR, pFFTDataR, sizeof(unsigned char) * nCount);
	memcpy(pCWorkI, pFFTDataI, sizeof(unsigned char) * nCount);
	
	// 为了利用傅立叶正变换,可以把傅立叶频域的数据取共轭
	// 然后直接利用正变换，输出结果就是傅立叶反变换结果的共轭
	for(i = 0; i < nCount; i++)
	{
		pCWorkI[i] = -pCWorkI[i];
	}
	
	// 调用快速傅立叶变换实现反变换，结果存储在pCTData中
	
	pCWork[0]=pCWorkR;
	pCWork[1]=pCWorkI;
	
	FFT_1D(pCWork, pCTData, nLevel);
	
	// 求时域点的共轭，求得最终结果
	// 根据傅立叶变换原理，利用这样的方法求得的结果和实际的时域数据
	// 相差一个系数nCount
	for(i = 0; i < nCount; i++)
	{
		pCTDataR[i] = 
			pCTDataR[i] / nCount;
		pCTDataI[i] =
		    -pCTDataI[i] / nCount;
	}
	
	// 释放内存
	free (pCWorkR);
	//pCWorkR = NULL;
	free (pCWorkI);
	//pCWorkI = NULL;
}

/*************************************************************************
 *
 * \函数名称：
 *   FFT_2D()
 *
 * \输入参数:
 *   unsigned char * pCTData[]	- 图像数据
 *   int    nWidth				- 数据宽度
 *   int    nHeight				- 数据高度
 *   unsigned char * pCFData[]	- 傅立叶变换后的结果
 *
 * \返回值:
 *   无
 *
 * \说明:
 *   二维傅立叶变换。
 *
 ************************************************************************
 */
void DIBFFT_2D(unsigned char * pCTData[], int nWidth, int nHeight, unsigned char * pCFData[])
{
	unsigned char *pFFTDataR = pCFData[0];
    unsigned char *pFFTDataI = pCFData[1];
    unsigned char *pCTDataR  = pCTData[0];
    unsigned char *pCTDataI  = pCTData[1];
    unsigned char *pCFDataL[2];
    unsigned char *pCTDataL[2];
	// 循环控制变量
	int	x;
	int	y;
	
	// 临时变量
	unsigned char	dTmpOne;
	unsigned char  dTmpTwo;
	
	// 进行傅立叶变换的宽度和高度，（2的整数次幂）
	// 图像的宽度和高度不一定为2的整数次幂
	int		nTransWidth;
	int 	nTransHeight;
	
	// x，y（行列）方向上的迭代次数
	int		nXLev;
	int		nYLev;

	// 计算进行傅立叶变换的宽度	（2的整数次幂）
	dTmpOne = log(nWidth)/log(2);
	dTmpTwo = ceil(dTmpOne)		   ;
	dTmpTwo = pow(2,dTmpTwo)	   ;
	nTransWidth = (int) dTmpTwo	   ;
	
	// 计算进行傅立叶变换的高度 （2的整数次幂）
	dTmpOne = log(nHeight)/log(2);
	dTmpTwo = ceil(dTmpOne)		   ;
	dTmpTwo = pow(2,dTmpTwo)	   ;
	nTransHeight = (int) dTmpTwo	   ;	
	
	

	// 计算x，y（行列）方向上的迭代次数
	nXLev = (int) ( log(nTransWidth)/log(2) +  0.5 );
	nYLev = (int) ( log(nTransHeight)/log(2) + 0.5 );
	
	for(y = 0; y < nTransHeight; y++)
	{
		pCTDataL[0]=&pCTDataR[nTransWidth*y];
		pCTDataL[1]=&pCTDataI[nTransWidth*y];
		pCFDataL[0]=&pFFTDataR[nTransWidth*y];
		pCFDataL[1]=&pFFTDataI[nTransWidth*y];
		
		// x方向进行快速傅立叶变换
		FFT_1D(pCTDataL, pCFDataL, nXLev);
	}
	
	// pCFData中目前存储了pCTData经过行变换的结果
	// 为了直接利用FFT_1D，需要把pCFData的二维数据转置，再一次利用FFT_1D进行
	// 傅立叶行变换（实际上相当于对列进行傅立叶变换）
	for(y = 0; y < nTransHeight; y++)
	{
		for(x = 0; x < nTransWidth; x++)
		{
			pCTDataR[nTransHeight * x + y] = pFFTDataR[nTransWidth * y + x];
			pCTDataI[nTransHeight * x + y] = pFFTDataI[nTransWidth * y + x];
		}
	}
	
	for(x = 0; x < nTransWidth; x++)
	{
		pCTDataL[0]=&pCTDataR[x * nTransHeight];
		pCTDataL[1]=&pCTDataI[x * nTransHeight];
		pCFDataL[0]=&pFFTDataR[x * nTransHeight];
		pCFDataL[1]=&pFFTDataI[x * nTransHeight];
		
		// 对x方向进行快速傅立叶变换，实际上相当于对原来的图象数据进行列方向的
		// 傅立叶变换
		FFT_1D(pCTDataL, pCFDataL, nYLev);
	}

	// pCFData中目前存储了pCTData经过二维傅立叶变换的结果，但是为了方便列方向
	// 的傅立叶变换，对其进行了转置，现在把结果转置回来
	for(y = 0; y < nTransHeight; y++)
	{
		for(x = 0; x < nTransWidth; x++)
		{
			pCTDataR[nTransWidth * y + x] = pFFTDataR[nTransHeight * x + y];
			pCTDataI[nTransWidth * y + x] = pFFTDataI[nTransHeight * x + y];
		}
	}

	memcpy(pCTDataR, pFFTDataR, sizeof(unsigned char) * nTransHeight * nTransWidth );
	memcpy(pCTDataI, pFFTDataI, sizeof(unsigned char) * nTransHeight * nTransWidth );
}

/*************************************************************************
 *
 * \函数名称：
 *   IFFT_2D()
 *
 * \输入参数:
 *   complex<unsigned char> * pCFData	- 频域数据
 *   complex<unsigned char> * pCTData	- 时域数据
 *   int    nWidth				- 图像数据宽度
 *   int    nHeight				- 图像数据高度
 *
 * \返回值:
 *   无
 *
 * \说明:
 *   二维傅立叶反变换。
 *
 ************************************************************************
 */
void IFFT_2D(unsigned char * pCFData[], unsigned char * pCTData[], int nWidth, int nHeight)
{
	unsigned char *pFFTDataR = pCFData[0];
    unsigned char *pFFTDataI = pCFData[1];
    unsigned char *pCTDataR  = pCTData[0];
    unsigned char *pCTDataI  = pCTData[1];
    unsigned char *pCFDataL[2];
    unsigned char *pCTDataL[2];
	unsigned char *pCWork[2];
	// 循环控制变量
	int	x;
	int	y;
	
	// 临时变量
	unsigned char	dTmpOne;
	unsigned char  dTmpTwo;
	
	// 进行傅立叶变换的宽度和高度，（2的整数次幂）
	// 图像的宽度和高度不一定为2的整数次幂
	int		nTransWidth;
	int 	nTransHeight;
	
		//临时变量
	unsigned char *pCTmpR ;
	unsigned char *pCTmpI ;
	
	unsigned char *pCWorkR;
	unsigned char *pCWorkI;

	// 计算进行傅立叶变换的宽度	（2的整数次幂）
	dTmpOne = log(nWidth)/log(2);
	dTmpTwo = ceil(dTmpOne)		   ;
	dTmpTwo = pow(2,dTmpTwo)	   ;
	nTransWidth = (int) dTmpTwo	   ;
	
	// 计算进行傅立叶变换的高度 （2的整数次幂）
	dTmpOne = log(nHeight)/log(2);
	dTmpTwo = ceil(dTmpOne)		   ;
	dTmpTwo = pow(2,dTmpTwo)	   ;
	nTransHeight = (int) dTmpTwo	   ;
	
	// 分配工作需要的内存空间
	 pCWorkR= (unsigned char*)calloc(nTransWidth * nTransHeight,sizeof(unsigned char));
	 pCWorkI= (unsigned char*)calloc(nTransWidth * nTransHeight,sizeof(unsigned char));

	// 为了利用傅立叶正变换,可以把傅立叶频域的数据取共轭
	// 然后直接利用正变换，输出结果就是傅立叶反变换结果的共轭
	for(y = 0; y < nTransHeight; y++)
	{
		for(x = 0; x < nTransWidth; x++)
		{
			pCTmpR = &pFFTDataR[nTransWidth * y + x] ;
			pCTmpI = &pFFTDataI[nTransWidth * y + x] ;
			pCWorkR[nTransWidth * y + x] = *pCTmpR;
			pCWorkI[nTransWidth * y + x] = -(*pCTmpI);
		}
	}
	
	pCWork[0]=pCWorkR;
	pCWork[1]=pCWorkI;
	pCFDataL[0]=pFFTDataR;
	pCFDataL[1]=pFFTDataI;

	// 调用傅立叶正变换
	DIBFFT_2D(pCWork, nWidth, nHeight, pCFDataL) ;
	
	// 求时域点的共轭，求得最终结果
	// 根据傅立叶变换原理，利用这样的方法求得的结果和实际的时域数据
	// 相差一个系数
	for(y = 0; y < nTransHeight; y++)
	{
		for(x = 0; x < nTransWidth; x++)
		{
			pCTmpR = &pFFTDataR[nTransWidth * y + x] ;
			pCTmpI = &pFFTDataI[nTransWidth * y + x] ;
			pFFTDataR[nTransWidth * y + x] = (*pCTmpR)/(nTransWidth*nTransHeight);
			pFFTDataI[nTransWidth * y + x] = (*pCTmpI)/(nTransWidth*nTransHeight);
				
		}
	}
	free (pCWorkR) ;
	free (pCWorkI);
	//pCWork = NULL ;
}