1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install, copy or use the software.
8 // Copyright (C) 2009, Farhad Dadgostar
9 // Intel Corporation and third party copyrights are property of their respective owners.
11 // Redistribution and use in source and binary forms, with or without modification,
12 // are permitted provided that the following conditions are met:
14 // * Redistribution's of source code must retain the above copyright notice,
15 // this list of conditions and the following disclaimer.
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.
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.
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.
39 CvFuzzyPoint::CvFuzzyPoint(double _x, double _y)
45 bool CvFuzzyCurve::between(double x, double x1, double x2)
47 if ((x >= x1) && (x <= x2))
49 else if ((x >= x2) && (x <= x1))
55 CvFuzzyCurve::CvFuzzyCurve()
60 CvFuzzyCurve::~CvFuzzyCurve()
65 void CvFuzzyCurve::setCentre(double _centre)
70 double CvFuzzyCurve::getCentre()
75 void CvFuzzyCurve::clear()
80 void CvFuzzyCurve::addPoint(double x, double y)
83 point = new CvFuzzyPoint(x, y);
84 points.push_back(*point);
87 double CvFuzzyCurve::calcValue(double param)
89 int size = (int)points.size();
90 double x1, y1, x2, y2, m, y;
91 for (int i = 1; i < size; i++)
95 if (between(param, x1, x2)) {
108 double CvFuzzyCurve::getValue()
113 void CvFuzzyCurve::setValue(double _value)
119 CvFuzzyFunction::CvFuzzyFunction()
124 CvFuzzyFunction::~CvFuzzyFunction()
129 void CvFuzzyFunction::addCurve(CvFuzzyCurve *curve, double value)
131 curves.push_back(*curve);
132 curve->setValue(value);
135 void CvFuzzyFunction::resetValues()
137 int numCurves = (int)curves.size();
138 for (int i = 0; i < numCurves; i++)
139 curves[i].setValue(0);
142 double CvFuzzyFunction::calcValue()
144 double s1 = 0, s2 = 0, v;
145 int numCurves = (int)curves.size();
146 for (int i = 0; i < numCurves; i++)
148 v = curves[i].getValue();
149 s1 += curves[i].getCentre() * v;
159 CvFuzzyCurve *CvFuzzyFunction::newCurve()
162 c = new CvFuzzyCurve();
167 CvFuzzyRule::CvFuzzyRule()
174 CvFuzzyRule::~CvFuzzyRule()
176 if (fuzzyInput1 != NULL)
179 if (fuzzyInput2 != NULL)
182 if (fuzzyOutput != NULL)
186 void CvFuzzyRule::setRule(CvFuzzyCurve *c1, CvFuzzyCurve *c2, CvFuzzyCurve *o1)
193 double CvFuzzyRule::calcValue(double param1, double param2)
196 v1 = fuzzyInput1->calcValue(param1);
197 if (fuzzyInput2 != NULL)
199 v2 = fuzzyInput2->calcValue(param2);
209 CvFuzzyCurve *CvFuzzyRule::getOutputCurve()
214 CvFuzzyController::CvFuzzyController()
219 CvFuzzyController::~CvFuzzyController()
221 int size = (int)rules.size();
222 for(int i = 0; i < size; i++)
226 void CvFuzzyController::addRule(CvFuzzyCurve *c1, CvFuzzyCurve *c2, CvFuzzyCurve *o1)
228 CvFuzzyRule *f = new CvFuzzyRule();
230 f->setRule(c1, c2, o1);
233 double CvFuzzyController::calcOutput(double param1, double param2)
236 CvFuzzyFunction list;
237 int size = (int)rules.size();
239 for(int i = 0; i < size; i++)
241 v = rules[i]->calcValue(param1, param2);
243 list.addCurve(rules[i]->getOutputCurve(), v);
245 v = list.calcValue();
249 CvFuzzyMeanShiftTracker::FuzzyResizer::FuzzyResizer()
251 CvFuzzyCurve *i1L, *i1M, *i1H;
252 CvFuzzyCurve *oS, *oZE, *oE;
255 double MedStart = 0.1, MedWidth = 0.15;
257 c = iInput.newCurve();
263 c = iInput.newCurve();
264 c->addPoint(0.05, 0);
265 c->addPoint(MedStart, 1);
266 c->addPoint(MedStart+MedWidth, 1);
267 c->addPoint(MedStart+MedWidth+0.05, 0);
268 c->setCentre(MedStart+(MedWidth/2));
271 c = iInput.newCurve();
272 c->addPoint(MedStart+MedWidth, 0);
274 c->addPoint(1000, 1);
278 c = iOutput.newCurve();
279 c->addPoint(-10000, 1);
281 c->addPoint(-0.5, 0);
285 c = iOutput.newCurve();
287 c->addPoint(-0.05, 1);
288 c->addPoint(0.05, 1);
293 c = iOutput.newCurve();
294 c->addPoint(-0.5, 0);
296 c->addPoint(1000, 1);
300 fuzzyController.addRule(i1L, NULL, oS);
301 fuzzyController.addRule(i1M, NULL, oZE);
302 fuzzyController.addRule(i1H, NULL, oE);
305 int CvFuzzyMeanShiftTracker::FuzzyResizer::calcOutput(double edgeDensity, double density)
307 return (int)fuzzyController.calcOutput(edgeDensity, density);
310 CvFuzzyMeanShiftTracker::SearchWindow::SearchWindow()
335 CvFuzzyMeanShiftTracker::SearchWindow::~SearchWindow()
337 if (fuzzyResizer != NULL)
341 void CvFuzzyMeanShiftTracker::SearchWindow::setSize(int _x, int _y, int _width, int _height)
354 if (x + width > maxWidth)
355 width = maxWidth - x;
357 if (y + height > maxHeight)
358 height = maxHeight - y;
361 void CvFuzzyMeanShiftTracker::SearchWindow::initDepthValues(IplImage *maskImage, IplImage *depthMap)
363 unsigned int d=0, mind = 0xFFFF, maxd = 0, m0 = 0, m1 = 0, mc, dd;
364 unsigned char *data = NULL;
365 unsigned short *depthData = NULL;
367 for (int j = 0; j < height; j++)
369 data = (unsigned char *)(maskImage->imageData + (maskImage->widthStep * (j + y)) + x);
371 depthData = (unsigned short *)(depthMap->imageData + (depthMap->widthStep * (j + y)) + x);
373 for (int i = 0; i < width; i++)
399 if ((mc - mind) > (maxd - mc))
414 bool CvFuzzyMeanShiftTracker::SearchWindow::shift()
416 if ((xGc != (width/2)) || (yGc != (height/2)))
418 setSize(x + (xGc-(width/2)), y + (yGc-(height/2)), width, height);
427 void CvFuzzyMeanShiftTracker::SearchWindow::extractInfo(IplImage *maskImage, IplImage *depthMap, bool initDepth)
439 maxWidth = maskImage->width;
440 maxHeight = maskImage->height;
443 initDepthValues(maskImage, depthMap);
445 unsigned char *maskData = NULL;
446 unsigned short *depthData = NULL, depth;
450 verticalEdgeLeft = 0;
451 verticalEdgeRight = 0;
452 horizontalEdgeTop = 0;
453 horizontalEdgeBottom = 0;
455 for (int j = 0; j < height; j++)
457 maskData = (unsigned char *)(maskImage->imageData + (maskImage->widthStep * (j + y)) + x);
459 depthData = (unsigned short *)(depthMap->imageData + (depthMap->widthStep * (j + y)) + x);
462 for (int i = 0; i < width; i++)
469 depth = (*depthData);
470 if ((depth > depthHigh) || (depth < depthLow))
487 else if (i == width-1)
491 else if (j == height-1)
492 horizontalEdgeBottom++;
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);
512 e2 = sqrt((b*b)+(e3*e3));
513 ellipseHeight = int(sqrt(0.5*(e1+e2)));
514 ellipseWidth = int(sqrt(0.5*(e1-e2)));
518 ellipseAngle = 0.5*atan(b/e3);
520 density = (double)m00/(double)(width * height);
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;
550 } else if (x1 < lx) {
555 resizeDh = resizeDy + 1;
556 } else if (x2 < lx) {
557 resizeDh = - (resizeDy + 1);
559 resizeDh = - resizeDy;
564 } else if (y1 < ly) {
569 resizeDw = resizeDx + 1;
570 } else if (y2 < ly) {
571 resizeDw = - (resizeDx + 1);
573 resizeDw = - resizeDx;
577 void CvFuzzyMeanShiftTracker::SearchWindow::getResizeAttribsInnerDensity(int &resizeDx, int &resizeDy, int &resizeDw, int &resizeDh)
579 int newWidth, newHeight, dx, dy;
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);
593 void CvFuzzyMeanShiftTracker::SearchWindow::getResizeAttribsEdgeDensityFuzzy(int &resizeDx, int &resizeDy, int &resizeDw, int &resizeDh)
595 double dx1=0, dx2, dy1, dy2;
602 if (fuzzyResizer == NULL)
603 fuzzyResizer = new FuzzyResizer();
605 dx2 = fuzzyResizer->calcOutput(double(verticalEdgeRight)/double(height), density);
608 resizeDx = int(-dx1);
609 resizeDw = int(dx1+dx2);
612 dy1 = fuzzyResizer->calcOutput(double(horizontalEdgeTop)/double(width), density);
613 dy2 = fuzzyResizer->calcOutput(double(horizontalEdgeBottom)/double(width), density);
615 dx1 = fuzzyResizer->calcOutput(double(verticalEdgeLeft)/double(height), density);
616 dx2 = fuzzyResizer->calcOutput(double(verticalEdgeRight)/double(height), density);
619 resizeDx = int(-dx1);
620 resizeDw = int(dx1+dx2);
623 dy1 = fuzzyResizer->calcOutput(double(horizontalEdgeTop)/double(width), density);
624 dy2 = fuzzyResizer->calcOutput(double(horizontalEdgeBottom)/double(width), density);
627 resizeDy = int(-dy1);
628 resizeDh = int(dy1+dy2);
632 bool CvFuzzyMeanShiftTracker::SearchWindow::meanShift(IplImage *maskImage, IplImage *depthMap, int maxIteration, bool initDepth)
637 extractInfo(maskImage, depthMap, initDepth);
640 } while (++numShifts < maxIteration);
645 void CvFuzzyMeanShiftTracker::findOptimumSearchWindow(SearchWindow &searchWindow, IplImage *maskImage, IplImage *depthMap, int maxIteration, int resizeMethod, bool initDepth)
647 int resizeDx, resizeDy, resizeDw, resizeDh;
652 searchWindow.numIters = 0;
653 for (int i = 0; i < maxIteration; i++)
655 searchWindow.numIters++;
656 searchWindow.meanShift(maskImage, depthMap, MaxMeanShiftIteration, initDepth);
657 switch (resizeMethod)
659 case rmEdgeDensityLinear :
660 searchWindow.getResizeAttribsEdgeDensityLinear(resizeDx, resizeDy, resizeDw, resizeDh);
662 case rmEdgeDensityFuzzy :
663 //searchWindow.getResizeAttribsEdgeDensityLinear(resizeDx, resizeDy, resizeDw, resizeDh);
664 searchWindow.getResizeAttribsEdgeDensityFuzzy(resizeDx, resizeDy, resizeDw, resizeDh);
666 case rmInnerDensity :
667 searchWindow.getResizeAttribsInnerDensity(resizeDx, resizeDy, resizeDw, resizeDh);
670 searchWindow.getResizeAttribsEdgeDensityLinear(resizeDx, resizeDy, resizeDw, resizeDh);
673 searchWindow.ldx = resizeDx;
674 searchWindow.ldy = resizeDy;
675 searchWindow.ldw = resizeDw;
676 searchWindow.ldh = resizeDh;
678 if ((resizeDx == 0) && (resizeDy == 0) && (resizeDw == 0) && (resizeDh == 0))
681 searchWindow.setSize(searchWindow.x + resizeDx, searchWindow.y + resizeDy, searchWindow.width + resizeDw, searchWindow.height + resizeDh);
685 CvFuzzyMeanShiftTracker::CvFuzzyMeanShiftTracker()
687 searchMode = tsSetWindow;
690 CvFuzzyMeanShiftTracker::~CvFuzzyMeanShiftTracker()
695 void CvFuzzyMeanShiftTracker::track(IplImage *maskImage, IplImage *depthMap, int resizeMethod, bool resetSearch, int minKernelMass)
697 bool initDepth = false;
700 searchMode = tsSetWindow;
709 kernel.maxWidth = maskImage->width;
710 kernel.maxHeight = maskImage->height;
711 kernel.setSize(0, 0, maskImage->width, maskImage->height);
714 searchMode = tsSearching;
715 findOptimumSearchWindow(kernel, maskImage, depthMap, MaxSetSizeIteration, resizeMethod, initDepth);
716 if ((kernel.density == 0) || (kernel.m00 < minKernelMass))
717 searchMode = tsSetWindow;
719 searchMode = tsTracking;