OpenCV相机姿态更新

使用OpenCV中的solvePnP函数计算相机姿态（旋转与平移）

#include “opencv2/core/core.hpp“
#include “opencv2/video/tracking.hpp“
#include “opencv2/imgproc/imgproc.hpp“
#include “opencv2/highgui/highgui.hpp“
#include 
#include “opencv2/features2d/features2d.hpp“
#include 
#include 

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（33CV_64FC1camD）;
Mat distortion_coefficients = Mat（51CV_64FC1distCoeffD）;

vector objP;
Mat objPM;
vector rv（3） tv（3）;
Mat rvec（rv）tvec（tv）; 
double rm[9];
Mat rotM（33CV_64FC1rm）;

Mat gray prevGray image frame;
vector points[2];
vector initPoints;
vector recoveryPoints;
vector projectedPoints;
vector 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 t1t2;
int framenum = 0;

TermCriteria termcrit（CV_TERMCRIT_ITER|CV_TERMCRIT_EPS 30 0.01）;
Size subPixWinSize（1010）winSize（3131）;

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 eventint xint yint flag void *param）
{
	if（event==CV_EVENT_LBUTTONDOWN）
	{
		if（needtomap && points[1].size（）<4）
		{
			for（size_t i = 0;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_WIDTH640）;
	cap.set（CV_CAP_PROP_frame_HEIGHT480）;

	if（ !cap.isOpened（） ）
	{
		cout << “Could not initialize capturing...\n“;
		return false;
	}

	namedWindow（ “PTAM_cc_LK_Tracking“ 1 ）;
	cvSetMouseCallback（ “PTAM_cc_LK_Tracking“on_mouseNULL ）;

	objP.push_back（Point3f（000））;    //三维坐标的单位是毫米
	objP.push_back（Point3f（500））;
	objP.push_back（Point3f（550））;
	objP.push_back（Point3f（050））;
	Mat（objP）.convertTo（objPMCV_32F）;
	
	return true;
}

void getPlanarSurface（vector& imgP）{

	Rodrigues（rotMrvec）;
	solvePnP（objPM Mat（imgP） camera_matrix distortion_coefficients rvec tvec）;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件     182272  2015-10-05 22:16  ptam_lk_tracking\Debug\ptam_lk_tracking.exe

     文件     823424  2015-10-05 22:16  ptam_lk_tracking\Debug\ptam_lk_tracking.ilk

     文件    2288640  2015-10-05 22:16  ptam_lk_tracking\Debug\ptam_lk_tracking.pdb

     文件        776  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\cl.command.1.tlog

     文件      32320  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\CL.read.1.tlog

     文件        576  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\CL.write.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link-cvtres.read.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link-cvtres.write.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link-rc.read.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link-rc.write.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.13052-cvtres.read.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.13052-cvtres.write.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.13052-rc.read.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.13052-rc.write.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.13052.read.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.13052.write.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.14772-cvtres.read.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.14772-cvtres.write.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.14772-rc.read.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.14772-rc.write.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.14772.read.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.14772.write.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.22636-cvtres.read.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.22636-cvtres.write.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.22636-rc.read.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.22636-rc.write.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.22636.read.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.22636.write.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.25252-cvtres.read.1.tlog

     文件          2  2015-10-05 22:16  ptam_lk_tracking\ptam_lk_tracking\Debug\link.25252-cvtres.write.1.tlog

............此处省略37个文件信息