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; } } }