基于KL的光流算法C语言代码

此处调用的头文件在前几次上传的附件中。

#include <math.h>
#include <iostream> 
#include <string.h>
#include "matrix.h"
#include <time.h>
#include <float.h>
#include "imageBase.h"

typedef short deriv_type ;
void pyrDown2Extract(unsigned char *src,unsigned char *dst,int width,int height)
{
	int i,j;
	int dHeight,dWidth;
	
	dHeight = height >> 1;
	dWidth = width >> 1;

	for(i = 0;i < dHeight;i++)
	{
		for(j = 0;j < dWidth;j++)
		{
			dst[i * dWidth + j] = src[2 * i * width + 2 * j];
		}
	}
}
int boarderInterpolate(int colIndex,int num)
{
	if(colIndex < 0)
	{
		return (-colIndex - 1);
	}
	else if(colIndex < num)
	{
		return colIndex;
	}
	else
	{
		return (2 * num - colIndex - 1);
	}
}
void pyrDownGauss2Extract(unsigned char *_src,unsigned char *_dst,int swidth,int sheight)
{
	const int PD_SZ = 5;
	int dWidth = swidth >> 1;
	int dHeight = sheight >> 1;
	int cn = 1;
	int bufstep = dWidth;

	unsigned short _buf[320 * 5 + 6];
	unsigned short * buf = _buf;
	unsigned short tabL[16],tabR[16];
	unsigned short *rows[PD_SZ];
	int k,x,sy0 = -PD_SZ/2,sy = sy0,width0 = dWidth - 1;

	tabL[0] = 1,tabL[1] = 0,tabL[2] = 0,tabL[3] = 1,tabL[4] = 2,tabL[5] = 3,tabL[6] = 4;
	x = (width0 << 1) + sy0;
	tabR[0] = x + 0;
	tabR[1] = x + 1;
	tabR[2] = x + 2;
	tabR[3] = x + 3;
	tabR[4] = x + 3;
	tabR[5] = x + 2;
	tabR[6] = x + 1;

	for(int y = 0; y < dHeight;y++)
	{
		unsigned char *dst = (unsigned char *)(_dst + dWidth * y);
		unsigned short *row0,*row1,*row2,*row3,*row4;

		for(;sy < y * 2 + 2;sy++)
		{
			unsigned short *row = buf + ((sy - sy0)%PD_SZ) * bufstep;
			int _sy = boarderInterpolate(sy,sheight);
			const unsigned char *src = (const unsigned char *)(_src + swidth * _sy);
			int limit = cn;
			const unsigned short * tab = tabL;

			for(x = 0;;)
			{
				for(;x < limit;x++)
				{
					row[x] = src[tab[x + 2]] * 6 + (src[tab[x + 1]] + src[tab[x + 3]]) * 4 
						+ src[tab[x + 0]] + src[tab[x + 4]];
				}
				if(x == dWidth)
					break;

				for(; x < width0;x++)
				{
					row[x] = src[x * 2] * 6 + (src[x * 2 - 1] + src[x * 2 + 1]) * 4 + src[x * 2 - 2] + src[x * 2 + 2];
				}
				limit = dWidth;
				tab = tabR - x;
			}
		}
		for(k = 0;k < PD_SZ;k++)
		{
			rows[k] = buf + ((y * 2 - PD_SZ/2 + k - sy0) % PD_SZ) * bufstep;
		}
		row0 = rows[0];
		row1 = rows[1];
		row2 = rows[2];
		row3 = rows[3];
		row4 = rows[4];
		x = 0;

		for(x = 0;x < dWidth;x++)
		{
			dst[x] = (row2[x] * 6 + (row1[x] + row3[x]) * 4 + row0[x] + row4[x]) >> 8;
		}
	}
	
		
}

