cvvecfacetracking.cpp.svn-base

来自「非结构化路识别」· SVN-BASE 代码 · 共 976 行 · 第 1/3 页

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

/****************************************************************************************\
      Contour-based face feature tracking
      The code was created by Tatiana Cherepanova ([e-mail])
\****************************************************************************************/

#include "_cvaux.h"
#include "_cvvectrack.h"

#define _ASSERT     assert
#define NUM_FACE_ELEMENTS   3
enum 
{
    MOUTH = 0,
    LEYE = 1,
    REYE = 2,
};

#define MAX_LAYERS      64

const double pi = 3.1415926535;

struct CvFaceTracker;
struct CvTrackingRect;
class CvFaceElement;

void ThresholdingParam(IplImage *imgGray, int iNumLayers, int &iMinLevel, int &iMaxLevel, float &step, float& power, int iHistMin /*= HIST_MIN*/);
int ChoiceTrackingFace3(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy);
int ChoiceTrackingFace2(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy, int noel);
inline int GetEnergy(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl);
inline int GetEnergy2(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl, int* element);
inline double CalculateTransformationLMS3_0( CvPoint* pTemplPoints, CvPoint* pSrcPoints);
inline double CalculateTransformationLMS3( CvPoint* pTemplPoints, 
                                   CvPoint* pSrcPoints,
                                   double*       pdbAverageScale,
                                   double*       pdbAverageRotate,
                                   double*       pdbAverageShiftX,
                                   double*       pdbAverageShiftY );

struct CvTrackingRect
{
    CvRect r;
    CvPoint ptCenter;
    int iColor;
    int iEnergy;
    int nRectsInThis;
    int nRectsOnLeft;
    int nRectsOnRight;
    int nRectsOnTop;
    int nRectsOnBottom;
    CvTrackingRect() { memset(this, 0, sizeof(CvTrackingRect)); };
    int Energy(const CvTrackingRect& prev)
    {
        int prev_color = 0 == prev.iColor ? iColor : prev.iColor;
        iEnergy =	1 * pow2(r.width - prev.r.width) + 
            1 * pow2(r.height - prev.r.height) + 
            1 * pow2(iColor - prev_color) / 4 + 
            - 1 * nRectsInThis + 
            - 0 * nRectsOnTop + 
            + 0 * nRectsOnLeft + 
            + 0 * nRectsOnRight + 
            + 0 * nRectsOnBottom;
        return iEnergy;
    }
};

struct CvFaceTracker
{
    CvTrackingRect face[NUM_FACE_ELEMENTS];
    int iTrackingFaceType;
    double dbRotateDelta;
    double dbRotateAngle;
    CvPoint ptRotate;
    
    CvPoint ptTempl[NUM_FACE_ELEMENTS];
    CvRect rTempl[NUM_FACE_ELEMENTS];
    
    IplImage* imgGray;
    IplImage* imgThresh;
    CvMemStorage* mstgContours;
    CvFaceTracker()
    {
        ptRotate.x = 0;
        ptRotate.y = 0;
        dbRotateDelta = 0;
        dbRotateAngle = 0;
        iTrackingFaceType = -1;
        imgThresh = NULL;
        imgGray = NULL;
        mstgContours = NULL;
    };
    ~CvFaceTracker()
    {
        if (NULL != imgGray)
            delete imgGray;
        if (NULL != imgThresh)
            delete imgThresh;
        if (NULL != mstgContours)
            cvReleaseMemStorage(&mstgContours);
    };
    int Init(CvRect* pRects, IplImage* imgGray)
    {
        for (int i = 0; i < NUM_FACE_ELEMENTS; i++)
        {
            face[i].r = pRects[i];
            face[i].ptCenter = Center(face[i].r);
            ptTempl[i] = face[i].ptCenter;
            rTempl[i] = face[i].r;
        }
        imgGray = cvCreateImage(cvSize(imgGray->width, imgGray->height), 8, 1);
        imgThresh = cvCreateImage(cvSize(imgGray->width, imgGray->height), 8, 1);
        mstgContours = cvCreateMemStorage();
        if ((NULL == imgGray) || 
            (NULL == imgThresh) || 
            (NULL == mstgContours))
            return FALSE;
        return TRUE;
    };
    int InitNextImage(IplImage* img)
    {
        CvSize sz = {img->width, img->height};
        ReallocImage(&imgGray, sz, 1);
        ReallocImage(&imgThresh, sz, 1);
        ptRotate = face[MOUTH].ptCenter;
        float m[6];
        CvMat mat = cvMat( 2, 3, CV_32FC1, m ); 

        if (NULL == imgGray || NULL == imgThresh)
            return FALSE;
        
        m[0] = (float)cos(-dbRotateAngle*CV_PI/180.);
        m[1] = (float)sin(-dbRotateAngle*CV_PI/180.);
        m[2] = (float)ptRotate.x;
        m[3] = -m[1];
        m[4] = m[0];
        m[5] = (float)ptRotate.y;
        
        /*iplGetRotateShift(ptRotate.x, ptRotate.y, angle, &xShift, &yShift);
        iplRotate(img, imgGray, angle, xShift, yShift, CV_INTER_LINEAR | IPL_SMOOTH_EDGE);*/
        cvGetQuadrangleSubPix( img, imgGray, &mat, 1, cvScalarAll(0));
        
        if (NULL == mstgContours)
            mstgContours = cvCreateMemStorage();
        else
            cvClearMemStorage(mstgContours);
        if (NULL == mstgContours)
            return FALSE;
        return TRUE;
    }
};

