Adding scale to partical filter - c++

Question

The following code is for partical filter for mouse and I change it for track on video with color, this works.
But I want to add scale to it now which only works with x and y. I tried to add scale to it but I failed. Please help me to add scale to the object detected to partical filter.
// Module "core"
#include <opencv2/core/core.hpp>
#include < opencv2/video/background_segm.hpp>
// Module "highgui"
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/legacy/legacy.hpp>
// Module "imgproc"
#include <opencv2/imgproc/imgproc.hpp>
#include "opencv2/videostab/videostab.hpp"
// Module "video"
#include <opencv2/video/video.hpp>
// Output
#include <iostream>
// Vector
#include <vector>
#define drawCross( center, color, d ) \
line( frame, cv::Point( center.x - d, center.y - d ), \
cv::Point( center.x + d, center.y + d ), color, 1, CV_AA, 0); \
line( frame, cv::Point( center.x + d, center.y - d ), \
cv::Point( center.x - d, center.y + d ), color, 1, CV_AA, 0 )
#define PLOT_PARTICLES 1
using namespace std;
using namespace cv;
// >>>>> Color to be tracked
#define MIN_H_BLUE 200
#define MAX_H_BLUE 300
// <<<<< Color to be tracked
vector<cv::Point> mouseV, particleV;
int main()
{
// Camera frame
cv::Mat frame;
char code = (char)-1;
cv::namedWindow("mouse particle");
cv::Mat_<float> measurement(2,1);
measurement.setTo(cv::Scalar(0));
int dim = 2;
int nParticles = 300;
float xRange = 650.0;
float yRange = 650.0;
float minRange[] = { 0, 0 };
float maxRange[] = { xRange, yRange };
CvMat LB, UB;
cvInitMatHeader(&LB, 2, 1, CV_32FC1, minRange);
cvInitMatHeader(&UB, 2, 1, CV_32FC1, maxRange);
CvConDensation* condens = cvCreateConDensation(dim, dim, nParticles);
cvConDensInitSampleSet(condens, &LB, &UB);
condens->DynamMatr[0] = 1.0;
condens->DynamMatr[1] = 0.0;
condens->DynamMatr[2] = 0.0;
condens->DynamMatr[3] = 1.0;
// Camera Index
string idx = "a.mp4";
// Camera Capture
cv::VideoCapture cap;
// >>>>> Camera Settings
if (!cap.open(idx))
{
cout << "Webcam not connected.\n" << "Please verify\n";
return EXIT_FAILURE;
}
cap.set(CV_CAP_PROP_FRAME_WIDTH, 1024);
cap.set(CV_CAP_PROP_FRAME_HEIGHT, 768);
// <<<<< Camera Settings
cout << "\nHit 'q' to exit...\n";
char ch = 0;
double ticks = 0;
bool found = false;
int notFoundCount = 0;
// >>>>> Main loop
while (ch != 'q' && ch != 'Q')
{
double precTick = ticks;
ticks = (double) cv::getTickCount();
double dT = (ticks - precTick) / cv::getTickFrequency(); //seconds
// Frame acquisition
cap >> frame;
mouseV.clear();
particleV.clear();
// >>>>> Noise smoothing
cv::Mat blur;
cv::GaussianBlur(frame, blur, cv::Size(5, 5), 3.0, 3.0);
// <<<<< Noise smoothing
// >>>>> HSV conversion
cv::Mat frmHsv;
cv::cvtColor(blur, frmHsv, CV_BGR2HSV);
// <<<<< HSV conversion
// >>>>> Color Thresholding
// Note: change parameters for different colors
cv::Mat rangeRes = cv::Mat::zeros(frame.size(), CV_8UC1);
cv::inRange(frmHsv, cv::Scalar(MIN_H_BLUE / 2, 100, 80),
cv::Scalar(MAX_H_BLUE / 2, 255, 255), rangeRes);
// <<<<< Color Thresholding
// >>>>> Improving the result
cv::erode(rangeRes, rangeRes, cv::Mat(), cv::Point(-1, -1), 2);
cv::dilate(rangeRes, rangeRes, cv::Mat(), cv::Point(-1, -1), 2);
// <<<<< Improving the result
// >>>>> Contours detection
vector<vector<cv::Point> > contours;
cv::findContours(rangeRes, contours, CV_RETR_EXTERNAL,
CV_CHAIN_APPROX_NONE);
// <<<<< Contours detection
// >>>>> Filtering
vector<vector<cv::Point> > balls;
vector<cv::Rect> ballsBox;
for (size_t i = 0; i < contours.size(); i++)
{
cv::Rect bBox;
bBox = cv::boundingRect(contours[i]);
float ratio = (float) bBox.width / (float) bBox.height;
if (ratio > 1.0f)
ratio = 1.0f / ratio;
// Searching for a bBox almost square
// if (ratio > 0.55 && bBox.area() >= 50)
// {
balls.push_back(contours[i]);
ballsBox.push_back(bBox);
measurement(0) = bBox.x;
measurement(1) = bBox.y;
measurement(2) = ballsBox.size();
//cout << "Balls found:" << bBox.x << endl;
// }
}
/*
cout << "Balls found:" << ballsBox.size() << endl;
*/
cv::Point measPt(measurement(0),measurement(1));
mouseV.push_back(measPt);
for (int i = 0; i < condens->SamplesNum; i++) {
float diffX = (measurement(0) - condens->flSamples[i][0])/xRange;
float diffY = (measurement(1) - condens->flSamples[i][1])/yRange;
condens->flConfidence[i] = 1.0 / (sqrt(diffX * diffX + diffY * diffY));
// plot particles
#ifdef PLOT_PARTICLES
cv::Point partPt(condens->flSamples[i][0], condens->flSamples[i][1]);
drawCross(partPt , cv::Scalar(255,0,255), 2);
#endif
}
cvConDensUpdateByTime(condens);
cv::Point statePt(condens->State[0], condens->State[1]);
particleV.push_back(statePt);
for (int i = 0; i < particleV.size() - 1; i++) {
line(frame, particleV[i], particleV[i+1], cv::Scalar(0,255,0), 1);
}
drawCross( statePt, cv::Scalar(255,255,255), 5 );
drawCross( measPt, cv::Scalar(0,0,255), 5 );
for (size_t i = 0; i < balls.size(); i++)
{
cv::drawContours(frame, balls, i, CV_RGB(20,150,20), 1);
cv::rectangle(frame, ballsBox[i], CV_RGB(0,255,0), 2);
cv::Point center;
center.x = ballsBox[i].x + ballsBox[i].width / 2;
center.y = ballsBox[i].y + ballsBox[i].height / 2;
cv::circle(frame, center, 2, CV_RGB(20,150,20), -1);
stringstream sstr;
sstr << "(" << center.x << "," << center.y << ")";
cv::putText(frame, sstr.str(),
cv::Point(center.x + 3, center.y - 3),
cv::FONT_HERSHEY_SIMPLEX, 0.5, CV_RGB(20,150,20), 2);
}
cv::imshow("mouse particle", frame);
cv::imshow("ssssssss", rangeRes);
ch = cv::waitKey(1);
}
// <<<<< Main loop
return EXIT_SUCCESS;
}