kalman.cpp

微软的基于HMM的人脸识别原代码, 非常经典的说

/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include <windows.h>
#include <streams.h>
#include <initguid.h>
#include <olectl.h>
#if (1100 > _MSC_VER)
#include <olectlid.h>
#endif
#include "iKalman.h"
#include "Kalmanprop.h"
#include "Kalman.h"
#include "Kalmanuids.h"

// setup data
const AMOVIESETUP_MEDIATYPE sudPinTypes =
{
    &MEDIATYPE_Video,       // Major type
    &MEDIASUBTYPE_RGB24      // Minor type
};

const AMOVIESETUP_PIN psudPins[] =
{
    {
        L"Input",           // String pin name
        FALSE,              // Is it rendered
        FALSE,              // Is it an output
        FALSE,              // Allowed none
        FALSE,              // Allowed many
        &CLSID_NULL,        // Connects to filter
        L"Output",          // Connects to pin
        1,                  // Number of types
        &sudPinTypes },     // The pin details
      { L"Output",          // String pin name
        FALSE,              // Is it rendered
        TRUE,               // Is it an output
        FALSE,              // Allowed none
        FALSE,              // Allowed many
        &CLSID_NULL,        // Connects to filter
        L"Input",           // Connects to pin
        1,                  // Number of types
        &sudPinTypes        // The pin details
    }
};


const AMOVIESETUP_FILTER sudCKalmTrack =
{
    &CLSID_CKalmTrack,        // Filter CLSID
    L"Kalman",                    // Filter name
    MERIT_DO_NOT_USE,               // Its merit
    2,                              // Number of pins
    psudPins                        // Pin details
};


// List of class IDs and creator functions for the class factory. This
// provides the link between the OLE entry point in the DLL and an object
// being created. The class factory will call the static CreateInstance

CFactoryTemplate g_Templates[2] = {

    { L"Kalman"
    , &CLSID_CKalmTrack
    , CKalmTrack::CreateInstance
    , NULL
    , &sudCKalmTrack }
  ,
    { L"Kalman Property Page"
    , &CLSID_CKalmTrackPropertyPage
    , CKalmTrackProperties::CreateInstance }
};
int g_cTemplates = sizeof(g_Templates) / sizeof(g_Templates[0]);



//
// Constructor
//
CKalmTrack::CKalmTrack(TCHAR *tszName,LPUNKNOWN punk,HRESULT *phr) :
    CTransInPlaceFilter(tszName, punk, CLSID_CKalmTrack,phr)    
   
{
    m_params.x = 0.4f; 
    m_params.y = 0.3f;
    m_params.width = 0.2f;
    m_params.height = 0.3f;

    m_params.Smin = 20;
    m_params.Vmin = 40;
    m_params.view = 0;

    IsTracking = false;
    IsInit =false;
	Kalman = cvCreateKalman(4,4);
	CvMat Dyn = cvMat(4,4,CV_MAT4x4_32F,Kalman->DynamMatr);
	CvMat Mes = cvMat(4,4,CV_MAT4x4_32F,Kalman->MeasurementMatr);
	CvMat PNC = cvMat(4,4,CV_MAT4x4_32F,Kalman->PNCovariance);
	CvMat MNC = cvMat(4,4,CV_MAT4x4_32F,Kalman->MNCovariance);
	CvMat PriErr = cvMat(4,4,CV_MAT4x4_32F,Kalman->PriorErrorCovariance);
	CvMat PostErr = cvMat(4,4,CV_MAT4x4_32F,Kalman->PosterErrorCovariance);
	CvMat PriState = cvMat(4,1,CV_MAT4x1_32F,Kalman->PriorState);
	cvmSetIdentity(&PNC);
	cvmSetIdentity(&PriErr);
	cvmSetIdentity(&PostErr);
	cvmSetZero(&MNC);
	cvmSetZero(&PriState);
    cvmSetIdentity(&Dyn);
    cvmSet(&Dyn,0,1,0.9f);
    cvmSet(&Dyn,2,3,0.9f);
    cvmSetIdentity(&Mes);
	MVect = cvMat(4,1,CV_MAT4x1_32F,Measurement);
	ASSERT(tszName);
    ASSERT(phr);
} // CKalmTrack

