camengine.cpp

摄像头控制引擎（内含畸变矫正算法） 本程序是在VC++6.0环境下开发的

// CamEngine.cpp : Defines the entry point for the DLL application.
//

#include "stdafx.h"
#include "CamEngine.h"
#include "math.h"

#define SAFE_RELEASE(x) { if (x) x->Release(); x = NULL; }
#define PI      3.1415926

IVideoWindow  * g_pVW = NULL;
IMediaControl * g_pMC = NULL;
IMediaEventEx * g_pME = NULL;
IGraphBuilder *g_pGraph = NULL;
ICaptureGraphBuilder2 *g_pCapture = NULL;
IBaseFilter *  g_pCap;
ISampleGrabber *g_pGrabber;
IAMStreamConfig *g_pConfig;



AM_MEDIA_TYPE g_StillMediaType;

BYTE *g_pBuffer = NULL;
VIDEOINFOHEADER *gvih = NULL;
BOOL   g_bOneShot = FALSE; 

#define SRCIMGW	640
#define SRCIMGH	480
#define RSTIMGW	256
#define RSTIMGH	360



BYTE g_Buffer[SRCIMGW*SRCIMGH];
BYTE g_Result[RSTIMGW*RSTIMGH];

int SaveBitmap(unsigned char *lpBits, int  width, int height, LPCTSTR lpszPathName)
{
	BITMAPFILEHEADER   bf;
	BITMAPINFOHEADER   bi;
	HFILE              hf;
	BYTE palatte[256*4];
	int i =0;

	//构造bmp文件头信息
	bf.bfType=19778;
	bf.bfSize=width*height+1078;
	bf.bfReserved1=0;
	bf.bfReserved2=0;
	bf.bfOffBits=1078;
	bi.biSize=40;
	bi.biHeight=height;
	bi.biWidth=width;
	bi.biPlanes=1;
	bi.biBitCount=8;
	bi.biCompression=0;
	bi.biSizeImage=width*height;
	bi.biXPelsPerMeter=0;
	bi.biYPelsPerMeter=0;
	bi.biClrUsed=256;
	bi.biClrImportant=0;
	for(i=0; i < 256; i++)
	{
		palatte[i*4]   = i;
		palatte[i*4+1] = i;
		palatte[i*4+2] = i;
		palatte[i*4+3] = 0;
	}
	
	hf = _lcreat(lpszPathName,0);
	_lwrite(hf,(LPSTR)&bf,sizeof(BITMAPFILEHEADER)); 
	_lwrite(hf,(LPSTR)&bi,sizeof(BITMAPINFOHEADER)); 
	_lwrite(hf,(LPSTR)palatte,1024); 
	_lwrite(hf,(LPSTR)lpBits, width*height);
	_lclose(hf);
	
	return 0; 
}

double computeY(double oldI, double W0, double W1, double TH)
{
	
	double oldTH = 15 * 1.414, u = 15;

	double RH = RSTIMGH, SH = SRCIMGH;

	double sina = 1.414 / 2, cosa = 1.414 / 2;

	double oY = SH * (W0 / (W0 + W1));

	double f = u / 2;//u - oldTH * cosa * (W1 / (W0 - W1));

	double u_f = (W1 / (W0 - W1) ) * oldTH * cosa;

	double a_b = oldTH * sina / 2 * f * (1 / (u_f + oldTH * cosa) + 1 / u_f);


	double oldY = ((oldI - RH / 2) / RH) * oldTH;

	double newY = - oldY * sina * f / (u_f + oldTH * sina / 2 - oldY * cosa);
	
	double newI = newY * SH / (a_b) + oY;

	newI = SH - newI;

	return newI;		

}
/*
		P2	  W1	P3
		-------------        ----
	   /             \
	  /               \      TH
	 /                 \
	/                   \
   -----------------------   ----
  P0          W0         P1

BMP图像左下角为坐标原点


	P0_X       P0横坐标
	P0_Y       P0纵坐标
	P1_X       P1横坐标
	P1_Y       P1纵坐标
	P2_X       P2横坐标
	P2_Y       P2纵坐标
	P3_X       P3横坐标
	P4_Y       P3纵坐标

*/
//矫正函数（源图指针，矫正图指针，参数指针）
void RectifyFunc(unsigned char *lpSrcBitmap, unsigned char *lpRstBitmap, int *pParam)
{
	int P0_X = pParam[0];
	int P0_Y = pParam[1];
	int P1_X = pParam[2];
	int P1_Y = pParam[3];
	int P2_X = pParam[4];
	int P2_Y = pParam[5];
	int P3_X = pParam[6];
	int P3_Y = pParam[7];
	unsigned short	YTable[RSTIMGH];//RSTIMGH=360
	int W0  = P1_X-P0_X;
	int W1  = P3_X-P2_X;
	int TH	= P2_Y-P0_Y;



	int i, j;
	double oldy;
	int		iy;
    
	for(i = 0; i < RSTIMGH; i++)//0--360
	{	
		oldy = computeY(i, W0, W1, TH);			
		iy = (int)oldy;
		YTable[i] = iy;
	}

	
	unsigned int x, y;
	short	sum;
    
	for(i = 0; i < RSTIMGH; i++)
	{
		for(j = 0; j < RSTIMGW; j++)
		{					
			x = j * (SRCIMGW - 100) / RSTIMGW + 50;
			y = YTable[i];

			sum =
				*(lpSrcBitmap+ (y+1) * SRCIMGW + x-1) + *(lpSrcBitmap+ (y+1) * SRCIMGW + x)*2 +
				*(lpSrcBitmap+ (y+1) * SRCIMGW + x+1) + *(lpSrcBitmap+ (y) * SRCIMGW + x-1)*2 +
				*(lpSrcBitmap+ (y) * SRCIMGW + x)*4 + *(lpSrcBitmap+ (y) * SRCIMGW + x-1)*2 +
				*(lpSrcBitmap+ (y-1) * SRCIMGW + x-1) + *(lpSrcBitmap+ (y-1) * SRCIMGW + x)*2 +
				*(lpSrcBitmap+ (y-1) * SRCIMGW + x+1);
			
			sum = (sum>>4);
			*(lpRstBitmap+ i* RSTIMGW + RSTIMGW-j-1) = (unsigned char)sum; //[i][RSTIMGW-j-1]
		}
	}


}



