calibfilter.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*/

#pragma warning( disable: 4201 4710 )

#define MIRROR_POINTS

#include <windows.h>
#include <streams.h>
#include <initguid.h>
#include <olectl.h>
#if (1100 > _MSC_VER)
#include <olectlid.h>
#endif
#include "iCalibFilter.h"
#include "CalibFilterprop.h"
#include "CalibFilter.h"
#include "CalibFilteruids.h"
#include <assert.h>
#include "math.h"
#include <stdio.h>
#include "Calib3DWindow.h"
#include "CV.h"

// setup data
const AMOVIESETUP_MEDIATYPE sudPinTypes =
{
    &MEDIATYPE_Video,       // Major type
    &MEDIASUBTYPE_NULL      // 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 sudCCalibFilter =
{
    &CLSID_CCalibFilter,        // Filter CLSID
    L"CalibFilter",                    // 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"CalibFilter"
    , &CLSID_CCalibFilter
    , CCalibFilter::CreateInstance
    , NULL
    , &sudCCalibFilter }
  ,
    { L"CalibFilter Property Page"
    , &CLSID_CCalibFilterPropertyPage
    , CCalibFilterProperties::CreateInstance }
};
int g_cTemplates = sizeof(g_Templates) / sizeof(g_Templates[0]);

static DWORD WINAPI _3DWindowThreadProc( void* window );

/* Constructor */
CCalibFilter::CCalibFilter(TCHAR *tszName,LPUNKNOWN punk,HRESULT *phr) :
        CTransInPlaceFilter(tszName, punk, CLSID_CCalibFilter,phr),
        CPersistStream(punk, phr)
{
    m_initial_params.etalon_type = CalibEtalon_ChessBoard;
    
    m_initial_params.etalon_params[0] = 6;
    m_initial_params.etalon_params[1] = 8;
    m_initial_params.etalon_params[2] = 3;
    m_initial_params.etalon_params_count = 3;
    
    m_initial_params.show_feature_points_flag = 1;
    m_initial_params.frame_interval = 1000;
    m_initial_params.frames_to_collect = 10;
    m_initial_params.frames_collected = 0;
    m_initial_params.frames_passed = 0;
    m_initial_params.calib_state = CalibState_Initial;
    m_initial_params.last_frame_time = -1e6f;

    m_initial_params.enable_undistortion    = 0;
    m_initial_params.show_3d_window         = 1;

    m_params = m_initial_params;

    m_max_points = 0;
    m_imagePoints = 0;
    m_objectPoints = 0;
    m_transVects = 0;
    m_rotMatrs = 0;

    m_gray_img   = cvCreateImage( cvSize(1,1), IPL_DEPTH_8U, 1 );
    m_thresh_img = cvCreateImage( cvSize(1,1), IPL_DEPTH_8U, 1 );
    m_rgb_img    = cvCreateImage( cvSize(1,1), IPL_DEPTH_8U, 3 );
    m_undist_img = cvCreateImage( cvSize(1,1), IPL_DEPTH_8U, 3 );

    m_window3D = 0;

    DWORD threadId;
    m_thread = CreateThread( 0, 0, _3DWindowThreadProc, &m_window3D, 0, &threadId );
}

CvSize  CCalibFilter::GetEtalonSize()
{
    return cvSize( cvRound(m_params.etalon_params[0]) - 1,
                      cvRound(m_params.etalon_params[1]) - 1 ); 
}

/* a whole life of 3D window in the single routine */
DWORD WINAPI _3DWindowThreadProc( void* window )
{
    CCalib3DWindow* window3D = new CCalib3DWindow;
    *((CCalib3DWindow**)window) = window3D;
    
    MSG msg;

    // Main message loop:
    while( GetMessage(&msg, NULL, 0, 0)) 
    {
        TranslateMessage(&msg);
        DispatchMessage(&msg);
    }

    *((CCalib3DWindow**)window) = 0;
    delete window3D;
    return 0;
}

CCalibFilter::~CCalibFilter()
{
    SendMessage( m_window3D->m_hwnd, WM_CLOSE, 0, 0 );
    WaitForSingleObject( m_thread, 100 );
    CloseHandle( m_thread );
}


/* CreateInstance */
CUnknown * WINAPI CCalibFilter::CreateInstance(LPUNKNOWN punk, HRESULT *phr)
{
    CCalibFilter *pNewObject = new CCalibFilter(NAME("CalibFilter"), punk, phr);
    if( !pNewObject ) *phr = E_OUTOFMEMORY;
    return pNewObject;
}

/* NonDelegatingQueryInterface */
STDMETHODIMP CCalibFilter::NonDelegatingQueryInterface(REFIID riid, void **ppv)
{
    CheckPointer(ppv,E_POINTER);

    if( riid == IID_ICalibFilter )
        return GetInterface((ICalibFilter *) this, ppv);
    else if( riid == IID_ISpecifyPropertyPages )
        return GetInterface((ISpecifyPropertyPages *) this, ppv);
    else if( riid == IID_IPersistStream )
        return GetInterface((IPersistStream *) this, ppv);
    else
        return CTransInPlaceFilter::NonDelegatingQueryInterface(riid, ppv);
}


