1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
42 /****************************************************************************************\
43 Contour-based face feature tracking
44 The code was created by Tatiana Cherepanova (tata@sl.iae.nsk.su)
45 \****************************************************************************************/
47 #include "precomp.hpp"
48 #include "_vectrack.h"
50 #define NUM_FACE_ELEMENTS 3
60 const double pi = 3.1415926535;
63 struct CvTrackingRect;
66 void ThresholdingParam(IplImage *imgGray, int iNumLayers, int &iMinLevel, int &iMaxLevel, float &step, float& power, int iHistMin /*= HIST_MIN*/);
67 int ChoiceTrackingFace3(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy);
68 int ChoiceTrackingFace2(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy, int noel);
69 inline int GetEnergy(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl);
70 inline int GetEnergy2(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl, int* element);
71 inline double CalculateTransformationLMS3_0( CvPoint* pTemplPoints, CvPoint* pSrcPoints);
72 inline double CalculateTransformationLMS3( CvPoint* pTemplPoints,
74 double* pdbAverageScale,
75 double* pdbAverageRotate,
76 double* pdbAverageShiftX,
77 double* pdbAverageShiftY );
90 CvTrackingRect() { memset(this, 0, sizeof(CvTrackingRect)); };
91 int Energy(const CvTrackingRect& prev)
93 int prev_color = 0 == prev.iColor ? iColor : prev.iColor;
94 iEnergy = 1 * pow2(r.width - prev.r.width) +
95 1 * pow2(r.height - prev.r.height) +
96 1 * pow2(iColor - prev_color) / 4 +
100 + 0 * nRectsOnRight +
101 + 0 * nRectsOnBottom;
108 CvTrackingRect face[NUM_FACE_ELEMENTS];
109 int iTrackingFaceType;
110 double dbRotateDelta;
111 double dbRotateAngle;
114 CvPoint ptTempl[NUM_FACE_ELEMENTS];
115 CvRect rTempl[NUM_FACE_ELEMENTS];
119 CvMemStorage* mstgContours;
126 iTrackingFaceType = -1;
135 if (NULL != imgThresh)
137 if (NULL != mstgContours)
138 cvReleaseMemStorage(&mstgContours);
140 int Init(CvRect* pRects, IplImage* imgray)
142 for (int i = 0; i < NUM_FACE_ELEMENTS; i++)
144 face[i].r = pRects[i];
145 face[i].ptCenter = Center(face[i].r);
146 ptTempl[i] = face[i].ptCenter;
147 rTempl[i] = face[i].r;
149 imgray = cvCreateImage(cvSize(imgray->width, imgray->height), 8, 1);
150 imgThresh = cvCreateImage(cvSize(imgray->width, imgray->height), 8, 1);
151 mstgContours = cvCreateMemStorage();
152 if ((NULL == imgray) ||
153 (NULL == imgThresh) ||
154 (NULL == mstgContours))
158 int InitNextImage(IplImage* img)
160 CvSize sz(img->width, img->height);
161 ReallocImage(&imgGray, sz, 1);
162 ReallocImage(&imgThresh, sz, 1);
163 ptRotate = face[MOUTH].ptCenter;
165 CvMat mat = cvMat( 2, 3, CV_32FC1, m );
167 if (NULL == imgGray || NULL == imgThresh)
170 /*m[0] = (float)cos(-dbRotateAngle*CV_PI/180.);
171 m[1] = (float)sin(-dbRotateAngle*CV_PI/180.);
172 m[2] = (float)ptRotate.x;
175 m[5] = (float)ptRotate.y;*/
176 cv2DRotationMatrix( cvPointTo32f(ptRotate), -dbRotateAngle, 1., &mat );
177 cvWarpAffine( img, imgGray, &mat );
179 if (NULL == mstgContours)
180 mstgContours = cvCreateMemStorage();
182 cvClearMemStorage(mstgContours);
183 if (NULL == mstgContours)
193 CvMemStorage* m_mstgRects;
195 CvTrackingRect m_trPrev;
196 inline CvFaceElement()
205 inline int Init(const CvRect& roi, const CvTrackingRect& prev, CvMemStorage* mstg = NULL)
211 if (NULL == m_mstgRects)
213 if (NULL == m_seqRects)
214 m_seqRects = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvTrackingRect), m_mstgRects);
216 cvClearSeq(m_seqRects);
217 if (NULL == m_seqRects)
221 void FindRects(IplImage* img, IplImage* thresh, int nLayers, int dMinSize);
223 void FindContours(IplImage* img, IplImage* thresh, int nLayers, int dMinSize);
224 void MergeRects(int d);
226 }; //class CvFaceElement
228 inline int CV_CDECL CompareEnergy(const void* el1, const void* el2, void*)
230 return ((CvTrackingRect*)el1)->iEnergy - ((CvTrackingRect*)el2)->iEnergy;
231 }// int CV_CDECL CompareEnergy(const void* el1, const void* el2, void*)
233 void CvFaceElement::FindRects(IplImage* img, IplImage* thresh, int nLayers, int dMinSize)
235 FindContours(img, thresh, nLayers, dMinSize / 4);
236 if (0 == m_seqRects->total)
239 cvSeqSort(m_seqRects, CompareEnergy, NULL);
240 CvTrackingRect* pR = (CvTrackingRect*)cvGetSeqElem(m_seqRects, 0);
241 if (m_seqRects->total < 32)
243 MergeRects(dMinSize / 8);
245 cvSeqSort(m_seqRects, CompareEnergy, NULL);
247 pR = (CvTrackingRect*)cvGetSeqElem(m_seqRects, 0);
248 if ((pR->iEnergy > 100 && m_seqRects->total < 32) || (m_seqRects->total < 16))
250 MergeRects(dMinSize / 4);
252 cvSeqSort(m_seqRects, CompareEnergy, NULL);
254 pR = (CvTrackingRect*)cvGetSeqElem(m_seqRects, 0);
255 if ((pR->iEnergy > 100 && m_seqRects->total < 16) || (pR->iEnergy > 200 && m_seqRects->total < 32))
257 MergeRects(dMinSize / 2);
259 cvSeqSort(m_seqRects, CompareEnergy, NULL);
262 }// void CvFaceElement::FindRects(IplImage* img, IplImage* thresh, int nLayers, int dMinSize)
264 void CvFaceElement::FindContours(IplImage* img, IplImage* thresh, int nLayers, int dMinSize)
269 cvSetImageROI(img, roi);
270 cvSetImageROI(thresh, roi);
272 int colors[MAX_LAYERS] = {0};
273 int iMinLevel = 0, iMaxLevel = 255;
275 ThresholdingParam(img, nLayers / 2, iMinLevel, iMaxLevel, step, power, 4);
276 int iMinLevelPrev = iMinLevel;
277 int iMaxLevelPrev = iMinLevel;
278 if (m_trPrev.iColor != 0)
280 iMinLevelPrev = m_trPrev.iColor - nLayers / 2;
281 iMaxLevelPrev = m_trPrev.iColor + nLayers / 2;
283 if (iMinLevelPrev < iMinLevel)
285 iMaxLevelPrev += iMinLevel - iMinLevelPrev;
286 iMinLevelPrev = iMinLevel;
288 if (iMaxLevelPrev > iMaxLevel)
290 iMinLevelPrev -= iMaxLevelPrev - iMaxLevel;
291 if (iMinLevelPrev < iMinLevel)
292 iMinLevelPrev = iMinLevel;
293 iMaxLevelPrev = iMaxLevel;
296 n -= (iMaxLevelPrev - iMinLevelPrev + 1) / 2;
297 step = float(iMinLevelPrev - iMinLevel + iMaxLevel - iMaxLevelPrev) / float(n);
300 for (level = (float)iMinLevel; level < iMinLevelPrev && j < nLayers; level += step, j++)
301 colors[j] = int(level + 0.5);
302 for (level = (float)iMinLevelPrev; level < iMaxLevelPrev && j < nLayers; level += 2.0, j++)
303 colors[j] = int(level + 0.5);
304 for (level = (float)iMaxLevelPrev; level < iMaxLevel && j < nLayers; level += step, j++)
305 colors[j] = int(level + 0.5);
307 for (int i = 0; i < nLayers; i++)
309 cvThreshold(img, thresh, colors[i], 255.0, CV_THRESH_BINARY);
310 if (cvFindContours(thresh, m_mstgRects, &seq, sizeof(CvContour), CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE))
313 for (CvSeq* external = seq; external; external = external->h_next)
315 cr.r = cvContourBoundingRect(external);
316 Move(cr.r, roi.x, roi.y);
317 if (RectInRect(cr.r, m_rROI) && cr.r.width > dMinSize && cr.r.height > dMinSize)
319 cr.ptCenter = Center(cr.r);
320 cr.iColor = colors[i];
321 cvSeqPush(m_seqRects, &cr);
323 for (CvSeq* internal = external->v_next; internal; internal = internal->h_next)
325 cr.r = cvContourBoundingRect(internal);
326 Move(cr.r, roi.x, roi.y);
327 if (RectInRect(cr.r, m_rROI) && cr.r.width > dMinSize && cr.r.height > dMinSize)
329 cr.ptCenter = Center(cr.r);
330 cr.iColor = colors[i];
331 cvSeqPush(m_seqRects, &cr);
338 cvResetImageROI(img);
339 cvResetImageROI(thresh);
340 }//void CvFaceElement::FindContours(IplImage* img, IplImage* thresh, int nLayers)
342 void CvFaceElement::MergeRects(int d)
344 int nRects = m_seqRects->total;
345 CvSeqReader reader, reader2;
346 cvStartReadSeq( m_seqRects, &reader );
348 for (i = 0; i < nRects; i++)
350 CvTrackingRect* pRect1 = (CvTrackingRect*)(reader.ptr);
351 cvStartReadSeq( m_seqRects, &reader2 );
352 cvSetSeqReaderPos(&reader2, i + 1);
353 for (j = i + 1; j < nRects; j++)
355 CvTrackingRect* pRect2 = (CvTrackingRect*)(reader2.ptr);
356 if (abs(pRect1->ptCenter.y - pRect2->ptCenter.y) < d &&
357 abs(pRect1->r.height - pRect2->r.height) < d)
360 rNew.iColor = (pRect1->iColor + pRect2->iColor + 1) / 2;
361 rNew.r.x = min(pRect1->r.x, pRect2->r.x);
362 rNew.r.y = min(pRect1->r.y, pRect2->r.y);
363 rNew.r.width = max(pRect1->r.x + pRect1->r.width, pRect2->r.x + pRect2->r.width) - rNew.r.x;
364 rNew.r.height = min(pRect1->r.y + pRect1->r.height, pRect2->r.y + pRect2->r.height) - rNew.r.y;
365 if (rNew.r != pRect1->r && rNew.r != pRect2->r)
367 rNew.ptCenter = Center(rNew.r);
368 cvSeqPush(m_seqRects, &rNew);
371 CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader2 );
373 CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader );
375 // delete equal rects
376 for (i = 0; i < m_seqRects->total; i++)
378 CvTrackingRect* pRect1 = (CvTrackingRect*)cvGetSeqElem(m_seqRects, i);
380 for (j = j_begin; j < m_seqRects->total;)
382 CvTrackingRect* pRect2 = (CvTrackingRect*)cvGetSeqElem(m_seqRects, j);
383 if (pRect1->r == pRect2->r)
384 cvSeqRemove(m_seqRects, j);
390 }//void CvFaceElement::MergeRects(int d)
392 void CvFaceElement::Energy()
394 CvSeqReader reader, reader2;
395 cvStartReadSeq( m_seqRects, &reader );
396 for (int i = 0; i < m_seqRects->total; i++)
398 CvTrackingRect* pRect = (CvTrackingRect*)(reader.ptr);
399 // outside and inside rects
400 cvStartReadSeq( m_seqRects, &reader2 );
401 for (int j = 0; j < m_seqRects->total; j++)
403 CvTrackingRect* pRect2 = (CvTrackingRect*)(reader2.ptr);
406 if (RectInRect(pRect2->r, pRect->r))
407 pRect->nRectsInThis ++;
408 else if (pRect2->r.y + pRect2->r.height <= pRect->r.y)
409 pRect->nRectsOnTop ++;
410 else if (pRect2->r.y >= pRect->r.y + pRect->r.height)
411 pRect->nRectsOnBottom ++;
412 else if (pRect2->r.x + pRect2->r.width <= pRect->r.x)
413 pRect->nRectsOnLeft ++;
414 else if (pRect2->r.x >= pRect->r.x + pRect->r.width)
415 pRect->nRectsOnRight ++;
417 CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader2 );
420 pRect->Energy(m_trPrev);
421 CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader );
423 }//void CvFaceElement::Energy()
425 CV_IMPL CvFaceTracker*
426 cvInitFaceTracker(CvFaceTracker* pFaceTracker, const IplImage* imgGray, CvRect* pRects, int nRects)
428 assert(NULL != imgGray);
429 assert(NULL != pRects);
430 assert(nRects >= NUM_FACE_ELEMENTS);
431 if ((NULL == imgGray) ||
433 (nRects < NUM_FACE_ELEMENTS))
437 CvFaceTracker* pFace = pFaceTracker;
440 pFace = new CvFaceTracker;
445 pFace->Init(pRects, (IplImage*)imgGray);
447 }//CvFaceTracker* InitFaceTracker(IplImage* imgGray, CvRect* pRects, int nRects)
450 cvReleaseFaceTracker(CvFaceTracker** ppFaceTracker)
452 if (NULL == *ppFaceTracker)
454 delete *ppFaceTracker;
455 *ppFaceTracker = NULL;
456 }//void ReleaseFaceTracker(CvFaceTracker** ppFaceTracker)
460 cvTrackFace(CvFaceTracker* pFaceTracker, IplImage* imgGray, CvRect* pRects, int nRects, CvPoint* ptRotate, double* dbAngleRotate)
462 assert(NULL != pFaceTracker);
463 assert(NULL != imgGray);
464 assert(NULL != pRects && nRects >= NUM_FACE_ELEMENTS);
465 if ((NULL == pFaceTracker) ||
468 pFaceTracker->InitNextImage(imgGray);
469 *ptRotate = pFaceTracker->ptRotate;
470 *dbAngleRotate = pFaceTracker->dbRotateAngle;
473 double dx = pFaceTracker->face[LEYE].ptCenter.x - pFaceTracker->face[REYE].ptCenter.x;
474 double dy = pFaceTracker->face[LEYE].ptCenter.y - pFaceTracker->face[REYE].ptCenter.y;
475 double d_eyes = sqrt(dx*dx + dy*dy);
476 int d = cvRound(0.25 * d_eyes);
482 CvFaceElement big_face[NUM_FACE_ELEMENTS];
485 for (elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
487 CvRect r = pFaceTracker->face[elem].r;
491 r.x -= (4*d - r.width) / 2;
492 r.width += 4*d - r.width;
496 r.y -= (3*d - r.height) / 2;
497 r.height += 3*d - r.height;
503 if (r.x + r.width > pFaceTracker->imgGray->width - 2)
504 r.width = pFaceTracker->imgGray->width - 2 - r.x;
505 if (r.y + r.height > pFaceTracker->imgGray->height - 2)
506 r.height = pFaceTracker->imgGray->height - 2 - r.y;
507 if (!big_face[elem].Init(r, pFaceTracker->face[elem], pFaceTracker->mstgContours))
511 for (elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
512 big_face[elem].FindRects(pFaceTracker->imgGray, pFaceTracker->imgThresh, 32, dMinSize);
514 CvTrackingRect new_face[NUM_FACE_ELEMENTS];
516 int found = ChoiceTrackingFace3(pFaceTracker, nElements, big_face, new_face, new_energy);
522 if (new_energy > 100000 && -1 != pFaceTracker->iTrackingFaceType)
524 else if (new_energy > 150000)
527 for (int el = 0; el < NUM_FACE_ELEMENTS; el++)
529 if (big_face[el].m_seqRects->total > 16 || (big_face[el].m_seqRects->total > 8 && new_face[el].iEnergy < 100))
542 if (-1 != pFaceTracker->iTrackingFaceType)
558 if (-1 != pFaceTracker->iTrackingFaceType)
559 noel = pFaceTracker->iTrackingFaceType;
560 int found2 = ChoiceTrackingFace2(pFaceTracker, nElements, big_face, new_face, new_energy, noel);
561 if (found2 && new_energy < 100000)
563 pFaceTracker->iTrackingFaceType = noel;
575 // angle by mouth & eyes
576 double vx_prev = double(pFaceTracker->face[LEYE].ptCenter.x + pFaceTracker->face[REYE].ptCenter.x) / 2.0 - pFaceTracker->face[MOUTH].ptCenter.x;
577 double vy_prev = double(pFaceTracker->face[LEYE].ptCenter.y + pFaceTracker->face[REYE].ptCenter.y) / 2.0 - pFaceTracker->face[MOUTH].ptCenter.y;
578 double vx_prev1 = vx_prev * cos(pFaceTracker->dbRotateDelta) - vy_prev * sin(pFaceTracker->dbRotateDelta);
579 double vy_prev1 = vx_prev * sin(pFaceTracker->dbRotateDelta) + vy_prev * cos(pFaceTracker->dbRotateDelta);
582 for (elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
583 pFaceTracker->face[elem] = new_face[elem];
584 double vx = double(pFaceTracker->face[LEYE].ptCenter.x + pFaceTracker->face[REYE].ptCenter.x) / 2.0 - pFaceTracker->face[MOUTH].ptCenter.x;
585 double vy = double(pFaceTracker->face[LEYE].ptCenter.y + pFaceTracker->face[REYE].ptCenter.y) / 2.0 - pFaceTracker->face[MOUTH].ptCenter.y;
586 pFaceTracker->dbRotateDelta = 0;
587 double n1_n2 = (vx * vx + vy * vy) * (vx_prev * vx_prev + vy_prev * vy_prev);
589 pFaceTracker->dbRotateDelta = asin((vx * vy_prev - vx_prev * vy) / sqrt(n1_n2));
590 pFaceTracker->dbRotateAngle -= pFaceTracker->dbRotateDelta;
594 pFaceTracker->dbRotateDelta = 0;
595 pFaceTracker->dbRotateAngle = 0;
597 if ((pFaceTracker->dbRotateAngle >= pi/2 && pFaceTracker->dbRotateAngle > 0) ||
598 (pFaceTracker->dbRotateAngle <= -pi/2 && pFaceTracker->dbRotateAngle < 0))
600 pFaceTracker->dbRotateDelta = 0;
601 pFaceTracker->dbRotateAngle = 0;
606 for (int i = 0; i < NUM_FACE_ELEMENTS && i < nRects; i++)
607 pRects[i] = pFaceTracker->face[i].r;
610 }//int FindFaceTracker(CvFaceTracker* pFaceTracker, IplImage* imgGray, CvRect* pRects, int nRects, CvPoint& ptRotate, double& dbAngleRotate)
612 void ThresholdingParam(IplImage *imgGray, int iNumLayers, int &iMinLevel, int &iMaxLevel, float &step, float& power, int iHistMin /*= HIST_MIN*/)
614 assert(imgGray != NULL);
615 assert(imgGray->nChannels == 1);
618 int histImg[256] = {0};
619 uchar* buffImg = (uchar*)imgGray->imageData;
620 CvRect rROI = cvGetImageROI(imgGray);
621 buffImg += rROI.y * imgGray->widthStep + rROI.x;
622 for (j = 0; j < rROI.height; j++)
624 for (i = 0; i < rROI.width; i++)
625 histImg[buffImg[i]] ++;
626 buffImg += imgGray->widthStep;
629 for (i = 0; i < 256; i++)
631 if (histImg[i] > iHistMin)
635 for (i = 255; i >= 0; i--)
637 if (histImg[i] > iHistMin)
641 if (iMaxLevel <= iMinLevel)
649 for (i = iMinLevel; i < (iMinLevel + iMaxLevel) / 2; i++)
651 for (i = (iMinLevel + iMaxLevel) / 2; i < iMaxLevel; i++)
653 power = float(black) / float(2 * white);
655 step = float(iMaxLevel - iMinLevel) / float(iNumLayers);
658 }// void ThresholdingParam(IplImage *imgGray, int iNumLayers, int &iMinLevel, int &iMaxLevel, int &iStep)
660 int ChoiceTrackingFace3(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy)
662 CvTrackingRect* curr_face[NUM_FACE_ELEMENTS] = {NULL};
663 CvTrackingRect* new_face[NUM_FACE_ELEMENTS] = {NULL};
664 new_energy = 0x7fffffff;
665 int curr_energy = 0x7fffffff;
668 CvSeqReader reader_m, reader_l, reader_r;
669 cvStartReadSeq( big_face[MOUTH].m_seqRects, &reader_m );
670 for (int i_mouth = 0; i_mouth < big_face[MOUTH].m_seqRects->total && i_mouth < nElements; i_mouth++)
672 curr_face[MOUTH] = (CvTrackingRect*)(reader_m.ptr);
673 cvStartReadSeq( big_face[LEYE].m_seqRects, &reader_l );
674 for (int i_left = 0; i_left < big_face[LEYE].m_seqRects->total && i_left < nElements; i_left++)
676 curr_face[LEYE] = (CvTrackingRect*)(reader_l.ptr);
677 if (curr_face[LEYE]->r.y + curr_face[LEYE]->r.height < curr_face[MOUTH]->r.y)
679 cvStartReadSeq( big_face[REYE].m_seqRects, &reader_r );
680 for (int i_right = 0; i_right < big_face[REYE].m_seqRects->total && i_right < nElements; i_right++)
682 curr_face[REYE] = (CvTrackingRect*)(reader_r.ptr);
683 if (curr_face[REYE]->r.y + curr_face[REYE]->r.height < curr_face[MOUTH]->r.y &&
684 curr_face[REYE]->r.x > curr_face[LEYE]->r.x + curr_face[LEYE]->r.width)
686 curr_energy = GetEnergy(curr_face, pTF->face, pTF->ptTempl, pTF->rTempl);
687 if (curr_energy < new_energy)
689 for (int elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
690 new_face[elem] = curr_face[elem];
691 new_energy = curr_energy;
702 for (int elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
703 face[elem] = *(new_face[elem]);
706 } // int ChoiceTrackingFace3(const CvTrackingRect* tr_face, CvTrackingRect* new_face, int& new_energy)
708 int ChoiceTrackingFace2(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy, int noel)
710 int element[NUM_FACE_ELEMENTS];
711 for (int i = 0, elem = 0; i < NUM_FACE_ELEMENTS; i++)
721 CvTrackingRect* curr_face[NUM_FACE_ELEMENTS] = {NULL};
722 CvTrackingRect* new_face[NUM_FACE_ELEMENTS] = {NULL};
723 new_energy = 0x7fffffff;
724 int curr_energy = 0x7fffffff;
727 CvSeqReader reader0, reader1;
728 cvStartReadSeq( big_face[element[0]].m_seqRects, &reader0 );
729 for (int i0 = 0; i0 < big_face[element[0]].m_seqRects->total && i0 < nElements; i0++)
731 curr_face[element[0]] = (CvTrackingRect*)(reader0.ptr);
732 cvStartReadSeq( big_face[element[1]].m_seqRects, &reader1 );
733 for (int i1 = 0; i1 < big_face[element[1]].m_seqRects->total && i1 < nElements; i1++)
735 curr_face[element[1]] = (CvTrackingRect*)(reader1.ptr);
736 curr_energy = GetEnergy2(curr_face, pTF->face, pTF->ptTempl, pTF->rTempl, element);
737 if (curr_energy < new_energy)
739 for (int elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
740 new_face[elem] = curr_face[elem];
741 new_energy = curr_energy;
749 face[element[0]] = *(new_face[element[0]]);
750 face[element[1]] = *(new_face[element[1]]);
751 // 3 element find by template
752 CvPoint templ_v01(pTF->ptTempl[element[1]].x - pTF->ptTempl[element[0]].x, pTF->ptTempl[element[1]].y - pTF->ptTempl[element[0]].y);
753 CvPoint templ_v02(pTF->ptTempl[element[2]].x - pTF->ptTempl[element[0]].x, pTF->ptTempl[element[2]].y - pTF->ptTempl[element[0]].y);
754 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);
755 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);
756 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);
757 double templ_d01 = sqrt((double)templ_v01.x*templ_v01.x + templ_v01.y*templ_v01.y);
758 double templ_d02 = sqrt((double)templ_v02.x*templ_v02.x + templ_v02.y*templ_v02.y);
759 double prev_d01 = sqrt((double)prev_v01.x*prev_v01.x + prev_v01.y*prev_v01.y);
760 double prev_d02 = sqrt((double)prev_v02.x*prev_v02.x + prev_v02.y*prev_v02.y);
761 double new_d01 = sqrt((double)new_v01.x*new_v01.x + new_v01.y*new_v01.y);
762 double scale = templ_d01 / new_d01;
763 double new_d02 = templ_d02 / scale;
764 double sin_a = double(prev_v01.x * prev_v02.y - prev_v01.y * prev_v02.x) / (prev_d01 * prev_d02);
765 double cos_a = cos(asin(sin_a));
766 double x = double(new_v01.x) * cos_a - double(new_v01.y) * sin_a;
767 double y = double(new_v01.x) * sin_a + double(new_v01.y) * cos_a;
768 x = x * new_d02 / new_d01;
769 y = y * new_d02 / new_d01;
770 CvPoint new_v02(int(x + 0.5), int(y + 0.5));
771 face[element[2]].iColor = 0;
772 face[element[2]].iEnergy = 0;
773 face[element[2]].nRectsInThis = 0;
774 face[element[2]].nRectsOnBottom = 0;
775 face[element[2]].nRectsOnLeft = 0;
776 face[element[2]].nRectsOnRight = 0;
777 face[element[2]].nRectsOnTop = 0;
778 face[element[2]].ptCenter.x = new_v02.x + new_face[element[0]]->ptCenter.x;
779 face[element[2]].ptCenter.y = new_v02.y + new_face[element[0]]->ptCenter.y;
780 face[element[2]].r.width = int(double(pTF->rTempl[element[2]].width) / (scale) + 0.5);
781 face[element[2]].r.height = int(double(pTF->rTempl[element[2]].height) / (scale) + 0.5);
782 face[element[2]].r.x = face[element[2]].ptCenter.x - (face[element[2]].r.width + 1) / 2;
783 face[element[2]].r.y = face[element[2]].ptCenter.y - (face[element[2]].r.height + 1) / 2;
784 assert(face[LEYE].r.x + face[LEYE].r.width <= face[REYE].r.x);
787 } // int ChoiceTrackingFace3(const CvTrackingRect* tr_face, CvTrackingRect* new_face, int& new_energy)
789 inline int GetEnergy(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl)
792 CvPoint ptNew[NUM_FACE_ELEMENTS];
793 CvPoint ptPrev[NUM_FACE_ELEMENTS];
794 for (int i = 0; i < NUM_FACE_ELEMENTS; i++)
796 ptNew[i] = ppNew[i]->ptCenter;
797 ptPrev[i] = pPrev[i].ptCenter;
798 energy += ppNew[i]->iEnergy - 2 * ppNew[i]->nRectsInThis;
800 double dx = 0, dy = 0, scale = 1, rotate = 0;
801 double e_templ = CalculateTransformationLMS3(ptTempl, ptNew, &scale, &rotate, &dx, &dy);
802 double e_prev = CalculateTransformationLMS3_0(ptPrev, ptNew);
803 double w_eye = double(ppNew[LEYE]->r.width + ppNew[REYE]->r.width) * scale / 2.0;
804 double h_eye = double(ppNew[LEYE]->r.height + ppNew[REYE]->r.height) * scale / 2.0;
805 double w_mouth = double(ppNew[MOUTH]->r.width) * scale;
806 double h_mouth = double(ppNew[MOUTH]->r.height) * scale;
808 int(512.0 * (e_prev + 16.0 * e_templ)) +
809 4 * pow2(ppNew[LEYE]->r.width - ppNew[REYE]->r.width) +
810 4 * pow2(ppNew[LEYE]->r.height - ppNew[REYE]->r.height) +
811 4 * (int)pow(w_eye - double(rTempl[LEYE].width + rTempl[REYE].width) / 2.0, 2) +
812 2 * (int)pow(h_eye - double(rTempl[LEYE].height + rTempl[REYE].height) / 2.0, 2) +
813 1 * (int)pow(w_mouth - double(rTempl[MOUTH].width), 2) +
814 1 * (int)pow(h_mouth - double(rTempl[MOUTH].height), 2) +
819 inline int GetEnergy2(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl, int* element)
821 CvPoint new_v(ppNew[element[0]]->ptCenter.x - ppNew[element[1]]->ptCenter.x,
822 ppNew[element[0]]->ptCenter.y - ppNew[element[1]]->ptCenter.y);
823 CvPoint prev_v(pPrev[element[0]].ptCenter.x - pPrev[element[1]].ptCenter.x,
824 pPrev[element[0]].ptCenter.y - pPrev[element[1]].ptCenter.y);
825 double new_d = sqrt((double)new_v.x*new_v.x + new_v.y*new_v.y);
826 double prev_d = sqrt((double)prev_v.x*prev_v.x + prev_v.y*prev_v.y);
827 double dx = ptTempl[element[0]].x - ptTempl[element[1]].x;
828 double dy = ptTempl[element[0]].y - ptTempl[element[1]].y;
829 double templ_d = sqrt(dx*dx + dy*dy);
830 double scale_templ = new_d / templ_d;
831 double w0 = (double)ppNew[element[0]]->r.width * scale_templ;
832 double h0 = (double)ppNew[element[0]]->r.height * scale_templ;
833 double w1 = (double)ppNew[element[1]]->r.width * scale_templ;
834 double h1 = (double)ppNew[element[1]]->r.height * scale_templ;
836 int energy = ppNew[element[0]]->iEnergy + ppNew[element[1]]->iEnergy +
837 - 2 * (ppNew[element[0]]->nRectsInThis - ppNew[element[1]]->nRectsInThis) +
838 (int)pow(w0 - (double)rTempl[element[0]].width, 2) +
839 (int)pow(h0 - (double)rTempl[element[0]].height, 2) +
840 (int)pow(w1 - (double)rTempl[element[1]].width, 2) +
841 (int)pow(h1 - (double)rTempl[element[1]].height, 2) +
842 (int)pow(new_d - prev_d, 2) +
848 inline double CalculateTransformationLMS3( CvPoint* pTemplPoints,
850 double* pdbAverageScale,
851 double* pdbAverageRotate,
852 double* pdbAverageShiftX,
853 double* pdbAverageShiftY )
856 double dbAverageScale = 1;
857 double dbAverageRotate = 0;
858 double dbAverageShiftX = 0;
859 double dbAverageShiftY = 0;
862 assert( NULL != pTemplPoints);
863 assert( NULL != pSrcPoints);
865 double dbXt = double(pTemplPoints[0].x + pTemplPoints[1].x + pTemplPoints[2].x) / 3.0;
866 double dbYt = double(pTemplPoints[0].y + pTemplPoints[1].y + pTemplPoints[2].y ) / 3.0;
867 double dbXs = double(pSrcPoints[0].x + pSrcPoints[1].x + pSrcPoints[2].x) / 3.0;
868 double dbYs = double(pSrcPoints[0].y + pSrcPoints[1].y + pSrcPoints[2].y) / 3.0;
870 double dbXtXt = double(pow2(pTemplPoints[0].x) + pow2(pTemplPoints[1].x) + pow2(pTemplPoints[2].x)) / 3.0;
871 double dbYtYt = double(pow2(pTemplPoints[0].y) + pow2(pTemplPoints[1].y) + pow2(pTemplPoints[2].y)) / 3.0;
873 double dbXsXs = double(pow2(pSrcPoints[0].x) + pow2(pSrcPoints[1].x) + pow2(pSrcPoints[2].x)) / 3.0;
874 double dbYsYs = double(pow2(pSrcPoints[0].y) + pow2(pSrcPoints[1].y) + pow2(pSrcPoints[2].y)) / 3.0;
876 double dbXtXs = double(pTemplPoints[0].x * pSrcPoints[0].x +
877 pTemplPoints[1].x * pSrcPoints[1].x +
878 pTemplPoints[2].x * pSrcPoints[2].x) / 3.0;
879 double dbYtYs = double(pTemplPoints[0].y * pSrcPoints[0].y +
880 pTemplPoints[1].y * pSrcPoints[1].y +
881 pTemplPoints[2].y * pSrcPoints[2].y) / 3.0;
883 double dbXtYs = double(pTemplPoints[0].x * pSrcPoints[0].y +
884 pTemplPoints[1].x * pSrcPoints[1].y +
885 pTemplPoints[2].x * pSrcPoints[2].y) / 3.0;
886 double dbYtXs = double(pTemplPoints[0].y * pSrcPoints[0].x +
887 pTemplPoints[1].y * pSrcPoints[1].x +
888 pTemplPoints[2].y * pSrcPoints[2].x ) / 3.0;
890 dbXtXt -= dbXt * dbXt;
891 dbYtYt -= dbYt * dbYt;
893 dbXsXs -= dbXs * dbXs;
894 dbYsYs -= dbYs * dbYs;
896 dbXtXs -= dbXt * dbXs;
897 dbYtYs -= dbYt * dbYs;
899 dbXtYs -= dbXt * dbYs;
900 dbYtXs -= dbYt * dbXs;
902 dbAverageRotate = atan2( dbXtYs - dbYtXs, dbXtXs + dbYtYs );
904 double cosR = cos(dbAverageRotate);
905 double sinR = sin(dbAverageRotate);
906 double del = dbXsXs + dbYsYs;
909 dbAverageScale = (double(dbXtXs + dbYtYs) * cosR + double(dbXtYs - dbYtXs) * sinR) / del;
910 dbLMS = dbXtXt + dbYtYt - ((double)pow(dbXtXs + dbYtYs,2) + (double)pow(dbXtYs - dbYtXs,2)) / del;
913 dbAverageShiftX = double(dbXt) - dbAverageScale * (double(dbXs) * cosR + double(dbYs) * sinR);
914 dbAverageShiftY = double(dbYt) - dbAverageScale * (double(dbYs) * cosR - double(dbXs) * sinR);
916 if( pdbAverageScale != NULL ) *pdbAverageScale = dbAverageScale;
917 if( pdbAverageRotate != NULL ) *pdbAverageRotate = dbAverageRotate;
918 if( pdbAverageShiftX != NULL ) *pdbAverageShiftX = dbAverageShiftX;
919 if( pdbAverageShiftY != NULL ) *pdbAverageShiftY = dbAverageShiftY;
925 inline double CalculateTransformationLMS3_0( CvPoint* pTemplPoints, CvPoint* pSrcPoints)
929 assert( NULL != pTemplPoints);
930 assert( NULL != pSrcPoints);
932 double dbXt = double(pTemplPoints[0].x + pTemplPoints[1].x + pTemplPoints[2].x) / 3.0;
933 double dbYt = double(pTemplPoints[0].y + pTemplPoints[1].y + pTemplPoints[2].y ) / 3.0;
934 double dbXs = double(pSrcPoints[0].x + pSrcPoints[1].x + pSrcPoints[2].x) / 3.0;
935 double dbYs = double(pSrcPoints[0].y + pSrcPoints[1].y + pSrcPoints[2].y) / 3.0;
937 double dbXtXt = double(pow2(pTemplPoints[0].x) + pow2(pTemplPoints[1].x) + pow2(pTemplPoints[2].x)) / 3.0;
938 double dbYtYt = double(pow2(pTemplPoints[0].y) + pow2(pTemplPoints[1].y) + pow2(pTemplPoints[2].y)) / 3.0;
940 double dbXsXs = double(pow2(pSrcPoints[0].x) + pow2(pSrcPoints[1].x) + pow2(pSrcPoints[2].x)) / 3.0;
941 double dbYsYs = double(pow2(pSrcPoints[0].y) + pow2(pSrcPoints[1].y) + pow2(pSrcPoints[2].y)) / 3.0;
943 double dbXtXs = double(pTemplPoints[0].x * pSrcPoints[0].x +
944 pTemplPoints[1].x * pSrcPoints[1].x +
945 pTemplPoints[2].x * pSrcPoints[2].x) / 3.0;
946 double dbYtYs = double(pTemplPoints[0].y * pSrcPoints[0].y +
947 pTemplPoints[1].y * pSrcPoints[1].y +
948 pTemplPoints[2].y * pSrcPoints[2].y) / 3.0;
950 double dbXtYs = double(pTemplPoints[0].x * pSrcPoints[0].y +
951 pTemplPoints[1].x * pSrcPoints[1].y +
952 pTemplPoints[2].x * pSrcPoints[2].y) / 3.0;
953 double dbYtXs = double(pTemplPoints[0].y * pSrcPoints[0].x +
954 pTemplPoints[1].y * pSrcPoints[1].x +
955 pTemplPoints[2].y * pSrcPoints[2].x ) / 3.0;
957 dbXtXt -= dbXt * dbXt;
958 dbYtYt -= dbYt * dbYt;
960 dbXsXs -= dbXs * dbXs;
961 dbYsYs -= dbYs * dbYs;
963 dbXtXs -= dbXt * dbXs;
964 dbYtYs -= dbYt * dbYs;
966 dbXtYs -= dbXt * dbYs;
967 dbYtXs -= dbYt * dbXs;
969 double del = dbXsXs + dbYsYs;
971 dbLMS = dbXtXt + dbYtYt - ((double)pow(dbXtXs + dbYtYs,2) + (double)pow(dbXtYs - dbYtXs,2)) / del;