Skip to main content
Computational Science

Return to Question

added 2456 characters in body
Source Link
Gulzar
Gulzar
  • 121
  • 4
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.

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
```

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.

Source Link
Gulzar
Gulzar
  • 121
  • 4

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

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
```