图像坐标逆变换

PowerDi123

于 2023-03-18 23:34:13 发布

阅读量170

点赞数

文章标签： python 机器学习 numpy

本文链接：https://blog.csdn.net/tengxunbc/article/details/129637784

版权

文章介绍了如何将图像坐标转换为世界坐标的过程，涉及到了相机内参矩阵K、旋转矩阵R、平移向量T的逆变换，以及在给定高度的情况下计算物体在世界坐标系中的位置。提供的Python代码示例展示了如何使用OpenCV的Rodrigues函数和线性代数操作实现这一转换。

摘要由CSDN通过智能技术生成

一个熟悉的世界坐标转到图像坐标系的公式：
$Z_{c} \begin{bmatrix}u \\v \\1\end{bmatrix} = \begin{bmatrix} \frac{1}{\mathrm{d} x} & 0 & u_{0} \\ 0 & \frac{1}{\mathrm{d} y} & v_{0}\\ 0 & 0 &1\end{bmatrix}\begin{bmatrix} f & 0 & 0 & 0\\ 0 & f & 0 & 0\\ 0& 0& 1&0\end{bmatrix}\begin{bmatrix} R & T\\ 0 & 1\end{bmatrix}\begin{bmatrix} X_{W} \\ Y_{W}\\ Z_{W}\\1\end{bmatrix}=\begin{bmatrix} f & 0 & U_{0} & 0\\ 0 & f & V_{0} & 0\\ 0& 0& 1&0\end{bmatrix}\begin{bmatrix} R & T\\ 0 & 1\end{bmatrix}\begin{bmatrix} X_{W} \\ Y_{W}\\ Z_{W}\\1\end{bmatrix}$

图像逆变换的前提：定义的世界坐标系的原点在地面上。

上述公式可以简写为：
$Z_{c} \begin{bmatrix}u \\v \\1\end{bmatrix} = K(R\begin{bmatrix} X_{W} \\ Y_{W}\\ Z_{W}\end{bmatrix}+T)$
进一步
$\begin{bmatrix} X_{W} \\ Y_{W}\\ Z_{W}\end{bmatrix}=R^{-1}\left ( K^{-1}Z_{c}\begin{bmatrix} u\\ v\\ 1\end{bmatrix}-T \right ) = R^{-1} K^{-1}Z_{c}\begin{bmatrix} u\\ v\\ 1\end{bmatrix}-R^{-1}T$
其中R，K，T均为已知值。将上述等式中的变量改写一下，
$R^{-1}k^{-1}\begin{bmatrix} u\\ v\\1\end{bmatrix}$ , $Mat2 = R^{-1}T$

我们只需要关注图像逆变换公式中等式两边的第三项，则
$Z_{w}=Z_{c}*Mat1(2,0)-Mat2(2,0)$
$Z_{c}=(Z_{w}+Mat1(2,0))/Mat1(2,0)$

其中，Mat2(2,0)表示3x1矩阵的第三项。如果目标物体在世界坐标系的水平面上则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