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