#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;
}
opencv相机标定和计算外参四元数
最新推荐文章于 2023-08-25 14:04:54 发布