void getImageDeriv(unsigned char *src,short *dst,int width,int height)
{
	int rows = width;
	int cols = height;
	int cn = 1;
	int colsn = cols;
	int x,y;
	int delta = (cols + 2) * cn;
	
	short *tmpBuf = (short *)malloc((colsn + 2) * 2 * sizeof(short));
	short *trow0 = &tmpBuf[cn],*trow1 = &tmpBuf[delta];
	short *drow;
	const unsigned char *srow0;
	const unsigned char *srow1;
	const unsigned char *srow2;

	for(y = 0;y < rows;y++)
	{
		if(y > 0)
		{
			srow0 = &src[(y - 1) * colsn];
		}
		else
		{
			srow0 = &src[1 * colsn];
		}
		srow1 = &src[y * colsn];
		if(y < (rows - 1))
		{
			srow2 = &src[(y + 1) * colsn];
		}
		else
		{
			srow2 = &src[(y - 1) * colsn];
		}

		for(x = 0; x < colsn;x++)
		{
			int t0 = (srow0[x] + srow2[x]) * 3 + srow1[x] * 10;
			int t1 = srow2[x] - srow0[x];
			trow0[x] = (short)t0;
			trow1[x] = (short)t1;
		}
		int x0 = 1;
		int x1 = cols - 2;

		trow0[-cn] = trow0[x0];trow0[colsn] = trow0[x0];
		trow1[-cn] = trow1[x0];trow1[colsn] = trow1[x0];
		drow = &dst[y * colsn * 2];

		for(x = 0;x < colsn;x++)
		{
			short t0 = (short)(trow0[x + cn] - trow0[x - cn]);
			short t1 = (short)((trow1[x + cn] + trow1[x - cn]) * 3 + trow1[x] * 10);
			drow[x * 2 + 0] = t0;
			drow[x * 2 + 1] = t1;
		}
		
	}
	free(tmpBuf);
}
void pyrLKIlterator(const unsigned char * 	prevImg,
					short * 				prevDeriv,
					const unsigned char * 	nextImg,
					int   					src_width,
					int						src_hight,
					const vect2f_t * 		prevPts,
					vect2f_t	*			nextPts,
					int						nPoint,
					char *					status,
					float *					err,
					SIZE_t					winSize,
					int						maxCount,
					float					epsilon,
					int						level,
					int						maxLevel,
					int						flags,
					float					minEigenThreshold)
{
	vect2f_t halfWin = {(winSize.width - 1) * 0.5f, (winSize.height - 1) * 0.5f};
    const unsigned char * I = (const unsigned char *)prevImg;
    const unsigned char * J = (const unsigned char *)nextImg;
    const short * derivI = (const short *)prevDeriv;

    int j = 0, cn = 1, cn2 = cn*2;
    deriv_type * _buf = (deriv_type *)malloc(winSize.area * (cn + cn2) * sizeof(deriv_type));
	deriv_type * IWinBuf = _buf;
	deriv_type * derivIWinBuf = _buf + winSize.area;
	float levelScale = (float)(1.0/(1 << level));
	
    for( int ptidx = 0; ptidx < nPoint; ptidx++ )
    {
        vect2f_t prevPt;
        vect2f_t nextPt;
		prevPt.x = prevPts[ptidx].x * levelScale;
		prevPt.y = prevPts[ptidx].y * levelScale;
		
        if( level == maxLevel )
        {
            if( flags & 0x80u )
            {
                nextPt.x = nextPts[ptidx].x * levelScale;
				nextPt.y = nextPts[ptidx].y * levelScale;
            }
            else
            {
                nextPt.x = prevPt.x;
				nextPt.y = prevPt.y;
            }
        }
        else
        {
            nextPt.x = nextPts[ptidx].x * 2.0f;
			nextPt.y = nextPts[ptidx].y * 2.0f;
        }
        nextPts[ptidx] = nextPt;

        vect2i_t iprevPt, inextPt;
        prevPt.x -= halfWin.x;
		prevPt.y -= halfWin.y;
		
        iprevPt.x = mvFloor(prevPt.x);
        iprevPt.y = mvFloor(prevPt.y);

        if( iprevPt.x < -winSize.width || iprevPt.x >= src_width ||
            iprevPt.y < -winSize.height || iprevPt.y >= src_hight )
        {
            if( level == 0 )
            {
                if( status )
                    status[ptidx] = false;
                if( err )
                    err[ptidx] = 0;
            }
            continue;
        }

        float a = prevPt.x - iprevPt.x;
        float b = prevPt.y - iprevPt.y;
        const int W_BITS = 14, W_BITS1 = 14;
        const float FLT_SCALE = 1.f/(1 << 20);
        int iw00 = mvRound((1.f - a)*(1.f - b)*(1 << W_BITS));
        int iw01 = mvRound(a*(1.f - b)*(1 << W_BITS));
        int iw10 = mvRound((1.f - a)*b*(1 << W_BITS));
        int iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;

        int dstep = src_width * 2;
        int stepI = src_width * 2;
        int stepJ = src_width * 2;
        float A11 = 0, A12 = 0, A22 = 0;

        // extract the patch from the first image, compute covariation matrix of derivatives
        int x, y;
        for( y = 0; y < winSize.height; y++ )
        {
            const unsigned char * src = (const unsigned char*)I + (y + iprevPt.y)*stepI + iprevPt.x*cn;
            const deriv_type* dsrc = (const deriv_type*)derivI + (y + iprevPt.y)*dstep + iprevPt.x*cn2;

            deriv_type* Iptr = (deriv_type*)(IWinBuf + y * winSize.width);
            deriv_type* dIptr = (deriv_type*)(derivIWinBuf + y * winSize.width * 2);

            x = 0;

            for( ; x < winSize.width*cn; x++, dsrc += 2, dIptr += 2 )
            {
                int ival = MV_DESCALE(src[x]*iw00 + src[x+cn]*iw01 +
                                      src[x+stepI]*iw10 + src[x+stepI+cn]*iw11, W_BITS1-5);
                int ixval = MV_DESCALE(dsrc[0]*iw00 + dsrc[cn2]*iw01 +
                                       dsrc[dstep]*iw10 + dsrc[dstep+cn2]*iw11, W_BITS1);
                int iyval = MV_DESCALE(dsrc[1]*iw00 + dsrc[cn2+1]*iw01 + dsrc[dstep+1]*iw10 +
                                       dsrc[dstep+cn2+1]*iw11, W_BITS1);

                Iptr[x] = (short)ival;
                dIptr[0] = (short)ixval;
                dIptr[1] = (short)iyval;

                A11 += (float)(ixval*ixval);
                A12 += (float)(ixval*iyval);
                A22 += (float)(iyval*iyval);
            }
        }


        A11 *= FLT_SCALE;
        A12 *= FLT_SCALE;
        A22 *= FLT_SCALE;

        float D = A11*A22 - A12*A12;
        float minEig = (A22 + A11 - sqrt((A11-A22)*(A11-A22) +
                        4.f*A12*A12))/(2*winSize.width*winSize.height);

        if( err && (flags & 0x40u) != 0 )
            err[ptidx] = (float)minEig;

        if( minEig < minEigenThreshold || D < FLT_EPSILON )
        {
            if( level == 0 && status )
                status[ptidx] = false;
            continue;
        }

        D = 1.f/D;

        nextPt.x -= halfWin.x;
		nextPt.y -= halfWin.y;
        vect2f_t prevDelta;

        for( j = 0; j < maxCount; j++ )
        {
            inextPt.x = mvFloor(nextPt.x);
            inextPt.y = mvFloor(nextPt.y);

            if( inextPt.x < -winSize.width || inextPt.x >= src_width ||
               inextPt.y < -winSize.height || inextPt.y >= src_hight )
            {
                if( level == 0 && status )
                    status[ptidx] = false;
                break;
            }

            a = nextPt.x - inextPt.x;
            b = nextPt.y - inextPt.y;
            iw00 = mvRound((1.f - a)*(1.f - b)*(1 << W_BITS));
            iw01 = mvRound(a*(1.f - b)*(1 << W_BITS));
            iw10 = mvRound((1.f - a)*b*(1 << W_BITS));
            iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
            float b1 = 0, b2 = 0;

            for( y = 0; y < winSize.height; y++ )
            {
                const unsigned char * Jptr = (const unsigned char *)J + (y + inextPt.y)*stepJ + inextPt.x*cn;
                const deriv_type* Iptr = (const deriv_type*)(IWinBuf + y * winSize.width);
                const deriv_type* dIptr = (const deriv_type*)(derivIWinBuf + y*winSize.width * 2);

                x = 0;

                for( ; x < winSize.width*cn; x++, dIptr += 2 )
                {
                    int diff = MV_DESCALE(Jptr[x]*iw00 + Jptr[x+cn]*iw01 +
                                          Jptr[x+stepJ]*iw10 + Jptr[x+stepJ+cn]*iw11,
                                          W_BITS1-5) - Iptr[x];
                    b1 += (float)(diff*dIptr[0]);
                    b2 += (float)(diff*dIptr[1]);
                }
            }


            b1 *= FLT_SCALE;
            b2 *= FLT_SCALE;

            vect2f_t delta = { (float)((A12*b2 - A22*b1) * D),
                          (float)((A12*b1 - A11*b2) * D)};
            //delta = -delta;

            nextPt.x += delta.x;
			nextPt.y += delta.y;
			
            nextPts[ptidx].x = nextPt.x + halfWin.x;
			nextPts[ptidx].y = nextPt.y + halfWin.y;
			
            if((delta.x * delta.x + delta.y * delta.y) <= epsilon )
                break;

            if( j > 0 && fabs(delta.x + prevDelta.x) < 0.01 &&
               fabs(delta.y + prevDelta.y) < 0.01 )
            {
                nextPts[ptidx].x -= delta.x * 0.5f;
				nextPts[ptidx].y -= delta.y * 0.5f;
                break;
            }
            prevDelta = delta;
        }

        if( status[ptidx] && err && level == 0 && (flags & 8) == 0 )
        {
            vect2f_t nextPoint = {nextPts[ptidx].x - halfWin.x,nextPts[ptidx].y - halfWin.y};
            vect2i_t inextPoint;

            inextPoint.x = mvFloor(nextPoint.x);
            inextPoint.y = mvFloor(nextPoint.y);

            if( inextPoint.x < -winSize.width || inextPoint.x >= src_width ||
                inextPoint.y < -winSize.height || inextPoint.y >= src_hight )
            {
                if( status )
                    status[ptidx] = false;
                continue;
            }

            float aa = nextPoint.x - inextPoint.x;
            float bb = nextPoint.y - inextPoint.y;
            iw00 = mvRound((1.f - aa)*(1.f - bb)*(1 << W_BITS));
            iw01 = mvRound(aa*(1.f - bb)*(1 << W_BITS));
            iw10 = mvRound((1.f - aa)*bb*(1 << W_BITS));
            iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
            float errval = 0.f;

            for( y = 0; y < winSize.height; y++ )
            {
                const unsigned char * Jptr = (const unsigned char*)J + (y + inextPoint.y)*stepJ + inextPoint.x*cn;
                const deriv_type* Iptr = (const deriv_type*)(IWinBuf + y*winSize.width);

                for( x = 0; x < winSize.width*cn; x++ )
                {
                    int diff = MV_DESCALE(Jptr[x]*iw00 + Jptr[x+cn]*iw01 +
                                          Jptr[x+stepJ]*iw10 + Jptr[x+stepJ+cn]*iw11,
                                          W_BITS1-5) - Iptr[x];
                    errval += fabs((float)diff);
                }
            }
            err[ptidx] = errval * 1.f/(32*winSize.width*cn*winSize.height);
        }
    }

	free(_buf);
}
int calcJitterPyrLK(const unsigned char * 	prevImg,
					const unsigned char * 	nextImg,
					int   					src_width,
					int						src_hight,
					vect2f_t * 				prevPts,
					vect2f_t	*			nextPts,
					int						nPoint,
					SIZE_t					winSize,
					int						maxCount,
					float					maxVal,
					float					epsilon,
					int						level,
					int						maxLevel,
					int						flags)
{
	int 		i,validCnt = 0;
	int			srcImgSize		= 		src_width * src_hight;
	int			levelAllSize	=		srcImgSize + (srcImgSize >> 2) + (srcImgSize >> 4);
	unsigned char * prevImgData	=		(unsigned char *)prevImg;
	unsigned char * nextImgData	=		(unsigned char *)nextImg;
	int			levelWidth,levelHeight;
	unsigned int 	levelSize;
	short * DerivI;
	float *flow_err = (float *)malloc(nPoint * sizeof(float));
	char *status = (char *)malloc(nPoint);
	short *g_difxy = (short *)malloc((src_width * src_hight) * sizeof(short));
	vect2f_t * gPrevPts = (vect2f_t *)malloc(nPoint * sizeof(vect2f_t));
	vect2f_t * gNextPts = (vect2f_t *)malloc(nPoint * sizeof(vect2f_t));
	
	pyrDownGauss2Extract(prevImgData,prevImgData + srcImgSize,src_width,src_hight);
	pyrDownGauss2Extract(prevImgData + srcImgSize,prevImgData + srcImgSize + (srcImgSize >> 2),src_width >> 1,src_hight >> 1);

	pyrDownGauss2Extract(nextImgData,nextImgData + srcImgSize,src_width,src_hight);
	pyrDownGauss2Extract(nextImgData + srcImgSize,nextImgData + srcImgSize + (srcImgSize >> 2),src_width >> 1,src_hight >> 1);

	memset(status,0x01,sizeof(status));
	memset(flow_err,0x00,nPoint * sizeof(float));

	for(i = level - 1;i >= 0;i--)
	{
		levelWidth = (src_width >> 1);
		levelHeight = (src_hight >> 1);
		levelSize = levelWidth * levelHeight;
		levelAllSize = levelAllSize - levelSize;

		DerivI = g_difxy;
		getImageDeriv(prevImgData + levelAllSize,DerivI,levelWidth,levelHeight);

		pyrLKIlterator(prevImgData + levelAllSize,
			DerivI,
			nextImgData + levelAllSize,
			levelWidth,levelHeight,
			gPrevPts,gNextPts,nPoint,status,NULL,winSize,maxCount,epsilon,i,maxLevel - 1,0,0.01f);
	}
	for(i = 0;i < nPoint;i++)
	{
		float x = fabs(gNextPts[i].x - gPrevPts[i].x);
		float y = fabs(gNextPts[i].y - gPrevPts[i].y);

		if(status[i] && (x < maxVal) && (y < maxVal))
		{
			prevPts[validCnt].x = gPrevPts[i].x;
			prevPts[validCnt].y = gPrevPts[i].y;
			nextPts[validCnt].x = gNextPts[i].x;
			nextPts[validCnt].y = gNextPts[i].y;
			validCnt++;
		}
		
	}


	free(flow_err);
	free(status);
	free(g_difxy);
	free(gPrevPts);
	free(gNextPts);

	return validCnt;

}