跟踪特征点并画AR物体

#include <opencv2/core/core.hpp> #include <opencv2/video/tracking.hpp> #include <opencv2/imgproc/imgproc.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/calib3d/calib3d.hpp> #include <opencv2/features2d/features2d.hpp> #include <windows.h> #include <iostream> #include <pthread.h> #include <GL/glut.h> #include <math.h> using namespace cv; using namespace std; VideoCapture cap(0); static int resolution_width = 640; static int resolution_height = 480; static int num_track = 4; double camD[9] = {618.526381968738, 0, 310.8963715614199, 0, 619.4548980786033, 248.6374860176724, 0, 0, 1}; double distCoeffD[5] = {0.09367405350511771, -0.08731677320554751, 0.002823563134787144, -1.246739177460954e-005, -0.0469061739387372}; Mat camera_matrix = Mat(3,3,CV_64FC1,camD); Mat distortion_coefficients = Mat(5,1,CV_64FC1,distCoeffD); vector<Point3f> objP; Mat objPM; vector<double> rv(3), tv(3); Mat rvecm(rv),tvecm(tv); Mat gray, prevGray, image, frame; vector<Point2f> points[2]; vector<KeyPoint> keypoints; vector<KeyPoint> initPoints; vector<Point2f> recoveryPoints; vector<Point2f> projectedPoints; vector<Point2f> goodfeatures; bool initflag = true; bool needToGetgf = false; bool needtomap = false; bool needtokeeptime = false; const int MAX_COUNT = 500; size_t trackingpoints = 0; Mat initdescriptors; string msg; int baseLine; Size textSize; DWORD t1,t2; int framenum = 0; TermCriteria termcrit(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 30, 0.01); Size subPixWinSize(10,10),winSize(21,21); static void help() { // print a welcome message, and the OpenCV version cout << "\n<Feature Tracking and Synchronous Scene Generation with a Single Camera>\n" "\nUser: cc.\n" "\nUsing OpenCV version " << CV_VERSION << endl; cout << "\nHot keys: \n" " Tracking: \n" "\tESC - quit the program\n" "\tg - get the good corners\n" "\tc - delete all the points\n" "\tm - choose four points to track and update camera pose then\n" "\t\t(To add a tracking point, please click that point)\n" " Generation: \n" "\tESC - quit the program\n" "\te - change the view mode\n"<< endl; } void on_mouse(int event,int x,int y,int flag, void *param) { if(event==CV_EVENT_LBUTTONDOWN) { if(needtomap && points[1].size()<num_track) { for(size_t i = 0;i<goodfeatures.size();i++) { if(abs(goodfeatures[i].x-x)+abs(goodfeatures[i].y-y)<5) { points[1].push_back(goodfeatures[i]); initPoints.push_back(keypoints[i]); trackingpoints++; break; } } Mat temp; image.copyTo(temp); for(size_t i = 0; i < points[1].size(); i++ ) { circle( temp, points[1][i], 3, Scalar(0,0,255), -1, 8); } msg = format( "Resolution: %d * %d. Corner number: %d. Tracked points: %d",(int)cap.get(CV_CAP_PROP_FRAME_WIDTH),(int)cap.get(CV_CAP_PROP_FRAME_HEIGHT),goodfeatures.size(),trackingpoints); baseLine = 0; textSize = getTextSize(msg, 1, 1, 1, &baseLine); Point textOrigin(temp.cols - textSize.width - 20, temp.rows - 2*baseLine - 10); putText(temp,msg,textOrigin,1,1,Scalar(0,0,255)); imshow("Tracking", temp); } } } void init3DP() { // objP.push_back(Point3f(-5,-5,0)); //三维坐标的单位是毫米 // objP.push_back(Point3f(0,-5,0)); // objP.push_back(Point3f(5,-5,0)); // objP.push_back(Point3f(5,0,0)); // objP.push_back(Point3f(5,5,0)); // objP.push_back(Point3f(0,5,0)); // objP.push_back(Point3f(-5,5,0)); // objP.push_back(Point3f(-5,0,0)); // objP.push_back(Point3f(0,0,0)); if(!objP.empty()) objP.clear(); objP.push_back(Point3f(-0.5,-0.5,0)); //三维坐标的单位是毫米 objP.push_back(Point3f(0.5,-0.5,0)); objP.push_back(Point3f(0.5,0.5,0)); objP.push_back(Point3f(-0.5,0.5,0)); objPM.setTo(0); Mat(objP).convertTo(objPM,CV_32F); } bool init() { cap.set(CV_CAP_PROP_FRAME_WIDTH,resolution_width); cap.set(CV_CAP_PROP_FRAME_HEIGHT,resolution_height); if( !cap.isOpened() ) { cout << "Could not initialize capturing...\n"; return false; } namedWindow( "Tracking", 1 ); cvSetMouseCallback( "Tracking",on_mouse,NULL ); init3DP(); return true; } void getPlanarSurface(vector<Point2f>& imgP){ solvePnP(objPM, Mat(imgP), camera_matrix, distortion_coefficients, rvecm, tvecm); projectedPoints.clear(); projectPoints(objPM, rvecm, tvecm, camera_matrix, distortion_coefficients, projectedPoints); for(unsigned int i = 0; i < projectedPoints.size(); ++i) { circle( image, projectedPoints[i], 3, Scalar(255,0,0), -1, 8); } } bool attemptoRecoveryTracking(vector<Point2f> missingPoints) { DWORD t_s,t_e,t = 0; ORB orb; BFMatcher bfm; Mat descriptors; Mat tempF,tempI,tempG; Mat top,bottom,left,right; vector<KeyPoint> newKP; vector<DMatch> matches; int locationflag = 0; t_s = GetTickCount(); while(t<5000) { cap >> tempF; if( tempF.empty() ) break; tempF.copyTo(tempI); if(missingPoints[0].x<30 && missingPoints[3].x<30) { left = tempI(Range(0,480),Range(0,200)); cvtColor(left, tempG, COLOR_BGR2GRAY); locationflag = 1; } else if(missingPoints[1].x>610 && missingPoints[2].x>610) { right = tempI(Range(0,480),Range(440,640)); cvtColor(right, tempG, COLOR_BGR2GRAY); locationflag = 2; } else if(missingPoints[0].y<30 && missingPoints[1].y<30) { top = tempI(Range(0,200),Range(0,640)); cvtColor(top, tempG, COLOR_BGR2GRAY); locationflag = 3; } else if(missingPoints[2].y>450 && missingPoints[3].y>450) { bottom = tempI(Range(280,480),Range(0,640)); cvtColor(bottom, tempG, COLOR_BGR2GRAY); locationflag = 4; } else{ cvtColor(tempI, tempG, COLOR_BGR2GRAY); } orb.detect(tempG,newKP); orb.compute(tempG,newKP,descriptors); bfm.match(initdescriptors,descriptors,matches); recoveryPoints.clear(); if(matches.size() == num_track) { for(size_t i = 0; i < num_track; i++ ) { Point t = newKP[matches[i].trainIdx].pt; if(locationflag == 2) { t.x += 440; } if(locationflag == 4) { t.y += 280; } recoveryPoints.push_back(t); } return true; } imshow("Tracking",tempI); waitKey(1); t_e = GetTickCount(); t = t_e - t_s; } return false; } bool attemptoRecoveryTracking2(int mpi) { vector<Point3f> newobjP; for(int i = 0;i<num_track;i++) { if(i == mpi) continue; newobjP.push_back(objP.at(i)); } switch (mpi) { case 0: newobjP.push_back(Point3f(1.5,-0.5,0)); break; case 1: newobjP.push_back(Point3f(0.5,1.5,0)); break; case 2: newobjP.push_back(Point3f(-1.5,0.5,0)); break; case 3: newobjP.push_back(Point3f(-0.5,-1.5,0)); break; default: break; } objPM.setTo(0); Mat(newobjP).convertTo(objPM,CV_32F); points[1].clear(); projectPoints(objPM, rvecm, tvecm, camera_matrix, distortion_coefficients, points[1]); objP = newobjP; return true; } int tracking_update() { ORB orb; for(;;) { if( needtomap && goodfeatures.size()>0 ) { needToGetgf = false; if(trackingpoints<num_track) { char c = waitKey(2); if(c == 'c') { points[0].clear(); points[1].clear(); trackingpoints = 0; goodfeatures.clear(); needToGetgf = false; needtomap = false; initflag = true; initPoints.clear(); init3DP(); } if(c == 27) break; continue; } if(initflag && initPoints.size()==num_track) { ORB orb; cvtColor(image, gray, COLOR_BGR2GRAY); orb.compute(gray,initPoints,initdescriptors); initflag = false; } cap >> frame; if( frame.empty() ) break; frame.copyTo(image); cvtColor(image, gray, COLOR_BGR2GRAY); if(!points[0].empty()) { vector<uchar> status; vector<float> err; if(prevGray.empty())