如何根据其他基本矩阵和相机运动找到基本矩阵？

Question

I am trying to speed up some multi-camera system that relies on calculation of fundamental matrices between each camera pair.

Please notice the following is pseudocode. @ means matrix multiplication, | means concatenation.

I have code to calculate F for each pair calculate_f(camera_matrix1_3x4, camera_matrix1_3x4) , and the naiive solution is

for c1 in cameras:
    for c2 in cameras:
        if c1 != c2:
            f = calculate_f(c1.proj_matrix, c2.proj_matrix)

This is slow, and I would like to speed it up. I have ~5000 cameras.

---

I have pre calculated all rotations and translations (in world coordinates) between every pair of cameras, and internal parameters k , such that for each camera c , it holds that c.matrix = c.k @ (c.rot | c.t)

Can I use the parameters r, t to help speed up following calculations for F ?

---

In mathematical form, for 3 different cameras c1 , c2 , c3 I have

f12=(c1.proj_matrix, c2.proj_matrix) , and I want f23=(c2.proj_matrix, c3.proj_matrix) , f13=(c1.proj_matrix, c3.proj_matrix) with some function f23, f13 = fast_f(f12, c1.r, c1.t, c2.r, c2.t, c3.r, c3.t) ?

---

A working function for calculating the fundamental matrix in numpy:

def fundamental_3x3_from_projections(p_left_3x4: np.array, p_right__3x4: np.array) -> np.array:
    # The following is based on OpenCv-contrib's c++ implementation.
    # see https://github.com/opencv/opencv_contrib/blob/master/modules/sfm/src/fundamental.cpp#L109
    # see https://sourishghosh.com/2016/fundamental-matrix-from-camera-matrices/
    # see https://answers.opencv.org/question/131017/how-do-i-compute-the-fundamental-matrix-from-2-projection-matrices/
    f_3x3 = np.zeros((3, 3))
    p1, p2 = p_left_3x4, p_right__3x4

    x = np.empty((3, 2, 4), dtype=np.float)
    x[0, :, :] = np.vstack([p1[1, :], p1[2, :]])
    x[1, :, :] = np.vstack([p1[2, :], p1[0, :]])
    x[2, :, :] = np.vstack([p1[0, :], p1[1, :]])

    y = np.empty((3, 2, 4), dtype=np.float)
    y[0, :, :] = np.vstack([p2[1, :], p2[2, :]])
    y[1, :, :] = np.vstack([p2[2, :], p2[0, :]])
    y[2, :, :] = np.vstack([p2[0, :], p2[1, :]])

    for i in range(3):
        for j in range(3):
            xy = np.vstack([x[j, :], y[i, :]])
            f_3x3[i, j] = np.linalg.det(xy)

    return f_3x3

Answer 1

Numpy is clearly not optimized for working on small matrices . The parsing of CPython input objects, internal checks and function calls introduce a significant overhead which is far bigger than the execution time need to perform the actual computation. Not to mention the creation of many temporary arrays is also expensive. One solution to solve this problem is to use Numba or Cython .

Moreover, the computation of the determinant can be optimized a lot since you know the exact size of the matrix and a part of the matrix does not always change. Indeed, using a basic algebraic expression for the 4x4 determinant help compilers to optimize a lot the overall computation thanks to the common sub-expression elimination (not performed by the CPython interpreter) and the removal of complex loops/conditionals in np.linalg.det .

Here is the resulting code:

import numba as nb

@nb.njit('float64(float64[:,::1])')
def det_4x4(mat):
    a, b, c, d = mat[0,0], mat[0,1], mat[0,2], mat[0,3]
    e, f, g, h = mat[1,0], mat[1,1], mat[1,2], mat[1,3]
    i, j, k, l = mat[2,0], mat[2,1], mat[2,2], mat[2,3]
    m, n, o, p = mat[3,0], mat[3,1], mat[3,2], mat[3,3]
    return a * (f * (k*p - l*o) + g * (l*n - j*p) + h * (j*o - k*n)) + \
            b * (e * (l*o - k*p) + g * (i*p - l*m) + h * (k*m - i*o)) + \
            c * (e * (j*p - l*n) + f * (l*m - i*p) + h * (i*n - j*m)) + \
            d * (e * (k*n - j*o) + f * (i*o - k*m) + g * (j*m - i*n))

@nb.njit('float64[:,::1](float64[:,::1], float64[:,::1])')
def fundamental_3x3_from_projections(p_left_3x4, p_right_3x4):
    f_3x3 = np.empty((3, 3))
    p1, p2 = p_left_3x4, p_right_3x4

    x = np.empty((3, 2, 4), dtype=np.float64)
    x[0, 0, :] = p1[1, :]
    x[0, 1, :] = p1[2, :]
    x[1, 0, :] = p1[2, :]
    x[1, 1, :] = p1[0, :]
    x[2, 0, :] = p1[0, :]
    x[2, 1, :] = p1[1, :]

    y = np.empty((3, 2, 4), dtype=np.float64)
    y[0, 0, :] = p2[1, :]
    y[0, 1, :] = p2[2, :]
    y[1, 0, :] = p2[2, :]
    y[1, 1, :] = p2[0, :]
    y[2, 0, :] = p2[0, :]
    y[2, 1, :] = p2[1, :]

    xy = np.empty((4, 4), dtype=np.float64)

    for i in range(3):
        xy[2:4, :] = y[i, :, :]
        for j in range(3):
            xy[0:2, :] = x[j, :, :]
            f_3x3[i, j] = det_4x4(xy)

    return f_3x3

This is 130 times faster on my machine (85.6 us VS 0.66 us).

You can speed up the process even more by a factor of two if the applied function is commutative (ie. f(c1, c2) == f(c2, c1) ). If so, you could compute only the upper part . It turns out that your function have some interesting property since f(c1, c2) == f(c2, c1).T appear to be always true. Another possible optimization is to run the loop in parallel .

With all these optimizations, the resulting program should be about 3 order of magnitude faster .