使用OpenCV的三角测量可以根据立体标定得到的相机参数，以及一个特征点在左右两个相机中的像素坐标，求解得到该特征点的三维世界坐标（z坐标即深度信息）。

使用双目相机进行立体重建时，利用物体点-光心-像点三者共线的原理，在找到左右匹配的像点并且完成去畸变之后，就能够在三维空间中形成两条直线，物体点就是这两个直线的“交点”，“交点”指最小二乘交点，因为这两个直线由于偏差通常不会交于一点。

1.测量原理

相机成像过程中，从世界坐标系到相机坐标系的转换式为式（1），从像素坐标系到相机坐标系的转换为式（2）

[xcyczc]=[R11R12R13TxR21R22R23TyR31R32R33Tz][XYZ](1)\begin{bmatrix}x_{c} \\y_{c} \\z_{c} \end{bmatrix}=\begin{bmatrix} R_{11} &R_{12} &R_{13} &T_{x} \\ R_{21} &R_{22} &R_{23} &T_{y} \\ R_{31} &R_{32} &R_{33} &T_{z} \end{bmatrix}\begin{bmatrix}X \\Y \\Z \end{bmatrix} (1)

zc[uv1]=[fx0cx0fycy001][xcyczc](2)z_{c}\begin{bmatrix}u \\v \\1 \end{bmatrix}=\begin{bmatrix} f_{x} & 0 &c_{x} \\ 0 &f_{y} &c_{y} \\ 0 &0 &1 \end{bmatrix} \begin{bmatrix}x_{c} \\y_{c} \\z_{c} \end{bmatrix} (2)

根据式（1）可以得到：

{xc′=xc/zc=R11xw+R12yw+R13zw+TxR31xw+R32yw+R33zw+Tzyc′=yc/zc=R21xw+R22yw+R23zw+TyR31xw+R32yw+R33zw+Tz(3)\begin{cases}x_{c}^{'} = x_{c} / z_{c}=\frac{R_{11}x_{w}+R_{12}y_{w}+R_{13}z_{w}+T_{x}}{R_{31}x_{w}+R_{32}y_{w}+R_{33}z_{w}+T_{z}} \\y_{c}^{'} = y_{c} / z_{c}=\frac{R_{21}x_{w}+R_{22}y_{w}+R_{23}z_{w}+T_{y}}{R_{31}x_{w}+R_{32}y_{w}+R_{33}z_{w}+T_{z}} \end{cases} (3)

根据式（3）可以发现，当已知左右两个相机对于同一个物体点的坐标xc1′,yc1′x_{c1}^{'},y_{c1}^{'}和xc2′,yc2′x_{c2}^{'},y_{c2}^{'}以及旋转矩阵和平移矩阵时（一般将第一个相机的相机坐标系当作世界坐标系，故第一个相机的旋转矩阵为单位矩阵，平移矩阵为0，第二个相机的旋转矩阵和平移矩阵即为两个相机之间的旋转和平移矩阵），这是有四个方程三个未知量，这是一个超定问题，OpenCV将其转换为“Ax=0"的矩阵形式，利用SVD分解解算未知数。

2.函数使用

OpenCV中三角测量对应函数为triangulatePoints(),函数原型及对应参数含义为：

1
2
3
4
5
6
void cv::triangulatePoints	(InputArray 	projMatr1,
InputArray 	projMatr2,
InputArray 	projPoints1,
InputArray 	projPoints2,
OutputArray points4D 
)
参数 含义
projMatr1 第一个相机的投影矩阵，大小为3*4，一般以第一个相机为基准（即以第一个相机的相机坐标系作为世界坐标系），故其为旋转对应为单位矩阵，平移对应0
projMatr2 第二个相机的投影矩阵，大小为3*4，参数为两个相机之间的旋转平移矩阵，一般通过立体标定函数stereoCalibrate()得到；由理论推导过程可知，R、T为从右相机坐标系到左相机坐标系的转换
projPoints1 第一个相机下的二维点集，需要将像素坐标归一化在相机坐标系下（从像素坐标系转换到相机坐标系）
projPoints2 第二个相机下的二维点集，需要将像素坐标归一化在相机坐标系下
points4D 输出的点集对应的齐次坐标，形式为4*N，需要将前三个维度除以第四个维度得到非齐次坐标的坐标

对于两个投影矩阵，其一般设为如下（R,T为立体标定得到参数）：

1
2
3
4
5
6
7
8
9
Mat T1 = (Mat_<float>(3, 4) <<
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0);
Mat T2 = (Mat_<float>(3, 4) <<
		R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), T.at<double>(0, 0),
		R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), T.at<double>(1, 0),
		R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), T.at<double>(2, 0)
		);

OpenCV的坐标经过了相机归一化平面的处理，也就是令xc′=xc/zcx_{c}^{'} = x_{c} / z_{c}，yc′=yc/zcy_{c}^{'} = y_{c} / z_{c}的处理，这里的xc′，yc′x_{c}^{'}，y_{c}^{'}就是函数中需要的归一化坐标。已知左右相机上匹配的同名点(u1，v1)(u_{1}，v_{1})和(u2，v2)(u_{2}，v_{2})，根据式 (2) 能便够得到分别对应的归一化坐标，转换式为xc′=(u−u0)/fxx_{c}^{'}=(u-u_{0})/f_{x}和yc′=(v−v0)/fyy_{c}^{'}=(v-v_{0})/f_{y}，封装成函数如下，输入的点p为像素坐标，矩阵K为相机内参矩阵：

1
2
3
4
5
6
7
8
Point2f pixel2cam(const Point2f& p, const Mat& K)
{
	return Point2f
		(
			(p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
			(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
			);
}

接下来就可以将构造好的参数输入函数中，在得到输出结果后再将四维齐次坐标转换为非齐次的世界坐标。

1
2
3
4
5
6
7
8
9
10
11
12
Mat pts_4d;
triangulatePoints(T1, T2, pts1, pts2, pts_4d);

// 转换成非齐次坐标
vector<Point3f> pts_result;
for (int i = 0; i < pts_4d.cols; i++)
{
	Mat x = pts_4d.col(i);
	x /= x.at<float>(3, 0); // 归一化
	Point3f p(x.at<float>(0, 0), x.at<float>(1, 0), x.at<float>(2, 0));
	pts_result.push_back(p);
}

主要参考博客链接：

triangulatePoints()函数使用方法

原理解析