--- /dev/null
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install, copy or use the software.
+//
+// Copyright (C) 2009, Farhad Dadgostar
+// Intel Corporation and third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+// * Redistribution's of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+//
+// * Redistribution's in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+//
+// * The name of Intel Corporation may not be used to endorse or promote products
+// derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "_cvaux.h"
+
+CvFuzzyPoint::CvFuzzyPoint(double _x, double _y)
+{
+ x = _x;
+ y = _y;
+};
+
+bool CvFuzzyCurve::between(double x, double x1, double x2)
+{
+ if ((x >= x1) && (x <= x2))
+ return true;
+ else if ((x >= x2) && (x <= x1))
+ return true;
+
+ return false;
+};
+
+CvFuzzyCurve::CvFuzzyCurve()
+{
+ value = 0;
+};
+
+CvFuzzyCurve::~CvFuzzyCurve()
+{
+ // nothing to do
+};
+
+void CvFuzzyCurve::setCentre(double _centre)
+{
+ centre = _centre;
+};
+
+double CvFuzzyCurve::getCentre()
+{
+ return centre;
+};
+
+void CvFuzzyCurve::clear()
+{
+ points.clear();
+};
+
+void CvFuzzyCurve::addPoint(double x, double y)
+{
+ CvFuzzyPoint *point;
+ point = new CvFuzzyPoint(x, y);
+ points.push_back(*point);
+};
+
+double CvFuzzyCurve::calcValue(double param)
+{
+ int size = (int)points.size();
+ double x1, y1, x2, y2, m, y;
+ for (int i = 1; i < size; i++)
+ {
+ x1 = points[i-1].x;
+ x2 = points[i].x;
+ if (between(param, x1, x2)) {
+ y1 = points[i-1].y;
+ y2 = points[i].y;
+ if (x2 == x1)
+ return y2;
+ m = (y2-y1)/(x2-x1);
+ y = m*(param-x1)+y1;
+ return y;
+ }
+ }
+ return 0;
+};
+
+double CvFuzzyCurve::getValue()
+{
+ return value;
+};
+
+void CvFuzzyCurve::setValue(double _value)
+{
+ value = _value;
+};
+
+
+CvFuzzyFunction::CvFuzzyFunction()
+{
+ // nothing to do
+};
+
+CvFuzzyFunction::~CvFuzzyFunction()
+{
+ curves.clear();
+};
+
+void CvFuzzyFunction::addCurve(CvFuzzyCurve *curve, double value)
+{
+ curves.push_back(*curve);
+ curve->setValue(value);
+};
+
+void CvFuzzyFunction::resetValues()
+{
+ int numCurves = (int)curves.size();
+ for (int i = 0; i < numCurves; i++)
+ curves[i].setValue(0);
+};
+
+double CvFuzzyFunction::calcValue()
+{
+ double s1 = 0, s2 = 0, v;
+ int numCurves = (int)curves.size();
+ for (int i = 0; i < numCurves; i++)
+ {
+ v = curves[i].getValue();
+ s1 += curves[i].getCentre() * v;
+ s2 += v;
+ }
+
+ if (s2 != 0)
+ return s1/s2;
+ else
+ return 0;
+};
+
+CvFuzzyCurve *CvFuzzyFunction::newCurve()
+{
+ CvFuzzyCurve *c;
+ c = new CvFuzzyCurve();
+ addCurve(c);
+ return c;
+};
+
+CvFuzzyRule::CvFuzzyRule()
+{
+ fuzzyInput1 = NULL;
+ fuzzyInput2 = NULL;
+ fuzzyOutput = NULL;
+};
+
+CvFuzzyRule::~CvFuzzyRule()
+{
+ if (fuzzyInput1 != NULL)
+ delete fuzzyInput1;
+
+ if (fuzzyInput2 != NULL)
+ delete fuzzyInput2;
+
+ if (fuzzyOutput != NULL)
+ delete fuzzyOutput;
+};
+
+void CvFuzzyRule::setRule(CvFuzzyCurve *c1, CvFuzzyCurve *c2, CvFuzzyCurve *o1)
+{
+ fuzzyInput1 = c1;
+ fuzzyInput2 = c2;
+ fuzzyOutput = o1;
+};
+
+double CvFuzzyRule::calcValue(double param1, double param2)
+{
+ double v1, v2;
+ v1 = fuzzyInput1->calcValue(param1);
+ if (fuzzyInput2 != NULL)
+ {
+ v2 = fuzzyInput2->calcValue(param2);
+ if (v1 < v2)
+ return v1;
+ else
+ return v2;
+ }
+ else
+ return v1;
+};
+
+CvFuzzyCurve *CvFuzzyRule::getOutputCurve()
+{
+ return fuzzyOutput;
+};
+
+CvFuzzyController::CvFuzzyController()
+{
+ // nothing to do
+};
+
+CvFuzzyController::~CvFuzzyController()
+{
+ int size = (int)rules.size();
+ for(int i = 0; i < size; i++)
+ delete rules[i];
+};
+
+void CvFuzzyController::addRule(CvFuzzyCurve *c1, CvFuzzyCurve *c2, CvFuzzyCurve *o1)
+{
+ CvFuzzyRule *f = new CvFuzzyRule();
+ rules.push_back(f);
+ f->setRule(c1, c2, o1);
+};
+
+double CvFuzzyController::calcOutput(double param1, double param2)
+{
+ double v;
+ CvFuzzyFunction list;
+ int size = (int)rules.size();
+
+ for(int i = 0; i < size; i++)
+ {
+ v = rules[i]->calcValue(param1, param2);
+ if (v != 0)
+ list.addCurve(rules[i]->getOutputCurve(), v);
+ }
+ v = list.calcValue();
+ return v;
+};
+
+CvFuzzyMeanShiftTracker::FuzzyResizer::FuzzyResizer()
+{
+ CvFuzzyCurve *i1L, *i1M, *i1H;
+ CvFuzzyCurve *oS, *oZE, *oE;
+ CvFuzzyCurve *c;
+
+ double MedStart = 0.1, MedWidth = 0.15;
+
+ c = iInput.newCurve();
+ c->addPoint(0, 1);
+ c->addPoint(0.1, 0);
+ c->setCentre(0);
+ i1L = c;
+
+ c = iInput.newCurve();
+ c->addPoint(0.05, 0);
+ c->addPoint(MedStart, 1);
+ c->addPoint(MedStart+MedWidth, 1);
+ c->addPoint(MedStart+MedWidth+0.05, 0);
+ c->setCentre(MedStart+(MedWidth/2));
+ i1M = c;
+
+ c = iInput.newCurve();
+ c->addPoint(MedStart+MedWidth, 0);
+ c->addPoint(1, 1);
+ c->addPoint(1000, 1);
+ c->setCentre(1);
+ i1H = c;
+
+ c = iOutput.newCurve();
+ c->addPoint(-10000, 1);
+ c->addPoint(-5, 1);
+ c->addPoint(-0.5, 0);
+ c->setCentre(-5);
+ oS = c;
+
+ c = iOutput.newCurve();
+ c->addPoint(-1, 0);
+ c->addPoint(-0.05, 1);
+ c->addPoint(0.05, 1);
+ c->addPoint(1, 0);
+ c->setCentre(0);
+ oZE = c;
+
+ c = iOutput.newCurve();
+ c->addPoint(-0.5, 0);
+ c->addPoint(5, 1);
+ c->addPoint(1000, 1);
+ c->setCentre(5);
+ oE = c;
+
+ fuzzyController.addRule(i1L, NULL, oS);
+ fuzzyController.addRule(i1M, NULL, oZE);
+ fuzzyController.addRule(i1H, NULL, oE);
+};
+
+int CvFuzzyMeanShiftTracker::FuzzyResizer::calcOutput(double edgeDensity, double density)
+{
+ return (int)fuzzyController.calcOutput(edgeDensity, density);
+};
+
+CvFuzzyMeanShiftTracker::SearchWindow::SearchWindow()
+{
+ x = 0;
+ y = 0;
+ width = 0;
+ height = 0;
+ maxWidth = 0;
+ maxHeight = 0;
+ xGc = 0;
+ yGc = 0;
+ m00 = 0;
+ m01 = 0;
+ m10 = 0;
+ m11 = 0;
+ m02 = 0;
+ m20 = 0;
+ ellipseHeight = 0;
+ ellipseWidth = 0;
+ ellipseAngle = 0;
+ density = 0;
+ depthLow = 0;
+ depthHigh = 0;
+ fuzzyResizer = NULL;
+};
+
+CvFuzzyMeanShiftTracker::SearchWindow::~SearchWindow()
+{
+ if (fuzzyResizer != NULL)
+ delete fuzzyResizer;
+}
+
+void CvFuzzyMeanShiftTracker::SearchWindow::setSize(int _x, int _y, int _width, int _height)
+{
+ x = _x;
+ y = _y;
+ width = _width;
+ height = _height;
+
+ if (x < 0)
+ x = 0;
+
+ if (y < 0)
+ y = 0;
+
+ if (x + width > maxWidth)
+ width = maxWidth - x;
+
+ if (y + height > maxHeight)
+ height = maxHeight - y;
+};
+
+void CvFuzzyMeanShiftTracker::SearchWindow::initDepthValues(IplImage *maskImage, IplImage *depthMap)
+{
+ unsigned int d=0, mind = 0xFFFF, maxd = 0, m0 = 0, m1 = 0, mc, dd;
+ unsigned char *data = NULL;
+ unsigned short *depthData = NULL;
+
+ for (int j = 0; j < height; j++)
+ {
+ data = (unsigned char *)(maskImage->imageData + (maskImage->widthStep * (j + y)) + x);
+ if (depthMap)
+ depthData = (unsigned short *)(depthMap->imageData + (depthMap->widthStep * (j + y)) + x);
+
+ for (int i = 0; i < width; i++)
+ {
+ if (*data)
+ {
+ m0 += 1;
+
+ if (depthData)
+ {
+ if (*depthData)
+ {
+ m1 += d;
+ if (d < mind)
+ mind = d;
+ if (d > maxd)
+ maxd = d;
+ }
+ depthData++;
+ }
+ }
+ data++;
+ }
+ }
+
+ if (m0 > 0)
+ {
+ mc = m1/m0;
+ if ((mc - mind) > (maxd - mc))
+ dd = maxd - mc;
+ else
+ dd = mc - mind;
+ dd = dd - dd/10;
+ depthHigh = mc + dd;
+ depthLow = mc - dd;
+ }
+ else
+ {
+ depthHigh = 32000;
+ depthLow = 0;
+ }
+};
+
+bool CvFuzzyMeanShiftTracker::SearchWindow::shift()
+{
+ if ((xGc != (width/2)) || (yGc != (height/2)))
+ {
+ setSize(x + (xGc-(width/2)), y + (yGc-(height/2)), width, height);
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+};
+
+void CvFuzzyMeanShiftTracker::SearchWindow::extractInfo(IplImage *maskImage, IplImage *depthMap, bool initDepth)
+{
+ m00 = 0;
+ m10 = 0;
+ m01 = 0;
+ m11 = 0;
+ density = 0;
+ m02 = 0;
+ m20 = 0;
+ ellipseHeight = 0;
+ ellipseWidth = 0;
+
+ maxWidth = maskImage->width;
+ maxHeight = maskImage->height;
+
+ if (initDepth)
+ initDepthValues(maskImage, depthMap);
+
+ unsigned char *maskData = NULL;
+ unsigned short *depthData = NULL, depth;
+ bool isOk;
+ unsigned long count;
+
+ verticalEdgeLeft = 0;
+ verticalEdgeRight = 0;
+ horizontalEdgeTop = 0;
+ horizontalEdgeBottom = 0;
+
+ for (int j = 0; j < height; j++)
+ {
+ maskData = (unsigned char *)(maskImage->imageData + (maskImage->widthStep * (j + y)) + x);
+ if (depthMap)
+ depthData = (unsigned short *)(depthMap->imageData + (depthMap->widthStep * (j + y)) + x);
+
+ count = 0;
+ for (int i = 0; i < width; i++)
+ {
+ if (*maskData)
+ {
+ isOk = true;
+ if (depthData)
+ {
+ depth = (*depthData);
+ if ((depth > depthHigh) || (depth < depthLow))
+ isOk = false;
+
+ depthData++;
+ }
+
+ if (isOk)
+ {
+ m00++;
+ m01 += j;
+ m10 += i;
+ m02 += (j * j);
+ m20 += (i * i);
+ m11 += (j * i);
+
+ if (i == 0)
+ verticalEdgeLeft++;
+ else if (i == width-1)
+ verticalEdgeRight++;
+ else if (j == 0)
+ horizontalEdgeTop++;
+ else if (j == height-1)
+ horizontalEdgeBottom++;
+
+ count++;
+ }
+ }
+ maskData++;
+ }
+ }
+
+ if (m00 > 0)
+ {
+ xGc = (m10 / m00);
+ yGc = (m01 / m00);
+
+ double a, b, c, e1, e2, e3;
+ a = ((double)m20/(double)m00)-(xGc * xGc);
+ b = 2*(((double)m11/(double)m00)-(xGc * yGc));
+ c = ((double)m02/(double)m00)-(yGc * yGc);
+ e1 = a+c;
+ e3 = a-c;
+ e2 = sqrt((b*b)+(e3*e3));
+ ellipseHeight = int(sqrt(0.5*(e1+e2)));
+ ellipseWidth = int(sqrt(0.5*(e1-e2)));
+ if (e3 == 0)
+ ellipseAngle = 0;
+ else
+ ellipseAngle = 0.5*atan(b/e3);
+
+ density = (double)m00/(double)(width * height);
+ }
+ else
+ {
+ xGc = width / 2;
+ yGc = height / 2;
+ ellipseHeight = 0;
+ ellipseWidth = 0;
+ ellipseAngle = 0;
+ density = 0;
+ }
+};
+
+void CvFuzzyMeanShiftTracker::SearchWindow::getResizeAttribsEdgeDensityLinear(int &resizeDx, int &resizeDy, int &resizeDw, int &resizeDh) {
+ int x1 = horizontalEdgeTop;
+ int x2 = horizontalEdgeBottom;
+ int y1 = verticalEdgeLeft;
+ int y2 = verticalEdgeRight;
+ int gx = (width*2)/5;
+ int gy = (height*2)/5;
+ int lx = width/10;
+ int ly = height/10;
+
+ resizeDy = 0;
+ resizeDh = 0;
+ resizeDx = 0;
+ resizeDw = 0;
+
+ if (x1 > gx) {
+ resizeDy = -1;
+ } else if (x1 < lx) {
+ resizeDy = +1;
+ }
+
+ if (x2 > gx) {
+ resizeDh = resizeDy + 1;
+ } else if (x2 < lx) {
+ resizeDh = - (resizeDy + 1);
+ } else {
+ resizeDh = - resizeDy;
+ }
+
+ if (y1 > gy) {
+ resizeDx = -1;
+ } else if (y1 < ly) {
+ resizeDx = +1;
+ }
+
+ if (y2 > gy) {
+ resizeDw = resizeDx + 1;
+ } else if (y2 < ly) {
+ resizeDw = - (resizeDx + 1);
+ } else {
+ resizeDw = - resizeDx;
+ }
+};
+
+void CvFuzzyMeanShiftTracker::SearchWindow::getResizeAttribsInnerDensity(int &resizeDx, int &resizeDy, int &resizeDw, int &resizeDh)
+{
+ int newWidth, newHeight, dx, dy;
+ double px, py;
+ newWidth = int(sqrt(double(m00)*1.3));
+ newHeight = int(newWidth*1.2);
+ dx = (newWidth - width);
+ dy = (newHeight - height);
+ px = (double)xGc/(double)width;
+ py = (double)yGc/(double)height;
+ resizeDx = (int)(px*dx);
+ resizeDy = (int)(py*dy);
+ resizeDw = (int)((1-px)*dx);
+ resizeDh = (int)((1-py)*dy);
+};
+
+void CvFuzzyMeanShiftTracker::SearchWindow::getResizeAttribsEdgeDensityFuzzy(int &resizeDx, int &resizeDy, int &resizeDw, int &resizeDh)
+{
+ double dx1=0, dx2, dy1, dy2;
+
+ resizeDy = 0;
+ resizeDh = 0;
+ resizeDx = 0;
+ resizeDw = 0;
+
+ if (fuzzyResizer == NULL)
+ fuzzyResizer = new FuzzyResizer();
+
+ dx2 = fuzzyResizer->calcOutput(double(verticalEdgeRight)/double(height), density);
+ if (dx1 == dx2)
+ {
+ resizeDx = int(-dx1);
+ resizeDw = int(dx1+dx2);
+ }
+
+ dy1 = fuzzyResizer->calcOutput(double(horizontalEdgeTop)/double(width), density);
+ dy2 = fuzzyResizer->calcOutput(double(horizontalEdgeBottom)/double(width), density);
+
+ dx1 = fuzzyResizer->calcOutput(double(verticalEdgeLeft)/double(height), density);
+ dx2 = fuzzyResizer->calcOutput(double(verticalEdgeRight)/double(height), density);
+ //if (dx1 == dx2)
+ {
+ resizeDx = int(-dx1);
+ resizeDw = int(dx1+dx2);
+ }
+
+ dy1 = fuzzyResizer->calcOutput(double(horizontalEdgeTop)/double(width), density);
+ dy2 = fuzzyResizer->calcOutput(double(horizontalEdgeBottom)/double(width), density);
+ //if (dy1 == dy2)
+ {
+ resizeDy = int(-dy1);
+ resizeDh = int(dy1+dy2);
+ }
+};
+
+bool CvFuzzyMeanShiftTracker::SearchWindow::meanShift(IplImage *maskImage, IplImage *depthMap, int maxIteration, bool initDepth)
+{
+ numShifts = 0;
+ do
+ {
+ extractInfo(maskImage, depthMap, initDepth);
+ if (! shift())
+ return true;
+ } while (++numShifts < maxIteration);
+
+ return false;
+};
+
+void CvFuzzyMeanShiftTracker::findOptimumSearchWindow(SearchWindow &searchWindow, IplImage *maskImage, IplImage *depthMap, int maxIteration, int resizeMethod, bool initDepth)
+{
+ int resizeDx, resizeDy, resizeDw, resizeDh;
+ resizeDx = 0;
+ resizeDy = 0;
+ resizeDw = 0;
+ resizeDh = 0;
+ searchWindow.numIters = 0;
+ for (int i = 0; i < maxIteration; i++)
+ {
+ searchWindow.numIters++;
+ searchWindow.meanShift(maskImage, depthMap, MaxMeanShiftIteration, initDepth);
+ switch (resizeMethod)
+ {
+ case rmEdgeDensityLinear :
+ searchWindow.getResizeAttribsEdgeDensityLinear(resizeDx, resizeDy, resizeDw, resizeDh);
+ break;
+ case rmEdgeDensityFuzzy :
+ //searchWindow.getResizeAttribsEdgeDensityLinear(resizeDx, resizeDy, resizeDw, resizeDh);
+ searchWindow.getResizeAttribsEdgeDensityFuzzy(resizeDx, resizeDy, resizeDw, resizeDh);
+ break;
+ case rmInnerDensity :
+ searchWindow.getResizeAttribsInnerDensity(resizeDx, resizeDy, resizeDw, resizeDh);
+ break;
+ default:
+ searchWindow.getResizeAttribsEdgeDensityLinear(resizeDx, resizeDy, resizeDw, resizeDh);
+ }
+
+ searchWindow.ldx = resizeDx;
+ searchWindow.ldy = resizeDy;
+ searchWindow.ldw = resizeDw;
+ searchWindow.ldh = resizeDh;
+
+ if ((resizeDx == 0) && (resizeDy == 0) && (resizeDw == 0) && (resizeDh == 0))
+ break;
+
+ searchWindow.setSize(searchWindow.x + resizeDx, searchWindow.y + resizeDy, searchWindow.width + resizeDw, searchWindow.height + resizeDh);
+ }
+};
+
+CvFuzzyMeanShiftTracker::CvFuzzyMeanShiftTracker()
+{
+ searchMode = tsSetWindow;
+};
+
+CvFuzzyMeanShiftTracker::~CvFuzzyMeanShiftTracker()
+{
+ // nothing to do
+};
+
+void CvFuzzyMeanShiftTracker::track(IplImage *maskImage, IplImage *depthMap, int resizeMethod, bool resetSearch, int minKernelMass)
+{
+ bool initDepth = false;
+
+ if (resetSearch)
+ searchMode = tsSetWindow;
+
+ switch (searchMode)
+ {
+ case tsDisabled:
+ return;
+ case tsSearching:
+ return;
+ case tsSetWindow:
+ kernel.maxWidth = maskImage->width;
+ kernel.maxHeight = maskImage->height;
+ kernel.setSize(0, 0, maskImage->width, maskImage->height);
+ initDepth = true;
+ case tsTracking:
+ searchMode = tsSearching;
+ findOptimumSearchWindow(kernel, maskImage, depthMap, MaxSetSizeIteration, resizeMethod, initDepth);
+ if ((kernel.density == 0) || (kernel.m00 < minKernelMass))
+ searchMode = tsSetWindow;
+ else
+ searchMode = tsTracking;
+ }
+};
+
projects / opencv / blobdiff