1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
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.
8 //
9 //
10 //                        Intel License Agreement
11 //                For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 //   * Redistribution's of source code must retain the above copyright notice,
20 //     this list of conditions and the following disclaimer.
21 //
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.
25 //
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.
28 //
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.
39 //
40 //M*/
41 
42 /****************************************************************************************\
43       Contour-based face feature tracking
44       The code was created by Tatiana Cherepanova (tata@sl.iae.nsk.su)
45 \****************************************************************************************/
46 
47 #include "_cvaux.h"
48 #include "_cvvectrack.h"
49 
50 #define _ASSERT     assert
51 #define NUM_FACE_ELEMENTS   3
52 enum
53 {
54     MOUTH = 0,
55     LEYE = 1,
56     REYE = 2,
57 };
58 
59 #define MAX_LAYERS      64
60 
61 const double pi = 3.1415926535;
62 
63 struct CvFaceTracker;
64 struct CvTrackingRect;
65 class CvFaceElement;
66 
67 void ThresholdingParam(IplImage *imgGray, int iNumLayers, int &iMinLevel, int &iMaxLevel, float &step, float& power, int iHistMin /*= HIST_MIN*/);
68 int ChoiceTrackingFace3(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy);
69 int ChoiceTrackingFace2(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy, int noel);
70 inline int GetEnergy(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl);
71 inline int GetEnergy2(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl, int* element);
72 inline double CalculateTransformationLMS3_0( CvPoint* pTemplPoints, CvPoint* pSrcPoints);
73 inline double CalculateTransformationLMS3( CvPoint* pTemplPoints,
74                                    CvPoint* pSrcPoints,
75                                    double*       pdbAverageScale,
76                                    double*       pdbAverageRotate,
77                                    double*       pdbAverageShiftX,
78                                    double*       pdbAverageShiftY );
79 
80 struct CvTrackingRect
81 {
82     CvRect r;
83     CvPoint ptCenter;
84     int iColor;
85     int iEnergy;
86     int nRectsInThis;
87     int nRectsOnLeft;
88     int nRectsOnRight;
89     int nRectsOnTop;
90     int nRectsOnBottom;
CvTrackingRectCvTrackingRect91     CvTrackingRect() { memset(this, 0, sizeof(CvTrackingRect)); };
EnergyCvTrackingRect92     int Energy(const CvTrackingRect& prev)
93     {
94         int prev_color = 0 == prev.iColor ? iColor : prev.iColor;
95         iEnergy =	1 * pow2(r.width - prev.r.width) +
96             1 * pow2(r.height - prev.r.height) +
97             1 * pow2(iColor - prev_color) / 4 +
98             - 1 * nRectsInThis +
99             - 0 * nRectsOnTop +
100             + 0 * nRectsOnLeft +
101             + 0 * nRectsOnRight +
102             + 0 * nRectsOnBottom;
103         return iEnergy;
104     }
105 };
106 
107 struct CvFaceTracker
108 {
109     CvTrackingRect face[NUM_FACE_ELEMENTS];
110     int iTrackingFaceType;
111     double dbRotateDelta;
112     double dbRotateAngle;
113     CvPoint ptRotate;
114 
115     CvPoint ptTempl[NUM_FACE_ELEMENTS];
116     CvRect rTempl[NUM_FACE_ELEMENTS];
117 
118     IplImage* imgGray;
119     IplImage* imgThresh;
120     CvMemStorage* mstgContours;
CvFaceTrackerCvFaceTracker121     CvFaceTracker()
122     {
123         ptRotate.x = 0;
124         ptRotate.y = 0;
125         dbRotateDelta = 0;
126         dbRotateAngle = 0;
127         iTrackingFaceType = -1;
128         imgThresh = NULL;
129         imgGray = NULL;
130         mstgContours = NULL;
131     };
~CvFaceTrackerCvFaceTracker132     ~CvFaceTracker()
133     {
134         if (NULL != imgGray)
135             delete imgGray;
136         if (NULL != imgThresh)
137             delete imgThresh;
138         if (NULL != mstgContours)
139             cvReleaseMemStorage(&mstgContours);
140     };
InitCvFaceTracker141     int Init(CvRect* pRects, IplImage* imgGray)
142     {
143         for (int i = 0; i < NUM_FACE_ELEMENTS; i++)
144         {
145             face[i].r = pRects[i];
146             face[i].ptCenter = Center(face[i].r);
147             ptTempl[i] = face[i].ptCenter;
148             rTempl[i] = face[i].r;
149         }
150         imgGray = cvCreateImage(cvSize(imgGray->width, imgGray->height), 8, 1);
151         imgThresh = cvCreateImage(cvSize(imgGray->width, imgGray->height), 8, 1);
152         mstgContours = cvCreateMemStorage();
153         if ((NULL == imgGray) ||
154             (NULL == imgThresh) ||
155             (NULL == mstgContours))
156             return FALSE;
157         return TRUE;
158     };
InitNextImageCvFaceTracker159     int InitNextImage(IplImage* img)
160     {
161         CvSize sz = {img->width, img->height};
162         ReallocImage(&imgGray, sz, 1);
163         ReallocImage(&imgThresh, sz, 1);
164         ptRotate = face[MOUTH].ptCenter;
165         float m[6];
166         CvMat mat = cvMat( 2, 3, CV_32FC1, m );
167 
168         if (NULL == imgGray || NULL == imgThresh)
169             return FALSE;
170 
171         /*m[0] = (float)cos(-dbRotateAngle*CV_PI/180.);
172         m[1] = (float)sin(-dbRotateAngle*CV_PI/180.);
173         m[2] = (float)ptRotate.x;
174         m[3] = -m[1];
175         m[4] = m[0];
176         m[5] = (float)ptRotate.y;*/
177         cv2DRotationMatrix( cvPointTo32f(ptRotate), -dbRotateAngle, 1., &mat );
178         cvWarpAffine( img, imgGray, &mat );
179 
180         if (NULL == mstgContours)
181             mstgContours = cvCreateMemStorage();
182         else
183             cvClearMemStorage(mstgContours);
184         if (NULL == mstgContours)
185             return FALSE;
186         return TRUE;
187     }
188 };
189 
190 class CvFaceElement
191 {
192 public:
193     CvSeq* m_seqRects;
194     CvMemStorage* m_mstgRects;
195     CvRect m_rROI;
196     CvTrackingRect m_trPrev;
CvFaceElement()197     inline CvFaceElement()
198     {
199         m_seqRects = NULL;
200         m_mstgRects = NULL;
201         m_rROI.x = 0;
202         m_rROI.y = 0;
203         m_rROI.width = 0;
204         m_rROI.height = 0;
205     };
Init(const CvRect & roi,const CvTrackingRect & prev,CvMemStorage * mstg=NULL)206     inline int Init(const CvRect& roi, const CvTrackingRect& prev, CvMemStorage* mstg = NULL)
207     {
208         m_rROI = roi;
209         m_trPrev = prev;
210         if (NULL != mstg)
211             m_mstgRects = mstg;
212         if (NULL == m_mstgRects)
213             return FALSE;
214         if (NULL == m_seqRects)
215             m_seqRects = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvTrackingRect), m_mstgRects);
216         else
217             cvClearSeq(m_seqRects);
218         if (NULL == m_seqRects)
219             return FALSE;
220         return TRUE;
221     };
222     void FindRects(IplImage* img, IplImage* thresh, int nLayers, int dMinSize);
223 protected:
224     void FindContours(IplImage* img, IplImage* thresh, int nLayers, int dMinSize);
225     void MergeRects(int d);
226     void Energy();
227 }; //class CvFaceElement
228 
CompareEnergy(const void * el1,const void * el2,void *)229 int CV_CDECL CompareEnergy(const void* el1, const void* el2, void*)
230 {
231     return ((CvTrackingRect*)el1)->iEnergy - ((CvTrackingRect*)el2)->iEnergy;
232 }// int CV_CDECL CompareEnergy(const void* el1, const void* el2, void*)
233 
FindRects(IplImage * img,IplImage * thresh,int nLayers,int dMinSize)234 void CvFaceElement::FindRects(IplImage* img, IplImage* thresh, int nLayers, int dMinSize)
235 {
236     FindContours(img, thresh, nLayers, dMinSize / 4);
237     if (0 == m_seqRects->total)
238         return;
239     Energy();
240     cvSeqSort(m_seqRects, CompareEnergy, NULL);
241     CvTrackingRect* pR = (CvTrackingRect*)cvGetSeqElem(m_seqRects, 0);
242     if (m_seqRects->total < 32)
243     {
244         MergeRects(dMinSize / 8);
245         Energy();
246         cvSeqSort(m_seqRects, CompareEnergy, NULL);
247     }
248     pR = (CvTrackingRect*)cvGetSeqElem(m_seqRects, 0);
249     if ((pR->iEnergy > 100 && m_seqRects->total < 32) || (m_seqRects->total < 16))
250     {
251         MergeRects(dMinSize / 4);
252         Energy();
253         cvSeqSort(m_seqRects, CompareEnergy, NULL);
254     }
255     pR = (CvTrackingRect*)cvGetSeqElem(m_seqRects, 0);
256     if ((pR->iEnergy > 100 && m_seqRects->total < 16) || (pR->iEnergy > 200 && m_seqRects->total < 32))
257     {
258         MergeRects(dMinSize / 2);
259         Energy();
260         cvSeqSort(m_seqRects, CompareEnergy, NULL);
261     }
262 
263 }// void CvFaceElement::FindRects(IplImage* img, IplImage* thresh, int nLayers, int dMinSize)
264 
FindContours(IplImage * img,IplImage * thresh,int nLayers,int dMinSize)265 void CvFaceElement::FindContours(IplImage* img, IplImage* thresh, int nLayers, int dMinSize)
266 {
267     CvSeq* seq;
268     CvRect roi = m_rROI;
269     Extend(roi, 1);
270     cvSetImageROI(img, roi);
271     cvSetImageROI(thresh, roi);
272     // layers
273     int colors[MAX_LAYERS] = {0};
274     int iMinLevel = 0, iMaxLevel = 255;
275     float step, power;
276     ThresholdingParam(img, nLayers / 2, iMinLevel, iMaxLevel, step, power, 4);
277     int iMinLevelPrev = iMinLevel;
278     int iMaxLevelPrev = iMinLevel;
279     if (m_trPrev.iColor != 0)
280     {
281         iMinLevelPrev = m_trPrev.iColor - nLayers / 2;
282         iMaxLevelPrev = m_trPrev.iColor + nLayers / 2;
283     }
284     if (iMinLevelPrev < iMinLevel)
285     {
286         iMaxLevelPrev += iMinLevel - iMinLevelPrev;
287         iMinLevelPrev = iMinLevel;
288     }
289     if (iMaxLevelPrev > iMaxLevel)
290     {
291         iMinLevelPrev -= iMaxLevelPrev - iMaxLevel;
292         if (iMinLevelPrev < iMinLevel)
293             iMinLevelPrev = iMinLevel;
294         iMaxLevelPrev = iMaxLevel;
295     }
296     int n = nLayers;
297     n -= (iMaxLevelPrev - iMinLevelPrev + 1) / 2;
298     step = float(iMinLevelPrev - iMinLevel + iMaxLevel - iMaxLevelPrev) / float(n);
299     int j = 0;
300     float level;
301     for (level = (float)iMinLevel; level < iMinLevelPrev && j < nLayers; level += step, j++)
302         colors[j] = int(level + 0.5);
303     for (level = (float)iMinLevelPrev; level < iMaxLevelPrev && j < nLayers; level += 2.0, j++)
304         colors[j] = int(level + 0.5);
305     for (level = (float)iMaxLevelPrev; level < iMaxLevel && j < nLayers; level += step, j++)
306         colors[j] = int(level + 0.5);
307     //
308     for (int i = 0; i < nLayers; i++)
309     {
310         cvThreshold(img, thresh, colors[i], 255.0, CV_THRESH_BINARY);
311         if (cvFindContours(thresh, m_mstgRects, &seq, sizeof(CvContour), CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE))
312         {
313             CvTrackingRect cr;
314             for (CvSeq* external = seq; external; external = external->h_next)
315             {
316                 cr.r = cvContourBoundingRect(external);
317                 Move(cr.r, roi.x, roi.y);
318                 if (RectInRect(cr.r, m_rROI) && cr.r.width > dMinSize  && cr.r.height > dMinSize)
319                 {
320                     cr.ptCenter = Center(cr.r);
321                     cr.iColor = colors[i];
322                     cvSeqPush(m_seqRects, &cr);
323                 }
324                 for (CvSeq* internal = external->v_next; internal; internal = internal->h_next)
325                 {
326                     cr.r = cvContourBoundingRect(internal);
327                     Move(cr.r, roi.x, roi.y);
328                     if (RectInRect(cr.r, m_rROI) && cr.r.width > dMinSize  && cr.r.height > dMinSize)
329                     {
330                         cr.ptCenter = Center(cr.r);
331                         cr.iColor = colors[i];
332                         cvSeqPush(m_seqRects, &cr);
333                     }
334                 }
335             }
336             cvClearSeq(seq);
337         }
338     }
339     cvResetImageROI(img);
340     cvResetImageROI(thresh);
341 }//void CvFaceElement::FindContours(IplImage* img, IplImage* thresh, int nLayers)
342 
MergeRects(int d)343 void CvFaceElement::MergeRects(int d)
344 {
345     int nRects = m_seqRects->total;
346     CvSeqReader reader, reader2;
347     cvStartReadSeq( m_seqRects, &reader );
348     int i, j;
349     for (i = 0; i < nRects; i++)
350     {
351         CvTrackingRect* pRect1 = (CvTrackingRect*)(reader.ptr);
352         cvStartReadSeq( m_seqRects, &reader2 );
353         cvSetSeqReaderPos(&reader2, i + 1);
354         for (j = i + 1; j < nRects; j++)
355         {
356             CvTrackingRect* pRect2 = (CvTrackingRect*)(reader2.ptr);
357             if (abs(pRect1->ptCenter.y - pRect2->ptCenter.y) < d &&
358                 abs(pRect1->r.height - pRect2->r.height) < d)
359             {
360                 CvTrackingRect rNew;
361                 rNew.iColor = (pRect1->iColor + pRect2->iColor + 1) / 2;
362                 rNew.r.x = min(pRect1->r.x, pRect2->r.x);
363                 rNew.r.y = min(pRect1->r.y, pRect2->r.y);
364                 rNew.r.width = max(pRect1->r.x + pRect1->r.width, pRect2->r.x + pRect2->r.width) - rNew.r.x;
365                 rNew.r.height = min(pRect1->r.y + pRect1->r.height, pRect2->r.y + pRect2->r.height) - rNew.r.y;
366                 if (rNew.r != pRect1->r && rNew.r != pRect2->r)
367                 {
368                     rNew.ptCenter = Center(rNew.r);
369                     cvSeqPush(m_seqRects, &rNew);
370                 }
371             }
372             CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader2 );
373         }
374         CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader );
375     }
376     // delete equal rects
377     for (i = 0; i < m_seqRects->total; i++)
378     {
379         CvTrackingRect* pRect1 = (CvTrackingRect*)cvGetSeqElem(m_seqRects, i);
380         int j_begin = i + 1;
381         for (j = j_begin; j < m_seqRects->total;)
382         {
383             CvTrackingRect* pRect2 = (CvTrackingRect*)cvGetSeqElem(m_seqRects, j);
384             if (pRect1->r == pRect2->r)
385                 cvSeqRemove(m_seqRects, j);
386             else
387                 j++;
388         }
389     }
390 
391 }//void CvFaceElement::MergeRects(int d)
392 
Energy()393 void CvFaceElement::Energy()
394 {
395     CvSeqReader reader, reader2;
396     cvStartReadSeq( m_seqRects, &reader );
397     for (int i = 0; i < m_seqRects->total; i++)
398     {
399         CvTrackingRect* pRect = (CvTrackingRect*)(reader.ptr);
400         // outside and inside rects
401         cvStartReadSeq( m_seqRects, &reader2 );
402         for (int j = 0; j < m_seqRects->total; j++)
403         {
404             CvTrackingRect* pRect2 = (CvTrackingRect*)(reader2.ptr);
405             if (i != j)
406             {
407                 if (RectInRect(pRect2->r, pRect->r))
408                     pRect->nRectsInThis ++;
409                 else if (pRect2->r.y + pRect2->r.height <= pRect->r.y)
410                     pRect->nRectsOnTop ++;
411                 else if (pRect2->r.y >= pRect->r.y + pRect->r.height)
412                     pRect->nRectsOnBottom ++;
413                 else if (pRect2->r.x + pRect2->r.width <= pRect->r.x)
414                     pRect->nRectsOnLeft ++;
415                 else if (pRect2->r.x >= pRect->r.x + pRect->r.width)
416                     pRect->nRectsOnRight ++;
417             }
418             CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader2 );
419         }
420         // energy
421         pRect->Energy(m_trPrev);
422         CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader );
423     }
424 }//void CvFaceElement::Energy()
425 
426 CV_IMPL CvFaceTracker*
cvInitFaceTracker(CvFaceTracker * pFaceTracker,const IplImage * imgGray,CvRect * pRects,int nRects)427 cvInitFaceTracker(CvFaceTracker* pFaceTracker, const IplImage* imgGray, CvRect* pRects, int nRects)
428 {
429     _ASSERT(NULL != imgGray);
430     _ASSERT(NULL != pRects);
431     _ASSERT(nRects >= NUM_FACE_ELEMENTS);
432     if ((NULL == imgGray) ||
433         (NULL == pRects) ||
434         (nRects < NUM_FACE_ELEMENTS))
435         return NULL;
436 
437     int new_face = FALSE;
438     CvFaceTracker* pFace = pFaceTracker;
439     if (NULL == pFace)
440     {
441         pFace = new CvFaceTracker;
442         if (NULL == pFace)
443             return NULL;
444         new_face = TRUE;
445     }
446     pFace->Init(pRects, (IplImage*)imgGray);
447     return pFace;
448 }//CvFaceTracker* InitFaceTracker(IplImage* imgGray, CvRect* pRects, int nRects)
449 
450 CV_IMPL void
cvReleaseFaceTracker(CvFaceTracker ** ppFaceTracker)451 cvReleaseFaceTracker(CvFaceTracker** ppFaceTracker)
452 {
453     if (NULL == *ppFaceTracker)
454         return;
455     delete *ppFaceTracker;
456     *ppFaceTracker = NULL;
457 }//void ReleaseFaceTracker(CvFaceTracker** ppFaceTracker)
458 
459 
460 CV_IMPL int
cvTrackFace(CvFaceTracker * pFaceTracker,IplImage * imgGray,CvRect * pRects,int nRects,CvPoint * ptRotate,double * dbAngleRotate)461 cvTrackFace(CvFaceTracker* pFaceTracker, IplImage* imgGray, CvRect* pRects, int nRects, CvPoint* ptRotate, double* dbAngleRotate)
462 {
463     _ASSERT(NULL != pFaceTracker);
464     _ASSERT(NULL != imgGray);
465     _ASSERT(NULL != pRects && nRects >= NUM_FACE_ELEMENTS);
466     if ((NULL == pFaceTracker) ||
467         (NULL == imgGray))
468         return FALSE;
469     pFaceTracker->InitNextImage(imgGray);
470     *ptRotate = pFaceTracker->ptRotate;
471     *dbAngleRotate = pFaceTracker->dbRotateAngle;
472 
473     int nElements = 16;
474     double dx = pFaceTracker->face[LEYE].ptCenter.x - pFaceTracker->face[REYE].ptCenter.x;
475     double dy = pFaceTracker->face[LEYE].ptCenter.y - pFaceTracker->face[REYE].ptCenter.y;
476     double d_eyes = sqrt(dx*dx + dy*dy);
477     int d = cvRound(0.25 * d_eyes);
478     int dMinSize = d;
479     int nRestarts = 0;
480 
481     int elem;
482 
483     CvFaceElement big_face[NUM_FACE_ELEMENTS];
484 START:
485     // init
486     for (elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
487     {
488         CvRect r = pFaceTracker->face[elem].r;
489         Extend(r, d);
490         if (r.width < 4*d)
491         {
492             r.x -= (4*d - r.width) / 2;
493             r.width += 4*d - r.width;
494         }
495         if (r.height < 3*d)
496         {
497             r.y -= (3*d - r.height) / 2;
498             r.height += 3*d - r.height;
499         }
500         if (r.x < 1)
501             r.x = 1;
502         if (r.y < 1)
503             r.y = 1;
504         if (r.x + r.width > pFaceTracker->imgGray->width - 2)
505             r.width = pFaceTracker->imgGray->width - 2 - r.x;
506         if (r.y + r.height > pFaceTracker->imgGray->height - 2)
507             r.height = pFaceTracker->imgGray->height - 2 - r.y;
508         if (!big_face[elem].Init(r, pFaceTracker->face[elem], pFaceTracker->mstgContours))
509             return FALSE;
510     }
511     // find contours
512     for (elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
513         big_face[elem].FindRects(pFaceTracker->imgGray, pFaceTracker->imgThresh, 32, dMinSize);
514     // candidats
515     CvTrackingRect new_face[NUM_FACE_ELEMENTS];
516     int new_energy = 0;
517     int found = ChoiceTrackingFace3(pFaceTracker, nElements, big_face, new_face, new_energy);
518     int restart = FALSE;
519     int find2 = FALSE;
520     int noel = -1;
521     if (found)
522     {
523         if (new_energy > 100000 && -1 != pFaceTracker->iTrackingFaceType)
524             find2 = TRUE;
525         else if (new_energy > 150000)
526         {
527             int elements = 0;
528             for (int el = 0; el < NUM_FACE_ELEMENTS; el++)
529             {
530                 if (big_face[el].m_seqRects->total > 16 || (big_face[el].m_seqRects->total > 8 && new_face[el].iEnergy < 100))
531                     elements++;
532                 else
533                     noel = el;
534             }
535             if (2 == elements)
536                 find2 = TRUE;
537             else
538                 restart = TRUE;
539         }
540     }
541     else
542     {
543         if (-1 != pFaceTracker->iTrackingFaceType)
544             find2 = TRUE;
545         else
546             restart = TRUE;
547     }
548 RESTART:
549     if (restart)
550     {
551         if (nRestarts++ < 2)
552         {
553             d = d + d/4;
554             goto START;
555         }
556     }
557     else if (find2)
558     {
559         if (-1 != pFaceTracker->iTrackingFaceType)
560             noel = pFaceTracker->iTrackingFaceType;
561         int found2 = ChoiceTrackingFace2(pFaceTracker, nElements, big_face, new_face, new_energy, noel);
562         if (found2 && new_energy < 100000)
563         {
564             pFaceTracker->iTrackingFaceType = noel;
565             found = TRUE;
566         }
567         else
568         {
569             restart = TRUE;
570             goto RESTART;
571         }
572     }
573 
574     if (found)
575     {
576         // angle by mouth & eyes
577         double vx_prev = double(pFaceTracker->face[LEYE].ptCenter.x + pFaceTracker->face[REYE].ptCenter.x) / 2.0 - pFaceTracker->face[MOUTH].ptCenter.x;
578         double vy_prev = double(pFaceTracker->face[LEYE].ptCenter.y + pFaceTracker->face[REYE].ptCenter.y) / 2.0 - pFaceTracker->face[MOUTH].ptCenter.y;
579         double vx_prev1 = vx_prev * cos(pFaceTracker->dbRotateDelta) - vy_prev * sin(pFaceTracker->dbRotateDelta);
580         double vy_prev1 = vx_prev * sin(pFaceTracker->dbRotateDelta) + vy_prev * cos(pFaceTracker->dbRotateDelta);
581         vx_prev = vx_prev1;
582         vy_prev = vy_prev1;
583         for (elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
584             pFaceTracker->face[elem] = new_face[elem];
585         double vx = double(pFaceTracker->face[LEYE].ptCenter.x + pFaceTracker->face[REYE].ptCenter.x) / 2.0 - pFaceTracker->face[MOUTH].ptCenter.x;
586         double vy = double(pFaceTracker->face[LEYE].ptCenter.y + pFaceTracker->face[REYE].ptCenter.y) / 2.0 - pFaceTracker->face[MOUTH].ptCenter.y;
587         pFaceTracker->dbRotateDelta = 0;
588         double n1_n2 = (vx * vx + vy * vy) * (vx_prev * vx_prev + vy_prev * vy_prev);
589         if (n1_n2 != 0)
590             pFaceTracker->dbRotateDelta = asin((vx * vy_prev - vx_prev * vy) / sqrt(n1_n2));
591         pFaceTracker->dbRotateAngle -= pFaceTracker->dbRotateDelta;
592     }
593     else
594     {
595         pFaceTracker->dbRotateDelta = 0;
596         pFaceTracker->dbRotateAngle = 0;
597     }
598     if ((pFaceTracker->dbRotateAngle >= pi/2 && pFaceTracker->dbRotateAngle > 0) ||
599         (pFaceTracker->dbRotateAngle <= -pi/2 && pFaceTracker->dbRotateAngle < 0))
600     {
601         pFaceTracker->dbRotateDelta = 0;
602         pFaceTracker->dbRotateAngle = 0;
603         found = FALSE;
604     }
605     if (found)
606     {
607         for (int i = 0; i < NUM_FACE_ELEMENTS && i < nRects; i++)
608             pRects[i] = pFaceTracker->face[i].r;
609     }
610     return found;
611 }//int FindFaceTracker(CvFaceTracker* pFaceTracker, IplImage* imgGray, CvRect* pRects, int nRects, CvPoint& ptRotate, double& dbAngleRotate)
612 
ThresholdingParam(IplImage * imgGray,int iNumLayers,int & iMinLevel,int & iMaxLevel,float & step,float & power,int iHistMin)613 void ThresholdingParam(IplImage *imgGray, int iNumLayers, int &iMinLevel, int &iMaxLevel, float &step, float& power, int iHistMin /*= HIST_MIN*/)
614 {
615     _ASSERT(imgGray != NULL);
616     _ASSERT(imgGray->nChannels == 1);
617     int i, j;
618     // create histogram
619     int histImg[256] = {0};
620     uchar* buffImg = (uchar*)imgGray->imageData;
621     CvRect rROI = cvGetImageROI(imgGray);
622     buffImg += rROI.y * imgGray->widthStep + rROI.x;
623     for (j = 0; j < rROI.height; j++)
624     {
625         for (i = 0; i < rROI.width; i++)
626             histImg[buffImg[i]] ++;
627         buffImg += imgGray->widthStep;
628     }
629     // params
630     for (i = 0; i < 256; i++)
631     {
632         if (histImg[i] > iHistMin)
633             break;
634     }
635     iMinLevel = i;
636     for (i = 255; i >= 0; i--)
637     {
638         if (histImg[i] > iHistMin)
639             break;
640     }
641     iMaxLevel = i;
642     if (iMaxLevel <= iMinLevel)
643     {
644         iMaxLevel = 255;
645         iMinLevel = 0;
646     }
647     // power
648     double black = 1;
649     double white = 1;
650     for (i = iMinLevel; i < (iMinLevel + iMaxLevel) / 2; i++)
651         black += histImg[i];
652     for (i = (iMinLevel + iMaxLevel) / 2; i < iMaxLevel; i++)
653         white += histImg[i];
654     power = float(black) / float(2 * white);
655     //
656     step = float(iMaxLevel - iMinLevel) / float(iNumLayers);
657     if (step < 1.0)
658         step = 1.0;
659 }// void ThresholdingParam(IplImage *imgGray, int iNumLayers, int &iMinLevel, int &iMaxLevel, int &iStep)
660 
ChoiceTrackingFace3(CvFaceTracker * pTF,const int nElements,const CvFaceElement * big_face,CvTrackingRect * face,int & new_energy)661 int ChoiceTrackingFace3(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy)
662 {
663     CvTrackingRect* curr_face[NUM_FACE_ELEMENTS] = {NULL};
664     CvTrackingRect* new_face[NUM_FACE_ELEMENTS] = {NULL};
665     new_energy = 0x7fffffff;
666     int curr_energy = 0x7fffffff;
667     int found = FALSE;
668     int N = 0;
669     CvSeqReader reader_m, reader_l, reader_r;
670     cvStartReadSeq( big_face[MOUTH].m_seqRects, &reader_m );
671     for (int i_mouth = 0; i_mouth < big_face[MOUTH].m_seqRects->total && i_mouth < nElements; i_mouth++)
672     {
673         curr_face[MOUTH] = (CvTrackingRect*)(reader_m.ptr);
674         cvStartReadSeq( big_face[LEYE].m_seqRects, &reader_l );
675         for (int i_left = 0; i_left < big_face[LEYE].m_seqRects->total && i_left < nElements; i_left++)
676         {
677             curr_face[LEYE] = (CvTrackingRect*)(reader_l.ptr);
678             if (curr_face[LEYE]->r.y + curr_face[LEYE]->r.height < curr_face[MOUTH]->r.y)
679             {
680                 cvStartReadSeq( big_face[REYE].m_seqRects, &reader_r );
681                 for (int i_right = 0; i_right < big_face[REYE].m_seqRects->total && i_right < nElements; i_right++)
682                 {
683                     curr_face[REYE] = (CvTrackingRect*)(reader_r.ptr);
684                     if (curr_face[REYE]->r.y + curr_face[REYE]->r.height < curr_face[MOUTH]->r.y &&
685                         curr_face[REYE]->r.x > curr_face[LEYE]->r.x + curr_face[LEYE]->r.width)
686                     {
687                         curr_energy = GetEnergy(curr_face, pTF->face, pTF->ptTempl, pTF->rTempl);
688                         if (curr_energy < new_energy)
689                         {
690                             for (int elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
691                                 new_face[elem] = curr_face[elem];
692                             new_energy = curr_energy;
693                             found = TRUE;
694                         }
695                         N++;
696                     }
697                 }
698             }
699         }
700     }
701     if (found)
702     {
703         for (int elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
704             face[elem] = *(new_face[elem]);
705     }
706     return found;
707 } // int ChoiceTrackingFace3(const CvTrackingRect* tr_face, CvTrackingRect* new_face, int& new_energy)
708 
ChoiceTrackingFace2(CvFaceTracker * pTF,const int nElements,const CvFaceElement * big_face,CvTrackingRect * face,int & new_energy,int noel)709 int ChoiceTrackingFace2(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy, int noel)
710 {
711     int element[NUM_FACE_ELEMENTS];
712     for (int i = 0, elem = 0; i < NUM_FACE_ELEMENTS; i++)
713     {
714         if (i != noel)
715         {
716             element[elem] = i;
717             elem ++;
718         }
719         else
720             element[2] = i;
721     }
722     CvTrackingRect* curr_face[NUM_FACE_ELEMENTS] = {NULL};
723     CvTrackingRect* new_face[NUM_FACE_ELEMENTS] = {NULL};
724     new_energy = 0x7fffffff;
725     int curr_energy = 0x7fffffff;
726     int found = FALSE;
727     int N = 0;
728     CvSeqReader reader0, reader1;
729     cvStartReadSeq( big_face[element[0]].m_seqRects, &reader0 );
730     for (int i0 = 0; i0 < big_face[element[0]].m_seqRects->total && i0 < nElements; i0++)
731     {
732         curr_face[element[0]] = (CvTrackingRect*)(reader0.ptr);
733         cvStartReadSeq( big_face[element[1]].m_seqRects, &reader1 );
734         for (int i1 = 0; i1 < big_face[element[1]].m_seqRects->total && i1 < nElements; i1++)
735         {
736             curr_face[element[1]] = (CvTrackingRect*)(reader1.ptr);
737             curr_energy = GetEnergy2(curr_face, pTF->face, pTF->ptTempl, pTF->rTempl, element);
738             if (curr_energy < new_energy)
739             {
740                 for (int elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
741                     new_face[elem] = curr_face[elem];
742                 new_energy = curr_energy;
743                 found = TRUE;
744             }
745             N++;
746         }
747     }
748     if (found)
749     {
750         face[element[0]] = *(new_face[element[0]]);
751         face[element[1]] = *(new_face[element[1]]);
752         // 3 element find by template
753         CvPoint templ_v01 = {pTF->ptTempl[element[1]].x - pTF->ptTempl[element[0]].x, pTF->ptTempl[element[1]].y - pTF->ptTempl[element[0]].y};
754         CvPoint templ_v02 = {pTF->ptTempl[element[2]].x - pTF->ptTempl[element[0]].x, pTF->ptTempl[element[2]].y - pTF->ptTempl[element[0]].y};
755         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};
756         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};
757         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};
758         double templ_d01 = sqrt((double)templ_v01.x*templ_v01.x + templ_v01.y*templ_v01.y);
759         double templ_d02 = sqrt((double)templ_v02.x*templ_v02.x + templ_v02.y*templ_v02.y);
760         double prev_d01 = sqrt((double)prev_v01.x*prev_v01.x + prev_v01.y*prev_v01.y);
761         double prev_d02 = sqrt((double)prev_v02.x*prev_v02.x + prev_v02.y*prev_v02.y);
762         double new_d01 = sqrt((double)new_v01.x*new_v01.x + new_v01.y*new_v01.y);
763         double scale = templ_d01 / new_d01;
764         double new_d02 = templ_d02 / scale;
765         double sin_a = double(prev_v01.x * prev_v02.y - prev_v01.y * prev_v02.x) / (prev_d01 * prev_d02);
766         double cos_a = cos(asin(sin_a));
767         double x = double(new_v01.x) * cos_a - double(new_v01.y) * sin_a;
768         double y = double(new_v01.x) * sin_a + double(new_v01.y) * cos_a;
769         x = x * new_d02 / new_d01;
770         y = y * new_d02 / new_d01;
771         CvPoint new_v02 = {int(x + 0.5), int(y + 0.5)};
772         face[element[2]].iColor = 0;
773         face[element[2]].iEnergy = 0;
774         face[element[2]].nRectsInThis = 0;
775         face[element[2]].nRectsOnBottom = 0;
776         face[element[2]].nRectsOnLeft = 0;
777         face[element[2]].nRectsOnRight = 0;
778         face[element[2]].nRectsOnTop = 0;
779         face[element[2]].ptCenter.x = new_v02.x + new_face[element[0]]->ptCenter.x;
780         face[element[2]].ptCenter.y = new_v02.y + new_face[element[0]]->ptCenter.y;
781         face[element[2]].r.width = int(double(pTF->rTempl[element[2]].width) / (scale) + 0.5);
782         face[element[2]].r.height = int(double(pTF->rTempl[element[2]].height) / (scale) + 0.5);
783         face[element[2]].r.x = face[element[2]].ptCenter.x - (face[element[2]].r.width + 1) / 2;
784         face[element[2]].r.y = face[element[2]].ptCenter.y - (face[element[2]].r.height + 1) / 2;
785         _ASSERT(face[LEYE].r.x + face[LEYE].r.width <= face[REYE].r.x);
786     }
787     return found;
788 } // int ChoiceTrackingFace3(const CvTrackingRect* tr_face, CvTrackingRect* new_face, int& new_energy)
789 
GetEnergy(CvTrackingRect ** ppNew,const CvTrackingRect * pPrev,CvPoint * ptTempl,CvRect * rTempl)790 inline int GetEnergy(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl)
791 {
792     int energy = 0;
793     CvPoint ptNew[NUM_FACE_ELEMENTS];
794     CvPoint ptPrev[NUM_FACE_ELEMENTS];
795     for (int i = 0; i < NUM_FACE_ELEMENTS; i++)
796     {
797         ptNew[i] = ppNew[i]->ptCenter;
798         ptPrev[i] = pPrev[i].ptCenter;
799         energy += ppNew[i]->iEnergy - 2 * ppNew[i]->nRectsInThis;
800     }
801     double dx = 0, dy = 0, scale = 1, rotate = 0;
802     double e_templ = CalculateTransformationLMS3(ptTempl, ptNew, &scale, &rotate, &dx, &dy);
803     double e_prev = CalculateTransformationLMS3_0(ptPrev, ptNew);
804     double w_eye = double(ppNew[LEYE]->r.width + ppNew[REYE]->r.width) * scale / 2.0;
805     double h_eye = double(ppNew[LEYE]->r.height + ppNew[REYE]->r.height) * scale / 2.0;
806     double w_mouth = double(ppNew[MOUTH]->r.width) * scale;
807     double h_mouth = double(ppNew[MOUTH]->r.height) * scale;
808     energy +=
809         int(512.0 * (e_prev + 16.0 * e_templ)) +
810         4 * pow2(ppNew[LEYE]->r.width - ppNew[REYE]->r.width) +
811         4 * pow2(ppNew[LEYE]->r.height - ppNew[REYE]->r.height) +
812         4 * (int)pow(w_eye - double(rTempl[LEYE].width + rTempl[REYE].width) / 2.0, 2) +
813         2 * (int)pow(h_eye - double(rTempl[LEYE].height + rTempl[REYE].height) / 2.0, 2) +
814         1 * (int)pow(w_mouth - double(rTempl[MOUTH].width), 2) +
815         1 * (int)pow(h_mouth - double(rTempl[MOUTH].height), 2) +
816         0;
817     return energy;
818 }
819 
GetEnergy2(CvTrackingRect ** ppNew,const CvTrackingRect * pPrev,CvPoint * ptTempl,CvRect * rTempl,int * element)820 inline int GetEnergy2(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl, int* element)
821 {
822     CvPoint new_v = {ppNew[element[0]]->ptCenter.x - ppNew[element[1]]->ptCenter.x,
823         ppNew[element[0]]->ptCenter.y - ppNew[element[1]]->ptCenter.y};
824     CvPoint prev_v = {pPrev[element[0]].ptCenter.x - pPrev[element[1]].ptCenter.x,
825         pPrev[element[0]].ptCenter.y - pPrev[element[1]].ptCenter.y};
826     double new_d = sqrt((double)new_v.x*new_v.x + new_v.y*new_v.y);
827     double prev_d = sqrt((double)prev_v.x*prev_v.x + prev_v.y*prev_v.y);
828     double dx = ptTempl[element[0]].x - ptTempl[element[1]].x;
829     double dy = ptTempl[element[0]].y - ptTempl[element[1]].y;
830     double templ_d = sqrt(dx*dx + dy*dy);
831     double scale_templ = new_d / templ_d;
832     double w0 = (double)ppNew[element[0]]->r.width * scale_templ;
833     double h0 = (double)ppNew[element[0]]->r.height * scale_templ;
834     double w1 = (double)ppNew[element[1]]->r.width * scale_templ;
835     double h1 = (double)ppNew[element[1]]->r.height * scale_templ;
836 
837     int energy = ppNew[element[0]]->iEnergy + ppNew[element[1]]->iEnergy +
838         - 2 * (ppNew[element[0]]->nRectsInThis - ppNew[element[1]]->nRectsInThis) +
839         (int)pow(w0 - (double)rTempl[element[0]].width, 2) +
840         (int)pow(h0 - (double)rTempl[element[0]].height, 2) +
841         (int)pow(w1 - (double)rTempl[element[1]].width, 2) +
842         (int)pow(h1 - (double)rTempl[element[1]].height, 2) +
843         (int)pow(new_d - prev_d, 2) +
844         0;
845 
846     return energy;
847 }
848 
CalculateTransformationLMS3(CvPoint * pTemplPoints,CvPoint * pSrcPoints,double * pdbAverageScale,double * pdbAverageRotate,double * pdbAverageShiftX,double * pdbAverageShiftY)849 inline double CalculateTransformationLMS3( CvPoint* pTemplPoints,
850                                    CvPoint* pSrcPoints,
851                                    double*       pdbAverageScale,
852                                    double*       pdbAverageRotate,
853                                    double*       pdbAverageShiftX,
854                                    double*       pdbAverageShiftY )
855 {
856 //    double WS = 0;
857     double dbAverageScale = 1;
858     double dbAverageRotate = 0;
859     double dbAverageShiftX = 0;
860     double dbAverageShiftY = 0;
861     double dbLMS = 0;
862 
863     _ASSERT( NULL != pTemplPoints);
864     _ASSERT( NULL != pSrcPoints);
865 
866     double dbXt = double(pTemplPoints[0].x + pTemplPoints[1].x + pTemplPoints[2].x) / 3.0;
867     double dbYt = double(pTemplPoints[0].y + pTemplPoints[1].y + pTemplPoints[2].y ) / 3.0;
868     double dbXs = double(pSrcPoints[0].x + pSrcPoints[1].x + pSrcPoints[2].x) / 3.0;
869     double dbYs = double(pSrcPoints[0].y + pSrcPoints[1].y + pSrcPoints[2].y) / 3.0;
870 
871     double dbXtXt = double(pow2(pTemplPoints[0].x) + pow2(pTemplPoints[1].x) + pow2(pTemplPoints[2].x)) / 3.0;
872     double dbYtYt = double(pow2(pTemplPoints[0].y) + pow2(pTemplPoints[1].y) + pow2(pTemplPoints[2].y)) / 3.0;
873 
874     double dbXsXs = double(pow2(pSrcPoints[0].x) + pow2(pSrcPoints[1].x) + pow2(pSrcPoints[2].x)) / 3.0;
875     double dbYsYs = double(pow2(pSrcPoints[0].y) + pow2(pSrcPoints[1].y) + pow2(pSrcPoints[2].y)) / 3.0;
876 
877     double dbXtXs = double(pTemplPoints[0].x * pSrcPoints[0].x +
878         pTemplPoints[1].x * pSrcPoints[1].x +
879         pTemplPoints[2].x * pSrcPoints[2].x) / 3.0;
880     double dbYtYs = double(pTemplPoints[0].y * pSrcPoints[0].y +
881         pTemplPoints[1].y * pSrcPoints[1].y +
882         pTemplPoints[2].y * pSrcPoints[2].y) / 3.0;
883 
884     double dbXtYs = double(pTemplPoints[0].x * pSrcPoints[0].y +
885         pTemplPoints[1].x * pSrcPoints[1].y +
886         pTemplPoints[2].x * pSrcPoints[2].y) / 3.0;
887     double dbYtXs = double(pTemplPoints[0].y * pSrcPoints[0].x +
888         pTemplPoints[1].y * pSrcPoints[1].x +
889         pTemplPoints[2].y * pSrcPoints[2].x ) / 3.0;
890 
891     dbXtXt -= dbXt * dbXt;
892     dbYtYt -= dbYt * dbYt;
893 
894     dbXsXs -= dbXs * dbXs;
895     dbYsYs -= dbYs * dbYs;
896 
897     dbXtXs -= dbXt * dbXs;
898     dbYtYs -= dbYt * dbYs;
899 
900     dbXtYs -= dbXt * dbYs;
901     dbYtXs -= dbYt * dbXs;
902 
903     dbAverageRotate = atan2( dbXtYs - dbYtXs, dbXtXs + dbYtYs );
904 
905     double cosR = cos(dbAverageRotate);
906     double sinR = sin(dbAverageRotate);
907     double del = dbXsXs + dbYsYs;
908     if( del != 0 )
909     {
910         dbAverageScale = (double(dbXtXs + dbYtYs) * cosR + double(dbXtYs - dbYtXs) * sinR) / del;
911         dbLMS = dbXtXt + dbYtYt - ((double)pow(dbXtXs + dbYtYs,2) + (double)pow(dbXtYs - dbYtXs,2)) / del;
912     }
913 
914     dbAverageShiftX = double(dbXt) - dbAverageScale * (double(dbXs) * cosR + double(dbYs) * sinR);
915     dbAverageShiftY = double(dbYt) - dbAverageScale * (double(dbYs) * cosR - double(dbXs) * sinR);
916 
917     if( pdbAverageScale != NULL ) *pdbAverageScale = dbAverageScale;
918     if( pdbAverageRotate != NULL ) *pdbAverageRotate = dbAverageRotate;
919     if( pdbAverageShiftX != NULL ) *pdbAverageShiftX = dbAverageShiftX;
920     if( pdbAverageShiftY != NULL ) *pdbAverageShiftY = dbAverageShiftY;
921 
922     _ASSERT(dbLMS >= 0);
923     return dbLMS;
924 }
925 
CalculateTransformationLMS3_0(CvPoint * pTemplPoints,CvPoint * pSrcPoints)926 inline double CalculateTransformationLMS3_0( CvPoint* pTemplPoints, CvPoint* pSrcPoints)
927 {
928     double dbLMS = 0;
929 
930     _ASSERT( NULL != pTemplPoints);
931     _ASSERT( NULL != pSrcPoints);
932 
933     double dbXt = double(pTemplPoints[0].x + pTemplPoints[1].x + pTemplPoints[2].x) / 3.0;
934     double dbYt = double(pTemplPoints[0].y + pTemplPoints[1].y + pTemplPoints[2].y ) / 3.0;
935     double dbXs = double(pSrcPoints[0].x + pSrcPoints[1].x + pSrcPoints[2].x) / 3.0;
936     double dbYs = double(pSrcPoints[0].y + pSrcPoints[1].y + pSrcPoints[2].y) / 3.0;
937 
938     double dbXtXt = double(pow2(pTemplPoints[0].x) + pow2(pTemplPoints[1].x) + pow2(pTemplPoints[2].x)) / 3.0;
939     double dbYtYt = double(pow2(pTemplPoints[0].y) + pow2(pTemplPoints[1].y) + pow2(pTemplPoints[2].y)) / 3.0;
940 
941     double dbXsXs = double(pow2(pSrcPoints[0].x) + pow2(pSrcPoints[1].x) + pow2(pSrcPoints[2].x)) / 3.0;
942     double dbYsYs = double(pow2(pSrcPoints[0].y) + pow2(pSrcPoints[1].y) + pow2(pSrcPoints[2].y)) / 3.0;
943 
944     double dbXtXs = double(pTemplPoints[0].x * pSrcPoints[0].x +
945         pTemplPoints[1].x * pSrcPoints[1].x +
946         pTemplPoints[2].x * pSrcPoints[2].x) / 3.0;
947     double dbYtYs = double(pTemplPoints[0].y * pSrcPoints[0].y +
948         pTemplPoints[1].y * pSrcPoints[1].y +
949         pTemplPoints[2].y * pSrcPoints[2].y) / 3.0;
950 
951     double dbXtYs = double(pTemplPoints[0].x * pSrcPoints[0].y +
952         pTemplPoints[1].x * pSrcPoints[1].y +
953         pTemplPoints[2].x * pSrcPoints[2].y) / 3.0;
954     double dbYtXs = double(pTemplPoints[0].y * pSrcPoints[0].x +
955         pTemplPoints[1].y * pSrcPoints[1].x +
956         pTemplPoints[2].y * pSrcPoints[2].x ) / 3.0;
957 
958     dbXtXt -= dbXt * dbXt;
959     dbYtYt -= dbYt * dbYt;
960 
961     dbXsXs -= dbXs * dbXs;
962     dbYsYs -= dbYs * dbYs;
963 
964     dbXtXs -= dbXt * dbXs;
965     dbYtYs -= dbYt * dbYs;
966 
967     dbXtYs -= dbXt * dbYs;
968     dbYtXs -= dbYt * dbXs;
969 
970     double del = dbXsXs + dbYsYs;
971     if( del != 0 )
972         dbLMS = dbXtXt + dbYtYt - ((double)pow(dbXtXs + dbYtYs,2) + (double)pow(dbXtYs - dbYtXs,2)) / del;
973     return dbLMS;
974 }
975 
976