//Destructor
CKalmTrack::~CKalmTrack()
{
	cvReleaseKalman(&Kalman);
}//~CKalmTrack

//
// CreateInstance
//
// Provide the way for COM to create a CKalmTrack object
//
CUnknown * WINAPI CKalmTrack::CreateInstance(LPUNKNOWN punk, HRESULT *phr) {

    CKalmTrack *pNewObject = new CKalmTrack(NAME("Kalman"), punk, phr);
    if (pNewObject == NULL) {
        *phr = E_OUTOFMEMORY;
    }
    return pNewObject;

} // CreateInstance


//
// NonDelegatingQueryInterface
//
// Reveals ICKalmTrack and ISpecifyPropertyPages
//
STDMETHODIMP CKalmTrack::NonDelegatingQueryInterface(REFIID riid, void **ppv)
{
    CheckPointer(ppv,E_POINTER);

    if (riid == IID_ICKalmTrack) {
        return GetInterface((ICKalmTrack *) this, ppv);
    } else if (riid == IID_ISpecifyPropertyPages) {
        return GetInterface((ISpecifyPropertyPages *) this, ppv);
    } else {
        return CTransInPlaceFilter::NonDelegatingQueryInterface(riid, ppv);
    }

} // NonDelegatingQueryInterface


void  CKalmTrack::ApplyCamShift( CvImage* image, bool initialize )
{
    CvSize size;
    int bins = 20;

    m_cCamShift.set_hist_dims( 1, &bins );
    m_cCamShift.set_thresh( 0, 1, 180 );
    m_cCamShift.set_min_ch_val( 1, m_params.Smin );
    m_cCamShift.set_max_ch_val( 1, 255 );
    m_cCamShift.set_min_ch_val( 2, m_params.Vmin );
    m_cCamShift.set_max_ch_val( 2, 255 );
    
    cvGetImageRawData( image, 0, 0, &size );

    if( m_object.x < 0 ) m_object.x = 0;
    if( m_object.x > size.width - m_object.width - 1 )
        m_object.x = size.width - m_object.width - 1;

    if( m_object.y < 0 ) m_object.y = 0;
    if( m_object.y > size.height - m_object.height - 1 )
        m_object.y = size.height - m_object.height - 1;

    m_cCamShift.set_window(m_object);
    
    if( initialize )
    {
        m_cCamShift.reset_histogram();
        m_cCamShift.update_histogram( image );
    }

    m_cCamShift.track_object( image );
    m_object = m_cCamShift.get_window();

    Measurement[0] = (float)m_object.x + m_object.width*0.5f;
    Measurement[1] = initialize ? 0 : Measurement[0] - m_Old.x;
    Measurement[2] = (float)m_object.y + m_object.height*0.5f;
    Measurement[3] = initialize ? 0 : Measurement[2] - m_Old.y;

    m_Old.x = cvRound( Measurement[0] );
    m_Old.y = cvRound( Measurement[2] );
    cvKalmanUpdateByMeasurement(Kalman,&MVect);
}


void  CKalmTrack::CheckBackProject( CvImage* image )
{
    if( m_params.view != 0 )
    {
        IplImage* src = m_cCamShift.get_back_project();
        if( src && src->imageData && image )
        {
            iplGrayToColor( src, image, 0, 0, 0 );
        }
    }
}

//
// Transform
// Transform the  sample 'in place'
//
HRESULT CKalmTrack::Transform(IMediaSample *pSample)
{
    BYTE*   pData;
    CvImage image;
    
    pSample->GetPointer(&pData);
    
    AM_MEDIA_TYPE* pType = &m_pInput->CurrentMediaType();
    VIDEOINFOHEADER *pvi = (VIDEOINFOHEADER *) pType->pbFormat;
    
    // Get the image properties from the BITMAPINFOHEADER
    CvSize size = cvSize( pvi->bmiHeader.biWidth, pvi->bmiHeader.biHeight );
    int stride = (size.width * 3 + 3) & -4;

    cvInitImageHeader( &image, size, IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4, 1 );
    cvSetImageData( &image, pData,stride );

    if(IsTracking == false)
    {
        if(IsInit == false)
        {
            CvPoint p1, p2;
            // Draw box
            p1.x = cvRound( size.width * m_params.x );