# 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)