OpenCV三角测量重建triangulatePoints原理解析
共线方程
双相机立体重建时，利用物体点-光心-像点三者共线的原理，在找到左右匹配的像点并且完成去畸变之后（OpenCV去畸变undistortPoints原理解析），就能够在三维空间中形成两条直线，物体点就是这两个直线的“交点”，“交点”指最小二乘交点，因为这两个直线由于偏差通常不会交于一点。共线方程描述了物体点-光心-像点三者共线的含义，也相当于三维点到二维点的投影过程，该投影过程可表达为
[ x c y c z c ] = [ R 11 R 12 R 13 T x R 21 R 22 R 23 T y R 31 R 32 R 33 T z ] [ x w y w z w 1 ] (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_w \\ y_w \\ z_w \\1\\ \end{bmatrix}\tag{1} ⎣⎡xcyczc⎦⎤=⎣⎡R11R21R31R12R22R32R13R23R33TxTyTz⎦⎤⎣⎢⎢⎡xwywzw1⎦⎥⎥⎤(1)
z c [ u v 1 ] = [ f x α c x 0 f y c y 0 0 1 ] [ x c y c z c ] (2) z_c \begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix}=\begin{bmatrix} f_x & \alpha & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \\ \end{bmatrix}\begin{bmatrix} x_c \\ y_c \\ z_c \\ \end{bmatrix}\tag{2} zc⎣⎡uv1⎦⎤=⎣⎡fx00αfy0cxcy1⎦⎤⎣⎡xcyczc⎦⎤(2)
解超定方程组
根据式（1）能够得到
{ x c = R 11 x w + R 12 y w + R 13 z w + T x y c = R 21 x w + R 22 y w + R 23 z w + T y z c = R 31 x w + R 32 y w + R 33 z w + T z (3) \left\{ \begin{array}{c} x_c=R_{11}x_w+R_{12}y_w+R_{13}z_w+T_x \\ y_c=R_{21}x_w+R_{22}y_w+R_{23}z_w+T_y \\ z_c=R_{31}x_w+R_{32}y_w+R_{33}z_w+T_z \\ \end{array} \right.\tag{3} ⎩⎨⎧xc=R11xw+R12yw+R13zw+Txyc=R21xw+R22yw+R23zw+Tyzc=R31xw+R32yw+R33zw+Tz(3)
进一步地，
{ x c ′ = x c / z c = 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 = 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 (4) \left\{ \begin{array}{c} 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{array} \right.\tag{4} {xc′=xc/zc=R31xw+R32yw+R33zw+TzR11xw+R12yw+R13zw+Txyc′=yc/zc=R31xw+R32yw+R33zw+TzR21xw+R22yw+R23zw+Ty(4)
opencv的坐标经过了相机归一化平面的处理，也就是上式中 x c / z c , y c / z c x_c/z_c, y_c/z_c xc/zc,yc/zc的处理。已知左右相机上匹配的同名点 u 1 , v 1 u_1, v_1 u1,v1和 u 2 , v 2 u_2,v_2 u2,v2, 根据式 (2) 能够得到分别在左右相机下的坐标 ( x c 1 , y c 1 , z c 1 ) (x_{c1},y_{c1},z_{c1}) (xc1,yc1,zc1)和 ( x c 2 , y c 2 , z c 2 ) (x_{c2},y_{c2},z_{c2}) (xc2,yc2,zc2)。经过归一化处理后，可以知道在相机归一化平面上的坐标 ( x c 1 ′ , y c 1 ′ , 1 ) (x'_{c1},y'_{c1},1) (xc1′,yc1′,1)和 ( x c 2 ′ , y c 2 ′ , 1 ) (x'_{c2},y'_{c2},1) (xc2′,yc2′,1)。根据式(4)，分别列出左右相机的等式如下
{ x c 1 ′ = R 11 1 x w + R 12 1 y w + R 13 1 z w + T x 1 R 31 1 x w + R 32 1 y w + R 33 1 z w + T z 1 y c 1 ′ = R 21 1 x w + R 22 1 y w + R 23 1 z w + T y 1 R 31 1 x w + R 32 1 y w + R 33 1 z w + T z 1 (5) \left\{ \begin{array}{c} x'_{c1}=\frac{R^1_{11}x_w+R^1_{12}y_w+R^1_{13}z_w+T^1_x}{R^1_{31}x_w+R^1_{32}y_w+R^1_{33}z_w+T^1_z} \\ y'_{c1}=\frac{R^1_{21}x_w+R^1_{22}y_w+R^1_{23}z_w+T^1_y}{R^1_{31}x_w+R^1_{32}y_w+R^1_{33}z_w+T^1_z} \\ \end{array} \right.\tag{5} ⎩⎨⎧xc1′=R311xw+R321yw+R331zw+Tz1R111xw+R121yw+R131zw+Tx1yc1′=R311xw+R321yw+R331zw+Tz1R211xw+R221yw+R231zw+Ty1(5)
{ x c 2 ′ = R 11 2 x w + R 12 2 y w + R 13 2 z w + T x 2 R 31 2 x w + R 32 2 y w + R 33 2 z w + T z 2 y c 2 ′ = R 21 2 x w + R 22 2 y w + R 23 2 z w + T y 2 R 31 2 x w + R 32 2 y w + R 33 2 z w + T z 2 (6) \left\{ \begin{array}{c} x'_{c2}=\frac{R^2_{11}x_w+R^2_{12}y_w+R^2_{13}z_w+T^2_x}{R^2_{31}x_w+R^2_{32}y_w+R^2_{33}z_w+T^2_z} \\ y'_{c2}=\frac{R^2_{21}x_w+R^2_{22}y_w+R^2_{23}z_w+T^2_y}{R^2_{31}x_w+R^2_{32}y_w+R^2_{33}z_w+T^2_z} \\ \end{array} \right.\tag{6} ⎩⎨⎧xc2′=R312xw+R322yw+R332zw+Tz2R112xw+R122yw+R132zw+Tx2yc2′=R312xw+R322yw+R332zw+Tz2R212xw+R222yw+R232zw+Ty2(6)
联立式（5）和（6）可以看出，这是一个已知左右相机的点、已知相机之间位姿关系，求解三维点坐标 ( x w , y w , z w ) (x_w,y_w,z_w) (xw,yw,zw)的问题。因为未知数是3，方程组的数量是4，这是一个超定问题，opencv将其转换为“Ax=0"的矩阵形式，利用SVD分解解算未知数。根据式（5）和（6）分别得到
{ ( x c 1 ′ R 31 1 − R 11 1 ) x w + ( x c 1 ′ R 32 1 − R 12 1 ) y w + ( x c 1 ′ R 33 1 − R 13 1 ) z w + ( x c 1 ′ T z 1 − T x 1 ) = 0 ( y c 1 ′ R 31 1 − R 21 1 ) x w + ( y c 1 ′ R 32 1 − R 22 1 ) y w + ( y c 1 ′ R 33 1 − R 23 1 ) z w + ( y c 1 ′ T z 1 − T y 1 ) = 0 (7) \left\{ \begin{array}{c} (x'_{c1}R^1_{31}-R^1_{11})x_w+(x'_{c1}R^1_{32}-R^1_{12})y_w+(x'_{c1}R^1_{33}-R^1_{13})z_w +(x'_{c1}T^1_z-T^1_x)= 0 \\ (y'_{c1}R^1_{31}-R^1_{21})x_w+(y'_{c1}R^1_{32}-R^1_{22})y_w+(y'_{c1}R^1_{33}-R^1_{23})z_w +(y'_{c1}T^1_z-T^1_y)= 0\\ \end{array} \right.\tag{7} {(xc1′R311−R111)xw+(xc1′R321−R121)yw+(xc1′R331−R131)zw+(xc1′Tz1−Tx1)=0(yc1′R311−R211)xw+(yc1′R321−R221)yw+(yc1′R331−R231)zw+(yc1′Tz1−Ty1)=0(7)
{ ( x c 2 ′ R 31 2 − R 11 2 ) x w + ( x c 2 ′ R 32 2 − R 12 2 ) y w + ( x c 2 ′ R 33 2 − R 13 2 ) z w + ( x c 2 ′ T z 2 − T x 2 ) = 0 ( y c 2 ′ R 31 2 − R 21 2 ) x w + ( y c 2 ′ R 32 2 − R 22 2 ) y w + ( y c 2 ′ R 33 2 − R 23 2 ) z w + ( y c 2 ′ T z 2 − T y 2 ) = 0 (8) \left\{ \begin{array}{c} (x'_{c2}R^2_{31}-R^2_{11})x_w+(x'_{c2}R^2_{32}-R^2_{12})y_w+(x'_{c2}R^2_{33}-R^2_{13})z_w +(x'_{c2}T^2_z-T^2_x)= 0 \\ (y'_{c2}R^2_{31}-R^2_{21})x_w+(y'_{c2}R^2_{32}-R^2_{22})y_w+(y'_{c2}R^2_{33}-R^2_{23})z_w +(y'_{c2}T^2_z-T^2_y)= 0 \\ \end{array} \right.\tag{8} {(xc2′R312−R112)xw+(xc2′R322−R122)yw+(xc2′R332−R132)zw+(xc2′Tz2−Tx2)=0(yc2′R312−R212)xw+(yc2′R322−R222)yw+(yc2′R332−R232)zw+(yc2′Tz2−Ty2)=0(8)
转换为矩阵形式为
[ x c 1 ′ R 31 1 − R 11 1 x c 1 ′ R 32 1 − R 12 1 x c 1 ′ R 33 1 − R 13 1 x c 1 ′ T z 1 − T x 1 y c 1 ′ R 31 1 − R 21 1 y c 1 ′ R 32 1 − R 22 1 y c 1 ′ R 33 1 − R 23 1 y c 1 ′ T z 1 − T y 1 x c 2 ′ R 31 2 − R 11 2 x c 2 ′ R 32 2 − R 12 2 x c 2 ′ R 33 2 − R 13 2 x c 2 ′ T z 2 − T x 2 y c 2 ′ R 31 2 − R 21 2 y c 2 ′ R 32 2 − R 22 2 y c 2 ′ R 33 2 − R 23 2 y c 2 ′ T z 2 − T y 2 ] [ x w y w z w 1 ] = [ 0 0 0 0 ] (9) \begin{bmatrix} x'_{c1}R^1_{31}-R^1_{11} & x'_{c1}R^1_{32}-R^1_{12} &x'_{c1}R^1_{33}-R^1_{13} & x'_{c1}T^1_z-T^1_x \\ y'_{c1}R^1_{31}-R^1_{21} & y'_{c1}R^1_{32}-R^1_{22} & y'_{c1}R^1_{33}-R^1_{23} & y'_{c1}T^1_z-T^1_y \\ x'_{c2}R^2_{31}-R^2_{11} & x'_{c2}R^2_{32}-R^2_{12} & x'_{c2}R^2_{33}-R^2_{13} & x'_{c2}T^2_z-T^2_x \\ y'_{c2}R^2_{31}-R^2_{21} & y'_{c2}R^2_{32}-R^2_{22} & y'_{c2}R^2_{33}-R^2_{23} & y'_{c2}T^2_z-T^2_y \\ \end{bmatrix} \begin{bmatrix} x_w \\ y_w \\ z_w \\ 1 \\ \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ \end{bmatrix} \tag{9} ⎣⎢⎢⎡xc1′R311−R111yc1′R311−R211xc2′R312−R112yc2′R312−R212xc1′R321−R121yc1′R321−R221xc2′R322−R122yc2′R322−R222xc1′R331−R131yc1′R331−R231xc2′R332−R132yc2′R332−R232xc1′Tz1−Tx1yc1′Tz1−Ty1xc2′Tz2−Tx2yc2′Tz2−Ty2⎦⎥⎥⎤⎣⎢⎢⎡xwywzw1⎦⎥⎥⎤=⎣⎢⎢⎡0000⎦⎥⎥⎤(9)
该式子就是形如 A X = 0 AX=0 AX=0，其中，
A = [ x c 1 ′ R 31 1 − R 11 1 x c 1 ′ R 32 1 − R 12 1 x c 1 ′ R 33 1 − R 13 1 x c 1 ′ T z 1 − T x 1 y c 1 ′ R 31 1 − R 21 1 y c 1 ′ R 32 1 − R 22 1 y c 1 ′ R 33 1 − R 23 1 y c 1 ′ T z 1 − T y 1 x c 2 ′ R 31 2 − R 11 2 x c 2 ′ R 32 2 − R 12 2 x c 2 ′ R 33 2 − R 13 2 x c 2 ′ T z 2 − T x 2 y c 2 ′ R 31 2 − R 21 2 y c 2 ′ R 32 2 − R 22 2 y c 2 ′ R 33 2 − R 23 2 y c 2 ′ T z 2 − T y 2 ] (10) A = \begin{bmatrix} x'_{c1}R^1_{31}-R^1_{11} & x'_{c1}R^1_{32}-R^1_{12} &x'_{c1}R^1_{33}-R^1_{13} & x'_{c1}T^1_z-T^1_x \\ y'_{c1}R^1_{31}-R^1_{21} & y'_{c1}R^1_{32}-R^1_{22} & y'_{c1}R^1_{33}-R^1_{23} & y'_{c1}T^1_z-T^1_y \\ x'_{c2}R^2_{31}-R^2_{11} & x'_{c2}R^2_{32}-R^2_{12} & x'_{c2}R^2_{33}-R^2_{13} & x'_{c2}T^2_z-T^2_x \\ y'_{c2}R^2_{31}-R^2_{21} & y'_{c2}R^2_{32}-R^2_{22} & y'_{c2}R^2_{33}-R^2_{23} & y'_{c2}T^2_z-T^2_y \\ \end{bmatrix} \tag{10} A=⎣⎢⎢⎡xc1′R311−R111yc1′R311−R211xc2′R312−R112yc2′R312−R212xc1′R321−R121yc1′R321−R221xc2′R322−R122yc2′R322−R222xc1′R331−R131yc1′R331−R231xc2′R332−R132yc2′R332−R232xc1′Tz1−Tx1yc1′Tz1−Ty1xc2′Tz2−Tx2yc2′Tz2−Ty2⎦⎥⎥⎤(10)
对该系数矩阵进行SVD分解， V V V矩阵的最后一列 [ X , Y , Z , W ] [X,Y,Z,W] [X,Y,Z,W]就是方程的解，但是SVD的解是一个模为1的特征向量，最终的三维坐标需要将特征向量的前三个除以最后一个[X/W,Y/W,Z/W,1], 即三维坐标为 ( x w , y w , z w ) = ( X / W , Y / W , Z / W ) (x_w,y_w,z_w)=(X/W,Y/W,Z/W) (xw,yw,zw)=(X/W,Y/W,Z/W)
opencv源代码注释
附上opencv三角测量函数的主要代码和注释
cvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, CvMat* projPoints1, CvMat* projPoints2, CvMat* points4D)
{
// preallocate SVD matrices on stack
cv::Matx<double, 4, 4> matrA;
cv::Matx<double, 4, 4> matrU;
cv::Matx<double, 4, 1> matrW;
cv::Matx<double, 4, 4> matrV;
CvMat* projPoints[2] = {projPoints1, projPoints2};
CvMat* projMatrs[2] = {projMatr1, projMatr2};
/* Solve system for each point */
for( int i = 0; i < numPoints; i++ )/* For each point */
{
/* Fill matrix for current point */
for( int j = 0; j < 2; j++ )/* For each view */
{
double x,y;
x = cvmGet(projPoints[j],0,i);
y = cvmGet(projPoints[j],1,i);
// 注1：构造系数矩阵A
for( int k = 0; k < 4; k++ )
{
matrA(j*2+0, k) = x * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],0,k);
matrA(j*2+1, k) = y * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],1,k);
}
}
// 注2：SVD分解A矩阵
/* Solve system for current point */
cv::SVD::compute(matrA, matrW, matrU, matrV);
/* Copy computed point */
cvmSet(points4D,0,i,matrV(3,0));/* X */
cvmSet(points4D,1,i,matrV(3,1));/* Y */
cvmSet(points4D,2,i,matrV(3,2));/* Z */
cvmSet(points4D,3,i,matrV(3,3));/* W */
}
}