相机姿态估计

一、相机姿态估计原理

首先介绍一下什么是世界坐标系和相机坐标系——世界坐标系是自己定义的一个坐标系，这里我定义世界坐标系是X轴垂直屏幕指向人，Y轴水平向右，Z轴竖直向上。相机坐标系有统一的规定，如图所示，x轴平行于相机镜头水平向左，y轴平行于相机镜头向下，z轴垂直于镜头水平指向人。
我们求相机在世界坐标系中的姿态的需求往往就是求一个相机坐标下的点在世界坐标系下的坐标，或者是一个世界坐标系下的点在相机坐标系的坐标。

首先思考一下为什么能在一个确定的世界坐标系下确定相机的姿态（姿态也就是指相机在世界坐标系中的具体位置，包括xyz坐标轴旋转量与平移量）。假如相机坐标系与和世界坐标系相对静止，那么两者之间的关系就是确定的，通过一定的方法肯定可以算出两者之间的关系。这里肯定有人要问算出两者之间的关系需要几个相机，答案是只需要一个（上面的示意图片只是因为我没有单目相机，整个实验过程只用到了左相机）。只用一个相机的前提是必须知道相机的内参，包括内参矩阵和畸变矩阵。至于为什么在知道内参矩阵和畸变矩阵条件下可以用一个相机算出相机的姿态暂时还没完全参透。

在上图中，小写的xyz是点在相机坐标系中的坐标，大写的XYZ是点在世界坐标系中的坐标。R33是旋转矩阵，T31是平移矩阵，这里非常值得注意的是，坐标的单位要统一，其次是当输入的世界坐标是实际的坐标数值时，计算出来的平移矩阵也是实际的平移量。我们的目标就转变为求旋转矩阵和是平移矩阵。
下面先介绍这两个矩阵：
①旋转矩阵：

现在要求A到B的旋转矩阵，XB、YB、ZB、XA、YA、ZA分别是指各个坐标轴方向的单位向量。A为世界坐标系，B为相机坐标系。


则有

知道旋转矩阵各项的意义，可以帮助我们大概地判断我们计算出来的旋转矩阵是否正确。
②平移矩阵
现在回到这个图：

如果要求一个点世界坐标系的点在相机坐标系的坐标，设点在世界坐标系中的坐标是[X;Y;Z] (这里用31的矩阵表示点)，那么在求出旋转矩阵后，由R[X;Y;Z]可以得出点在旋转后的世界坐标系中的坐标，此时的世界坐标系变为三个坐标轴分别与相机坐标系三个坐标轴平行的坐标系，如下图所示：

而其中的▲x,▲y分别是上图中两个坐标系对应轴的距离（z轴也一样，有点难画就不画了）。平移矩阵T=[▲x;▲y;▲z]。通过几何意义我们也可以大概判断计算出来的平移矩阵是否正确。

二、相机姿态估计实现

首先我们需要得到相机的内参，可以通过张正友标定法得到相机的内参矩阵和畸变矩阵。具体步骤这里就不展开了，下面是我的得到的内参矩阵和畸变矩阵：

相机拍摄图片的分辨率是19201080，相机的像元尺寸是2.2um2.2um。
旋转矩阵和平移矩阵的求取通过Opencv的PnP算法，采用的函数是solvePnP，下面介绍一下这个函数：






函数的介绍可以在这里找到
通过上面的函数介绍可知，我们需要做的是取得至少4对的匹配点，即图像的二维坐标点和世界坐标系中的三维坐标点。如何比较准确地得出点在世界坐标系中的坐标需要我们好好想一想。由于这里我的世界坐标系比较特殊，我采用了一根碳素杆，在碳素杆上标记一个小白点，让碳素杆平整的一段垂直于屏幕紧靠，紧靠的点在屏幕上的世界坐标可以通过手工测量得出，而小白点的X坐标就是碳素杆平整一段到小白点的距离，YZ坐标值与紧靠的点相同。
从文章开头到现在我们都是求世界坐标系的点到相机坐标系的点的旋转矩阵和平移矩阵，那么如果要求相机坐标系的点到世界坐标系的点的旋转矩阵和平移矩阵，那么我们可以先求出世界坐标系的点到相机坐标系的点的旋转矩阵R和平移矩阵T,然后通过下式求得相机坐标系的点在世界坐标系中的坐标：

实现代码如下：
这里我的目的是求两个相机的光心的世界坐标。

