opencv相机标定和计算外参四元数

#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"

#include <cctype>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <iostream>

using namespace cv;
using namespace std;



float squareSize = 1.f, aspectRatio = 1.f;
const char* outputFilename = "out_camera_data.yml";

int  nframes = 1;
int flags = 0;
int m_width = 640;
int m_height = 480;
int cameraId = 0;
int delay = 1000;

Mat cameraMatrix, distCoeffs, my_frameRBG, r, t;
Size boardSize(6, 9), imageSize(640, 480);
clock_t prevTimestamp = 0;
vector<vector<Point2f> > imagePoints;
vector<string> imageList;
vector<Mat> rvecs, tvecs;


enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };
enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
Pattern pattern = CHESSBOARD;
int mode = DETECTION;


//计算外参并转化为unity3d能用的
int sign(float x)
{
	return x >= 0 ? 1 : -1;
}
float myMax(float x, float y)
{
	return x >y ? x : y;

}
//计算四元数
void QuaternionFromMatrix(const Mat& R, float quat[])
{
	// Adapted from: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm

	quat[0] = (float)sqrt(myMax(0.0, 1 + R.at<float>(0, 0) + R.at<float>(1, 1) + R.at<float>(2, 2))) / 2;

	quat[1] = (float)sqrt(myMax(0.0, 1 + R.at<float>(0, 0) - R.at<float>(1, 1) - R.at<float>(2, 2))) / 2;
	quat[2] = (float)sqrt(myMax(0.0, 1 - R.at<float>(0, 0) + R.at<float>(1, 1) - R.at<float>(2, 2))) / 2;
	quat[3] = (float)sqrt(myMax(0.0, 1 - R.at<float>(0, 0) - R.at<float>(1, 1) + R.at<float>(2, 2))) / 2;

	quat[1] *= sign(R.at<float>(2, 1) - R.at<float>(1, 2));
	quat[2] *= sign(R.at<float>(0, 2) - R.at<float>(2, 0));
	quat[3] *= sign(R.at<float>(1, 0) - R.at<float>(0, 1));
}

//左右手坐标系转化,计算四元数
void CcalinitEctrinsicMat(float* m_fExtrinsic, Mat rr, Mat tt)
{
	rr.convertTo(rr, CV_32F);
	tt.convertTo(tt, CV_32F);
	Mat R = Mat(3, 3, CV_32F), T = Mat(3, 1, CV_32F);
	T = tt;
	R = rr;
	mrightEX = mrightEX.inv();
	//for (size_t i = 0; i < 3; i++)
	//{
	//	for (size_t j = 0; j < 3; j++)
	//	{
	//		R.at<float>(i, j) = mrightEX.at<float>(i, j);
	//		//T.at<float>(i, 0) = mrightEX.at<float>(i, 3);
	//	}
	//	T.at<float>(i, 0) = mrightEX.at<float>(i, 3);
	//}
	左右手坐标系转化

	Mat R_CPP, R_inv, T_CPP;
	R_CPP = R;
	R_inv = R_CPP.inv();
	T_CPP = T;
	QuaternionFromMatrix(R_inv, m_fExtrinsic);

	// We need to invert rotations on X and Z axis
	m_fExtrinsic[1] = -m_fExtrinsic[1];
	m_fExtrinsic[3] = -m_fExtrinsic[3];
	Mat Tt = -R_inv * T_CPP;
	m_fExtrinsic[4] = (float)Tt.at<float>(0);
	m_fExtrinsic[5] = -(float)Tt.at<float>(1);
	m_fExtrinsic[6] = (float)Tt.at<float>(2);
}




