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

傷城~ 2023-06-28 04:58 140阅读 0赞

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

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

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

上述公式可以简写为：  
![在这里插入图片描述][20210420093817438.png_pic_center]

进一步：

![在这里插入图片描述][20210420093911417.png_pic_center]

其中 R，K，T均为已知值。将上述等式中的变量做以下改写，方便描述：  
![在这里插入图片描述][20210420093926961.png_pic_center]

只观察世界坐标计算公式中等式两边的第三项，则：  
![在这里插入图片描述][20210420094108275.png_pic_center]  
![在这里插入图片描述][20210420094121978.png_pic_center]

其中，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

[Link 1]: https://blog.csdn.net/liubing8609/article/details/85339009
[20200107160022648.png]: https://img-blog.csdnimg.cn/20200107160022648.png
[20210420093817438.png_pic_center]: https://img-blog.csdnimg.cn/20210420093817438.png#pic_center
[20210420093911417.png_pic_center]: https://img-blog.csdnimg.cn/20210420093911417.png#pic_center
[20210420093926961.png_pic_center]: https://img-blog.csdnimg.cn/20210420093926961.png#pic_center
[20210420094108275.png_pic_center]: https://img-blog.csdnimg.cn/20210420094108275.png#pic_center
[20210420094121978.png_pic_center]: https://img-blog.csdnimg.cn/20210420094121978.png#pic_center