#include <opencv2\opencv.hpp>
#include <fstream> 
#include <iostream>
#include <vector>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>
using namespace std;
using namespace cv;
using namespace Eigen;

int main()
{
	Mat rvec_L, tvec_L, rvec_R, tvec_R;
	//左相机的内参矩阵
	Mat cameraMatrix_L = (Mat_<double>(3, 3) << 1492.320675688276, 0, 1042.383048569903,
											   0, 1443.277689279584, 561.8151909811569,
											   0, 0, 1);
	//左相机的畸变矩阵
	Mat disCoeffs_L = (Mat_<double>(1,5) << 0.8855691467581785, -3.76931898492866, 0.001722608150198718, 0.09559525553142016, 8.767109962143531);
	//右相机的内参矩阵
	Mat cameraMatrix_R = (Mat_<double>(3, 3) << 1489.387347765696, 0, 1155.32002834218,
												0, 1446.451497184327, 499.0095775742494,
												0, 0, 1);
	//右相机的畸变矩阵
	Mat disCoeffs_R = (Mat_<double>(1, 5) << 1.010043485302243, -8.566544837424154, -0.002706277601760965, 0.09466761963281151, 32.5513838899965);
	//匹配点的坐标，这里取5对，不要取6对
	vector<Point3d> PointSets = { Point3d(415,115,195),Point3d(415,230,195),Point3d(415,345,195),Point3d(415,115,130),Point3d(415,230,130) };
	vector<Point2f> imgPoints_L = { Point2f(1230,659),Point2f(841,662) ,Point2f(436,683) ,Point2f(1266,893) ,Point2f(824,864) }; 
    vector<Point2f> imgPoints_R = { Point2f(1428,604),Point2f(1039,607) ,Point2f(640,627) ,Point2f(1483,838) ,Point2f(1040,808) }; 
	solvePnP(PointSets, imgPoints_L, cameraMatrix_L, disCoeffs_L, rvec_L, tvec_L);
	solvePnP(PointSets, imgPoints_R, cameraMatrix_R, disCoeffs_R, rvec_R, tvec_R);
	Mat R_L, R_R;
	//转换为旋转矩阵，但是格式仍然是Mat类型
	Rodrigues(rvec_L, R_L);
	Rodrigues(rvec_R, R_R);
	Matrix<double, 3, 3> Matrix_R_L;//世界坐标系到左相机坐标系的旋转矩阵
	Matrix<double, 3, 3> Matrix_R_R;//世界坐标系到右相机坐标系的旋转矩阵
	Matrix<double, 3, 1> Matrix_T_L;//世界坐标系到左相机坐标系的平移矩阵
	Matrix<double, 3, 1> Matrix_T_R;//世界坐标系到右相机坐标系的平移矩阵
	Matrix<double, 3, 1> oL_in_world;//左相机光心在世界坐标系中的坐标
	Matrix<double, 3, 1> oR_in_world;//右相机光心在世界坐标系中的坐标
	Matrix<double, 3, 1> o_in_cameraL;//左相机光心在左相机坐标系中的坐标
	Matrix<double, 3, 1> o_in_cameraR;//右相机光心在右相机坐标系中的坐标
	//光心就是各自相机坐标系的原点
	o_in_cameraL(0, 0) = 0;
	o_in_cameraL(1, 0) = 0;
	o_in_cameraL(2, 0) = 0;
	o_in_cameraR(0, 0) = 0;
	o_in_cameraR(1, 0) = 0;
	o_in_cameraR(2, 0) = 0;
	//将Mat类型的旋转矩阵和平移矩阵转化为Matrix类型
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			Matrix_R_L(i, j) = R_L.at<double>(i, j);
			Matrix_R_R(i, j) = R_R.at<double>(i, j);
		}
		Matrix_T_L(i, 0) = tvec_L.at<double>(i, 0);
		Matrix_T_R(i, 0) = tvec_R.at<double>(i, 0);
	}
	oL_in_world = Matrix_R_L.inverse()*(o_in_cameraL - Matrix_T_L);
	oR_in_world = Matrix_R_R.inverse()*(o_in_cameraR - Matrix_T_R);
	cout << "oL_in_world=" << oL_in_world << endl;
	cout << "oR_in_world=" << oR_in_world << endl;
	return 0;
}

中间求解的旋转矩阵和平移矩阵如下：