openc基于SIFT和Surf特征的目标跟踪

#include "FeatureTraker.h" #include <stdio.h> #include <iostream> #include "opencv2/calib3d/calib3d.hpp" #include "opencv2/contrib/contrib.hpp" #include "opencv2/features2d/features2d.hpp" #include "opencv2/objdetect/objdetect.hpp" #include "opencv2/imgproc/imgproc.hpp" #include "opencv2/imgproc/imgproc_c.h" #include "opencv2/core/internal.hpp" #include "opencv2/nonfree/nonfree.hpp" using namespace cv; FeatureTraker::FeatureTraker(CvFeatureTrackerParams _params) : params(_params) { initModule_nonfree(); switch (params.feature_type) { case CvFeatureTrackerParams::SIFT: dd = Algorithm::create<Feature2D>("Feature2D.SIFT"); if( dd.empty() ) CV_Error(CV_StsNotImplemented, "OpenCV has been compiled without SIFT support"); dd->set("nOctaveLayers", 5); dd->set("contrastThreshold", 0.04); dd->set("edgeThreshold", 10.7); break; case CvFeatureTrackerParams::SURF: dd = Algorithm::create<Feature2D>("Feature2D.SURF"); if( dd.empty() ) CV_Error(CV_StsNotImplemented, "OpenCV has been compiled without SURF support"); dd->set("hessianThreshold", 400); dd->set("nOctaves", 3); dd->set("nOctaveLayers", 4); break; default: CV_Error(CV_StsBadArg, "Unknown feature type"); break; } matcher = new BFMatcher(NORM_L2); } FeatureTraker::FeatureTraker() { } FeatureTraker::~FeatureTraker() { } void FeatureTraker::newTrackingWindow(Mat image, Rect selection) { image.copyTo(prev_image); cvtColor(prev_image, prev_image_bw, CV_BGR2GRAY); prev_trackwindow = selection; prev_center.x = selection.x; prev_center.y = selection.y; ittr = 0; } Rect FeatureTraker::updateTrackingWindow(Mat image) { if(params.feature_type == CvFeatureTrackerParams::OPTICAL_FLOW) return updateTrackingWindowWithFlow(image); else return updateTrackingWindowWithSIFT(image); } Rect FeatureTraker::updateTrackingWindowWithSIFT(Mat image) { ittr++; vector<KeyPoint> prev_keypoints, curr_keypoints; vector<Point2f> prev_keys, curr_keys; Mat prev_desc, curr_desc; Rect window = prev_trackwindow; Mat mask = Mat::zeros(image.size(), CV_8UC1); rectangle(mask, Point(window.x, window.y), Point(window.x + window.width, window.y + window.height), Scalar(255), CV_FILLED); dd->operator()(prev_image, mask, prev_keypoints, prev_desc); window.x -= params.window_size; window.y -= params.window_size; window.width += params.window_size; window.height += params.window_size; rectangle(mask, Point(window.x, window.y), Point(window.x + window.width, window.y + window.height), Scalar(255), CV_FILLED); dd->operator()(image, mask, curr_keypoints, curr_desc); if (prev_keypoints.size() > 4 && curr_keypoints.size() > 4) { //descriptor->compute(prev_image, prev_keypoints, prev_desc); //descriptor->compute(image, curr_keypoints, curr_desc); matcher->match(prev_desc, curr_desc, matches); for (int i = 0; i < (int)matches.size(); i++) { prev_keys.push_back(prev_keypoints[matches[i].queryIdx].pt); curr_keys.push_back(curr_keypoints[matches[i].trainIdx].pt); } Mat T = findHomography(prev_keys, curr_keys, CV_LMEDS); prev_trackwindow.x += cvRound(T.at<double> (0, 2)); prev_trackwindow.y += cvRound(T.at<double> (1, 2)); } prev_center.x = prev_trackwindow.x; prev_center.y = prev_trackwindow.y; prev_image = image; return prev_trackwindow; } Rect FeatureTraker::updateTrackingWindowWithFlow(Mat image) { ittr++; Size subPixWinSize(10,10), winSize(31,31); Mat image_bw; TermCriteria termcrit(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03); vector<uchar> status; vector<float> err; cvtColor(image, image_bw, CV_BGR2GRAY); cvtColor(prev_image, prev_image_bw, CV_BGR2GRAY); if (ittr == 1) { Mat mask = Mat::zeros(image.size(), CV_8UC1); rectangle(mask, Point(prev_trackwindow.x, prev_trackwindow.y), Point( prev_trackwindow.x + prev_trackwindow.width, prev_trackwindow.y + prev_trackwindow.height), Scalar(255), CV_FILLED); goodFeaturesToTrack(image_bw, features[1], 500, 0.01, 20, mask, 3, 0, 0.04); cornerSubPix(image_bw, features[1], subPixWinSize, Size(-1, -1), termcrit); } else { calcOpticalFlowPyrLK(prev_image_bw, image_bw, features[0], features[1], status, err, winSize, 3, termcrit); Point2f feature0_center(0, 0); Point2f feature1_center(0, 0); int goodtracks = 0; for (int i = 0; i < (int)features[1].size(); i++) { if (status[i] == 1) { feature0_center.x += features[0][i].x; feature0_center.y += features[0][i].y; feature1_center.x += features[1][i].x; feature1_center.y += features[1][i].y; goodtracks++; } } feature0_center.x /= goodtracks; feature0_center.y /= goodtracks; feature1_center.x /= goodtracks; feature1_center.y /= goodtracks; prev_center.x += (feature1_center.x - feature0_center.x); prev_center.y += (feature1_center.y - feature0_center.y); prev_trackwindow.x = (int)prev_center.x; prev_trackwindow.y = (int)prev_center.y; } swap(features[0], features[1]); image.copyTo(prev_image); return prev_trackwindow; } void FeatureTraker::setTrackingWindow(Rect _window) { prev_trackwindow = _window; } Rect FeatureTraker::getTrackingWindow() { return prev_trackwindow; } Point2f FeatureTraker::getTrackingCenter() { Point2f center(0, 0); center.x = (float)(prev_center.x + prev_trackwindow.width/2.0); center.y = (float)(prev_center.y + prev_trackwindow.height/2.0); return center; }