OPENCV之從calibrateCamera到solvePnP（二）
阿新 • • 發佈：2019-02-11
接上一篇。
看函式
第一個引數和第二個引數與上一篇類似，不過上一篇的是多幅圖的多組點，型別為雙重vector，這裡是一副圖中的一組點，型別為單重vector。
vector<Point3f> calcBoardCornerPositions(int gridW, int gridH, float squareSize)
{
vector<Point3f> objectPoints;
for (int i = 0; i < gridH; i++)
for (int j = 0; j < gridW; j++)
objectPoints.push_back(Point3f(float 第二個引數
vector<Point2f> corners;
bool found = findChessboardCorners(images, grids, corners, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK); 第三四個引數既是上篇的結果，讀入之。
再後兩個引數是我們要得到的結果，聲明後帶入即可。
再其他引數預設就好。
那麼得到rvecs, tvecs之後怎麼得到棋盤的空間姿態呢，姿態座標系又是什麼呢？
rvecs是旋轉向量，使用Rodrigues轉成旋轉矩陣。原理百度可查。
現在得到了旋轉矩陣和平移矩陣了，那麼根據標定公式就可以得到世界座標了。
接下來求解三軸旋轉角。原理如下：
這個矩陣對應與旋轉矩陣，由旋轉矩陣即可反解尤拉角。
最終效果：
X,Y,Z是以鏡頭為原點的世界座標單位0.1mm，ang_X,Y,Z是棋盤沿三軸的旋轉角度，單位度。
程式碼實時執行，速度超過20FPS，精度1cm以內。
完整程式碼：
#include <cv.h>
#include <highgui.h>
#include <iostream>
#include <math.h>
#define pi 3.14159265358979323846
using namespace std;
using namespace cv;
vector<Point3f> calcBoardCornerPositions(int gridW, int gridH, float squareSize)
{
vector<Point3f> objectPoints;
for (int i = 0; i < gridH; i++)
for (int j = 0; j < gridW; j++)
objectPoints.push_back(Point3f(float(j*squareSize), float(i*squareSize), 0));
return objectPoints;
}
vector<Point3f> calcBoardCornerPositions(int gridW, int gridH)
{
vector<Point3f> objectPoints;
for (int i = 0; i < gridH; i++)
for (int j = 0; j < gridW; j++)
objectPoints.push_back(Point3f(float(j * 89), float(i * 83), 0));
return objectPoints;
}
int main()
{
VideoCapture cap(1);
if (!cap.isOpened()) return -1;
//cap.set(CV_CAP_PROP_FRAME_WIDTH, 1920);
//cap.set(CV_CAP_PROP_FRAME_HEIGHT, 1080);
Mat images, gray;
Size grids(9, 6);
int Grids_Size = 260;
float Ang_X, Ang_Y, Ang_Z;
float X, Y, Z;
char key; int i = 0;
//float A[][3] = { { 644.8137843176841, 0, 302.6526363184274 },{0, 649.3562275395091, 286.5283609342574 },{0, 0, 1 }};
//float B[] = { 0.01655500980525433, 0.1901812353222618, 0.003461616464410258, 0.002455084197033077, -1.444734184159016 };
float A[][3] = { { 988.74755, 0, 309.709197 }, { 0, 988.2410178, 239.85705 }, { 0, 0, 1 } };
float B[] = { -0.41287433, 1.80600373, 0.00250586, 0.0013610796, -7.6232044988 };
Mat rvecs(3, 1, CV_32F), tvecs(3, 1, CV_32F), cameraMatrix(3, 3, CV_32F), distCoeffs(1, 5, CV_32F), R(3, 3, CV_32FC1);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
{
cameraMatrix.at<float>(i, j) = A[i][j];
R.at<float>(i, j) = 0;
}
for (int i = 0; i < 5;i++)
distCoeffs.at<float>(0, i) = B[i];
namedWindow("chessboard", 0);
namedWindow("qqq", 0);
cap >> images;
while (1)
{
cap >> images;
imshow("qqq", images);
vector<Point2f> corners;
bool found = findChessboardCorners(images, grids, corners, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);
if (found)
{
cvtColor(images, gray, CV_BGR2GRAY);
cornerSubPix(gray, corners, Size(11, 11), Size(-1, -1),
TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));//0.1為精度
drawChessboardCorners(images, grids, corners, found);
vector<Point3f> objectPoints = calcBoardCornerPositions(grids.width, grids.height, Grids_Size);
//vector<Point3f> objectPoints = calcBoardCornerPositions(grids.width, grids.height);
solvePnP(objectPoints, corners, cameraMatrix, distCoeffs, rvecs, tvecs);
Rodrigues(rvecs, R);
Ang_X = asin(R.at<double>(1, 0) / cos(asin(-R.at<double>(2, 0)))) / pi * 180;
Ang_Y = asin(-R.at<double>(2, 0)) / pi * 180;
Ang_Z = asin(R.at<double>(2, 1) / cos(asin(-R.at<double>(2, 0)))) / pi * 180;
X = R.at<double>(0, 0) *objectPoints[22].x + R.at<double>(0, 1) * objectPoints[22].y + R.at<double>(0,2) * objectPoints[22].z + tvecs.at<double>(0,0);
Y = R.at<double>(1, 0) *objectPoints[22].x + R.at<double>(1, 1) * objectPoints[22].y + R.at<double>(1, 2) * objectPoints[22].z + tvecs.at<double>(1, 0);
Z = R.at<double>(2, 0) *objectPoints[22].x + R.at<double>(2, 1) * objectPoints[22].y + R.at<double>(2, 2) * objectPoints[22].z + tvecs.at<double>(2, 0);
putText(images, "X:"+to_string(X), { 1, 50 }, 0, 1.0f, CV_RGB(255, 0, 0), 2);
putText(images, "Y:"+to_string(Y), { 1, 150 }, 0, 1.0f, CV_RGB(0, 255, 0), 2);
putText(images, "Z:"+to_string(Z), { 1, 250 }, 0, 1.0f, CV_RGB(0, 0, 255), 2);
putText(images, "Ang_X:" + to_string(Ang_X), { 300, 50 }, 0, 1.0f, CV_RGB(255, 0, 0), 2);
putText(images, "Ang_Y:" + to_string(Ang_Y), { 300, 150 }, 0, 1.0f, CV_RGB(0, 255, 0), 2);
putText(images, "Ang_Z:" + to_string(Ang_Z), { 300, 250 }, 0, 1.0f, CV_RGB(0, 0, 255), 2);
imshow("chessboard", images);
}
key=waitKey(20);
if (key == ' ')
break;
}
return 0;
}
注意，程式碼中取objectPoints[22]計算是求解棋盤較為中間的點的座標，若取0則是計算的棋盤第一個點座標，這個不重要。
完了。
接下來如果有空的話把雙目的也寫上來。還有單目標定也可以用MATLAB工具箱，結果差不多。