modules/legacy/src/vecfacetracking.cpp

   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 "precomp.hpp"
  48 #include "_vectrack.h"
  49
  50 #define NUM_FACE_ELEMENTS   3
  51 enum
  52 {
  53     MOUTH = 0,
  54     LEYE = 1,
  55     REYE = 2,
  56 };
  57
  58 #define MAX_LAYERS      64
  59
  60 const double pi = 3.1415926535;
  61
  62 struct CvFaceTracker;
  63 struct CvTrackingRect;
  64 class CvFaceElement;
  65
  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,
  73                                    CvPoint* pSrcPoints,
  74                                    double*       pdbAverageScale,
  75                                    double*       pdbAverageRotate,
  76                                    double*       pdbAverageShiftX,
  77                                    double*       pdbAverageShiftY );
  78
  79 struct CvTrackingRect
  80 {
  81     CvRect r;
  82     CvPoint ptCenter;
  83     int iColor;
  84     int iEnergy;
  85     int nRectsInThis;
  86     int nRectsOnLeft;
  87     int nRectsOnRight;
  88     int nRectsOnTop;
  89     int nRectsOnBottom;
  90     CvTrackingRect() { memset(this, 0, sizeof(CvTrackingRect)); };
  91     int Energy(const CvTrackingRect& prev)
  92     {
  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 +
  97             - 1 * nRectsInThis +
  98             - 0 * nRectsOnTop +
  99             + 0 * nRectsOnLeft +
 100             + 0 * nRectsOnRight +
 101             + 0 * nRectsOnBottom;
 102         return iEnergy;
 103     }
 104 };
 105
 106 struct CvFaceTracker
 107 {
 108     CvTrackingRect face[NUM_FACE_ELEMENTS];
 109     int iTrackingFaceType;
 110     double dbRotateDelta;
 111     double dbRotateAngle;
 112     CvPoint ptRotate;
 113
 114     CvPoint ptTempl[NUM_FACE_ELEMENTS];
 115     CvRect rTempl[NUM_FACE_ELEMENTS];
 116
 117     IplImage* imgGray;
 118     IplImage* imgThresh;
 119     CvMemStorage* mstgContours;
 120     CvFaceTracker()
 121     {
 122         ptRotate.x = 0;
 123         ptRotate.y = 0;
 124         dbRotateDelta = 0;
 125         dbRotateAngle = 0;
 126         iTrackingFaceType = -1;
 127         imgThresh = NULL;
 128         imgGray = NULL;
 129         mstgContours = NULL;
 130     };
 131     ~CvFaceTracker()
 132     {
 133         if (NULL != imgGray)
 134             delete imgGray;
 135         if (NULL != imgThresh)
 136             delete imgThresh;
 137         if (NULL != mstgContours)
 138             cvReleaseMemStorage(&mstgContours);
 139     };
 140     int Init(CvRect* pRects, IplImage* imgGray)
 141     {
 142         for (int i = 0; i < NUM_FACE_ELEMENTS; i++)
 143         {
 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;
 148         }
 149         imgGray = cvCreateImage(cvSize(imgGray->width, imgGray->height), 8, 1);
 150         imgThresh = cvCreateImage(cvSize(imgGray->width, imgGray->height), 8, 1);
 151         mstgContours = cvCreateMemStorage();
 152         if ((NULL == imgGray) ||
 153             (NULL == imgThresh) ||
 154             (NULL == mstgContours))
 155             return FALSE;
 156         return TRUE;
 157     };
 158     int InitNextImage(IplImage* img)
 159     {
 160         CvSize sz = {img->width, img->height};
 161         ReallocImage(&imgGray, sz, 1);
 162         ReallocImage(&imgThresh, sz, 1);
 163         ptRotate = face[MOUTH].ptCenter;
 164         float m[6];
 165         CvMat mat = cvMat( 2, 3, CV_32FC1, m );
 166
 167         if (NULL == imgGray || NULL == imgThresh)
 168             return FALSE;
 169
 170         /*m[0] = (float)cos(-dbRotateAngle*CV_PI/180.);
 171         m[1] = (float)sin(-dbRotateAngle*CV_PI/180.);
 172         m[2] = (float)ptRotate.x;
 173         m[3] = -m[1];
 174         m[4] = m[0];
 175         m[5] = (float)ptRotate.y;*/
 176         cv2DRotationMatrix( cvPointTo32f(ptRotate), -dbRotateAngle, 1., &mat );
 177         cvWarpAffine( img, imgGray, &mat );
 178
 179         if (NULL == mstgContours)
 180             mstgContours = cvCreateMemStorage();
 181         else
 182             cvClearMemStorage(mstgContours);
 183         if (NULL == mstgContours)
 184             return FALSE;
 185         return TRUE;
 186     }
 187 };
 188
 189 class CvFaceElement
 190 {
 191 public:
 192     CvSeq* m_seqRects;
 193     CvMemStorage* m_mstgRects;
 194     CvRect m_rROI;
 195     CvTrackingRect m_trPrev;
 196     inline CvFaceElement()
 197     {
 198         m_seqRects = NULL;
 199         m_mstgRects = NULL;
 200         m_rROI.x = 0;
 201         m_rROI.y = 0;
 202         m_rROI.width = 0;
 203         m_rROI.height = 0;
 204     };
 205     inline int Init(const CvRect& roi, const CvTrackingRect& prev, CvMemStorage* mstg = NULL)
 206     {
 207         m_rROI = roi;
 208         m_trPrev = prev;
 209         if (NULL != mstg)
 210             m_mstgRects = mstg;
 211         if (NULL == m_mstgRects)
 212             return FALSE;
 213         if (NULL == m_seqRects)
 214             m_seqRects = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvTrackingRect), m_mstgRects);
 215         else
 216             cvClearSeq(m_seqRects);
 217         if (NULL == m_seqRects)
 218             return FALSE;
 219         return TRUE;
 220     };
 221     void FindRects(IplImage* img, IplImage* thresh, int nLayers, int dMinSize);
 222 protected:
 223     void FindContours(IplImage* img, IplImage* thresh, int nLayers, int dMinSize);
 224     void MergeRects(int d);
 225     void Energy();
 226 }; //class CvFaceElement
 227
 228 int CV_CDECL CompareEnergy(const void* el1, const void* el2, void*)
 229 {
 230     return ((CvTrackingRect*)el1)->iEnergy - ((CvTrackingRect*)el2)->iEnergy;
 231 }// int CV_CDECL CompareEnergy(const void* el1, const void* el2, void*)
 232
 233 void CvFaceElement::FindRects(IplImage* img, IplImage* thresh, int nLayers, int dMinSize)
 234 {
 235     FindContours(img, thresh, nLayers, dMinSize / 4);
 236     if (0 == m_seqRects->total)
 237         return;
 238     Energy();
 239     cvSeqSort(m_seqRects, CompareEnergy, NULL);
 240     CvTrackingRect* pR = (CvTrackingRect*)cvGetSeqElem(m_seqRects, 0);
 241     if (m_seqRects->total < 32)
 242     {
 243         MergeRects(dMinSize / 8);
 244         Energy();
 245         cvSeqSort(m_seqRects, CompareEnergy, NULL);
 246     }
 247     pR = (CvTrackingRect*)cvGetSeqElem(m_seqRects, 0);
 248     if ((pR->iEnergy > 100 && m_seqRects->total < 32) || (m_seqRects->total < 16))
 249     {
 250         MergeRects(dMinSize / 4);
 251         Energy();
 252         cvSeqSort(m_seqRects, CompareEnergy, NULL);
 253     }
 254     pR = (CvTrackingRect*)cvGetSeqElem(m_seqRects, 0);
 255     if ((pR->iEnergy > 100 && m_seqRects->total < 16) || (pR->iEnergy > 200 && m_seqRects->total < 32))
 256     {
 257         MergeRects(dMinSize / 2);
 258         Energy();
 259         cvSeqSort(m_seqRects, CompareEnergy, NULL);
 260     }
 261
 262 }// void CvFaceElement::FindRects(IplImage* img, IplImage* thresh, int nLayers, int dMinSize)
 263
 264 void CvFaceElement::FindContours(IplImage* img, IplImage* thresh, int nLayers, int dMinSize)
 265 {
 266     CvSeq* seq;
 267     CvRect roi = m_rROI;
 268     Extend(roi, 1);
 269     cvSetImageROI(img, roi);
 270     cvSetImageROI(thresh, roi);
 271     // layers
 272     int colors[MAX_LAYERS] = {0};
 273     int iMinLevel = 0, iMaxLevel = 255;
 274     float step, power;
 275     ThresholdingParam(img, nLayers / 2, iMinLevel, iMaxLevel, step, power, 4);
 276     int iMinLevelPrev = iMinLevel;
 277     int iMaxLevelPrev = iMinLevel;
 278     if (m_trPrev.iColor != 0)
 279     {
 280         iMinLevelPrev = m_trPrev.iColor - nLayers / 2;
 281         iMaxLevelPrev = m_trPrev.iColor + nLayers / 2;
 282     }
 283     if (iMinLevelPrev < iMinLevel)
 284     {
 285         iMaxLevelPrev += iMinLevel - iMinLevelPrev;
 286         iMinLevelPrev = iMinLevel;
 287     }
 288     if (iMaxLevelPrev > iMaxLevel)
 289     {
 290         iMinLevelPrev -= iMaxLevelPrev - iMaxLevel;
 291         if (iMinLevelPrev < iMinLevel)
 292             iMinLevelPrev = iMinLevel;
 293         iMaxLevelPrev = iMaxLevel;
 294     }
 295     int n = nLayers;
 296     n -= (iMaxLevelPrev - iMinLevelPrev + 1) / 2;
 297     step = float(iMinLevelPrev - iMinLevel + iMaxLevel - iMaxLevelPrev) / float(n);
 298     int j = 0;
 299     float level;
 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);
 306     //
 307     for (int i = 0; i < nLayers; i++)
 308     {
 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))
 311         {
 312             CvTrackingRect cr;
 313             for (CvSeq* external = seq; external; external = external->h_next)
 314             {
 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)
 318                 {
 319                     cr.ptCenter = Center(cr.r);
 320                     cr.iColor = colors[i];
 321                     cvSeqPush(m_seqRects, &cr);
 322                 }
 323                 for (CvSeq* internal = external->v_next; internal; internal = internal->h_next)
 324                 {
 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)
 328                     {
 329                         cr.ptCenter = Center(cr.r);
 330                         cr.iColor = colors[i];
 331                         cvSeqPush(m_seqRects, &cr);
 332                     }
 333                 }
 334             }
 335             cvClearSeq(seq);
 336         }
 337     }
 338     cvResetImageROI(img);
 339     cvResetImageROI(thresh);
 340 }//void CvFaceElement::FindContours(IplImage* img, IplImage* thresh, int nLayers)
 341
 342 void CvFaceElement::MergeRects(int d)
 343 {
 344     int nRects = m_seqRects->total;
 345     CvSeqReader reader, reader2;
 346     cvStartReadSeq( m_seqRects, &reader );
 347     int i, j;
 348     for (i = 0; i < nRects; i++)
 349     {
 350         CvTrackingRect* pRect1 = (CvTrackingRect*)(reader.ptr);
 351         cvStartReadSeq( m_seqRects, &reader2 );
 352         cvSetSeqReaderPos(&reader2, i + 1);
 353         for (j = i + 1; j < nRects; j++)
 354         {
 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)
 358             {
 359                 CvTrackingRect rNew;
 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)
 366                 {
 367                     rNew.ptCenter = Center(rNew.r);
 368                     cvSeqPush(m_seqRects, &rNew);
 369                 }
 370             }
 371             CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader2 );
 372         }
 373         CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader );
 374     }
 375     // delete equal rects
 376     for (i = 0; i < m_seqRects->total; i++)
 377     {
 378         CvTrackingRect* pRect1 = (CvTrackingRect*)cvGetSeqElem(m_seqRects, i);
 379         int j_begin = i + 1;
 380         for (j = j_begin; j < m_seqRects->total;)
 381         {
 382             CvTrackingRect* pRect2 = (CvTrackingRect*)cvGetSeqElem(m_seqRects, j);
 383             if (pRect1->r == pRect2->r)
 384                 cvSeqRemove(m_seqRects, j);
 385             else
 386                 j++;
 387         }
 388     }
 389
 390 }//void CvFaceElement::MergeRects(int d)
 391
 392 void CvFaceElement::Energy()
 393 {
 394     CvSeqReader reader, reader2;
 395     cvStartReadSeq( m_seqRects, &reader );
 396     for (int i = 0; i < m_seqRects->total; i++)
 397     {
 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++)
 402         {
 403             CvTrackingRect* pRect2 = (CvTrackingRect*)(reader2.ptr);
 404             if (i != j)
 405             {
 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 ++;
 416             }
 417             CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader2 );
 418         }
 419         // energy
 420         pRect->Energy(m_trPrev);
 421         CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader );
 422     }
 423 }//void CvFaceElement::Energy()
 424
 425 CV_IMPL CvFaceTracker*
 426 cvInitFaceTracker(CvFaceTracker* pFaceTracker, const IplImage* imgGray, CvRect* pRects, int nRects)
 427 {
 428     assert(NULL != imgGray);
 429     assert(NULL != pRects);
 430     assert(nRects >= NUM_FACE_ELEMENTS);
 431     if ((NULL == imgGray) ||
 432         (NULL == pRects) ||
 433         (nRects < NUM_FACE_ELEMENTS))
 434         return NULL;
 435
 436     //int new_face = FALSE;
 437     CvFaceTracker* pFace = pFaceTracker;
 438     if (NULL == pFace)
 439     {
 440         pFace = new CvFaceTracker;
 441         if (NULL == pFace)
 442             return NULL;
 443         //new_face = TRUE;
 444     }
 445     pFace->Init(pRects, (IplImage*)imgGray);
 446     return pFace;
 447 }//CvFaceTracker* InitFaceTracker(IplImage* imgGray, CvRect* pRects, int nRects)
 448
 449 CV_IMPL void
 450 cvReleaseFaceTracker(CvFaceTracker** ppFaceTracker)
 451 {
 452     if (NULL == *ppFaceTracker)
 453         return;
 454     delete *ppFaceTracker;
 455     *ppFaceTracker = NULL;
 456 }//void ReleaseFaceTracker(CvFaceTracker** ppFaceTracker)
 457
 458
 459 CV_IMPL int
 460 cvTrackFace(CvFaceTracker* pFaceTracker, IplImage* imgGray, CvRect* pRects, int nRects, CvPoint* ptRotate, double* dbAngleRotate)
 461 {
 462     assert(NULL != pFaceTracker);
 463     assert(NULL != imgGray);
 464     assert(NULL != pRects && nRects >= NUM_FACE_ELEMENTS);
 465     if ((NULL == pFaceTracker) ||
 466         (NULL == imgGray))
 467         return FALSE;
 468     pFaceTracker->InitNextImage(imgGray);
 469     *ptRotate = pFaceTracker->ptRotate;
 470     *dbAngleRotate = pFaceTracker->dbRotateAngle;
 471
 472     int nElements = 16;
 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);
 477     int dMinSize = d;
 478     int nRestarts = 0;
 479
 480     int elem;
 481
 482     CvFaceElement big_face[NUM_FACE_ELEMENTS];
 483 START:
 484     // init
 485     for (elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
 486     {
 487         CvRect r = pFaceTracker->face[elem].r;
 488         Extend(r, d);
 489         if (r.width < 4*d)
 490         {
 491             r.x -= (4*d - r.width) / 2;
 492             r.width += 4*d - r.width;
 493         }
 494         if (r.height < 3*d)
 495         {
 496             r.y -= (3*d - r.height) / 2;
 497             r.height += 3*d - r.height;
 498         }
 499         if (r.x < 1)
 500             r.x = 1;
 501         if (r.y < 1)
 502             r.y = 1;
 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))
 508             return FALSE;
 509     }
 510     // find contours
 511     for (elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
 512         big_face[elem].FindRects(pFaceTracker->imgGray, pFaceTracker->imgThresh, 32, dMinSize);
 513     // candidats
 514     CvTrackingRect new_face[NUM_FACE_ELEMENTS];
 515     int new_energy = 0;
 516     int found = ChoiceTrackingFace3(pFaceTracker, nElements, big_face, new_face, new_energy);
 517     int restart = FALSE;
 518     int find2 = FALSE;
 519     int noel = -1;
 520     if (found)
 521     {
 522         if (new_energy > 100000 && -1 != pFaceTracker->iTrackingFaceType)
 523             find2 = TRUE;
 524         else if (new_energy > 150000)
 525         {
 526             int elements = 0;
 527             for (int el = 0; el < NUM_FACE_ELEMENTS; el++)
 528             {
 529                 if (big_face[el].m_seqRects->total > 16 || (big_face[el].m_seqRects->total > 8 && new_face[el].iEnergy < 100))
 530                     elements++;
 531                 else
 532                     noel = el;
 533             }
 534             if (2 == elements)
 535                 find2 = TRUE;
 536             else
 537                 restart = TRUE;
 538         }
 539     }
 540     else
 541     {
 542         if (-1 != pFaceTracker->iTrackingFaceType)
 543             find2 = TRUE;
 544         else
 545             restart = TRUE;
 546     }
 547 RESTART:
 548     if (restart)
 549     {
 550         if (nRestarts++ < 2)
 551         {
 552             d = d + d/4;
 553             goto START;
 554         }
 555     }
 556     else if (find2)
 557     {
 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)
 562         {
 563             pFaceTracker->iTrackingFaceType = noel;
 564             found = TRUE;
 565         }
 566         else
 567         {
 568             restart = TRUE;
 569             goto RESTART;
 570         }
 571     }
 572
 573     if (found)
 574     {
 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);
 580         vx_prev = vx_prev1;
 581         vy_prev = vy_prev1;
 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);
 588         if (n1_n2 != 0)
 589             pFaceTracker->dbRotateDelta = asin((vx * vy_prev - vx_prev * vy) / sqrt(n1_n2));
 590         pFaceTracker->dbRotateAngle -= pFaceTracker->dbRotateDelta;
 591     }
 592     else
 593     {
 594         pFaceTracker->dbRotateDelta = 0;
 595         pFaceTracker->dbRotateAngle = 0;
 596     }
 597     if ((pFaceTracker->dbRotateAngle >= pi/2 && pFaceTracker->dbRotateAngle > 0) ||
 598         (pFaceTracker->dbRotateAngle <= -pi/2 && pFaceTracker->dbRotateAngle < 0))
 599     {
 600         pFaceTracker->dbRotateDelta = 0;
 601         pFaceTracker->dbRotateAngle = 0;
 602         found = FALSE;
 603     }
 604     if (found)
 605     {
 606         for (int i = 0; i < NUM_FACE_ELEMENTS && i < nRects; i++)
 607             pRects[i] = pFaceTracker->face[i].r;
 608     }
 609     return found;
 610 }//int FindFaceTracker(CvFaceTracker* pFaceTracker, IplImage* imgGray, CvRect* pRects, int nRects, CvPoint& ptRotate, double& dbAngleRotate)
 611
 612 void ThresholdingParam(IplImage *imgGray, int iNumLayers, int &iMinLevel, int &iMaxLevel, float &step, float& power, int iHistMin /*= HIST_MIN*/)
 613 {
 614     assert(imgGray != NULL);
 615     assert(imgGray->nChannels == 1);
 616     int i, j;
 617     // create histogram
 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++)
 623     {
 624         for (i = 0; i < rROI.width; i++)
 625             histImg[buffImg[i]] ++;
 626         buffImg += imgGray->widthStep;
 627     }
 628     // params
 629     for (i = 0; i < 256; i++)
 630     {
 631         if (histImg[i] > iHistMin)
 632             break;
 633     }
 634     iMinLevel = i;
 635     for (i = 255; i >= 0; i--)
 636     {
 637         if (histImg[i] > iHistMin)
 638             break;
 639     }
 640     iMaxLevel = i;
 641     if (iMaxLevel <= iMinLevel)
 642     {
 643         iMaxLevel = 255;
 644         iMinLevel = 0;
 645     }
 646     // power
 647     double black = 1;
 648     double white = 1;
 649     for (i = iMinLevel; i < (iMinLevel + iMaxLevel) / 2; i++)
 650         black += histImg[i];
 651     for (i = (iMinLevel + iMaxLevel) / 2; i < iMaxLevel; i++)
 652         white += histImg[i];
 653     power = float(black) / float(2 * white);
 654     //
 655     step = float(iMaxLevel - iMinLevel) / float(iNumLayers);
 656     if (step < 1.0)
 657         step = 1.0;
 658 }// void ThresholdingParam(IplImage *imgGray, int iNumLayers, int &iMinLevel, int &iMaxLevel, int &iStep)
 659
 660 int ChoiceTrackingFace3(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy)
 661 {
 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;
 666     int found = FALSE;
 667     int N = 0;
 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++)
 671     {
 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++)
 675         {
 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)
 678             {
 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++)
 681                 {
 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)
 685                     {
 686                         curr_energy = GetEnergy(curr_face, pTF->face, pTF->ptTempl, pTF->rTempl);
 687                         if (curr_energy < new_energy)
 688                         {
 689                             for (int elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
 690                                 new_face[elem] = curr_face[elem];
 691                             new_energy = curr_energy;
 692                             found = TRUE;
 693                         }
 694                         N++;
 695                     }
 696                 }
 697             }
 698         }
 699     }
 700     if (found)
 701     {
 702         for (int elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
 703             face[elem] = *(new_face[elem]);
 704     }
 705     return found;
 706 } // int ChoiceTrackingFace3(const CvTrackingRect* tr_face, CvTrackingRect* new_face, int& new_energy)
 707
 708 int ChoiceTrackingFace2(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy, int noel)
 709 {
 710     int element[NUM_FACE_ELEMENTS];
 711     for (int i = 0, elem = 0; i < NUM_FACE_ELEMENTS; i++)
 712     {
 713         if (i != noel)
 714         {
 715             element[elem] = i;
 716             elem ++;
 717         }
 718         else
 719             element[2] = i;
 720     }
 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;
 725     int found = FALSE;
 726     int N = 0;
 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++)
 730     {
 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++)
 734         {
 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)
 738             {
 739                 for (int elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
 740                     new_face[elem] = curr_face[elem];
 741                 new_energy = curr_energy;
 742                 found = TRUE;
 743             }
 744             N++;
 745         }
 746     }
 747     if (found)
 748     {
 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);
 785     }
 786     return found;
 787 } // int ChoiceTrackingFace3(const CvTrackingRect* tr_face, CvTrackingRect* new_face, int& new_energy)
 788
 789 inline int GetEnergy(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl)
 790 {
 791     int energy = 0;
 792     CvPoint ptNew[NUM_FACE_ELEMENTS];
 793     CvPoint ptPrev[NUM_FACE_ELEMENTS];
 794     for (int i = 0; i < NUM_FACE_ELEMENTS; i++)
 795     {
 796         ptNew[i] = ppNew[i]->ptCenter;
 797         ptPrev[i] = pPrev[i].ptCenter;
 798         energy += ppNew[i]->iEnergy - 2 * ppNew[i]->nRectsInThis;
 799     }
 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;
 807     energy +=
 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) +
 815         0;
 816     return energy;
 817 }
 818
 819 inline int GetEnergy2(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl, int* element)
 820 {
 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;
 835
 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) +
 843         0;
 844
 845     return energy;
 846 }
 847
 848 inline double CalculateTransformationLMS3( CvPoint* pTemplPoints,
 849                                    CvPoint* pSrcPoints,
 850                                    double*       pdbAverageScale,
 851                                    double*       pdbAverageRotate,
 852                                    double*       pdbAverageShiftX,
 853                                    double*       pdbAverageShiftY )
 854 {
 855 //    double WS = 0;
 856     double dbAverageScale = 1;
 857     double dbAverageRotate = 0;
 858     double dbAverageShiftX = 0;
 859     double dbAverageShiftY = 0;
 860     double dbLMS = 0;
 861
 862     assert( NULL != pTemplPoints);
 863     assert( NULL != pSrcPoints);
 864
 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;
 869
 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;
 872
 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;
 875
 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;
 882
 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;
 889
 890     dbXtXt -= dbXt * dbXt;
 891     dbYtYt -= dbYt * dbYt;
 892
 893     dbXsXs -= dbXs * dbXs;
 894     dbYsYs -= dbYs * dbYs;
 895
 896     dbXtXs -= dbXt * dbXs;
 897     dbYtYs -= dbYt * dbYs;
 898
 899     dbXtYs -= dbXt * dbYs;
 900     dbYtXs -= dbYt * dbXs;
 901
 902     dbAverageRotate = atan2( dbXtYs - dbYtXs, dbXtXs + dbYtYs );
 903
 904     double cosR = cos(dbAverageRotate);
 905     double sinR = sin(dbAverageRotate);
 906     double del = dbXsXs + dbYsYs;
 907     if( del != 0 )
 908     {
 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;
 911     }
 912
 913     dbAverageShiftX = double(dbXt) - dbAverageScale * (double(dbXs) * cosR + double(dbYs) * sinR);
 914     dbAverageShiftY = double(dbYt) - dbAverageScale * (double(dbYs) * cosR - double(dbXs) * sinR);
 915
 916     if( pdbAverageScale != NULL ) *pdbAverageScale = dbAverageScale;
 917     if( pdbAverageRotate != NULL ) *pdbAverageRotate = dbAverageRotate;
 918     if( pdbAverageShiftX != NULL ) *pdbAverageShiftX = dbAverageShiftX;
 919     if( pdbAverageShiftY != NULL ) *pdbAverageShiftY = dbAverageShiftY;
 920
 921     assert(dbLMS >= 0);
 922     return dbLMS;
 923 }
 924
 925 inline double CalculateTransformationLMS3_0( CvPoint* pTemplPoints, CvPoint* pSrcPoints)
 926 {
 927     double dbLMS = 0;
 928
 929     assert( NULL != pTemplPoints);
 930     assert( NULL != pSrcPoints);
 931
 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;
 936
 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;
 939
 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;
 942
 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;
 949
 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;
 956
 957     dbXtXt -= dbXt * dbXt;
 958     dbYtYt -= dbYt * dbYt;
 959
 960     dbXsXs -= dbXs * dbXs;
 961     dbYsYs -= dbYs * dbYs;
 962
 963     dbXtXs -= dbXt * dbXs;
 964     dbYtYs -= dbYt * dbYs;
 965
 966     dbXtYs -= dbXt * dbYs;
 967     dbYtXs -= dbYt * dbXs;
 968
 969     double del = dbXsXs + dbYsYs;
 970     if( del != 0 )
 971         dbLMS = dbXtXt + dbYtYt - ((double)pow(dbXtXs + dbYtYs,2) + (double)pow(dbXtYs - dbYtXs,2)) / del;
 972     return dbLMS;
 973 }
 974