第七章 相机坐标系中的Zc值求解(车道线感知)

@[TOC](第七章 相机坐标系中的Zc值求解(车道线感知))

一 前言

近期参与到了手写AI的车道线检测的学习中去，以此系列笔记记录学习与思考的全过程。车道线检测系列会持续更新，力求完整精炼，引人启示。所需前期知识，可以结合手写AI进行系统的学习。

二 推导$Z_c$

像素坐标转到世界坐标时相机坐标系中的Zc值求解
坐标转换参考：https://blog.csdn.net/weixin_44302770/article/details/133024739

求解具体过程：
前提是世界坐标的原点在地面上
上述公式可以简写为：
Z c [ u v 1 ] = K ( R [ X w Y w Z w ] + T ) Z_c\begin{bmatrix}u\\v\\1\end{bmatrix}=K(R\begin{bmatrix}X_w\\Y_w\\Z_w\end{bmatrix}+T) Zc​ ​uv1​ ​=K(R ​Xw​Yw​Zw​​ ​+T)
进一步：
[ X w Y w Z w ] = R − 1 ( K − 1 Z c [ u v 1 ] − T ) = R − 1 K − 1 Z c [ u v 1 ] − R − 1 T \left.\begin{bmatrix}X_w\\Y_w\\Z_w\end{bmatrix}=R^{-1}\left(K^{-1}Z_c\begin{bmatrix}u\\v\\1\end{bmatrix}\right.-T\right)=R^{-1}K^{-1}Z_c\begin{bmatrix}u\\v\\1\end{bmatrix}-R^{-1}T ​Xw​Yw​Zw​​ ​=R−1 ​K−1Zc​ ​uv1​ ​−T ​=R−1K−1Zc​ ​uv1​ ​−R−1T
其中 R，K，T均为已知值。将上述等式中的变量做以下改写，方便描述：
$$Mat1=R^{-1}{K}^{-1}\left[\begin{array}{c}u\\ v\\ 1\end{array}\right],Mat2=R^{-1}T$$
只观察世界坐标计算公式中等式两边的第三项，则：
$$\begin{gathered} Z_w=Z_c\ast Mat1(2,0)-Mat2(2,0) \\ {Z}_c=(Z_w+Mat2(2,0))/Mat1(2,0) \end{gathered}$$
其中，Mat2(2,0)表示3×1矩阵的第三项。如果目标物体在地面则Zw=0；如果目标物体有一定高度，则Zw=实际物体高度，求出Zc后即可根据像素坐标求得世界坐标。

三 代码实现

def 2Dto3Dpts(point2D, rVec, tVec, cameraMat, height):
    """
       Function used to convert given 2D points back to real-world 3D points    
       point2D  : An array of 2D points
       rVec     : Rotation vector
       tVec     : Translation vector
       cameraMat: Camera Matrix used in solvePnP
       height   : Height in real-world 3D space    
       Return   : output_array: Output array of 3D points      
             
    """
    point3D = []
    point2D = (np.array(point2D, dtype='float32')).reshape(-1, 2)
    numPts = point2D.shape[0]
    point2D_op = np.hstack((point2D, np.ones((numPts, 1))))
    rMat = cv2.Rodrigues(rVec)[0]
    rMat_inv = np.linalg.inv(rMat)
    kMat_inv = np.linalg.inv(cameraMat)
    for point in range(numPts):
        uvPoint = point2D_op[point, :].reshape(3, 1)
        tempMat = np.matmul(rMat_inv, kMat_inv)
        tempMat1 = np.matmul(tempMat, uvPoint)
        tempMat2 = np.matmul(rMat_inv, tVec)
        s = (height + tempMat2[2]) / tempMat1[2]
        p = tempMat1 * s - tempMat2
        point3D.append(p)

    point3D = (np.array(point3D, dtype='float32')).reshape([-1, 1, 3])
    return point3D