Opencv--从CalibrateCamera到SolvePnp（二）-技術動態-匯編語言學習筆記

Opencv--从CalibrateCamera到SolvePnp（二）

2023-09-07 阅读 21 评论 0

摘要：接上一篇。看函数第一个参数和第二个参数与上一篇类似，不过上一篇的是多幅图的多组点，类型为双重vector，这里是一副图中的一组点，类型为单重vector。 vector<Point3f> calcBoardCornerPositions(int gridW, int gridH, float squareSize)

接上一篇。

看函数
这里写图片描述

第一个参数和第二个参数与上一篇类似，不过上一篇的是多幅图的多组点，类型为双重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(j*squareSize), float(i*squareSize), 0));return objectPoints;
}vector<Point3f> objectPoints = calcBoardCornerPositions(grids.width, grids.height);1
2
3
4
5
6
7
8
9

第二个参数

        vector<Point2f> corners;bool found = findChessboardCorners(images, grids, corners, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);1
2

第三四个参数既是上篇的结果，读入之。

再后两个参数是我们要得到的结果，声明后带入即可。

再其他参数默认就好。

那么得到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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91