OpenCV相机姿态更新

#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> using namespace cv; using namespace std; 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 rvec(rv),tvec(tv); double rm[9]; Mat rotM(3,3,CV_64FC1,rm); Mat gray, prevGray, image, frame; vector<Point2f> points[2]; vector<Point2f> initPoints; vector<Point2f> recoveryPoints; vector<Point2f> projectedPoints; vector<Point2f> goodfeatures; bool initflag = false; bool needToGetgf = false; bool needtomap = false; bool needtokeeptime = false; const int MAX_COUNT = 500; size_t trackingpoints = 0; BriefDescriptorExtractor brief; 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(31,31); VideoCapture cap(0); static void help() { // print a welcome message, and the OpenCV version cout << "\nThis is the cPTAM .\n" "Using OpenCV version " << CV_VERSION << endl; cout << "\nHot keys: \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 it and press SPACE)\n" "\tt - keep the time to get FPS\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()<4) { for(size_t i = 0;i<goodfeatures.size();i++) { if(abs(goodfeatures[i].x-x)+abs(goodfeatures[i].y-y)<3) { points[1].push_back(goodfeatures[i]); trackingpoints++; break; } } } } } bool init() { cap.set(CV_CAP_PROP_FRAME_WIDTH,640); cap.set(CV_CAP_PROP_FRAME_HEIGHT,480); if( !cap.isOpened() ) { cout << "Could not initialize capturing...\n"; return false; } namedWindow( "PTAM_cc_LK_Tracking", 1 ); cvSetMouseCallback( "PTAM_cc_LK_Tracking",on_mouse,NULL ); objP.push_back(Point3f(0,0,0)); //三维坐标的单位是毫米 objP.push_back(Point3f(5,0,0)); objP.push_back(Point3f(5,5,0)); objP.push_back(Point3f(0,5,0)); Mat(objP).convertTo(objPM,CV_32F); return true; } void getPlanarSurface(vector<Point2f>& imgP){ Rodrigues(rotM,rvec); solvePnP(objPM, Mat(imgP), camera_matrix, distortion_coefficients, rvec, tvec); Rodrigues(rvec,rotM); cout<<"rotation matrix: "<<endl<<rotM<<endl; cout<<"translation matrix: "<<endl<<tv[0]<<" "<<tv[1]<<" "<<tv[2]<<endl; projectedPoints.clear(); projectPoints(objPM, rvec, tvec, 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); } } int tracking_update() { ORB orb; vector<KeyPoint> keypoints; for(;;) { if( needtomap && goodfeatures.size()>0 ) { needToGetgf = false; while(trackingpoints<4) { 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("PTAM_cc_LK_Tracking", temp); initflag = true; if(waitKey(0) == 27) return 0; } 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()) gray.copyTo(prevGray); calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err); size_t i,k; for(i = k = 0; i < points[1].size(); i++ ) { if( !status[i] ) continue; points[1][k++] = points[1][i]; circle( image, points[1][i], 3, Scalar(0,0,255), -1, 8); } if(k == 4) getPlanarSurface(points[0]); else { needtomap = false; needtokeeptime = false; trackingpoints = 0; goodfeatures.clear(); points[0].clear(); points[1].clear(); } } framenum++; } else { cap >> frame; if( frame.empty() ) break; frame.copyTo(image); if(needToGetgf) { cvtColor(image, gray, COLOR_BGR2GRAY); // automatic initialization orb.detect(gray, keypoints); goodfeatures.clear(); for( size_t i = 0; i < keypoints.size(); i++ ) { goodfeatures.push_back(keypoints[i].pt); } cornerSubPix(gray, goodfeatures, subPixWinSize, Size(-1,-1), termcrit); for(size_t i = 0; i < goodfeatures.size(); i++ ) { circle( image, goodfeatures[i], 3, Scalar(0,255,0), -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(image.cols - textSize.width - 20, image.rows - 2*baseLine - 10); putText(image,msg,textOrigin,1,1,Scalar(0,0,255)); imshow("PTAM_cc_LK_Tracking", image); char c = (char)waitKey(5); if( c == 27 ) { cap.release(); return 0; } switch( c ) { case 'g': needToGetgf = true; needtomap = false; break; case 'c': points[0].clear(); points[1].clear(); goodfeatures.clear(); needToGetgf = false; needtomap = false; break; case 'm': trackingpoints = 0; needtomap = true; break; case 't': needtokeeptime = true; framenum=0; t1 = GetTickCount(); break; } std::swap(points[1], points[0]); cv::swap(prevGray, gray); framenum++; } if(needtokeeptime) { t2 = GetTickCount(); cout<<endl<<"fps:"<<framenum/((t2-t1)*1.0/1000)<<"\n"; } return 0; } int main() { help(); if(!init()) return 0; tracking_update(); return 0; }