利用SolovePnP求解相机位姿，并且验证9点手眼标定法

9点标定法

用来将相机坐标系转到一个已知坐标系的方法，也就是求相机坐标系到已知世界坐标系下的坐标变换。

如图所示，寻找R,T即为所求目标。

SolvePnP

图片来源于Opencv官网。对于图中我们最终的目标是求解出红色区域，可以采用SVD分解的方法解这个方程。
对于普通的任意两个点集P和Q之间的关系可以表达为：

对t求偏导，t对加法是封闭的，所以可以对t求偏导，但是不能对R直接求偏导


2. 然后再求解R

代码验证

1. 首先我们获取到左相机的图像，计算出利用SlovePnP, R,T；

2. 将世界坐标系建立在标定板的一个点上，然后计算对应的每个点的坐标，同采用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