3DComputerVision/optimisation.py at main · Vlhermitte/3DComputerVision · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
import numpy as np
import tools as tb
from scipy.optimize import fmin
from common import Camera

def minimize_sampson(Rt_params, u1, u2, R, t, K) -> float:
    assert Rt_params.shape == (6,), f"Rt_params shape is {Rt_params.shape}, expected (6,)"
    if u1.shape[0] != 3 or u2.shape[0] != 3:
        u1, u2 = tb.e2p(u1), tb.e2p(u2)

    R_params, t_params = Rt_params[:3], Rt_params[3:]

    R_new = R @ rodrigues(R_params)
    t_new = t + t_params.reshape(3, 1)
    K_inv = np.linalg.inv(K)

    F = K_inv.T @ tb.sqc(-t_new) @ R_new @ K_inv

    return np.sum(tb.err_F_sampson(F, u1, u2))

def minimize_reproj(Rt_params, X, u, R_rans, t_rans, K) -> float:
    assert Rt_params.shape == (6,), f"Rt_params shape is {Rt_params.shape}, expected (6,)"
    if X.shape[0] == 3:
        X = tb.e2p(X)
    if u.shape[0] == 2:
        u = tb.e2p(u)
    R = R_rans @ rodrigues(Rt_params[0:3])
    t = t_rans + Rt_params[3:].reshape(3, 1)
    P = K @ np.hstack([R, t])
    X_proj = P @ X
    X_proj /= X_proj[2, :]
    e_reprj = np.linalg.norm(X_proj - u, axis=0)
    return np.sum(e_reprj)

def rodrigues(axis_angle) -> np.ndarray:
    """
    Convert the axis-angle representation to a rotation matrix using Rodrigues' formula.
    :param axis_angle: Axis-angle representation
    :return: Rotation matrix (3x3)
    """
    assert len(axis_angle) == 3, f"Axis_angle length is {len(axis_angle)}, expected 3"
    theta = np.linalg.norm(axis_angle)
    if theta == 0:
        return np.eye(3)
    u = axis_angle / theta
    K = tb.sqc(u)
    R = np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * K @ K
    return R

def optimise_extrinsic_reproj(
        camera: Camera,
        X: np.ndarray,
        u: np.ndarray,) -> (np.ndarray, np.ndarray):
    """
    Optimise the extrinsic parameters of the second camera given the first camera using the reprojection error method.
    :param camera: Camera
    :param R: Initial rotation matrix (3x3)
    :param t: Initial translation vector (3x1)
    :param X: 3D points
    :param u: image 2D points
    :return: Optimised rotation matrix and translation vector
    """
    K = camera.K
    R = camera.R
    t = camera.t

    Rt_params = np.zeros(6)
    opt_Rt_params = fmin(minimize_reproj, Rt_params, (X, u, R, t, K))

    best_Rp, best_tp = opt_Rt_params[:3], opt_Rt_params[3:]

    R_opt = R @ rodrigues(best_Rp)
    t_opt = t + best_tp.reshape(3, 1)

    return R_opt, t_opt

def optimise_extrinsic_sampson(
        camera1: Camera,
        camera2: Camera,
        inliers: np.ndarray,
        correspondences: np.ndarray,) -> (np.ndarray, np.ndarray):
    """
    Optimise the extrinsic parameters of the second camera given the first camera using sampson error.
    :param camera1: First camera
    :param camera2: Second camera
    :param R: Initial rotation matrix (3x3)
    :param t: Initial translation vector (3x1)
    :param inliers: Inliers
    :param correspondences: Correspondences
    :param method: Method to use for optimisation (reproj or sampson)
    :return: Optimised rotation matrix and translation vector
    """

    # Extract the inliers
    corresp = correspondences[camera1.id][camera2.id]
    u1 = camera1.features[:, corresp[inliers, 0]]
    u2 = camera2.features[:, corresp[inliers, 1]]

    # Get the camera matrix (assuming the camera matrix is the same for all cameras)
    K = camera2.K
    R = camera2.R
    t = camera2.t

    # Looking for an R_opt such that R_opt = R @ R_r and t_opt = t + t_i
    Rt_params = np.zeros(6)
    opt_Rt_params = fmin(minimize_sampson, Rt_params, (u1, u2, R, t, K))

    best_Rp, best_tp = opt_Rt_params[:3], opt_Rt_params[3:]

    R_opt = R @ rodrigues(best_Rp)
    t_opt = t + best_tp.reshape(3, 1)

    return R_opt, t_opt