9点标定法
用来将相机坐标系转到一个已知坐标系的方法,也就是求相机坐标系到已知世界坐标系下的坐标变换。
如图所示,寻找R,T即为所求目标。
SolvePnP
图片来源于Opencv官网。对于图中我们最终的目标是求解出红色区域,可以采用SVD分解的方法解这个方程。
对于普通的任意两个点集P和Q之间的关系可以表达为:
对t求偏导,t对加法是封闭的,所以可以对t求偏导,但是不能对R直接求偏导
2. 然后再求解R
代码验证
-
首先我们获取到左相机的图像,计算出利用SlovePnP, R,T;
-
将世界坐标系建立在标定板的一个点上,然后计算对应的每个点的坐标,同采用solvePnP描述的一样。这样一来就有了P和Q,下面就是SVD分解了。
3.
标定板用来标定,标记点用来验证。最后的结果如下:
理论计算Z轴应该为0,但是实际计算的结果还是有一点误差的,可能和标定有关,但是在x,y轴的结果还是很不错的,误差在1-2mm以内。
部分核心代码
#include<opencv.hpp>
#include<iostream>
#include<calib3d.hpp>
using namespace std;
using namespace cv;
Size board_size = Size(7, 8);//棋盘格角点数目
Size square_size = Size(23, 23); /* 实际测量得到的标定板上每个棋盘格的大小 */
float floatdata = 0.15;
//第一步,读取左相机图像,获取到corner点
void getImageCorner(Mat& imageLeft, vector<Point2f>& cornerL) {
if (imageLeft.empty())return;
Mat grayL;
cvtColor(imageLeft, grayL, COLOR_BGR2GRAY);
auto isFindL = findChessboardCorners(grayL, board_size, cornerL, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);
if (isFindL) {
cornerSubPix(grayL, cornerL, Size(5, 5), Size(-1, 1), TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.0001));
drawChessboardCorners(imageLeft, board_size, cornerL, isFindL);
namedWindow("chessboardL", WINDOW_NORMAL);
imshow("chessboardL", imageLeft);
waitKey(10);
}
else
{
return;
}
}
//第二步利用solvePnP计算旋转,平移向量
void getRT(vector<Point2f>& cornerL, Mat& cameraMatrix, Mat &distCoeffs, Mat&R, Mat&T) {
//首先获取到世界坐标的点
if (cornerL.empty() || cameraMatrix.empty() || distCoeffs.empty())return;
vector<Point3f> objectPoints;
for (int i = 0; i<board_size.height; i++)
{
for (int j = 0; j<board_size.width; j++)
{
Point3f realPoint;
/* 假设标定板放在世界坐标系中z=0的平面上 */
realPoint.x = (float)i*(float)(square_size.width + floatdata);
realPoint.y = (float)j*(float)(square_size.width + floatdata);
realPoint.z = 0;
objectPoints.push_back(realPoint);
//realPoint.x = (float)i*(float)(square_size.width);
//realPoint.y = (float)j*(float)(square_size.width );
//realPoint.z = 0;
//tempPointSet.push_back(realPoint);
}
}
Mat rvec;
solvePnP(objectPoints, cornerL, cameraMatrix, distCoeffs, rvec, T);//计算rvec,T,这个时候的rvec是雅可比形式的rvec,需要一个转换
Rodrigues(rvec, R); //这个时候的R是3x3的一个矩阵。
}
void transform3Dto3D(Mat& R, Mat& T, vector<Point3f>& input, vector<Point3f>& outPut) {
if (R.empty() || T.empty() || input.empty()) {
return;
}
//P=R*P+t;
for (auto p : input) {
Mat point1 = (Mat_<double>(3, 1) << p.x, p.y, p.z);
point1 = R.inv()*(point1 - T);
outPut.push_back(Point3f(point1.at<double>(0), point1.at<double>(1), point1.at<double>(2)));
}
}
int main() {
//第一步,读图
string adressL = ".//left//left_1.jpg";
string adressR = ".//right//right_1.jpg";
Mat img = imread(adressL);
Mat imgR = imread(adressR);
//获取图2的点
vector<Point2f> cornerL;
getImageCorner(img, cornerL);
TriangulationTest tt; //创建对象
auto cameraMatrixL = tt.getCameraMatrixL();//需要根据自己的内参调用函数
auto cameraDistcoffe = tt.getCameradistCoeffsL();
//获取RT
Mat R, T;
getRT(cornerL, cameraMatrixL, cameraDistcoffe, R, T);
cout << "R= " << endl << R << endl;
cout << "T= " << endl << T << endl;
//验证坐标
std::map<int, std::vector<cv::Point3f>> out;
std::map<int, Point3f>centerout;
tt.get3Dpoints(img, imgR, out, centerout);//利用左右相机计算出世界坐标系的点(需要根据自己的实际计算,这部分未给出)
//3D TO 3D 验证计算的R T是否正确
for (auto temp : out) {
cout << "ID =" << temp.first << "转换前的坐标为(x,y,z):" << endl;
for (auto pointBefore : temp.second) {
cout << "(" << pointBefore.x << "," << pointBefore.y << "," << pointBefore.z << ")" << endl;
}
vector<Point3f> outPut;
transform3Dto3D(R, T, temp.second, outPut);
cout << "ID =" << temp.first << "转换后的坐标为(x,y,z):" << endl;
for (auto point : outPut) {
cout << "(" << point.x << "," << point.y << "," << point.z << ")" << endl;
}
}
//结束,然后实际评测
system("pause");
return 1;
}
代码不能直接运行,部分地方需要加入计算左相机下特征点的三维坐标,需要根据自己实际情况更改,核心就是getRT这个函数,内部调用了SlovePnP,计算出R,T此时R是用罗德利斯公式表达的,所以需要一个转换函数Rodrigues()官方提供。详细可以查看官网例程https://docs.opencv.org/master/d9/d0c/group__calib3d.html#gae5af86788e99948d40b39a03f6acf623