# res = np.zeros([2, ]) res = [] b2k = [] b2k.append(opt.t_4_4__rvec(x[0:3], x[3:6])) b2k.append(opt.t_4_4__rvec(x[6:9], x[9:12])) # i-th photo j-th marker k-th corner for i in range(5): for j in range(2): for k in range(4): corner = d415.C_ext_r.dot(c2e).dot(e2b[i]).dot(b2k[j]).dot( P(k)) res.append(corner[0, 0] / corner[2, 0] - p[j][i][k, 0]) res.append(corner[1, 0] / corner[2, 0] - p[j][i][k, 1]) return np.array(res) ls = opt.least_squares(residual, x0, jac="2-point") b2k_0 = opt.t_4_4__rvec(ls.x[0:3], ls.x[3:6]) b2k_1 = opt.t_4_4__rvec(ls.x[6:9], ls.x[9:12]) w2k_01 = opt.inv(b2k_0).dot(b2k_1) # print(opt.rvec__t_4_4(w2k_01)) print(ls.cost / 40.0) print(ls.optimality) w2k = [] w2k.append(opt.rvec6__t_4_4(np.eye(4))) w2k.append(opt.rvec6__t_4_4(w2k_01)) print(w2k) np.save("w2k", w2k)
x0 = np.hstack((c2w, w2k)) A = lil_matrix((N*16, N*6+(m-1)*6), dtype=int) for i in range(N): A[np.arrange(i*16, i*16+16), np.arrange(i*6, i*6+6)] = 1 index_m0 = ids_all[i][0, 0] - 1 index_m1 = ids_all[i][1, 0] - 1 if index_m0 != -1: A[np.arrange(i*16, i*16+8), np.arrange(6*N+index_m0*6, 6*N+index_m0*6+6)] = 1 if index_m1 != -1: A[np.arrange(i*16+8, i*16+16), np.arrange(6*N+index_m1*6, 6*N+index_m1*6+6)] = 1 # ls = opt.least_squares(residual, x0, jac_sparsity=A, verbose=2, x_scale='jac', ftol=1e-4, method='trf') ls = opt.least_squares(residual, x0, verbose=2, x_scale='jac', ftol=1e-4, method='trf') print(ls.x[6*N: 6*N+6]) print(ls.x[6*N+6: 6*N+12]) print("------------------") print(ls.cost) print(ls.optimality) final_w2k = [] final_w2k.append(opt.rvec6__t_4_4(np.eye(4))) for i in range(m-1): final_w2k.append(ls.x[6*(N+i): 6*(N+i+1)]) np.save("w2k", final_w2k)