multiTracker.cpp 21.5 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41
/*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.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
// 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 the copyright holders 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*/

42
#include "multiTracker.hpp"
43 44 45 46

namespace cv
{
	//Multitracker
47
    bool MultiTracker_Alt::addTarget(InputArray image, const Rect2d& boundingBox, Ptr<Tracker> tracker_algorithm)
48
	{
49
        Ptr<Tracker> tracker = tracker_algorithm;
50 51 52 53 54 55 56 57 58 59 60 61 62
		if (tracker == NULL)
			return false;

		if (!tracker->init(image, boundingBox))
			return false;

		//Add BB of target
		boundingBoxes.push_back(boundingBox);

		//Add Tracker to stack
		trackers.push_back(tracker);

		//Assign a random color to target
63 64 65 66 67
		if (targetNum == 1)
			colors.push_back(Scalar(0, 0, 255));
		else
			colors.push_back(Scalar(rand() % 256, rand() % 256, rand() % 256));

68 69


70 71 72 73 74 75
		//Target counter
		targetNum++;

		return true;
	}

76
    bool MultiTracker_Alt::update(InputArray image)
77
	{
78
		for (int i = 0; i < (int)trackers.size(); i++)
79 80 81 82 83
			if (!trackers[i]->update(image, boundingBoxes[i]))
				return false;

		return true;
	}
84

85 86
	//Multitracker TLD
	/*Optimized update method for TLD Multitracker */
87
    bool MultiTrackerTLD::update_opt(InputArray _image)
88
	{
89
        Mat image = _image.getMat();
90 91 92 93 94
		//Get parameters from first object
		//TLD Tracker data extraction
		Tracker* trackerPtr = trackers[0];
		tld::TrackerTLDImpl* tracker = static_cast<tld::TrackerTLDImpl*>(trackerPtr);
		//TLD Model Extraction
95
		tld::TrackerTLDModel* tldModel = ((tld::TrackerTLDModel*)static_cast<TrackerModel*>(tracker->getModel()));
96 97
		Ptr<tld::Data> data = tracker->data;
		double scale = data->getScale();
98

99 100
		Mat image_gray, image_blurred, imageForDetector;
		cvtColor(image, image_gray, COLOR_BGR2GRAY);
101

102
		if (scale > 1.0)
103
            resize(image_gray, imageForDetector, Size(cvRound(image_gray.cols*scale), cvRound(image_gray.rows*scale)), 0, 0, tld::DOWNSCALE_MODE);
104 105 106 107 108 109 110
		else
			imageForDetector = image_gray;
		GaussianBlur(imageForDetector, image_blurred, tld::GaussBlurKernelSize, 0.0);

		//best overlap around 92%
		Mat_<uchar> standardPatch(tld::STANDARD_PATCH_SIZE, tld::STANDARD_PATCH_SIZE);

Vladimir's avatar
Vladimir committed
111 112 113
		std::vector<std::vector<tld::TLDDetector::LabeledPatch> > detectorResults(targetNum);
		std::vector<std::vector<Rect2d> > candidates(targetNum);
		std::vector<std::vector<double> > candidatesRes(targetNum);
114 115 116 117 118 119 120 121 122
		std::vector<Rect2d> tmpCandidates(targetNum);
		std::vector<bool> detect_flgs(targetNum);
		std::vector<bool> trackerNeedsReInit(targetNum);

		bool DETECT_FLG = false;

		//Detect all
		for (int k = 0; k < targetNum; k++)
			tmpCandidates[k] = boundingBoxes[k];
123
#ifdef HAVE_OPENCL
124 125 126
		if (ocl::haveOpenCL())
			ocl_detect_all(imageForDetector, image_blurred, tmpCandidates, detectorResults, detect_flgs, trackers);
		else
127
#endif
128 129 130 131 132
			detect_all(imageForDetector, image_blurred, tmpCandidates, detectorResults, detect_flgs, trackers);

		for (int k = 0; k < targetNum; k++)
		{
			//TLD Tracker data extraction
133 134
			trackerPtr = trackers[k];
			tracker = static_cast<tld::TrackerTLDImpl*>(trackerPtr);
135
			//TLD Model Extraction
136
			tldModel = ((tld::TrackerTLDModel*)static_cast<TrackerModel*>(tracker->getModel()));
137
			data = tracker->data;
138 139

			data->frameNum++;
140

141 142
			for (int i = 0; i < 2; i++)
			{
143
				Rect2d tmpCandid = boundingBoxes[k];
144

145
				//if (i == 1)
146
				{
147 148
					DETECT_FLG = detect_flgs[k];
					tmpCandid = tmpCandidates[k];
149 150 151 152
				}

				if (((i == 0) && !data->failedLastTime && tracker->trackerProxy->update(image, tmpCandid)) || (DETECT_FLG))
				{
153
					candidates[k].push_back(tmpCandid);
154 155 156 157
					if (i == 0)
						tld::resample(image_gray, tmpCandid, standardPatch);
					else
						tld::resample(imageForDetector, tmpCandid, standardPatch);
158
					candidatesRes[k].push_back(tldModel->detector->Sc(standardPatch));
159 160 161 162
				}
				else
				{
					if (i == 0)
163 164 165
						trackerNeedsReInit[k] = true;
					else
						trackerNeedsReInit[k] = false;
166 167
				}
			}
168

169
			std::vector<double>::iterator it = std::max_element(candidatesRes[k].begin(), candidatesRes[k].end());
170 171


172
			if (it == candidatesRes[k].end())
173
			{
174

175 176
				data->confident = false;
				data->failedLastTime = true;
177
				return false;
178 179 180
			}
			else
			{
181
				boundingBoxes[k] = candidates[k][it - candidatesRes[k].begin()];
182
				data->failedLastTime = false;
183 184
				if (trackerNeedsReInit[k] || it != candidatesRes[k].begin())
					tracker->trackerProxy->init(image, boundingBoxes[k]);
185 186 187
			}

#if 1
188 189
			if (it != candidatesRes[k].end())
				tld::resample(imageForDetector, candidates[k][it - candidatesRes[k].begin()], standardPatch);
190 191 192 193 194 195 196
#endif

			if (*it > tld::CORE_THRESHOLD)
				data->confident = true;

			if (data->confident)
			{
197 198
				tld::TrackerTLDImpl::Pexpert pExpert(imageForDetector, image_blurred, boundingBoxes[k], tldModel->detector, tracker->params, data->getMinSize());
				tld::TrackerTLDImpl::Nexpert nExpert(imageForDetector, boundingBoxes[k], tldModel->detector, tracker->params);
199 200 201
				std::vector<Mat_<uchar> > examplesForModel, examplesForEnsemble;
				examplesForModel.reserve(100); examplesForEnsemble.reserve(100);
				int negRelabeled = 0;
202
				for (int i = 0; i < (int)detectorResults[k].size(); i++)
203 204
				{
					bool expertResult;
205
					if (detectorResults[k][i].isObject)
206
					{
207 208
						expertResult = nExpert(detectorResults[k][i].rect);
						if (expertResult != detectorResults[k][i].isObject)
209 210 211 212
							negRelabeled++;
					}
					else
					{
213
						expertResult = pExpert(detectorResults[k][i].rect);
214 215
					}

216 217
					detectorResults[k][i].shouldBeIntegrated = detectorResults[k][i].shouldBeIntegrated || (detectorResults[k][i].isObject != expertResult);
					detectorResults[k][i].isObject = expertResult;
218
				}
219
				tldModel->integrateRelabeled(imageForDetector, image_blurred, detectorResults[k]);
220
				pExpert.additionalExamples(examplesForModel, examplesForEnsemble);
221
#ifdef HAVE_OPENCL
222 223 224
				if (ocl::haveOpenCL())
					tldModel->ocl_integrateAdditional(examplesForModel, examplesForEnsemble, true);
				else
225
#endif
226 227 228 229
					tldModel->integrateAdditional(examplesForModel, examplesForEnsemble, true);
				examplesForModel.clear(); examplesForEnsemble.clear();
				nExpert.additionalExamples(examplesForModel, examplesForEnsemble);

230
#ifdef HAVE_OPENCL
231 232 233
				if (ocl::haveOpenCL())
					tldModel->ocl_integrateAdditional(examplesForModel, examplesForEnsemble, false);
				else
234
#endif
235 236 237 238 239 240 241 242 243
					tldModel->integrateAdditional(examplesForModel, examplesForEnsemble, false);
			}
			else
			{
#ifdef CLOSED_LOOP
				tldModel->integrateRelabeled(imageForDetector, image_blurred, detectorResults);
#endif
			}

244

245
		}
246

247 248
		return true;
	}
249

Vladimir's avatar
Vladimir committed
250 251
	void detect_all(const Mat& img, const Mat& imgBlurred, std::vector<Rect2d>& res, std::vector < std::vector < tld::TLDDetector::LabeledPatch > > &patches, std::vector<bool> &detect_flgs,
		std::vector<Ptr<Tracker> > &trackers)
252 253 254 255 256
	{
		//TLD Tracker data extraction
		Tracker* trackerPtr = trackers[0];
		cv::tld::TrackerTLDImpl* tracker = static_cast<tld::TrackerTLDImpl*>(trackerPtr);
		//TLD Model Extraction
257
		tld::TrackerTLDModel* tldModel = ((tld::TrackerTLDModel*)static_cast<TrackerModel*>(tracker->getModel()));
258 259
		Size initSize = tldModel->getMinSize();

260
		for (int k = 0; k < (int)trackers.size(); k++)
261 262 263 264 265 266 267 268 269 270 271 272 273
			patches[k].clear();

		Mat_<uchar> standardPatch(tld::STANDARD_PATCH_SIZE, tld::STANDARD_PATCH_SIZE);
		Mat tmp;
		int dx = initSize.width / 10, dy = initSize.height / 10;
		Size2d size = img.size();
		double scale = 1.0;
		int npos = 0, nneg = 0;
		double maxSc = -5.0;
		Rect2d maxScRect;
		int scaleID;
		std::vector <Mat> resized_imgs, blurred_imgs;

Vladimir's avatar
Vladimir committed
274 275
		std::vector <std::vector <Point> > varBuffer(trackers.size()), ensBuffer(trackers.size());
		std::vector <std::vector <int> > varScaleIDs(trackers.size()), ensScaleIDs(trackers.size());
276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314

		std::vector <Point> tmpP;
		std::vector <int> tmpI;

		//Detection part
		//Generate windows and filter by variance
		scaleID = 0;
		resized_imgs.push_back(img);
		blurred_imgs.push_back(imgBlurred);
		do
		{
			Mat_<double> intImgP, intImgP2;
			tld::TLDDetector::computeIntegralImages(resized_imgs[scaleID], intImgP, intImgP2);
			for (int i = 0, imax = cvFloor((0.0 + resized_imgs[scaleID].cols - initSize.width) / dx); i < imax; i++)
			{
				for (int j = 0, jmax = cvFloor((0.0 + resized_imgs[scaleID].rows - initSize.height) / dy); j < jmax; j++)
				{
					//Optimized variance calculation
					int x = dx * i,
						y = dy * j,
						width = initSize.width,
						height = initSize.height;
					double p = 0, p2 = 0;
					double A, B, C, D;

					A = intImgP(y, x);
					B = intImgP(y, x + width);
					C = intImgP(y + height, x);
					D = intImgP(y + height, x + width);
					p = (A + D - B - C) / (width * height);

					A = intImgP2(y, x);
					B = intImgP2(y, x + width);
					C = intImgP2(y + height, x);
					D = intImgP2(y + height, x + width);
					p2 = (A + D - B - C) / (width * height);
					double windowVar = p2 - p * p;

					//Loop for on all objects
315
					for (int k = 0; k < (int)trackers.size(); k++)
316 317
					{
						//TLD Tracker data extraction
318 319
						trackerPtr = trackers[k];
						tracker = static_cast<tld::TrackerTLDImpl*>(trackerPtr);
320
						//TLD Model Extraction
321
						tldModel = ((tld::TrackerTLDModel*)static_cast<TrackerModel*>(tracker->getModel()));
322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343

						//Optimized variance calculation
						bool varPass = (windowVar > tld::VARIANCE_THRESHOLD * *tldModel->detector->originalVariancePtr);

						if (!varPass)
							continue;
						varBuffer[k].push_back(Point(dx * i, dy * j));
						varScaleIDs[k].push_back(scaleID);
					}
				}
			}
			scaleID++;
			size.width /= tld::SCALE_STEP;
			size.height /= tld::SCALE_STEP;
			scale *= tld::SCALE_STEP;
			resize(img, tmp, size, 0, 0, tld::DOWNSCALE_MODE);
			resized_imgs.push_back(tmp);
			GaussianBlur(resized_imgs[scaleID], tmp, tld::GaussBlurKernelSize, 0.0f);
			blurred_imgs.push_back(tmp);
		} while (size.width >= initSize.width && size.height >= initSize.height);

		//Encsemble classification
344
		for (int k = 0; k < (int)trackers.size(); k++)
345 346
		{
			//TLD Tracker data extraction
347 348
			trackerPtr = trackers[k];
			tracker = static_cast<tld::TrackerTLDImpl*>(trackerPtr);
349
			//TLD Model Extraction
350
			tldModel = ((tld::TrackerTLDModel*)static_cast<TrackerModel*>(tracker->getModel()));
351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385


			for (int i = 0; i < (int)varBuffer[k].size(); i++)
			{
				tldModel->detector->prepareClassifiers(static_cast<int> (blurred_imgs[varScaleIDs[k][i]].step[0]));

				double ensRes = 0;
				uchar* data = &blurred_imgs[varScaleIDs[k][i]].at<uchar>(varBuffer[k][i].y, varBuffer[k][i].x);
				for (int x = 0; x < (int)tldModel->detector->classifiers.size(); x++)
				{
					int position = 0;
					for (int n = 0; n < (int)tldModel->detector->classifiers[x].measurements.size(); n++)
					{
						position = position << 1;
						if (data[tldModel->detector->classifiers[x].offset[n].x] < data[tldModel->detector->classifiers[x].offset[n].y])
							position++;
					}
					double posNum = (double)tldModel->detector->classifiers[x].posAndNeg[position].x;
					double negNum = (double)tldModel->detector->classifiers[x].posAndNeg[position].y;
					if (posNum == 0.0 && negNum == 0.0)
						continue;
					else
						ensRes += posNum / (posNum + negNum);
				}
				ensRes /= tldModel->detector->classifiers.size();
				ensRes = tldModel->detector->ensembleClassifierNum(&blurred_imgs[varScaleIDs[k][i]].at<uchar>(varBuffer[k][i].y, varBuffer[k][i].x));

				if ( ensRes <= tld::ENSEMBLE_THRESHOLD)
					continue;
				ensBuffer[k].push_back(varBuffer[k][i]);
				ensScaleIDs[k].push_back(varScaleIDs[k][i]);
			}
		}

		//NN classification
386
		for (int k = 0; k < (int)trackers.size(); k++)
387 388
		{
			//TLD Tracker data extraction
389 390
			trackerPtr = trackers[k];
			tracker = static_cast<tld::TrackerTLDImpl*>(trackerPtr);
391
			//TLD Model Extraction
392
			tldModel = ((tld::TrackerTLDModel*)static_cast<TrackerModel*>(tracker->getModel()));
393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410

			npos = 0;
			nneg = 0;
			maxSc = -5.0;

			for (int i = 0; i < (int)ensBuffer[k].size(); i++)
			{
				tld::TLDDetector::LabeledPatch labPatch;
				double curScale = pow(tld::SCALE_STEP, ensScaleIDs[k][i]);
				labPatch.rect = Rect2d(ensBuffer[k][i].x*curScale, ensBuffer[k][i].y*curScale, initSize.width * curScale, initSize.height * curScale);
				tld::resample(resized_imgs[ensScaleIDs[k][i]], Rect2d(ensBuffer[k][i], initSize), standardPatch);

				double srValue, scValue;
				srValue = tldModel->detector->Sr(standardPatch);

				////To fix: Check the paper, probably this cause wrong learning
				//
				labPatch.isObject = srValue > tld::THETA_NN;
411
                labPatch.shouldBeIntegrated = abs(srValue - tld::THETA_NN) < tld::CLASSIFIER_MARGIN;
412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431
				patches[k].push_back(labPatch);
				//

				if (!labPatch.isObject)
				{
					nneg++;
					continue;
				}
				else
				{
					npos++;
				}
				scValue = tldModel->detector->Sc(standardPatch);
				if (scValue > maxSc)
				{
					maxSc = scValue;
					maxScRect = labPatch.rect;
				}
			}

432 433 434 435 436 437 438 439 440 441 442 443


			if (maxSc < 0)
				detect_flgs[k] = false;
			else
			{
				res[k] = maxScRect;
				detect_flgs[k] = true;
			}
		}
	}

444
#ifdef HAVE_OPENCL
Vladimir's avatar
Vladimir committed
445 446
	void ocl_detect_all(const Mat& img, const Mat& imgBlurred, std::vector<Rect2d>& res, std::vector < std::vector < tld::TLDDetector::LabeledPatch > > &patches, std::vector<bool> &detect_flgs,
		std::vector<Ptr<Tracker> > &trackers)
447 448 449 450 451
	{
		//TLD Tracker data extraction
		Tracker* trackerPtr = trackers[0];
		cv::tld::TrackerTLDImpl* tracker = static_cast<tld::TrackerTLDImpl*>(trackerPtr);
		//TLD Model Extraction
452
		tld::TrackerTLDModel* tldModel = ((tld::TrackerTLDModel*)static_cast<TrackerModel*>(tracker->getModel()));
453 454
		Size initSize = tldModel->getMinSize();

455
		for (int k = 0; k < (int)trackers.size(); k++)
456 457 458 459 460 461 462 463 464 465 466 467 468
			patches[k].clear();

		Mat_<uchar> standardPatch(tld::STANDARD_PATCH_SIZE, tld::STANDARD_PATCH_SIZE);
		Mat tmp;
		int dx = initSize.width / 10, dy = initSize.height / 10;
		Size2d size = img.size();
		double scale = 1.0;
		int npos = 0, nneg = 0;
		double maxSc = -5.0;
		Rect2d maxScRect;
		int scaleID;
		std::vector <Mat> resized_imgs, blurred_imgs;

Vladimir's avatar
Vladimir committed
469 470
		std::vector <std::vector <Point> > varBuffer(trackers.size()), ensBuffer(trackers.size());
		std::vector <std::vector <int> > varScaleIDs(trackers.size()), ensScaleIDs(trackers.size());
471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509

		std::vector <Point> tmpP;
		std::vector <int> tmpI;

		//Detection part
		//Generate windows and filter by variance
		scaleID = 0;
		resized_imgs.push_back(img);
		blurred_imgs.push_back(imgBlurred);
		do
		{
			Mat_<double> intImgP, intImgP2;
			tld::TLDDetector::computeIntegralImages(resized_imgs[scaleID], intImgP, intImgP2);
			for (int i = 0, imax = cvFloor((0.0 + resized_imgs[scaleID].cols - initSize.width) / dx); i < imax; i++)
			{
				for (int j = 0, jmax = cvFloor((0.0 + resized_imgs[scaleID].rows - initSize.height) / dy); j < jmax; j++)
				{
					//Optimized variance calculation
					int x = dx * i,
						y = dy * j,
						width = initSize.width,
						height = initSize.height;
					double p = 0, p2 = 0;
					double A, B, C, D;

					A = intImgP(y, x);
					B = intImgP(y, x + width);
					C = intImgP(y + height, x);
					D = intImgP(y + height, x + width);
					p = (A + D - B - C) / (width * height);

					A = intImgP2(y, x);
					B = intImgP2(y, x + width);
					C = intImgP2(y + height, x);
					D = intImgP2(y + height, x + width);
					p2 = (A + D - B - C) / (width * height);
					double windowVar = p2 - p * p;

					//Loop for on all objects
510
					for (int k = 0; k < (int)trackers.size(); k++)
511 512
					{
						//TLD Tracker data extraction
513 514
						trackerPtr = trackers[k];
						tracker = static_cast<tld::TrackerTLDImpl*>(trackerPtr);
515
						//TLD Model Extraction
516
						tldModel = ((tld::TrackerTLDModel*)static_cast<TrackerModel*>(tracker->getModel()));
517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538

						//Optimized variance calculation
						bool varPass = (windowVar > tld::VARIANCE_THRESHOLD * *tldModel->detector->originalVariancePtr);

						if (!varPass)
							continue;
						varBuffer[k].push_back(Point(dx * i, dy * j));
						varScaleIDs[k].push_back(scaleID);
					}
				}
			}
			scaleID++;
			size.width /= tld::SCALE_STEP;
			size.height /= tld::SCALE_STEP;
			scale *= tld::SCALE_STEP;
			resize(img, tmp, size, 0, 0, tld::DOWNSCALE_MODE);
			resized_imgs.push_back(tmp);
			GaussianBlur(resized_imgs[scaleID], tmp, tld::GaussBlurKernelSize, 0.0f);
			blurred_imgs.push_back(tmp);
		} while (size.width >= initSize.width && size.height >= initSize.height);

		//Encsemble classification
539
		for (int k = 0; k < (int)trackers.size(); k++)
540 541
		{
			//TLD Tracker data extraction
542 543
			trackerPtr = trackers[k];
			tracker = static_cast<tld::TrackerTLDImpl*>(trackerPtr);
544
			//TLD Model Extraction
545
			tldModel = ((tld::TrackerTLDModel*)static_cast<TrackerModel*>(tracker->getModel()));
546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580


			for (int i = 0; i < (int)varBuffer[k].size(); i++)
			{
				tldModel->detector->prepareClassifiers(static_cast<int> (blurred_imgs[varScaleIDs[k][i]].step[0]));

				double ensRes = 0;
				uchar* data = &blurred_imgs[varScaleIDs[k][i]].at<uchar>(varBuffer[k][i].y, varBuffer[k][i].x);
				for (int x = 0; x < (int)tldModel->detector->classifiers.size(); x++)
				{
					int position = 0;
					for (int n = 0; n < (int)tldModel->detector->classifiers[x].measurements.size(); n++)
					{
						position = position << 1;
						if (data[tldModel->detector->classifiers[x].offset[n].x] < data[tldModel->detector->classifiers[x].offset[n].y])
							position++;
					}
					double posNum = (double)tldModel->detector->classifiers[x].posAndNeg[position].x;
					double negNum = (double)tldModel->detector->classifiers[x].posAndNeg[position].y;
					if (posNum == 0.0 && negNum == 0.0)
						continue;
					else
						ensRes += posNum / (posNum + negNum);
				}
				ensRes /= tldModel->detector->classifiers.size();
				ensRes = tldModel->detector->ensembleClassifierNum(&blurred_imgs[varScaleIDs[k][i]].at<uchar>(varBuffer[k][i].y, varBuffer[k][i].x));

				if (ensRes <= tld::ENSEMBLE_THRESHOLD)
					continue;
				ensBuffer[k].push_back(varBuffer[k][i]);
				ensScaleIDs[k].push_back(varScaleIDs[k][i]);
			}
		}

		//NN classification
581
		for (int k = 0; k < (int)trackers.size(); k++)
582 583
		{
			//TLD Tracker data extraction
584 585
			trackerPtr = trackers[k];
			tracker = static_cast<tld::TrackerTLDImpl*>(trackerPtr);
586
			//TLD Model Extraction
587
			tldModel = ((tld::TrackerTLDModel*)static_cast<TrackerModel*>(tracker->getModel()));
588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644
			npos = 0;
			nneg = 0;
			maxSc = -5.0;

			//Prepare batch of patches
			int numOfPatches = (int)ensBuffer[k].size();
			Mat_<uchar> stdPatches(numOfPatches, 225);
			double *resultSr = new double[numOfPatches];
			double *resultSc = new double[numOfPatches];

			uchar *patchesData = stdPatches.data;
			for (int i = 0; i < (int)ensBuffer.size(); i++)
			{
				tld::resample(resized_imgs[ensScaleIDs[k][i]], Rect2d(ensBuffer[k][i], initSize), standardPatch);
				uchar *stdPatchData = standardPatch.data;
				for (int j = 0; j < 225; j++)
					patchesData[225 * i + j] = stdPatchData[j];
			}
			//Calculate Sr and Sc batches
			tldModel->detector->ocl_batchSrSc(stdPatches, resultSr, resultSc, numOfPatches);

			for (int i = 0; i < (int)ensBuffer[k].size(); i++)
			{
				tld::TLDDetector::LabeledPatch labPatch;
				standardPatch.data = &stdPatches.data[225 * i];
				double curScale = pow(tld::SCALE_STEP, ensScaleIDs[k][i]);
				labPatch.rect = Rect2d(ensBuffer[k][i].x*curScale, ensBuffer[k][i].y*curScale, initSize.width * curScale, initSize.height * curScale);
				tld::resample(resized_imgs[ensScaleIDs[k][i]], Rect2d(ensBuffer[k][i], initSize), standardPatch);

				double srValue, scValue;
				srValue = resultSr[i];

				////To fix: Check the paper, probably this cause wrong learning
				//
				labPatch.isObject = srValue > tld::THETA_NN;
				labPatch.shouldBeIntegrated = abs(srValue - tld::THETA_NN) < 0.1;
				patches[k].push_back(labPatch);
				//

				if (!labPatch.isObject)
				{
					nneg++;
					continue;
				}
				else
				{
					npos++;
				}
				scValue = resultSc[i];
				if (scValue > maxSc)
				{
					maxSc = scValue;
					maxScRect = labPatch.rect;
				}
			}


645 646 647 648 649 650 651 652 653 654

			if (maxSc < 0)
				detect_flgs[k] = false;
			else
			{
				res[k] = maxScRect;
				detect_flgs[k] = true;
			}
		}
	}
655
#endif
656

657
}