像素坐标转到世界坐标时相机坐标系中的Zc值求解

        世界坐标系、相机坐标系、图像坐标系、像素坐标系之间的转换公式参考：世界坐标系、相机坐标系、图像坐标系、像素坐标系之间的转换

        其中图像坐标到世界坐标的转化公式作者讲解的也比较清楚，但是对于Zc的值，作者并没有给出进一步讲解

        自己项目中有涉及到图像坐标到世界坐标的转化，故这里想写出来供大家参考，如有理解有误，欢迎指正。这里的前提是世界坐标的原点在地面上。

        上述公式可以简写为：

        进一步：

        其中 R，K，T均为已知值。将上述等式中的变量做以下改写，方便描述：

        只观察世界坐标计算公式中等式两边的第三项，则：

        其中，Mat2(2,0)表示3×1矩阵的第三项。如果目标物体在地面则Zw=0；如果目标物体有一定高度，则Zw=实际物体高度，求出Zc后即可根据像素坐标求得世界坐标。

       最后贴上2D转3D的计算代码供大家理解：

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