OpenCV camera coordinates to OpenGL NDC matrix

cv2ndc.py

import numpy as np


def cv_to_ndc(fx, fy, cx, cy, near, far, w, h):
    """Returns the 4x4 projection matrix that converts from OpenCV camera
    coordinates to OpenGL NDC coordinates.

    This takes into account that cameras in
        - OpenCV has +z into scene, +y down the image
        - OpenGL has -z into scene, +y up the image

    See:
        - OpenGL (cam and NDC)
            https://www.songho.ca/opengl/files/gl_projectionmatrix01.png
        - OpenCV
            https://docs.opencv.org/3.4/pinhole_camera_model.png
    Params
        fx: focal length in x [px]
        fy: focal length in y [px]
        cx: principal point in x [px]
        cy: principal point in y [px]
        near: near plane [m]
        far: far plane [m]
        width: width of image associated with +x [px]
        height: width of image associated with +y [px]

    Returns
        m: (4,4) matrix M such that x' = Mx are normalized device coordinates [-1,1]
            after division by x'[3].
    """
    fm = far - near
    glm = np.array(
        [
            [2 * fx / w, 0.0, (w - 2 * cx) / w, 0.0],
            [0.0, -2 * fy / h, (h - 2 * cy) / h, 0.0],
            [0.0, 0.0, (-far - near) / fm, -2.0 * far * near / fm],
            [0.0, 0.0, -1.0, 0.0],
        ]
    )
    glm = glm @ np.diag([1.0, 1.0, -1.0, 1.0])  # rotatex(pi) and flip y
    return glm


def test_cv_to_ndc():
    # run with pytest cv2ndc.py

    alpha = np.radians(60)
    width, height = 320, 200
    fx = width / (2 * np.tan(alpha / 2))
    fy = fx
    cx = width * 0.5  # doesn't account for sub-pixel acc
    cy = height * 0.5
    near = 2.0
    far = 10.0

    M = cv_to_ndc(fx, fy, cx, cy, near, far, width, height)

    # test points in cv cam coords
    x = np.array(
        [
            [-width / (2 * fx), height / (2 * fy), 1],
            [width / (2 * fx), -height / (2 * fy), 1],
            [0, 0, 1],
        ]
    )

    def hom(x):
        return np.concatenate((x, np.ones((len(x), 1), dtype=x.dtype)), -1)

    def dehom(x):
        return x[..., :-1] / x[..., -1:]

    # near plane (note, M.T comes from (M @ x.T).T is equiv. to (x @ M.T)
    q = dehom(hom(x * near) @ M.T)
    np.testing.assert_allclose(q, np.array([[-1, -1, -1], [1, 1, -1], [0, 0, -1]]))

    # far plane
    q = dehom(hom(x * far) @ M.T)
    np.testing.assert_allclose(q, np.array([[-1, -1, 1], [1, 1, 1], [0, 0, 1]]))