void doTracking(std::vector<cv::LatentSvmDetector::ObjectDetection>& detections, int frameNumber,
std::vector<kstate>& kstates, std::vector<bool>& active, cv::Mat& image,
std::vector<kstate>& trackedDetections, std::vector<cv::Scalar> & colors)
{
std::vector<cv::LatentSvmDetector::ObjectDetection> objects;
//vector<LatentSvmDetector::ObjectDetection> tracked_objects;
std::vector<bool> predict_indices;//this will correspond to kstates i
std::vector<bool> correct_indices;//this will correspond to kstates i
std::vector<int> correct_detection_indices;//this will correspond to kstates i, used to store the index of the corresponding object
std::vector<bool> add_as_new_indices;//this will correspond to detections j
//predict_indices.assign(kstates.size(), true);//always predict
correct_indices.assign(kstates.size(), false);//correct only those matched
correct_detection_indices.assign(kstates.size(), false);//correct only those matched
add_as_new_indices.assign(detections.size(), true);//if the detection was not found add as new
//Convert Bounding box coordinates from (x1,y1,w,h) to (BoxCenterX, BoxCenterY, width, height)
objects = detections;//bboxToPosScale(detections);
std::vector<int> already_matched;
//compare detections from this frame with tracked objects
for (unsigned int j = 0; j < detections.size(); j++)
{
for (unsigned int i = 0; i < kstates.size(); i++)
{
//compare only to active tracked objects(not too old)
if (kstates[i].active)
{
//extend the roi 20%
int new_x = (detections[j].rect.x - detections[j].rect.width*.1);
int new_y = (detections[j].rect.y - detections[j].rect.height*.1);
if (new_x < 0) new_x = 0;
if (new_x > image.cols) new_x = image.cols;
if (new_y < 0) new_y = 0;
if (new_y > image.rows) new_y = image.rows;
int new_width = detections[j].rect.width*1.2;
int new_height = detections[j].rect.height*1.2;
if (new_width + new_x > image.cols) new_width = image.cols - new_x;
if (new_height + new_y > image.rows) new_height = image.rows - new_y;
cv::Rect roi_20(new_x, new_y, new_width, new_height);
//cv::Rect roi(detections[j].rect);
cv::Rect roi(roi_20);
cv::Mat currentObjectROI = image(roi).clone();//Crop image and obtain only object (ROI)
//cv::Rect intersect = detections[j].rect & kstates[i].pos;//check overlapping
cv::Rect boundingbox;
bool matched = false;
//try to match with previous frame
if ( !USE_ORB )
matched = ( !alreadyMatched(j, already_matched) && crossCorr(kstates[i].image, currentObjectROI));
else
matched = (!alreadyMatched(j, already_matched) && orbMatch(currentObjectROI, kstates[i].image, boundingbox, ORB_MIN_MATCHES, ORB_KNN_RATIO));
if(matched)
{
correct_indices[i] = true;//if ROI on this frame is matched to a previous object, correct
correct_detection_indices[i] = j;//store the index of the detection corresponding to matched kstate
add_as_new_indices[j] = false;//if matched do not add as new
//kstates[i].image = currentObjectROI;//update image with current frame data
kstates[i].score = detections[j].score;
kstates[i].range = _ranges[j];
already_matched.push_back(j);
}//crossCorr
}//kstates[i].active
}//for (int i = 0; i < kstates.size(); i++)
}//for (int j = 0; j < detections.size(); j++)
//do prediction and correction for the marked states
for (unsigned int i = 0; i < kstates.size(); i++)
{
if (kstates[i].active)//predict and correct only active states
{
//update params before predicting
cv::setIdentity(kstates[i].KF.measurementMatrix);
cv::setIdentity(kstates[i].KF.processNoiseCov, cv::Scalar::all(NOISE_COV));//1e-4
cv::setIdentity(kstates[i].KF.measurementNoiseCov, cv::Scalar::all(MEAS_NOISE_COV));//1e-3
cv::setIdentity(kstates[i].KF.errorCovPost, cv::Scalar::all(ERROR_ESTIMATE_COV));//100
cv::Mat prediction = kstates[i].KF.predict();
cv::Mat correction;
kstates[i].pos.x = prediction.at<float>(0);
kstates[i].pos.y = prediction.at<float>(1);
kstates[i].pos.width = prediction.at<float>(2);
kstates[i].pos.height = prediction.at<float>(3);
kstates[i].real_data = 0;
kstates[i].range = 0.0f;//fixed to zero temporarily as this is not real_data
kstates[i].min_height = 0.0f;//fixed to zero temporarily as this is not real_data
kstates[i].max_height = 0.0f;//fixed to zero temporarily as this is not real_data
//now do respective corrections on KFs (updates)
if (correct_indices[i])
{
//a match was found hence update KF measurement
int j = correct_detection_indices[i];//obtain the index of the detection
//cv::Mat_<float> measurement = (cv::Mat_<float>(2, 1) << objects[j].rect.x, //XY ONLY
// objects[j].rect.y);
cv::Mat_<float> measurement = (cv::Mat_<float>(4, 1) << objects[j].rect.x,
objects[j].rect.y,
objects[j].rect.width,
objects[j].rect.height);
correction = kstates[i].KF.correct(measurement);//UPDATE KF with new info
kstates[i].lifespan = DEFAULT_LIFESPAN; //RESET Lifespan of object
//kstates[i].pos.width = objects[j].rect.width;//XY ONLY
//kstates[i].pos.height = objects[j].rect.height;//XY ONLY
//use real data instead of predicted if set
kstates[i].pos.x = objects[j].rect.x;
kstates[i].pos.y = objects[j].rect.y;
kstates[i].pos.width = objects[j].rect.width;
kstates[i].pos.height = objects[j].rect.height;
kstates[i].real_data = 1;
//cv::Mat im1 = image(kstates[i].pos);
//cv::Mat im2 = image(objects[j].rect);
kstates[i].range = _ranges[j];
kstates[i].min_height = _min_heights[j];
kstates[i].max_height = _max_heights[j];
}
//check that new widths and heights don't go beyond the image size
if (kstates[i].pos.width + kstates[i].pos.x > image.cols)
kstates[i].pos.width = image.cols - kstates[i].pos.x;
if (kstates[i].pos.height + kstates[i].pos.y > image.rows)
kstates[i].pos.height = image.rows - kstates[i].pos.y;
//check that predicted positions are inside the image
if (kstates[i].pos.x < 0)
kstates[i].pos.x = 0;
if (kstates[i].pos.x > image.cols)
kstates[i].pos.x = image.cols;
if (kstates[i].pos.y < 0)
kstates[i].pos.y = 0;
if (kstates[i].pos.y > image.rows)
kstates[i].pos.y = image.rows;
//remove those where the dimensions of are unlikely to be real
if (kstates[i].pos.width > kstates[i].pos.height*4)
kstates[i].active = false;
if (kstates[i].pos.height > kstates[i].pos.width*2)
kstates[i].active = false;
kstates[i].lifespan--;//reduce lifespan
if (kstates[i].lifespan <= 0)
{
kstates[i].active = false; //Too old, stop tracking.
}
}
}
//finally add non matched detections as new
for (unsigned int i = 0; i < add_as_new_indices.size(); i++)
{
if (add_as_new_indices[i])
{
initTracking(objects[i], kstates, detections[i], image, colors, _ranges[i]);
}
}
/*
//check overlapping states and remove them
float overlap = (OVERLAPPING_PERC/100);
std::vector<unsigned int> removedIndices;
for (unsigned int i = 0; i < kstates.size() ; i++)
{
for (unsigned int j = kstates.size() - 1; j > 0; j--)
{
if (i==j || isInRemoved(removedIndices, i) || isInRemoved(removedIndices, j))
continue;
//cout << "i:" << i << " j:" << j << endl;
cv::Rect intersection = kstates[i].pos & kstates[j].pos;
if ( ( (intersection.width >= kstates[i].pos.width * overlap) && (intersection.height >= kstates[i].pos.height * overlap) ) ||
( (intersection.width >= kstates[j].pos.width * overlap) && (intersection.height >= kstates[j].pos.height * overlap) ) )
{
//if one state is overlapped by "overlap" % remove it (mark it as unused
if (kstates[i].real_data && !(kstates[j].real_data))
{
kstates[j].active = false;
removedIndices.push_back(j);
}
else if (!(kstates[i].real_data) && (kstates[j].real_data))
{
kstates[i].active = false;
removedIndices.push_back(i);
}
else
{
kstates[j].active = false;
removedIndices.push_back(j);
}
}
}
}*/
ApplyNonMaximumSuppresion(kstates, OVERLAPPING_PERC);
removeUnusedObjects(kstates);
//return to x,y,w,h
posScaleToBbox(kstates, trackedDetections);
}
void cv::matchTemplate( InputArray _img, InputArray _templ, OutputArray _result, int method )
{
CV_Assert( CV_TM_SQDIFF <= method && method <= CV_TM_CCOEFF_NORMED );
int numType = method == CV_TM_CCORR || method == CV_TM_CCORR_NORMED ? 0 :
method == CV_TM_CCOEFF || method == CV_TM_CCOEFF_NORMED ? 1 : 2;
bool isNormed = method == CV_TM_CCORR_NORMED ||
method == CV_TM_SQDIFF_NORMED ||
method == CV_TM_CCOEFF_NORMED;
Mat img = _img.getMat(), templ = _templ.getMat();
if( img.rows < templ.rows || img.cols < templ.cols )
std::swap(img, templ);
CV_Assert( (img.depth() == CV_8U || img.depth() == CV_32F) &&
img.type() == templ.type() );
Size corrSize(img.cols - templ.cols + 1, img.rows - templ.rows + 1);
_result.create(corrSize, CV_32F);
Mat result = _result.getMat();
int cn = img.channels();
crossCorr( img, templ, result, result.size(), result.type(), Point(0,0), 0, 0);
if( method == CV_TM_CCORR )
return;
double invArea = 1./((double)templ.rows * templ.cols);
Mat sum, sqsum;
Scalar templMean, templSdv;
double *q0 = 0, *q1 = 0, *q2 = 0, *q3 = 0;
double templNorm = 0, templSum2 = 0;
if( method == CV_TM_CCOEFF )
{
integral(img, sum, CV_64F);
templMean = mean(templ);
}
else
{
integral(img, sum, sqsum, CV_64F);
meanStdDev( templ, templMean, templSdv );
templNorm = CV_SQR(templSdv[0]) + CV_SQR(templSdv[1]) +
CV_SQR(templSdv[2]) + CV_SQR(templSdv[3]);
if( templNorm < DBL_EPSILON && method == CV_TM_CCOEFF_NORMED )
{
result = Scalar::all(1);
return;
}
templSum2 = templNorm +
CV_SQR(templMean[0]) + CV_SQR(templMean[1]) +
CV_SQR(templMean[2]) + CV_SQR(templMean[3]);
if( numType != 1 )
{
templMean = Scalar::all(0);
templNorm = templSum2;
}
templSum2 /= invArea;
templNorm = sqrt(templNorm);
templNorm /= sqrt(invArea); // care of accuracy here
q0 = (double*)sqsum.data;
q1 = q0 + templ.cols*cn;
q2 = (double*)(sqsum.data + templ.rows*sqsum.step);
q3 = q2 + templ.cols*cn;
}
double* p0 = (double*)sum.data;
double* p1 = p0 + templ.cols*cn;
double* p2 = (double*)(sum.data + templ.rows*sum.step);
double* p3 = p2 + templ.cols*cn;
int sumstep = sum.data ? (int)(sum.step / sizeof(double)) : 0;
int sqstep = sqsum.data ? (int)(sqsum.step / sizeof(double)) : 0;
int i, j, k;
for( i = 0; i < result.rows; i++ )
{
float* rrow = (float*)(result.data + i*result.step);
int idx = i * sumstep;
int idx2 = i * sqstep;
for( j = 0; j < result.cols; j++, idx += cn, idx2 += cn )
{
double num = rrow[j], t;
double wndMean2 = 0, wndSum2 = 0;
if( numType == 1 )
{
for( k = 0; k < cn; k++ )
{
t = p0[idx+k] - p1[idx+k] - p2[idx+k] + p3[idx+k];
wndMean2 += CV_SQR(t);
num -= t*templMean[k];
}
wndMean2 *= invArea;
}
if( isNormed || numType == 2 )
{
for( k = 0; k < cn; k++ )
{
t = q0[idx2+k] - q1[idx2+k] - q2[idx2+k] + q3[idx2+k];
wndSum2 += t;
}
if( numType == 2 )
num = wndSum2 - 2*num + templSum2;
}
if( isNormed )
{
t = sqrt(MAX(wndSum2 - wndMean2,0))*templNorm;
if( fabs(num) < t )
num /= t;
else if( fabs(num) < t*1.125 )
num = num > 0 ? 1 : -1;
else
num = method != CV_TM_SQDIFF_NORMED ? 0 : 1;
}
rrow[j] = (float)num;
}
}
}
