OpenCV三角测量重建triangulatePoints原理解析

leaf_csdn

已于 2022-04-19 10:48:33 修改

阅读量1.1w

点赞数 19

分类专栏：结构光三维重建文章标签： c++ 计算机视觉算法

于 2022-04-19 10:29:16 首次发布

本文链接：https://blog.csdn.net/weixin_43956164/article/details/124266267

版权

结构光三维重建专栏收录该内容

16 篇文章 47 订阅

订阅专栏

共线方程

双相机立体重建时，利用物体点-光心-像点三者共线的原理，在找到左右匹配的像点并且完成去畸变之后（OpenCV去畸变undistortPoints原理解析），就能够在三维空间中形成两条直线，物体点就是这两个直线的“交点”，“交点”指最小二乘交点，因为这两个直线由于偏差通常不会交于一点。共线方程描述了物体点-光心-像点三者共线的含义，也相当于三维点到二维点的投影过程，该投影过程可表达为
$\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}$

$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}$

解超定方程组

根据式（1）能够得到

$\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}$
进一步地，
$\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}$
opencv的坐标经过了相机归一化平面的处理，也就是上式中 $x_c/z_c, y_c/z_c$ 的处理。已知左右相机上匹配的同名点 $u_1, v_1$ 和 $u_2,v_2$ , 根据式 (2) 能够得到分别在左右相机下的坐标 $x_{c1},y_{c1},z_{c1})$ 和 $x_{c2},y_{c2},z_{c2})$ 。经过归一化处理后，可以知道在相机归一化平面上的坐标 $x'_{c1},y'_{c1},1)$ 和 $x'_{c2},y'_{c2},1)$ 。根据式(4)，分别列出左右相机的等式如下
$\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}$

$\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}$
联立式（5）和（6）可以看出，这是一个已知左右相机的点、已知相机之间位姿关系，求解三维点坐标 $x_w,y_w,z_w)$ 的问题。因为未知数是3，方程组的数量是4，这是一个超定问题，opencv将其转换为“Ax=0"的矩阵形式，利用SVD分解解算未知数。根据式（5）和（6）分别得到
$\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}$
$\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}$
转换为矩阵形式为
$\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}$
该式子就是形如 $A X = 0$ ，其中，
$\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}$
对该系数矩阵进行SVD分解， $V$ 矩阵的最后一列 $[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)$

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 */
    }
}