imgPointToWorldCoord.py 文件源码

def imgPointToWorldCoord(XY, rvec,
                         tvec, cameraMatrix, zconst=0):
    '''
    @returns 3d object coords

    :param (ix,iy): list of 2d points (x,y) in image
    :param zconst: height above image plane (if 0, than on image plane)

    http://answers.opencv.org/question/62779/image-coordinate-to-world-coordinate-opencv/
    and
    http://stackoverflow.com/questions/12299870/computing-x-y-coordinate-3d-from-image-point
    '''
    (ix, iy) = XY
    uvPoint = np.array([ix.ravel(), iy.ravel(), np.ones(
        shape=ix.size)]).reshape(3, ix.size)

    R = cv2.Rodrigues(rvec)[0]

    iR = inv(R)
    iC = inv(cameraMatrix)

    t = iR.dot(iC).dot(uvPoint)
    t2 = iR.dot(tvec)
    s = (zconst + t2[2]) / t[2]

    objP = (iR.dot(s * iC.dot(uvPoint) - tvec))
    return objP