(Q,R,s,Abar,lam)
| 2 | from scipy.linalg import eigh, svd |
| 3 | |
| 4 | def recover_XM(Q,R,s,Abar,lam): |
| 5 | N = s.shape[0] |
| 6 | |
| 7 | sR = np.zeros((3*N, R.shape[1])) |
| 8 | for i in range(N): |
| 9 | sR[3*i:3*i+3,:] = s[i] * R[3*i:3*i+3,:] |
| 10 | |
| 11 | if R.shape[1] > 3: |
| 12 | X = sR @ sR.T |
| 13 | |
| 14 | eig_vals, eig_vecs = eigh(X) |
| 15 | |
| 16 | idx = np.argsort(eig_vals)[::-1] |
| 17 | eig_vals = eig_vals[idx] |
| 18 | eig_vecs = eig_vecs[:, idx] |
| 19 | |
| 20 | top_3_eig_vecs = eig_vecs[:, :3] |
| 21 | |
| 22 | sR_real = top_3_eig_vecs * np.sqrt(eig_vals[:3]) |
| 23 | sR_real = sR_real.T |
| 24 | |
| 25 | if(abs(eig_vals[3]/eig_vals[2]) < 1e-3): |
| 26 | print("Optimal rank is 3") |
| 27 | else: |
| 28 | X_new = sR_real.T @ sR_real |
| 29 | suboptimality = np.sum(Q * (X_new - X)) + lam * np.sum((np.diag(X_new)-1)**2)/3 - lam * np.sum((np.diag(X) - 1)**2)/3 |
| 30 | print("suboptimality: ", suboptimality) |
| 31 | |
| 32 | else: |
| 33 | s_real = s.squeeze() |
| 34 | R_real = R.T |
| 35 | sR_real = np.zeros((3, 3*N)) |
| 36 | for i in range(N): |
| 37 | sR_real[:,3*i:3*i+3] = s_real[i] * R_real[:,3*i:3*i+3] |
| 38 | |
| 39 | s_real = np.zeros(N) |
| 40 | R_real = np.zeros((3, 3*N)) |
| 41 | |
| 42 | for i in range(N): |
| 43 | s_real[i] = np.linalg.norm(sR_real[:, 3*i:3*i+3], 'fro') / np.sqrt(3) |
| 44 | R_real[:, 3*i:3*i+3] = sR_real[:, 3*i:3*i+3] / s_real[i] |
| 45 | |
| 46 | # because of anchoring |
| 47 | R1 = R_real[:,:3] |
| 48 | R_real = R1.T @ R_real |
| 49 | |
| 50 | negative_R = 0 |
| 51 | for i in range(N): |
| 52 | U, S, Vt = svd(R_real[:,3*i:3*i+3]) |
| 53 | |
| 54 | if np.linalg.det(U @ Vt) < 0: |
| 55 | negative_R = negative_R + 1 |
| 56 | |
| 57 | # not projecting the SO3, sometimes worse than the original in Ceres |
| 58 | if negative_R > 0: |
| 59 | print("warning: some det(R) < 0") |
| 60 | |
| 61 | # judge which is the largest component |
no outgoing calls
no test coverage detected