#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相机标定和计算外参四元数
猜你喜欢
转载自blog.csdn.net/moonlightpeng/article/details/80345537
今日推荐
周排行