class CvFaceElement
{
public:
    CvSeq* m_seqRects;
    CvMemStorage* m_mstgRects;
    CvRect m_rROI;
    CvTrackingRect m_trPrev;
    inline CvFaceElement()
    {
        m_seqRects = NULL;
        m_mstgRects = NULL;
        m_rROI.x = 0;
        m_rROI.y = 0;
        m_rROI.width = 0;
        m_rROI.height = 0;
    };
    inline int Init(const CvRect& roi, const CvTrackingRect& prev, CvMemStorage* mstg = NULL)
    {
        m_rROI = roi;
        m_trPrev = prev;
        if (NULL != mstg)
            m_mstgRects = mstg;
        if (NULL == m_mstgRects)
            return FALSE;
        if (NULL == m_seqRects)
            m_seqRects = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvTrackingRect), m_mstgRects);
        else
            cvClearSeq(m_seqRects);
        if (NULL == m_seqRects)
            return FALSE;
        return TRUE;
    };
    void FindRects(IplImage* img, IplImage* thresh, int nLayers, int dMinSize);
protected:
    void FindContours(IplImage* img, IplImage* thresh, int nLayers, int dMinSize);
    void MergeRects(int d);
    void Energy();
}; //class CvFaceElement

int CV_CDECL CompareEnergy(const void* el1, const void* el2, void*)
{
    return ((CvTrackingRect*)el1)->iEnergy - ((CvTrackingRect*)el2)->iEnergy;
}// int CV_CDECL CompareEnergy(const void* el1, const void* el2, void*)

void CvFaceElement::FindRects(IplImage* img, IplImage* thresh, int nLayers, int dMinSize)
{
    FindContours(img, thresh, nLayers, dMinSize / 4);
    if (0 == m_seqRects->total)
        return;
    Energy();
    cvSeqSort(m_seqRects, CompareEnergy, NULL);
    CvTrackingRect* pR = (CvTrackingRect*)cvGetSeqElem(m_seqRects, 0);
    if (m_seqRects->total < 32)
    {
        MergeRects(dMinSize / 8);
        Energy();
        cvSeqSort(m_seqRects, CompareEnergy, NULL);
    }
    pR = (CvTrackingRect*)cvGetSeqElem(m_seqRects, 0);
    if ((pR->iEnergy > 100 && m_seqRects->total < 32) || (m_seqRects->total < 16))
    {
        MergeRects(dMinSize / 4);
        Energy();
        cvSeqSort(m_seqRects, CompareEnergy, NULL);
    }
    pR = (CvTrackingRect*)cvGetSeqElem(m_seqRects, 0);
    if ((pR->iEnergy > 100 && m_seqRects->total < 16) || (pR->iEnergy > 200 && m_seqRects->total < 32))
    {
        MergeRects(dMinSize / 2);
        Energy();
        cvSeqSort(m_seqRects, CompareEnergy, NULL);
    }

}// void CvFaceElement::FindRects(IplImage* img, IplImage* thresh, int nLayers, int dMinSize)

void CvFaceElement::FindContours(IplImage* img, IplImage* thresh, int nLayers, int dMinSize)
{
    CvSeq* seq;
    CvRect roi = m_rROI;
    Extend(roi, 1);
    cvSetImageROI(img, roi);
    cvSetImageROI(thresh, roi);
    // layers
    int colors[MAX_LAYERS] = {0};
    int iMinLevel = 0, iMaxLevel = 255;
    float step, power;
    ThresholdingParam(img, nLayers / 2, iMinLevel, iMaxLevel, step, power, 4);
    int iMinLevelPrev = iMinLevel;
    int iMaxLevelPrev = iMinLevel;
    if (m_trPrev.iColor != 0)
    {
        iMinLevelPrev = m_trPrev.iColor - nLayers / 2;
        iMaxLevelPrev = m_trPrev.iColor + nLayers / 2;
    }
    if (iMinLevelPrev < iMinLevel)
    {
        iMaxLevelPrev += iMinLevel - iMinLevelPrev;
        iMinLevelPrev = iMinLevel;
    }
    if (iMaxLevelPrev > iMaxLevel)
    {
        iMinLevelPrev -= iMaxLevelPrev - iMaxLevel;
        if (iMinLevelPrev < iMinLevel)
            iMinLevelPrev = iMinLevel;
        iMaxLevelPrev = iMaxLevel;
    }
    int n = nLayers;
    n -= (iMaxLevelPrev - iMinLevelPrev + 1) / 2;
    step = float(iMinLevelPrev - iMinLevel + iMaxLevel - iMaxLevelPrev) / float(n);
    int j = 0;
    float level;
    for (level = (float)iMinLevel; level < iMinLevelPrev && j < nLayers; level += step, j++)
        colors[j] = int(level + 0.5);
    for (level = (float)iMinLevelPrev; level < iMaxLevelPrev && j < nLayers; level += 2.0, j++)
        colors[j] = int(level + 0.5);
    for (level = (float)iMaxLevelPrev; level < iMaxLevel && j < nLayers; level += step, j++)
        colors[j] = int(level + 0.5);
    //
    for (int i = 0; i < nLayers; i++)
    {
        cvThreshold(img, thresh, colors[i], 255.0, CV_THRESH_BINARY);
        if (cvFindContours(thresh, m_mstgRects, &seq, sizeof(CvContour), CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE))
        {
            CvTrackingRect cr;
            for (CvSeq* external = seq; external; external = external->h_next)
            {
                cr.r = cvContourBoundingRect(external);
                Move(cr.r, roi.x, roi.y);
                if (RectInRect(cr.r, m_rROI) && cr.r.width > dMinSize  && cr.r.height > dMinSize)
                {
                    cr.ptCenter = Center(cr.r);
                    cr.iColor = colors[i];
                    cvSeqPush(m_seqRects, &cr);
                }
                for (CvSeq* internal = external->v_next; internal; internal = internal->h_next)