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

@

目录

• 一 前言
• 二 推导$Z_c$
• 三 代码实现

一 前言

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

二 推导$Z_c$

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

求解具体过程：
前提是世界坐标的原点在地面上
上述公式可以简写为：
$$Z_c\begin{bmatrix}u\v\1\end{bmatrix}=K(R\begin{bmatrix}X_w\Y_w\Z_w\end{bmatrix}+T)$$
进一步：
$$\left.\begin{bmatrix}X_w\Y_w\Z_w\end{bmatrix}=R^{-1}\left(KZ_c\begin{bmatrix}u\v\1\end{bmatrix}\right.-T\right)=R^{-1}KZ_c\begin{bmatrix}u\v\1\end{bmatrix}-R^{-1}T$$
其中 R，K，T均为已知值。将上述等式中的变量做以下改写，方便描述：
$$Mat1=R^{-1}K\Big[\begin{matrix}u\v\1\end{matrix}\Big],Mat2=R^{-1}T$$
只观察世界坐标计算公式中等式两边的第三项，则：
$$Z_w=Z_c*Mat1(2,0)-Mat2(2,0)\\Z_c=(Z_w+Mat2(2,0))/Mat1(2,0)$$
其中，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