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

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

Please notice F is the matrix that holds (x.T)Fy=0 for pixel coordinates x, y, where x is in image "left" and y is in image "right", if x, y correspond to the same 3d object on the two images.

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 algorithmically (using significantly less computations, not just parallelizing, or optimizing code). 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

---

I managed to create a vectorized version of this

def fundamental_3x3_from_projections_vectorized(p_left_nx3x4: np.array, p_right_mx3x4: 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/

    assert p_left_nx3x4.shape[1:] == p_right_mx3x4.shape[1:] and p_left_nx3x4.shape[1:] == (3, 4)

    n = p_left_nx3x4.shape[0]
    m = p_right_mx3x4.shape[0]
    f_nxmx3x3 = np.empty((n, m, 3, 3), np.float)

    p1_nx3x4, p2_mx3x4 = p_left_nx3x4, p_right_mx3x4

    x_nx3x2x4 = np.empty((n, 3, 2, 4), dtype=np.float)
    x_nx3x2x4[:, 0, :, :] = np.stack([p1_nx3x4[:, 1, :], p1_nx3x4[:, 2, :]], axis=1)  # (n, 2, 4)
    x_nx3x2x4[:, 1, :, :] = np.stack([p1_nx3x4[:, 2, :], p1_nx3x4[:, 0, :]], axis=1)  # (n, 2, 4)
    x_nx3x2x4[:, 2, :, :] = np.stack([p1_nx3x4[:, 0, :], p1_nx3x4[:, 1, :]], axis=1)  # (n, 2, 4)

    y_mx3x2x4 = np.empty((m, 3, 2, 4), dtype=np.float)
    y_mx3x2x4[:, 0, :, :] = np.stack([p2_mx3x4[:, 1, :], p2_mx3x4[:, 2, :]], axis=1)  # (m, 2, 4)
    y_mx3x2x4[:, 1, :, :] = np.stack([p2_mx3x4[:, 2, :], p2_mx3x4[:, 0, :]], axis=1)  # (m, 2, 4)
    y_mx3x2x4[:, 2, :, :] = np.stack([p2_mx3x4[:, 0, :], p2_mx3x4[:, 1, :]], axis=1)  # (m, 2, 4)

    x_nx1x3x2x4 = x_nx3x2x4[:, np.newaxis, :, :, :]
    y_1xmx3x2x4 = y_mx3x2x4[np.newaxis, :, :, :, :]

    xtile_nxmx3x2x4 = np.broadcast_to(x_nx1x3x2x4, (n, m, 3, 2, 4))
    ytile_nxmx3x2x4 = np.broadcast_to(y_1xmx3x2x4, (n, m, 3, 2, 4))

    for i in range(3):
        for j in range(3):
            x_mesh_nxmx2x4 = xtile_nxmx3x2x4[:, :, j, :, :]
            y_mesh_nxmx2x4 = ytile_nxmx3x2x4[:, :, i, :, :]
            # TODO this concatenate can be done manually and avoid re-allocation 9 times
            xy_nxmx4x4 = np.concatenate([x_mesh_nxmx2x4, y_mesh_nxmx2x4], axis=2)
            xy_nmx4x4 = xy_nxmx4x4.reshape((n*m, 4, 4))
            det_nm = np.linalg.det(xy_nmx4x4)
            det_nxm = np.reshape(det_nm, (n, m))
            f_nxmx3x3[:, :, i, j] = det_nxm

    return f_nxmx3x3

It runs much faster.

I still would like to be able to calculate this incrementally.

Answer 1

I cannot help but suspect that the working function you've provided is less efficient than it can be, but I must also admit that I have only dabbled in computer vision myself. Here's a suggestion for how it might be improved.

As you've mentioned, the fundamental matrix $F$ is defined as the matrix that satisfies $x_{1}^{T} F x_{2} = 0$ for all points $x_{1}$ and $x_{2}$, in the first and second images, respectively, which correspond to the same point $z$ on the 3D object of interest. That is, for all $z$, $F$ satisfies $$z^{T} P_{1}^{T} F P_{2} z = 0.$$

A quadratic form is constantly $0$ if and only if its matrix is skew-symmetric (See here for justification). Therefore $F$ is the matrix such that $$P_{1}^{T} F P_{2} + P_{2}^{T} F^{T} P_{1} = 0.$$ Applying the vectorization operator to both sides of this equation and using the compatibility of vectorization with kronecker product (see here for justification), we have $$(P_{2}^{T} \otimes P_{1}^{T})\, \text{vec}(F) + (P_{1}^{T} \otimes P^{T}_{2}) \, \text{vec}(F^{T}) = 0.$$ Denote by $K$ the commutation matrix of $F$'s dimension. Then $$(P_{2}^{T} \otimes P_{1}^{T})\, \text{vec}(F) + (P_{1}^{T} \otimes P^{T}_{2}) \, K^{T} \, \text{vec}(F) = 0.$$

Finally, $F$ can be solved for by unvectorizing the solution to $$\left((P_{2}^{T} \otimes P_{1}^{T}) + (P_{1}^{T} \otimes P^{T}_{2}) \, K^{T} \right) \, \text{vec}(F) = 0,$$ which you can solve with scipy. Of course, the commutation matrix should be computed once and used throughout the for loop. I prefer this approach because it works with the camera matrices directly. I suspect it will be faster but cannot confirm.

Note also that the fundamental matrix between cameras $i$ and $j$ is the transpose of the fundamental matrix between cameras $j$ and $i$. This allows you to cut the number of fundamental matrices you are computing in half.

Answer 2

This is not what you asked for, but I'll point out the low hanging fruit in your implementation anyway. You have many for loops, which will be extremely slow in Python and can be easily sped up with minimal effort using Cython. In my experience the many vstacks will also be slow, since iirc numpy allocates a new contiguous array when concatenating. It would be better to allocate a single array containing all the stacked camera matrices, and then subselect the relevant xy matrix. This should be much faster than vstacking the individual matrices together.