modules/contrib/src/fuzzymeanshifttracker.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, copy or use the software.
   7 //
   8 // Copyright (C) 2009, Farhad Dadgostar
   9 // Intel Corporation and third party copyrights are property of their respective owners.
  10 //
  11 // Redistribution and use in source and binary forms, with or without modification,
  12 // are permitted provided that the following conditions are met:
  13 //
  14 //   * Redistribution's of source code must retain the above copyright notice,
  15 //     this list of conditions and the following disclaimer.
  16 //
  17 //   * Redistribution's in binary form must reproduce the above copyright notice,
  18 //     this list of conditions and the following disclaimer in the documentation
  19 //     and/or other materials provided with the distribution.
  20 //
  21 //   * The name of Intel Corporation may not be used to endorse or promote products
  22 //     derived from this software without specific prior written permission.
  23 //
  24 // This software is provided by the copyright holders and contributors "as is" and
  25 // any express or implied warranties, including, but not limited to, the implied
  26 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  27 // In no event shall the Intel Corporation or contributors be liable for any direct,
  28 // indirect, incidental, special, exemplary, or consequential damages
  29 // (including, but not limited to, procurement of substitute goods or services;
  30 // loss of use, data, or profits; or business interruption) however caused
  31 // and on any theory of liability, whether in contract, strict liability,
  32 // or tort (including negligence or otherwise) arising in any way out of
  33 // the use of this software, even if advised of the possibility of such damage.
  34 //
  35 //M*/
  36
  37 #include "precomp.hpp"
  38
  39 CvFuzzyPoint::CvFuzzyPoint(double _x, double _y)
  40 {
  41     x = _x;
  42     y = _y;
  43 }
  44
  45 bool CvFuzzyCurve::between(double x, double x1, double x2)
  46 {
  47     if ((x >= x1) && (x <= x2))
  48         return true;
  49     else if ((x >= x2) && (x <= x1))
  50         return true;
  51
  52     return false;
  53 }
  54
  55 CvFuzzyCurve::CvFuzzyCurve()
  56 {
  57     value = 0;
  58 }
  59
  60 CvFuzzyCurve::~CvFuzzyCurve()
  61 {
  62     // nothing to do
  63 }
  64
  65 void CvFuzzyCurve::setCentre(double _centre)
  66 {
  67     centre = _centre;
  68 }
  69
  70 double CvFuzzyCurve::getCentre()
  71 {
  72     return centre;
  73 }
  74
  75 void CvFuzzyCurve::clear()
  76 {
  77     points.clear();
  78 }
  79
  80 void CvFuzzyCurve::addPoint(double x, double y)
  81 {
  82     points.push_back(CvFuzzyPoint(x, y));
  83 }
  84
  85 double CvFuzzyCurve::calcValue(double param)
  86 {
  87     int size = (int)points.size();
  88     double x1, y1, x2, y2, m, y;
  89     for (int i = 1; i < size; i++)
  90     {
  91         x1 = points[i-1].x;
  92         x2 = points[i].x;
  93         if (between(param, x1, x2)) {
  94             y1 = points[i-1].y;
  95             y2 = points[i].y;
  96             if (x2 == x1)
  97                 return y2;
  98             m = (y2-y1)/(x2-x1);
  99             y = m*(param-x1)+y1;
 100             return y;
 101         }
 102     }
 103     return 0;
 104 }
 105
 106 double CvFuzzyCurve::getValue()
 107 {
 108     return value;
 109 }
 110
 111 void CvFuzzyCurve::setValue(double _value)
 112 {
 113     value = _value;
 114 }
 115
 116
 117 CvFuzzyFunction::CvFuzzyFunction()
 118 {
 119     // nothing to do
 120 }
 121
 122 CvFuzzyFunction::~CvFuzzyFunction()
 123 {
 124     curves.clear();
 125 }
 126
 127 void CvFuzzyFunction::addCurve(CvFuzzyCurve *curve, double value)
 128 {
 129     curves.push_back(*curve);
 130     curve->setValue(value);
 131 }
 132
 133 void CvFuzzyFunction::resetValues()
 134 {
 135     int numCurves = (int)curves.size();
 136     for (int i = 0; i < numCurves; i++)
 137         curves[i].setValue(0);
 138 }
 139
 140 double CvFuzzyFunction::calcValue()
 141 {
 142     double s1 = 0, s2 = 0, v;
 143     int numCurves = (int)curves.size();
 144     for (int i = 0; i < numCurves; i++)
 145     {
 146         v = curves[i].getValue();
 147         s1 += curves[i].getCentre() * v;
 148         s2 += v;
 149     }
 150
 151     if (s2 != 0)
 152         return s1/s2;
 153     else
 154         return 0;
 155 }
 156
 157 CvFuzzyCurve *CvFuzzyFunction::newCurve()
 158 {
 159     CvFuzzyCurve *c;
 160     c = new CvFuzzyCurve();
 161     addCurve(c);
 162     return c;
 163 }
 164
 165 CvFuzzyRule::CvFuzzyRule()
 166 {
 167     fuzzyInput1 = NULL;
 168     fuzzyInput2 = NULL;
 169     fuzzyOutput = NULL;
 170 }
 171
 172 CvFuzzyRule::~CvFuzzyRule()
 173 {
 174     if (fuzzyInput1 != NULL)
 175         delete fuzzyInput1;
 176
 177     if (fuzzyInput2 != NULL)
 178         delete fuzzyInput2;
 179
 180     if (fuzzyOutput != NULL)
 181         delete fuzzyOutput;
 182 }
 183
 184 void CvFuzzyRule::setRule(CvFuzzyCurve *c1, CvFuzzyCurve *c2, CvFuzzyCurve *o1)
 185 {
 186     fuzzyInput1 = c1;
 187     fuzzyInput2 = c2;
 188     fuzzyOutput = o1;
 189 }
 190
 191 double CvFuzzyRule::calcValue(double param1, double param2)
 192 {
 193     double v1, v2;
 194     v1 = fuzzyInput1->calcValue(param1);
 195     if (fuzzyInput2 != NULL)
 196     {
 197         v2 = fuzzyInput2->calcValue(param2);
 198         if (v1 < v2)
 199             return v1;
 200         else
 201             return v2;
 202     }
 203     else
 204         return v1;
 205 }
 206
 207 CvFuzzyCurve *CvFuzzyRule::getOutputCurve()
 208 {
 209     return fuzzyOutput;
 210 }
 211
 212 CvFuzzyController::CvFuzzyController()
 213 {
 214     // nothing to do
 215 }
 216
 217 CvFuzzyController::~CvFuzzyController()
 218 {
 219     int size = (int)rules.size();
 220     for(int i = 0; i < size; i++)
 221         delete rules[i];
 222 }
 223
 224 void CvFuzzyController::addRule(CvFuzzyCurve *c1, CvFuzzyCurve *c2, CvFuzzyCurve *o1)
 225 {
 226     CvFuzzyRule *f = new CvFuzzyRule();
 227     rules.push_back(f);
 228     f->setRule(c1, c2, o1);
 229 }
 230
 231 double CvFuzzyController::calcOutput(double param1, double param2)
 232 {
 233     double v;
 234     CvFuzzyFunction list;
 235     int size = (int)rules.size();
 236
 237     for(int i = 0; i < size; i++)
 238     {
 239         v = rules[i]->calcValue(param1, param2);
 240         if (v != 0)
 241             list.addCurve(rules[i]->getOutputCurve(), v);
 242     }
 243     v = list.calcValue();
 244     return v;
 245 }
 246
 247 CvFuzzyMeanShiftTracker::FuzzyResizer::FuzzyResizer()
 248 {
 249     CvFuzzyCurve *i1L, *i1M, *i1H;
 250     CvFuzzyCurve *oS, *oZE, *oE;
 251     CvFuzzyCurve *c;
 252
 253     double MedStart = 0.1, MedWidth = 0.15;
 254
 255     c = iInput.newCurve();
 256     c->addPoint(0, 1);
 257     c->addPoint(0.1, 0);
 258     c->setCentre(0);
 259     i1L = c;
 260
 261     c = iInput.newCurve();
 262     c->addPoint(0.05, 0);
 263     c->addPoint(MedStart, 1);
 264     c->addPoint(MedStart+MedWidth, 1);
 265     c->addPoint(MedStart+MedWidth+0.05, 0);
 266     c->setCentre(MedStart+(MedWidth/2));
 267     i1M = c;
 268
 269     c = iInput.newCurve();
 270     c->addPoint(MedStart+MedWidth, 0);
 271     c->addPoint(1, 1);
 272     c->addPoint(1000, 1);
 273     c->setCentre(1);
 274     i1H = c;
 275
 276     c = iOutput.newCurve();
 277     c->addPoint(-10000, 1);
 278     c->addPoint(-5, 1);
 279     c->addPoint(-0.5, 0);
 280     c->setCentre(-5);
 281     oS = c;
 282
 283     c = iOutput.newCurve();
 284     c->addPoint(-1, 0);
 285     c->addPoint(-0.05, 1);
 286     c->addPoint(0.05, 1);
 287     c->addPoint(1, 0);
 288     c->setCentre(0);
 289     oZE = c;
 290
 291     c = iOutput.newCurve();
 292     c->addPoint(-0.5, 0);
 293     c->addPoint(5, 1);
 294     c->addPoint(1000, 1);
 295     c->setCentre(5);
 296     oE = c;
 297
 298     fuzzyController.addRule(i1L, NULL, oS);
 299     fuzzyController.addRule(i1M, NULL, oZE);
 300     fuzzyController.addRule(i1H, NULL, oE);
 301 }
 302
 303 int CvFuzzyMeanShiftTracker::FuzzyResizer::calcOutput(double edgeDensity, double density)
 304 {
 305     return (int)fuzzyController.calcOutput(edgeDensity, density);
 306 }
 307
 308 CvFuzzyMeanShiftTracker::SearchWindow::SearchWindow()
 309 {
 310     x = 0;
 311     y = 0;
 312     width = 0;
 313     height = 0;
 314     maxWidth = 0;
 315     maxHeight = 0;
 316     xGc = 0;
 317     yGc = 0;
 318     m00 = 0;
 319     m01 = 0;
 320     m10 = 0;
 321     m11 = 0;
 322     m02 = 0;
 323     m20 = 0;
 324     ellipseHeight = 0;
 325     ellipseWidth = 0;
 326     ellipseAngle = 0;
 327     density = 0;
 328     depthLow = 0;
 329     depthHigh = 0;
 330     fuzzyResizer = NULL;
 331 }
 332
 333 CvFuzzyMeanShiftTracker::SearchWindow::~SearchWindow()
 334 {
 335     if (fuzzyResizer != NULL)
 336         delete fuzzyResizer;
 337 }
 338
 339 void CvFuzzyMeanShiftTracker::SearchWindow::setSize(int _x, int _y, int _width, int _height)
 340 {
 341     x = _x;
 342     y = _y;
 343     width = _width;
 344     height = _height;
 345
 346     if (x < 0)
 347         x = 0;
 348
 349     if (y < 0)
 350         y = 0;
 351
 352     if (x + width > maxWidth)
 353         width = maxWidth - x;
 354
 355     if (y + height > maxHeight)
 356         height = maxHeight - y;
 357 }
 358
 359 void CvFuzzyMeanShiftTracker::SearchWindow::initDepthValues(IplImage *maskImage, IplImage *depthMap)
 360 {
 361     unsigned int d=0, mind = 0xFFFF, maxd = 0, m0 = 0, m1 = 0, mc, dd;
 362     unsigned char *data = NULL;
 363     unsigned short *depthData = NULL;
 364
 365     for (int j = 0; j < height; j++)
 366     {
 367         data = (unsigned char *)(maskImage->imageData + (maskImage->widthStep * (j + y)) + x);
 368         if (depthMap)
 369             depthData = (unsigned short *)(depthMap->imageData + (depthMap->widthStep * (j + y)) + x);
 370
 371         for (int i = 0; i < width; i++)
 372         {
 373             if (*data)
 374             {
 375                 m0 += 1;
 376
 377                 if (depthData)
 378                 {
 379                     if (*depthData)
 380                     {
 381                         d = *depthData;
 382                         m1 += d;
 383                         if (d < mind)
 384                             mind = d;
 385                         if (d > maxd)
 386                             maxd = d;
 387                     }
 388                     depthData++;
 389                 }
 390             }
 391             data++;
 392         }
 393     }
 394
 395     if (m0 > 0)
 396     {
 397         mc = m1/m0;
 398         if ((mc - mind) > (maxd - mc))
 399             dd = maxd - mc;
 400         else
 401             dd = mc - mind;
 402         dd = dd - dd/10;
 403         depthHigh = mc + dd;
 404         depthLow = mc - dd;
 405     }
 406     else
 407     {
 408         depthHigh = 32000;
 409         depthLow = 0;
 410     }
 411 }
 412
 413 bool CvFuzzyMeanShiftTracker::SearchWindow::shift()
 414 {
 415     if ((xGc != (width/2)) || (yGc != (height/2)))
 416     {
 417         setSize(x + (xGc-(width/2)), y + (yGc-(height/2)), width, height);
 418         return true;
 419     }
 420     else
 421     {
 422         return false;
 423     }
 424 }
 425
 426 void CvFuzzyMeanShiftTracker::SearchWindow::extractInfo(IplImage *maskImage, IplImage *depthMap, bool initDepth)
 427 {
 428     m00 = 0;
 429     m10 = 0;
 430     m01 = 0;
 431     m11 = 0;
 432     density = 0;
 433     m02 = 0;
 434     m20 = 0;
 435     ellipseHeight = 0;
 436     ellipseWidth = 0;
 437
 438     maxWidth = maskImage->width;
 439     maxHeight = maskImage->height;
 440
 441     if (initDepth)
 442         initDepthValues(maskImage, depthMap);
 443
 444     unsigned char *maskData = NULL;
 445     unsigned short *depthData = NULL, depth;
 446     bool isOk;
 447     unsigned long count;
 448
 449     verticalEdgeLeft = 0;
 450     verticalEdgeRight = 0;
 451     horizontalEdgeTop = 0;
 452     horizontalEdgeBottom = 0;
 453
 454     for (int j = 0; j < height; j++)
 455     {
 456         maskData = (unsigned char *)(maskImage->imageData + (maskImage->widthStep * (j + y)) + x);
 457         if (depthMap)
 458             depthData = (unsigned short *)(depthMap->imageData + (depthMap->widthStep * (j + y)) + x);
 459
 460         count = 0;
 461         for (int i = 0; i < width; i++)
 462         {
 463             if (*maskData)
 464             {
 465                 isOk = true;
 466                 if (depthData)
 467                 {
 468                     depth = (*depthData);
 469                     if ((depth > depthHigh) || (depth < depthLow))
 470                         isOk = false;
 471
 472                     depthData++;
 473                 }
 474
 475                 if (isOk)
 476                 {
 477                     m00++;
 478                     m01 += j;
 479                     m10 += i;
 480                     m02 += (j * j);
 481                     m20 += (i * i);
 482                     m11 += (j * i);
 483
 484                     if (i == 0)
 485                         verticalEdgeLeft++;
 486                     else if (i == width-1)
 487                         verticalEdgeRight++;
 488                     else if (j == 0)
 489                         horizontalEdgeTop++;
 490                     else if (j == height-1)
 491                         horizontalEdgeBottom++;
 492
 493                     count++;
 494                 }
 495             }
 496             maskData++;
 497         }
 498     }
 499
 500     if (m00 > 0)
 501     {
 502         xGc = (m10 / m00);
 503         yGc = (m01 / m00);
 504
 505         double a, b, c, e1, e2, e3;
 506         a = ((double)m20/(double)m00)-(xGc * xGc);
 507         b = 2*(((double)m11/(double)m00)-(xGc * yGc));
 508         c = ((double)m02/(double)m00)-(yGc * yGc);
 509         e1 = a+c;
 510         e3 = a-c;
 511         e2 = sqrt((b*b)+(e3*e3));
 512         ellipseHeight = int(sqrt(0.5*(e1+e2)));
 513         ellipseWidth = int(sqrt(0.5*(e1-e2)));
 514         if (e3 == 0)
 515             ellipseAngle = 0;
 516         else
 517             ellipseAngle = 0.5*atan(b/e3);
 518
 519         density = (double)m00/(double)(width * height);
 520     }
 521     else
 522     {
 523         xGc = width / 2;
 524         yGc = height / 2;
 525         ellipseHeight = 0;
 526         ellipseWidth = 0;
 527         ellipseAngle = 0;
 528         density = 0;
 529     }
 530 }
 531
 532 void CvFuzzyMeanShiftTracker::SearchWindow::getResizeAttribsEdgeDensityLinear(int &resizeDx, int &resizeDy, int &resizeDw, int &resizeDh) {
 533     int x1 = horizontalEdgeTop;
 534     int x2 = horizontalEdgeBottom;
 535     int y1 = verticalEdgeLeft;
 536     int y2 = verticalEdgeRight;
 537     int gx = (width*2)/5;
 538     int gy = (height*2)/5;
 539     int lx = width/10;
 540     int ly = height/10;
 541
 542     resizeDy = 0;
 543     resizeDh = 0;
 544     resizeDx = 0;
 545     resizeDw = 0;
 546
 547     if (x1 > gx) {
 548         resizeDy = -1;
 549     } else if (x1 < lx) {
 550         resizeDy = +1;
 551     }
 552
 553     if (x2 > gx) {
 554         resizeDh = resizeDy + 1;
 555     } else if (x2 < lx) {
 556         resizeDh = - (resizeDy + 1);
 557     } else {
 558         resizeDh = - resizeDy;
 559     }
 560
 561     if (y1 > gy) {
 562         resizeDx = -1;
 563     } else if (y1 < ly) {
 564         resizeDx = +1;
 565     }
 566
 567     if (y2 > gy) {
 568         resizeDw = resizeDx + 1;
 569     } else if (y2 < ly) {
 570         resizeDw = - (resizeDx + 1);
 571     } else {
 572         resizeDw = - resizeDx;
 573     }
 574 }
 575
 576 void CvFuzzyMeanShiftTracker::SearchWindow::getResizeAttribsInnerDensity(int &resizeDx, int &resizeDy, int &resizeDw, int &resizeDh)
 577 {
 578     int newWidth, newHeight, dx, dy;
 579     double px, py;
 580     newWidth = int(sqrt(double(m00)*1.3));
 581     newHeight = int(newWidth*1.2);
 582     dx = (newWidth - width);
 583     dy = (newHeight - height);
 584     px = (double)xGc/(double)width;
 585     py = (double)yGc/(double)height;
 586     resizeDx = (int)(px*dx);
 587     resizeDy = (int)(py*dy);
 588     resizeDw = (int)((1-px)*dx);
 589     resizeDh = (int)((1-py)*dy);
 590 }
 591
 592 void CvFuzzyMeanShiftTracker::SearchWindow::getResizeAttribsEdgeDensityFuzzy(int &resizeDx, int &resizeDy, int &resizeDw, int &resizeDh)
 593 {
 594     double dx1=0, dx2, dy1, dy2;
 595
 596     resizeDy = 0;
 597     resizeDh = 0;
 598     resizeDx = 0;
 599     resizeDw = 0;
 600
 601     if (fuzzyResizer == NULL)
 602         fuzzyResizer = new FuzzyResizer();
 603
 604     dx2 = fuzzyResizer->calcOutput(double(verticalEdgeRight)/double(height), density);
 605     if (dx1 == dx2)
 606     {
 607         resizeDx = int(-dx1);
 608         resizeDw = int(dx1+dx2);
 609     }
 610
 611     dy1 = fuzzyResizer->calcOutput(double(horizontalEdgeTop)/double(width), density);
 612     dy2 = fuzzyResizer->calcOutput(double(horizontalEdgeBottom)/double(width), density);
 613
 614     dx1 = fuzzyResizer->calcOutput(double(verticalEdgeLeft)/double(height), density);
 615     dx2 = fuzzyResizer->calcOutput(double(verticalEdgeRight)/double(height), density);
 616     //if (dx1 == dx2)
 617     {
 618         resizeDx = int(-dx1);
 619         resizeDw = int(dx1+dx2);
 620     }
 621
 622     dy1 = fuzzyResizer->calcOutput(double(horizontalEdgeTop)/double(width), density);
 623     dy2 = fuzzyResizer->calcOutput(double(horizontalEdgeBottom)/double(width), density);
 624     //if (dy1 == dy2)
 625     {
 626         resizeDy = int(-dy1);
 627         resizeDh = int(dy1+dy2);
 628     }
 629 }
 630
 631 bool CvFuzzyMeanShiftTracker::SearchWindow::meanShift(IplImage *maskImage, IplImage *depthMap, int maxIteration, bool initDepth)
 632 {
 633     numShifts = 0;
 634     do
 635     {
 636         extractInfo(maskImage, depthMap, initDepth);
 637         if (! shift())
 638             return true;
 639     } while (++numShifts < maxIteration);
 640
 641     return false;
 642 }
 643
 644 void CvFuzzyMeanShiftTracker::findOptimumSearchWindow(SearchWindow &searchWindow, IplImage *maskImage, IplImage *depthMap, int maxIteration, int resizeMethod, bool initDepth)
 645 {
 646     int resizeDx, resizeDy, resizeDw, resizeDh;
 647     resizeDx = 0;
 648     resizeDy = 0;
 649     resizeDw = 0;
 650     resizeDh = 0;
 651     searchWindow.numIters = 0;
 652     for (int i = 0; i < maxIteration; i++)
 653     {
 654         searchWindow.numIters++;
 655         searchWindow.meanShift(maskImage, depthMap, MaxMeanShiftIteration, initDepth);
 656         switch (resizeMethod)
 657         {
 658             case rmEdgeDensityLinear :
 659                 searchWindow.getResizeAttribsEdgeDensityLinear(resizeDx, resizeDy, resizeDw, resizeDh);
 660                 break;
 661             case rmEdgeDensityFuzzy :
 662                 //searchWindow.getResizeAttribsEdgeDensityLinear(resizeDx, resizeDy, resizeDw, resizeDh);
 663                 searchWindow.getResizeAttribsEdgeDensityFuzzy(resizeDx, resizeDy, resizeDw, resizeDh);
 664                 break;
 665             case rmInnerDensity :
 666                 searchWindow.getResizeAttribsInnerDensity(resizeDx, resizeDy, resizeDw, resizeDh);
 667                 break;
 668             default:
 669                 searchWindow.getResizeAttribsEdgeDensityLinear(resizeDx, resizeDy, resizeDw, resizeDh);
 670         }
 671
 672         searchWindow.ldx = resizeDx;
 673         searchWindow.ldy = resizeDy;
 674         searchWindow.ldw = resizeDw;
 675         searchWindow.ldh = resizeDh;
 676
 677         if ((resizeDx == 0) && (resizeDy == 0) && (resizeDw == 0) && (resizeDh == 0))
 678             break;
 679
 680         searchWindow.setSize(searchWindow.x + resizeDx, searchWindow.y + resizeDy, searchWindow.width + resizeDw, searchWindow.height + resizeDh);
 681     }
 682 }
 683
 684 CvFuzzyMeanShiftTracker::CvFuzzyMeanShiftTracker()
 685 {
 686     searchMode = tsSetWindow;
 687 }
 688
 689 CvFuzzyMeanShiftTracker::~CvFuzzyMeanShiftTracker()
 690 {
 691     // nothing to do
 692 }
 693
 694 void CvFuzzyMeanShiftTracker::track(IplImage *maskImage, IplImage *depthMap, int resizeMethod, bool resetSearch, int minKernelMass)
 695 {
 696     bool initDepth = false;
 697
 698     if (resetSearch)
 699         searchMode = tsSetWindow;
 700
 701     switch (searchMode)
 702     {
 703         case tsDisabled:
 704             return;
 705         case tsSearching:
 706             return;
 707         case tsSetWindow:
 708             kernel.maxWidth = maskImage->width;
 709             kernel.maxHeight = maskImage->height;
 710             kernel.setSize(0, 0, maskImage->width, maskImage->height);
 711             initDepth = true;
 712         case tsTracking:
 713             searchMode = tsSearching;
 714             findOptimumSearchWindow(kernel, maskImage, depthMap, MaxSetSizeIteration, resizeMethod, initDepth);
 715             if ((kernel.density == 0) || (kernel.m00 < minKernelMass))
 716                 searchMode = tsSetWindow;
 717             else
 718                 searchMode = tsTracking;
 719     }
 720 }