static void calcChessboardCorners(Size boardSize, float squareSize,
				vector<Point3f>& corners, Pattern patternType = CHESSBOARD)
{
	corners.resize(0);

	switch (patternType)
	{
	case CHESSBOARD:
	case CIRCLES_GRID:
		for (int i = 0; i < boardSize.height; i++)
			for (int j = 0; j < boardSize.width; j++)
				corners.push_back(Point3f(float(j*squareSize),
				float(i*squareSize), 0));
		break;

	case ASYMMETRIC_CIRCLES_GRID:
		for (int i = 0; i < boardSize.height; i++)
			for (int j = 0; j < boardSize.width; j++)
				corners.push_back(Point3f(float((2 * j + i % 2)*squareSize),
				float(i*squareSize), 0));
		break;

	default:
		CV_Error(CV_StsBadArg, "Unknown pattern type\n");
	}
}


void main()
{

	VideoCapture capture(cameraId);
	float m_fExtrinsic[7];
	memset(m_fExtrinsic, 0, 7 * sizeof(float));

	for (int i = 0;; i++)
{
	Mat view, viewGray;
	bool blink = false;
	capture.set(CV_CAP_PROP_FRAME_WIDTH, m_width);
	capture.set(CV_CAP_PROP_FRAME_HEIGHT, m_height);

	if (capture.isOpened())
	{
		Mat view0;
		capture >> view0;
		view0.copyTo(view);
	}

	//flip(view, view, 0);
	vector<Point2f> pointbuf;
	cvtColor(view, viewGray, COLOR_BGR2GRAY);

	bool found;
	switch (pattern)
	{
	case CHESSBOARD:
		found = findChessboardCorners(view, boardSize, pointbuf,
			CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
		break;
	case CIRCLES_GRID:
		found = findCirclesGrid(view, boardSize, pointbuf);
		break;
	case ASYMMETRIC_CIRCLES_GRID:
		found = findCirclesGrid(view, boardSize, pointbuf, CALIB_CB_ASYMMETRIC_GRID);
		break;
	default:
		break;
	}

	// improve the found corners' coordinate accuracy
	if (pattern == CHESSBOARD && found) cornerSubPix(viewGray, pointbuf, Size(11, 11),
		Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
	mode = CAPTURING;

	if (mode == CAPTURING && found && (!capture.isOpened() || clock() - prevTimestamp > delay*1e-3*CLOCKS_PER_SEC))
	{
		imagePoints.push_back(pointbuf);
		prevTimestamp = clock();
		blink = capture.isOpened();
	}

	if (found)
		drawChessboardCorners(view, boardSize, Mat(pointbuf), found);

	imshow("Image View", view);
	waitKey(1);
	//if (view.data)
	//{
	//	cvtColor(view, my_frameRBG, CV_BGR2RGB);
	//	memcpy(texturePtr, my_frameRBG.data, my_frameRBG.cols*my_frameRBG.rows*my_frameRBG.channels()*sizeof(uchar));
	//}

	if (mode == CAPTURING && imagePoints.size() >= (unsigned)nframes)
	{
			double totalAvgErr = 0;
			cameraMatrix = Mat::eye(3, 3, CV_64F);
			if (flags & CV_CALIB_FIX_ASPECT_RATIO)
				cameraMatrix.at<double>(0, 0) = aspectRatio;

			distCoeffs = Mat::zeros(8, 1, CV_64F);
			vector<vector<Point3f> > objectPoints(1);
			calcChessboardCorners(boardSize, squareSize, objectPoints[0], pattern);
			objectPoints.resize(imagePoints.size(), objectPoints[0]);

			double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix,
				distCoeffs, rvecs, tvecs, flags | CV_CALIB_FIX_K4 | CV_CALIB_FIX_K5);		
			mode = CALIBRATED;
		}	
		else
			mode = DETECTION;
		if (!capture.isOpened())
			break;

		if (mode == CALIBRATED)
		{
			Rodrigues(rvecs[0], r);
			cout << rvecs[0] << endl;
			CcalinitEctrinsicMat(m_fExtrinsic, r, tvecs[0]);
			rvecs.clear();
		}
		for each (float var in m_fExtrinsic)
		{
			cout << var << endl;
		}
	}
	return;
}