cvvecfacetracking.cpp.svn-base

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

    for (i = (iMinLevel + iMaxLevel) / 2; i < iMaxLevel; i++)
        white += histImg[i];
    power = float(black) / float(2 * white);
    //
    step = float(iMaxLevel - iMinLevel) / float(iNumLayers);
    if (step < 1.0)
        step = 1.0;
}// void ThresholdingParam(IplImage *imgGray, int iNumLayers, int &iMinLevel, int &iMaxLevel, int &iStep)

int ChoiceTrackingFace3(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy)
{
    CvTrackingRect* curr_face[NUM_FACE_ELEMENTS] = {NULL};
    CvTrackingRect* new_face[NUM_FACE_ELEMENTS] = {NULL};
    new_energy = 0x7fffffff;
    int curr_energy = 0x7fffffff;
    int found = FALSE;
    int N = 0;
    CvSeqReader reader_m, reader_l, reader_r;
    cvStartReadSeq( big_face[MOUTH].m_seqRects, &reader_m );
    for (int i_mouth = 0; i_mouth < big_face[MOUTH].m_seqRects->total && i_mouth < nElements; i_mouth++)
    {
        curr_face[MOUTH] = (CvTrackingRect*)(reader_m.ptr);
        cvStartReadSeq( big_face[LEYE].m_seqRects, &reader_l );
        for (int i_left = 0; i_left < big_face[LEYE].m_seqRects->total && i_left < nElements; i_left++)
        {
            curr_face[LEYE] = (CvTrackingRect*)(reader_l.ptr);
            if (curr_face[LEYE]->r.y + curr_face[LEYE]->r.height < curr_face[MOUTH]->r.y)
            {
                cvStartReadSeq( big_face[REYE].m_seqRects, &reader_r );
                for (int i_right = 0; i_right < big_face[REYE].m_seqRects->total && i_right < nElements; i_right++)
                {
                    curr_face[REYE] = (CvTrackingRect*)(reader_r.ptr);
                    if (curr_face[REYE]->r.y + curr_face[REYE]->r.height < curr_face[MOUTH]->r.y &&
                        curr_face[REYE]->r.x > curr_face[LEYE]->r.x + curr_face[LEYE]->r.width)
                    {
                        curr_energy = GetEnergy(curr_face, pTF->face, pTF->ptTempl, pTF->rTempl);
                        if (curr_energy < new_energy)
                        {
                            for (int elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
                                new_face[elem] = curr_face[elem];
                            new_energy = curr_energy;
                            found = TRUE;
                        }
                        N++;
                    }
                }
            }
        }
    }
    if (found)
    {
        for (int elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
            face[elem] = *(new_face[elem]);
    }
    return found;
}; // int ChoiceTrackingFace3(const CvTrackingRect* tr_face, CvTrackingRect* new_face, int& new_energy)

int ChoiceTrackingFace2(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy, int noel)
{
    int element[NUM_FACE_ELEMENTS];
    for (int i = 0, elem = 0; i < NUM_FACE_ELEMENTS; i++)
    {
        if (i != noel)
        {
            element[elem] = i;
            elem ++;
        }
        else
            element[2] = i;
    }
    CvTrackingRect* curr_face[NUM_FACE_ELEMENTS] = {NULL};
    CvTrackingRect* new_face[NUM_FACE_ELEMENTS] = {NULL};
    new_energy = 0x7fffffff;
    int curr_energy = 0x7fffffff;
    int found = FALSE;
    int N = 0;
    CvSeqReader reader0, reader1;
    cvStartReadSeq( big_face[element[0]].m_seqRects, &reader0 );
    for (int i0 = 0; i0 < big_face[element[0]].m_seqRects->total && i0 < nElements; i0++)
    {
        curr_face[element[0]] = (CvTrackingRect*)(reader0.ptr);
        cvStartReadSeq( big_face[element[1]].m_seqRects, &reader1 );
        for (int i1 = 0; i1 < big_face[element[1]].m_seqRects->total && i1 < nElements; i1++)
        {
            curr_face[element[1]] = (CvTrackingRect*)(reader1.ptr);
            curr_energy = GetEnergy2(curr_face, pTF->face, pTF->ptTempl, pTF->rTempl, element);
            if (curr_energy < new_energy)
            {
                for (int elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
                    new_face[elem] = curr_face[elem];
                new_energy = curr_energy;
                found = TRUE;
            }
            N++;
        }
    }
    if (found)
    {
        face[element[0]] = *(new_face[element[0]]);
        face[element[1]] = *(new_face[element[1]]);
        // 3 element find by template
        CvPoint templ_v01 = {pTF->ptTempl[element[1]].x - pTF->ptTempl[element[0]].x, pTF->ptTempl[element[1]].y - pTF->ptTempl[element[0]].y};
        CvPoint templ_v02 = {pTF->ptTempl[element[2]].x - pTF->ptTempl[element[0]].x, pTF->ptTempl[element[2]].y - pTF->ptTempl[element[0]].y};
        CvPoint prev_v01 = {pTF->face[element[1]].ptCenter.x - pTF->face[element[0]].ptCenter.x, pTF->face[element[1]].ptCenter.y - pTF->face[element[0]].ptCenter.y};
        CvPoint prev_v02 = {pTF->face[element[2]].ptCenter.x - pTF->face[element[0]].ptCenter.x, pTF->face[element[2]].ptCenter.y - pTF->face[element[0]].ptCenter.y};
        CvPoint new_v01 = {new_face[element[1]]->ptCenter.x - new_face[element[0]]->ptCenter.x, new_face[element[1]]->ptCenter.y - new_face[element[0]]->ptCenter.y};
        double templ_d01 = sqrt(pow2(templ_v01.x) + pow2(templ_v01.y));
        double templ_d02 = sqrt(pow2(templ_v02.x) + pow2(templ_v02.y));
        double prev_d01 = sqrt(pow2(prev_v01.x) + pow2(prev_v01.y));
        double prev_d02 = sqrt(pow2(prev_v02.x) + pow2(prev_v02.y));
        double new_d01 = sqrt(pow2(new_v01.x) + pow2(new_v01.y));
        double scale = templ_d01 / new_d01;
        double new_d02 = templ_d02 / scale; 
        double sin_a = double(prev_v01.x * prev_v02.y - prev_v01.y * prev_v02.x) / (prev_d01 * prev_d02);
        double cos_a = cos(asin(sin_a));
        double x = double(new_v01.x) * cos_a - double(new_v01.y) * sin_a;
        double y = double(new_v01.x) * sin_a + double(new_v01.y) * cos_a;
        x = x * new_d02 / new_d01;
        y = y * new_d02 / new_d01;
        CvPoint new_v02 = {int(x + 0.5), int(y + 0.5)};
        face[element[2]].iColor = 0;
        face[element[2]].iEnergy = 0;
        face[element[2]].nRectsInThis = 0;
        face[element[2]].nRectsOnBottom = 0;
        face[element[2]].nRectsOnLeft = 0;
        face[element[2]].nRectsOnRight = 0;
        face[element[2]].nRectsOnTop = 0;
        face[element[2]].ptCenter.x = new_v02.x + new_face[element[0]]->ptCenter.x;
        face[element[2]].ptCenter.y = new_v02.y + new_face[element[0]]->ptCenter.y;
        face[element[2]].r.width = int(double(pTF->rTempl[element[2]].width) / (scale) + 0.5);
        face[element[2]].r.height = int(double(pTF->rTempl[element[2]].height) / (scale) + 0.5);
        face[element[2]].r.x = face[element[2]].ptCenter.x - (face[element[2]].r.width + 1) / 2;
        face[element[2]].r.y = face[element[2]].ptCenter.y - (face[element[2]].r.height + 1) / 2;
        _ASSERT(face[LEYE].r.x + face[LEYE].r.width <= face[REYE].r.x);
    }
    return found;
}; // int ChoiceTrackingFace3(const CvTrackingRect* tr_face, CvTrackingRect* new_face, int& new_energy)

inline int GetEnergy(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl)
{
    int energy = 0;
    CvPoint ptNew[NUM_FACE_ELEMENTS];
    CvPoint ptPrev[NUM_FACE_ELEMENTS];
    for (int i = 0; i < NUM_FACE_ELEMENTS; i++)
    {
        ptNew[i] = ppNew[i]->ptCenter;
        ptPrev[i] = pPrev[i].ptCenter;
        energy += ppNew[i]->iEnergy - 2 * ppNew[i]->nRectsInThis;
    }
    double dx = 0, dy = 0, scale = 1, rotate = 0;
    double e_templ = CalculateTransformationLMS3(ptTempl, ptNew, &scale, &rotate, &dx, &dy);
    double e_prev = CalculateTransformationLMS3_0(ptPrev, ptNew);
    double w_eye = double(ppNew[LEYE]->r.width + ppNew[REYE]->r.width) * scale / 2.0;
    double h_eye = double(ppNew[LEYE]->r.height + ppNew[REYE]->r.height) * scale / 2.0;
    double w_mouth = double(ppNew[MOUTH]->r.width) * scale;
    double h_mouth = double(ppNew[MOUTH]->r.height) * scale;
    energy +=
        int(512.0 * (e_prev + 16.0 * e_templ)) +
        4 * pow2(ppNew[LEYE]->r.width - ppNew[REYE]->r.width) + 
        4 * pow2(ppNew[LEYE]->r.height - ppNew[REYE]->r.height) + 
        4 * (int)pow(w_eye - double(rTempl[LEYE].width + rTempl[REYE].width) / 2.0, 2) + 
        2 * (int)pow(h_eye - double(rTempl[LEYE].height + rTempl[REYE].height) / 2.0, 2) + 
        1 * (int)pow(w_mouth - double(rTempl[MOUTH].width), 2) + 
        1 * (int)pow(h_mouth - double(rTempl[MOUTH].height), 2) + 
        0;
    return energy;
};

inline int GetEnergy2(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl, int* element)
{
    CvPoint new_v = {ppNew[element[0]]->ptCenter.x - ppNew[element[1]]->ptCenter.x,
        ppNew[element[0]]->ptCenter.y - ppNew[element[1]]->ptCenter.y};
    CvPoint prev_v = {pPrev[element[0]].ptCenter.x - pPrev[element[1]].ptCenter.x,
        pPrev[element[0]].ptCenter.y - pPrev[element[1]].ptCenter.y};
    double new_d = sqrt(pow2(new_v.x) + pow2(new_v.y));
    double prev_d = sqrt(pow2(prev_v.x) + pow2(prev_v.y));
    double templ_d = sqrt(pow2(ptTempl[element[0]].x - ptTempl[element[1]].x) +
        pow2(ptTempl[element[0]].y - ptTempl[element[1]].y));
    double scale_templ = new_d / templ_d;
    double w0 = (double)ppNew[element[0]]->r.width * scale_templ;
    double h0 = (double)ppNew[element[0]]->r.height * scale_templ;
    double w1 = (double)ppNew[element[1]]->r.width * scale_templ;
    double h1 = (double)ppNew[element[1]]->r.height * scale_templ;
    
    int energy = ppNew[element[0]]->iEnergy + ppNew[element[1]]->iEnergy +
        - 2 * (ppNew[element[0]]->nRectsInThis - ppNew[element[1]]->nRectsInThis) + 
        (int)pow(w0 - (double)rTempl[element[0]].width, 2) +
        (int)pow(h0 - (double)rTempl[element[0]].height, 2) +
        (int)pow(w1 - (double)rTempl[element[1]].width, 2) +
        (int)pow(h1 - (double)rTempl[element[1]].height, 2) +
        (int)pow(new_d - prev_d, 2) +
        0;
    
    return energy;
};

inline double CalculateTransformationLMS3( CvPoint* pTemplPoints, 
                                   CvPoint* pSrcPoints,
                                   double*       pdbAverageScale,
                                   double*       pdbAverageRotate,
                                   double*       pdbAverageShiftX,
                                   double*       pdbAverageShiftY )
{
//    double WS = 0;
    double dbAverageScale = 1;
    double dbAverageRotate = 0;
    double dbAverageShiftX = 0;
    double dbAverageShiftY = 0;
    double dbLMS = 0;

    _ASSERT( NULL != pTemplPoints);
    _ASSERT( NULL != pSrcPoints);

    double dbXt = double(pTemplPoints[0].x + pTemplPoints[1].x + pTemplPoints[2].x) / 3.0;
    double dbYt = double(pTemplPoints[0].y + pTemplPoints[1].y + pTemplPoints[2].y ) / 3.0;
    double dbXs = double(pSrcPoints[0].x + pSrcPoints[1].x + pSrcPoints[2].x) / 3.0;
    double dbYs = double(pSrcPoints[0].y + pSrcPoints[1].y + pSrcPoints[2].y) / 3.0;
    
    double dbXtXt = double(pow2(pTemplPoints[0].x) + pow2(pTemplPoints[1].x) + pow2(pTemplPoints[2].x)) / 3.0;
    double dbYtYt = double(pow2(pTemplPoints[0].y) + pow2(pTemplPoints[1].y) + pow2(pTemplPoints[2].y)) / 3.0;
    
    double dbXsXs = double(pow2(pSrcPoints[0].x) + pow2(pSrcPoints[1].x) + pow2(pSrcPoints[2].x)) / 3.0;
    double dbYsYs = double(pow2(pSrcPoints[0].y) + pow2(pSrcPoints[1].y) + pow2(pSrcPoints[2].y)) / 3.0;
    
    double dbXtXs = double(pTemplPoints[0].x * pSrcPoints[0].x + 
        pTemplPoints[1].x * pSrcPoints[1].x + 
        pTemplPoints[2].x * pSrcPoints[2].x) / 3.0;
    double dbYtYs = double(pTemplPoints[0].y * pSrcPoints[0].y + 
        pTemplPoints[1].y * pSrcPoints[1].y + 
        pTemplPoints[2].y * pSrcPoints[2].y) / 3.0;
    
    double dbXtYs = double(pTemplPoints[0].x * pSrcPoints[0].y + 
        pTemplPoints[1].x * pSrcPoints[1].y + 
        pTemplPoints[2].x * pSrcPoints[2].y) / 3.0;
    double dbYtXs = double(pTemplPoints[0].y * pSrcPoints[0].x + 
        pTemplPoints[1].y * pSrcPoints[1].x + 
        pTemplPoints[2].y * pSrcPoints[2].x ) / 3.0;
    
    dbXtXt -= dbXt * dbXt;
    dbYtYt -= dbYt * dbYt;
    
    dbXsXs -= dbXs * dbXs;
    dbYsYs -= dbYs * dbYs;
    
    dbXtXs -= dbXt * dbXs;
    dbYtYs -= dbYt * dbYs;
    
    dbXtYs -= dbXt * dbYs;
    dbYtXs -= dbYt * dbXs;
    
    dbAverageRotate = atan2( dbXtYs - dbYtXs, dbXtXs + dbYtYs );
    
    double cosR = cos(dbAverageRotate);
    double sinR = sin(dbAverageRotate);
    double del = dbXsXs + dbYsYs;
    if( del != 0 )
    {
        dbAverageScale = (double(dbXtXs + dbYtYs) * cosR + double(dbXtYs - dbYtXs) * sinR) / del;
        dbLMS = dbXtXt + dbYtYt - ((double)pow(dbXtXs + dbYtYs,2) + (double)pow(dbXtYs - dbYtXs,2)) / del;
    }
    
    dbAverageShiftX = double(dbXt) - dbAverageScale * (double(dbXs) * cosR + double(dbYs) * sinR);
    dbAverageShiftY = double(dbYt) - dbAverageScale * (double(dbYs) * cosR - double(dbXs) * sinR);
    
    if( pdbAverageScale != NULL ) *pdbAverageScale = dbAverageScale;
    if( pdbAverageRotate != NULL ) *pdbAverageRotate = dbAverageRotate;
    if( pdbAverageShiftX != NULL ) *pdbAverageShiftX = dbAverageShiftX;
    if( pdbAverageShiftY != NULL ) *pdbAverageShiftY = dbAverageShiftY;
    
    _ASSERT(dbLMS >= 0);
    return dbLMS;
}

inline double CalculateTransformationLMS3_0( CvPoint* pTemplPoints, CvPoint* pSrcPoints)
{
    double dbLMS = 0;

    _ASSERT( NULL != pTemplPoints);
    _ASSERT( NULL != pSrcPoints);

    double dbXt = double(pTemplPoints[0].x + pTemplPoints[1].x + pTemplPoints[2].x) / 3.0;
    double dbYt = double(pTemplPoints[0].y + pTemplPoints[1].y + pTemplPoints[2].y ) / 3.0;
    double dbXs = double(pSrcPoints[0].x + pSrcPoints[1].x + pSrcPoints[2].x) / 3.0;
    double dbYs = double(pSrcPoints[0].y + pSrcPoints[1].y + pSrcPoints[2].y) / 3.0;
    
    double dbXtXt = double(pow2(pTemplPoints[0].x) + pow2(pTemplPoints[1].x) + pow2(pTemplPoints[2].x)) / 3.0;
    double dbYtYt = double(pow2(pTemplPoints[0].y) + pow2(pTemplPoints[1].y) + pow2(pTemplPoints[2].y)) / 3.0;
    
    double dbXsXs = double(pow2(pSrcPoints[0].x) + pow2(pSrcPoints[1].x) + pow2(pSrcPoints[2].x)) / 3.0;
    double dbYsYs = double(pow2(pSrcPoints[0].y) + pow2(pSrcPoints[1].y) + pow2(pSrcPoints[2].y)) / 3.0;
    
    double dbXtXs = double(pTemplPoints[0].x * pSrcPoints[0].x + 
        pTemplPoints[1].x * pSrcPoints[1].x + 
        pTemplPoints[2].x * pSrcPoints[2].x) / 3.0;
    double dbYtYs = double(pTemplPoints[0].y * pSrcPoints[0].y + 
        pTemplPoints[1].y * pSrcPoints[1].y + 
        pTemplPoints[2].y * pSrcPoints[2].y) / 3.0;
    
    double dbXtYs = double(pTemplPoints[0].x * pSrcPoints[0].y + 
        pTemplPoints[1].x * pSrcPoints[1].y + 
        pTemplPoints[2].x * pSrcPoints[2].y) / 3.0;
    double dbYtXs = double(pTemplPoints[0].y * pSrcPoints[0].x + 
        pTemplPoints[1].y * pSrcPoints[1].x + 
        pTemplPoints[2].y * pSrcPoints[2].x ) / 3.0;
    
    dbXtXt -= dbXt * dbXt;
    dbYtYt -= dbYt * dbYt;
    
    dbXsXs -= dbXs * dbXs;
    dbYsYs -= dbYs * dbYs;
    
    dbXtXs -= dbXt * dbXs;
    dbYtYs -= dbYt * dbYs;
    
    dbXtYs -= dbXt * dbYs;
    dbYtXs -= dbYt * dbXs;
    
    double del = dbXsXs + dbYsYs;
    if( del != 0 )
        dbLMS = dbXtXt + dbYtYt - ((double)pow(dbXtXs + dbYtYs,2) + (double)pow(dbXtYs - dbYtXs,2)) / del;
    return dbLMS;
}