void Rectify()
{


	RectifyFunc((unsigned char *)&g_Buffer[0], (unsigned char *)&g_Result[0], &g_RectifyParm[g_SensorID][0]); 
	return;

}


void FreeMediaType(AM_MEDIA_TYPE& mt)
{
    if (mt.cbFormat != 0) {
        CoTaskMemFree((PVOID)mt.pbFormat);

        // Strictly unnecessary but tidier
        mt.cbFormat = 0;
        mt.pbFormat = NULL;
    }
    if (mt.pUnk != NULL) {
        mt.pUnk->Release();
        mt.pUnk = NULL;
    }
}

void DeleteMediaType(AM_MEDIA_TYPE *pmt)
{
    // allow NULL pointers for coding simplicity

    if (pmt == NULL) {
        return;
    }

    FreeMediaType(*pmt);
    CoTaskMemFree((PVOID)pmt);
}

HRESULT InitCaptureGraphBuilder(
  IGraphBuilder **ppGraph,  // Receives the pointer.
  ICaptureGraphBuilder2 **ppBuild  // Receives the pointer.
)
{
    if (!ppGraph || !ppBuild)
    {
        return E_POINTER;
    }
    IGraphBuilder *pGraph = NULL;
    ICaptureGraphBuilder2 *pBuild = NULL;

    // Create the Capture Graph Builder.
    HRESULT hr = CoCreateInstance(CLSID_CaptureGraphBuilder2, NULL, 
        CLSCTX_INPROC_SERVER, IID_ICaptureGraphBuilder2, (void**)&pBuild);
    if (SUCCEEDED(hr))
    {
        // Create the Filter Graph Manager.
        hr = CoCreateInstance(CLSID_FilterGraph, 0, CLSCTX_INPROC_SERVER,
            IID_IGraphBuilder, (void**)&pGraph);
        if (SUCCEEDED(hr))
        {
            // Initialize the Capture Graph Builder.
            pBuild->SetFiltergraph(pGraph);

            // Return both interface pointers to the caller.
            *ppBuild = pBuild;
            *ppGraph = pGraph; // The caller must release both interfaces.
            return S_OK;
        }
        else
        {
            pBuild->Release();
        }
    }
    return hr; // Failed
}


HRESULT FindCaptureDevice(IBaseFilter ** ppSrcFilter, IGraphBuilder *pGraph)
{
    HRESULT hr;
    IBaseFilter * pSrc = NULL;
    IMoniker  * pMoniker =NULL;
    ULONG cFetched;

    if (!ppSrcFilter)
        return E_POINTER;
   
    // Create the system device enumerator
    ICreateDevEnum * pDevEnum =NULL;

    hr = CoCreateInstance (CLSID_SystemDeviceEnum, NULL, CLSCTX_INPROC_SERVER,
                           IID_ICreateDevEnum, (void **) &pDevEnum);
    if (FAILED(hr))
    {
        return hr;
    }

    // Create an enumerator for the video capture devices
    IEnumMoniker * pClassEnum = NULL;

    hr = pDevEnum->CreateClassEnumerator (CLSID_VideoInputDeviceCategory, &pClassEnum, 0);
    if (FAILED(hr))
    {
        return hr;
    }

    // If there are no enumerators for the requested type, then 
    // CreateClassEnumerator will succeed, but pClassEnum will be NULL.
    if (pClassEnum == NULL)
    {
		pDevEnum->Release();
        return E_FAIL;
    }

    // Use the first video capture device on the device list.
    // Note that if the Next() call succeeds but there are no monikers,
    // it will return S_FALSE (which is not a failure).  Therefore, we
    // check that the return code is S_OK instead of using SUCCEEDED() macro.
    if (S_OK == (pClassEnum->Next (1, &pMoniker, &cFetched)))
    {
        // Bind Moniker to a filter object
        hr = pMoniker->BindToObject(0,0,IID_IBaseFilter, (void**)&pSrc);
        if (FAILED(hr))
        {
			pMoniker->Release();
            return hr;
        }
    }
    else
    {
        return E_FAIL;
    }



    // Copy the found filter pointer to the output parameter.
    // Do NOT Release() the reference, since it will still be used
    // by the calling function.
    *ppSrcFilter = pSrc;

	pGraph->AddFilter(pSrc, L"Cap");

    pClassEnum->Release();
    pMoniker->Release();

    return hr;
}


class SampleGrabberCallback : public ISampleGrabberCB
{
public:
    // Fake referance counting.
    STDMETHODIMP_(ULONG) AddRef() { return 1; }
    STDMETHODIMP_(ULONG) Release() { return 2; }

    STDMETHODIMP QueryInterface(REFIID riid, void **ppvObject)
    {
        if (NULL == ppvObject) return E_POINTER;
        if (riid == __uuidof(IUnknown))
        {
            *ppvObject = static_cast<IUnknown*>(this);
             return S_OK;
        }
        if (riid == __uuidof(ISampleGrabberCB))
        {
            *ppvObject = static_cast<ISampleGrabberCB*>(this);
             return S_OK;