/* CheckReallocBuffers */
void  CCalibFilter::CheckReallocBuffers( IplImage* rgb_img )
{
    CvSize etalon_size = GetEtalonSize();
    int etalon_points = etalon_size.width*etalon_size.height;

    if( m_gray_img->imageData == 0 ||
        m_gray_img->width != rgb_img->width ||
        m_gray_img->height != rgb_img->height )
    {
        cvReleaseImageData( m_gray_img );
        cvInitImageHeader( m_gray_img, cvSize(rgb_img->width, rgb_img->height),
                            IPL_DEPTH_8U, 1, IPL_ORIGIN_TL, 4, 1 );
        cvCreateImageData( m_gray_img ); 

        cvReleaseImageData( m_thresh_img );
        cvInitImageHeader( m_thresh_img, cvSize(rgb_img->width, rgb_img->height),
                            IPL_DEPTH_8U, 1, IPL_ORIGIN_TL, 4, 1 );
        cvCreateImageData( m_thresh_img );

        cvReleaseImageData( m_undist_img );
        cvInitImageHeader( m_undist_img, cvSize(rgb_img->width, rgb_img->height),
                            IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4, 1 );
        
        cvCreateImageData( m_undist_img ); 
    }

    if( etalon_points * m_params.frames_to_collect > m_max_points )
    {
        int  new_max_points = etalon_points * (m_params.frames_to_collect + 1) + 128;
        CvPoint2D32f* imagePoints = (CvPoint2D32f*)malloc( new_max_points * 
                                                           sizeof(CvPoint2D32f));

        memcpy( imagePoints, m_imagePoints, m_max_points * sizeof(CvPoint2D32f));

        free( m_imagePoints );
        free( m_objectPoints );
        free( m_transVects );
        free( m_rotMatrs );

        m_imagePoints  = imagePoints;
        m_objectPoints  = (CvPoint3D32f*)malloc( new_max_points * sizeof(CvPoint3D32f) );
        m_transVects = (CvPoint3D32f*)malloc( new_max_points * sizeof(CvPoint2D32f) );
        m_rotMatrs = (float*)malloc( new_max_points * 9 * sizeof(float));
        m_numsPoints = (int*)calloc( m_params.frames_to_collect,sizeof(int));

        m_max_points = new_max_points;
	}
}


void  CCalibFilter::DrawEtalon( IplImage* rgb_img, CvPoint2D32f* corners,
                                int corner_count, CvSize etalon_size,
                                int draw_ordered )
{
    const int r = 4;
    int i;

    if( corner_count == etalon_size.width * etalon_size.height && draw_ordered )
    {
        int x, y;
        CvPoint prev_pt = { 0, 0};
        const int line_max = 7;
        static const int line_colors[] = {
            CV_RGB(255,0,0),
            CV_RGB(255,128,0),
            CV_RGB(200,200,0),
            CV_RGB(0,255,0),
            CV_RGB(0,200,200),
            CV_RGB(0,0,255),
            CV_RGB(255,0,255) };

        for( y = 0, i = 0; y < etalon_size.height; y++ )
        {
            int color = line_colors[y % line_max];
            for( x = 0; x < etalon_size.width; x++, i++ )
            {
                CvPoint pt;
                pt.x = cvRound(corners[i].x);
                pt.y = cvRound(corners[i].y);

                if( i != 0 )
                {
                    cvLineAA( rgb_img, prev_pt, pt, color );
                }

                cvLineAA( rgb_img,
                          cvPoint( pt.x - r, pt.y - r ),
                          cvPoint( pt.x + r, pt.y + r ), color );
                cvLineAA( rgb_img,
                          cvPoint( pt.x - r, pt.y + r),
                          cvPoint( pt.x + r, pt.y - r), color );
                cvCircleAA( rgb_img, pt, r+1, color );
                prev_pt = pt;
            }
        }
    }
    else
    {
        const int color = CV_RGB(255,0,0);
        for( i = 0; i < corner_count; i++ )
        {
            CvPoint pt;
            pt.x = cvRound(corners[i].x);
            pt.y = cvRound(corners[i].y);
            cvLineAA( rgb_img,
                      cvPoint( pt.x - r, pt.y - r ),
                      cvPoint( pt.x + r, pt.y + r ), color );
            cvLineAA( rgb_img,
                      cvPoint( pt.x - r, pt.y + r),
                      cvPoint( pt.x + r, pt.y - r), color );
            cvCircleAA( rgb_img, pt, r+1, color );
        }
    }
}


void  CCalibFilter::FillEtalonObjPoints( CvPoint3D32f* obj_points,
                                         CvSize etalon_size,
                                         float square_size )
{
    int x, y, i;

    for( y = 0, i = 0; y < etalon_size.height; y++ )
    {
        for( x = 0; x < etalon_size.width; x++, i++ )
        {
            obj_points[i].x = square_size * x;
            obj_points[i].y = square_size * y;
            obj_points[i].z = 0;
        }
    }
}


#ifdef MIRROR_POINTS
void MirrorPoints( CvPoint2D32f* points, int frames, CvSize etalon_size, CvSize imgSize )
{
    int i, j, k;
    for( i = 0; i < frames; i++ )
    {