How to find fundamental matrix based on other fundamental matrix and camera movement?-CodePudding

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

CodePudding user response：

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.

Analysis of the accuracy of the approach

The precision provided appear to be similar than the original one. Regarding the input matrix, results are sometime more accurate and sometimes less accurate than the Numpy method. This is specifically due to the computation of the determinant. With 24-digit decimals, there is no visible error compared to the reliable result of Wolphram Alpha. This show that the method is correct, results as not the same due to numerical stability details. Here is the code used to test the accuracy of the methods:

# Imports

from decimal import Decimal
import numba as nb

# Definitions

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(float64[:,::1])')
def det_4x4_numba(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))


# Example matrix

precise_xy = np.array(
     [[Decimal('42'),Decimal('-6248'),Decimal('4060'),Decimal('845')],
      [Decimal('-0.00992'),Decimal('-0.704'),Decimal('-0.71173298417'),Decimal('300.532')],
      [Decimal('-8.94274'),Decimal('-7554.39'),Decimal('604.57'),Decimal('706282')],
      [Decimal('-0.0132'),Decimal('-0.2757'),Decimal('-0.961'),Decimal('247.65')]]
)

xy = precise_xy.astype(np.float64)

res_numpy = Decimal(np.linalg.det(xy))
res_numba = Decimal(det_4x4_numba(xy))
res_precise = det_4x4(precise_xy)

# The Wolphram Alpha expression used is:
# det({{42,-6248,4060,845},
#     {-0.00992,-0.704,-0.71173298417,300.532},
#     {-8.94274,-7554.39,604.57,706282},
#     {-0.0132,-0.2757,-0.961,247.65}})
res_wolframalpha = Decimal('-323312.2164828991329828243')

# The result got from Wolfram-Alpha have a 25-digit precision
# and is exactly the same than the one of det_4x4 using 24-digit decimals.
assert res_precise == res_wolframalpha

print(abs((res_numpy-res_precise)/res_precise))  # 1.7E-14
print(abs((res_numba-res_precise)/res_precise))  # 3.1E-14
# => Similar relative error (Numba slightly less accurate
#    but both are not close to the 1e-16 relative epsilon)