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
| def estimate_affine_matrix_3d22d(X, x): ''' Using Golden Standard Algorithm for estimating an affine camera matrix P from world to image correspondences. See Alg.7.2. in MVGCV Code Ref: https://github.com/patrikhuber/eos/blob/master/include/eos/fitting/affine_camera_estimation.hpp x_homo = X_homo.dot(P_Affine) Args: X: [n, 3]. corresponding 3d points(fixed) x: [n, 2]. n>=4. 2d points(moving). x = PX Returns: P_Affine: [3, 4]. Affine camera matrix ''' X = X.T; x = x.T assert(x.shape[1] == X.shape[1]) n = x.shape[1] assert(n >= 4)
mean = np.mean(x, 1) x = x - np.tile(mean[:, np.newaxis], [1, n]) average_norm = np.mean(np.sqrt(np.sum(x**2, 0))) scale = np.sqrt(2) / average_norm x = scale * x
T = np.zeros((3,3), dtype = np.float32) T[0, 0] = T[1, 1] = scale T[:2, 2] = -mean*scale T[2, 2] = 1
X_homo = np.vstack((X, np.ones((1, n)))) mean = np.mean(X, 1) X = X - np.tile(mean[:, np.newaxis], [1, n]) m = X_homo[:3,:] - X average_norm = np.mean(np.sqrt(np.sum(X**2, 0))) scale = np.sqrt(3) / average_norm X = scale * X
U = np.zeros((4,4), dtype = np.float32) U[0, 0] = U[1, 1] = U[2, 2] = scale U[:3, 3] = -mean*scale U[3, 3] = 1
A = np.zeros((n*2, 8), dtype = np.float32); X_homo = np.vstack((X, np.ones((1, n)))).T A[:n, :4] = X_homo A[n:, 4:] = X_homo b = np.reshape(x, [-1, 1])
p_8 = np.linalg.pinv(A).dot(b) P = np.zeros((3, 4), dtype = np.float32) P[0, :] = p_8[:4, 0] P[1, :] = p_8[4:, 0] P[-1, -1] = 1
P_Affine = np.linalg.inv(T).dot(P.dot(U)) return P_Affine
|