A module for operations on camera calibration parameters

calibration.py

import numpy as np
import numpy.typing as np_typing


Vector = np_typing.NDArray[np.float64]
Matrix = np_typing.NDArray[np.float64]


def extrinsics_to_rotmat_and_posvec(E: Matrix) -> tuple[Matrix, Vector]:
    R = E[:, :3]
    return R, -R.T @ E[:, 3]


def rotmat_and_posvec_to_extrinsics(R: Matrix, t: Vector) -> Matrix:
    return np.hstack((R, -R @ t[..., None]))


def projection_to_intrinsics_and_extrinsics(P: Matrix) -> tuple[Matrix, Matrix, Vector]:
    """It decomposes a camera projection matrix into intrinsics and extrinsics
    
    The code is inspired by P. Sturm's lecture notes,
    https://hal.inria.fr/cel-02129241/file/poly_3D_eng.pdf, Section 2.2
    
    Parameters
    ----------
    P
        The 3-by-4 camera projection matrix

    Returns
    -------
    K
        The 3-by-3 camera intrinsics
    R
        The 3-by-4 camera rotation matrix
    t
        The 3-by-1 camera position vector
    """
    M = P[0:3, 0:3]  # P bar
    Q = np.eye(3)[::-1]

    M_M_trs = Q @ M @ M.T @ Q
    B = Q @ np.linalg.cholesky(M_M_trs) @ Q

    K = B / B[2, 2]
    K_inv = np.linalg.pinv(K)

    A = K_inv @ M

    det_A = np.linalg.det(A)
    s = (1 / det_A)**(1/3)  # lambda

    R = s * A
    t = s * (-R.T @ K_inv @ P[:3, 3])

    return K, R, t


def intrinsics_and_extrinsics_to_projection(K: Matrix, R: Matrix, t: Vector) -> Matrix:
    E = rotmat_and_posvec_to_extrinsics(R, t)
    return K @ E

test_calibration.py

import pytest

from calibration import (
    Vector, 
    Matrix, 
    extrinsics_to_rotmat_and_posvec, 
    rotmat_and_posvec_to_extrinsics, 
    projection_to_intrinsics_and_extrinsics,
    intrinsics_and_extrinsics_to_projection
)


@pytest.fixture()
def K() -> Matrix:
    """The 3-by-4 camera intrinsics"""
    return np.array([
        [ 6.310000,  0.000000,  3.840000],
        [ 0.000000,  6.310000,  2.880000],
        [ 0.000000,  0.000000,  1.000000]
    ])


@pytest.fixture()
def R() -> Matrix:
    """The 3-by-3 camera rotation matrix"""
    return np.array([
        [-0.301649,  0.682824, -0.665401],
        [-0.634173,  0.377434,  0.674809],
        [ 0.711921,  0.625535,  0.319176]
    ])


@pytest.fixture()
def t() -> Vector:
    """The 3-by-1 camera position vector"""
    return np.array([3.750824, -1.180895,  1.061387])


@pytest.fixture()
def E() -> Vector:
    """The 3-by-4 camera extrinsics"""
    return np.array([
        [-0.301649,  0.682824, -0.665401,  2.644023],
        [-0.634173,  0.377434,  0.674809,  2.108147],
        [ 0.711921,  0.625535,  0.319176, -2.270368]
    ])


@pytest.fixture()
def P() -> Vector:
    """The 3-by-4 camera projection matrix"""
    return np.array([
        [ 0.830371,  6.710673, -2.973044,  7.965574]
        [-1.951299,  4.183149,  5.177271,  6.763750]
        [ 0.711921,  0.625535,  0.319176, -2.270368]
    ])


def test_rotmat_and_posvec_to_extrinsics(R: Matrix, t: Vector, E: Matrix) -> None:
    E_hat = rotmat_and_posvec_to_extrinsics(R, t)
    assert np.allclose(E, E_hat, atol=1e-6)
    
    
def test_extrinsics_to_rotmat_and_posvec(E: Matrix, R: Matrix, t: Vector) -> None:
    R_hat, t_hat = extrinsics_to_rotmat_and_posvec(E)
    
    assert np.allclose(R, R_hat, atol=1e-6)
    assert np.allclose(t, t_hat, atol=1e-6)
    
    
def test_projection_to_intrinsics_and_extrinsics(P: Matrix, K: Matrix, R: Matrix, t: Vector) -> None:
    K_hat, R_hat, t_hat = projection_to_intrinsics_and_extrinsics(P)

    assert np.allclose(K, K_hat, atol=1e-6)
    assert np.allclose(R, R_hat, atol=1e-6)
    assert np.allclose(t, t_hat, atol=1e-6)
    
    
def test_intrinsics_and_extrinsics_to_projection(K: Matrix, R: Matrix, t: Vector, P: Matrix) -> None:
    P_hat = intrinsics_and_extrinsics_to_projection(K, R, t)
    assert np.allclose(P, P_hat, atol=1e-6)