import numpy as np
import libs.lib_rs as rs
import cv2
import libs.lib_optimization as opt
# -----------------
# load poses
# -----------------
b2e_pos = np.loadtxt("..\\data_calib\\20200617_b2e_pos.txt") / 1000.
b2e_ori = np.loadtxt("..\\data_calib\\20200617_b2e_ori.txt")
# swap quaternions
e2b = opt.inv_t_4_4s__quats(opt.swap_quats(b2e_ori), b2e_pos)
e2c_rvec = np.loadtxt("..\\data_calib\\20200612_result\\final_result.txt")
c2e = opt.inv(opt.t_4_4__rvec(e2c_rvec[0:3], e2c_rvec[3:6]))
# np.save("c2e", c2e)
# ---------------
# load image data
# ---------------
d415 = rs.D415()
marker_size = 0.2
p = [[], []]
img_path = "..\\data_2D\\20200617_marker\\marker_%d.png"
for i in range(5):
img = cv2.imread(img_path % i)
corners, ids, _ = cv2.aruco.detectMarkers(img, d415.aruco_dict)
for j in range(len(corners)):
p[ids[j, 0]].append(corners[j].reshape([4, 2]))
# ----------------
# load data
# ----------------
print("loading data...")
# c [3, 4] * 1
c = np.load("..\\data_locate_base\\c.npy")
print("c:")
print(c)
# c2e [4, 4] * 1
c2e = np.load("..\\data_locate_base\\c2e.npy")
print("c2e:")
print(c2e)
print("e2c:")
print(opt.inv(c2e))
# e2b [4, 4] * number of photos
b2e_pos = np.loadtxt("..\\data_locate_base\\b2e_pos.txt") / 1000.
b2e_ori = np.loadtxt("..\\data_locate_base\\b2e_ori.txt")
e2b = opt.inv_t_4_4s__quats(opt.swap_quats(b2e_ori), b2e_pos)
n = len(e2b)
print("e2b:")
for i in range(len(e2b)):
print(e2b[i])
# w2k [4, 4] * number of markers
w2k = opt.t_4_4s__rvecs6(np.load("..\\data_locate_base\\w2k.npy"))
print("w2k:")
for i in range(len(w2k)):
print(w2k[i])
b2e_ext2 = np.zeros(c2o_ext2.shape)
for i in range(c2o_ext2.shape[0]):
b2e_ext2[i] = opt.t_4_4__quat(b2e_ori_ext2[i], b2e_pos_ext2[i]).reshape([16, ])
# -------------------------
# solve AX = XB
# A = T2(B_E).inv * T1(B_E)
# B = T2(C_O) * T2(C_O).inv
# -------------------------
A_whole, B_whole = [], []
A, B = [], []
for i in range(num_data-1):
A_whole.append(opt.inv(b2e[i + 1].reshape([4, 4])).dot(b2e[i].reshape([4, 4])))
B_whole.append(c2o[i + 1].reshape([4, 4]).dot(opt.inv(c2o[i].reshape([4, 4]))))
for i in range(c2o_ext.shape[0]-1):
A_whole.append(opt.inv(b2e_ext[i + 1].reshape([4, 4])).dot(b2e_ext[i].reshape([4, 4])))
B_whole.append(c2o_ext[i + 1].reshape([4, 4]).dot(opt.inv(c2o_ext[i].reshape([4, 4]))))
for i in range(c2o_ext2.shape[0]-1):
A_whole.append(opt.inv(b2e_ext2[i + 1].reshape([4, 4])).dot(b2e_ext2[i].reshape([4, 4])))
B_whole.append(c2o_ext2[i + 1].reshape([4, 4]).dot(opt.inv(c2o_ext2[i].reshape([4, 4]))))
x_plot = range(140, len(A_whole))
y0_plot = []
y1_plot = []
y2_plot = []
y3_plot = []
y4_plot = []
y5_plot = []
corners_all.append(corners)
ids_all.append(ids)
count_marker += len(ids)
print(count_marker)
# c2w = np.load("..\\data_2D\\c2w.npy").flatten()
w2k = np.load("..\\data_2D\\w2k.npy")
w2k_4_4 = opt.t_4_4s__rvecs6(w2k)
c2w = []
for i in range(len(ids_all)):
ids = ids_all[i]
corners = corners_all[i]
rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(corners[0], 0.2, d415.C_r, d415.coeffs_r)
c2k = opt.t_4_4__rvec(rvec.reshape([3, ]), tvec.reshape([3, ]))
c2w.append(opt.rvec6__t_4_4(c2k.dot(opt.inv(w2k_4_4[ids[0, 0]]))))
c2w = np.array(c2w)
c2w = c2w.flatten()
w2k = w2k[1:].flatten()
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:
# ---------------
# load image data
# ---------------
d415 = rs.D415()
c2k_0 = []
c2k_1 = []
img_path = "..\\data_2D\\20200617_marker\\marker_%d.png"
for i in range(5):
img = cv2.imread(img_path % i)
corners, ids, _ = cv2.aruco.detectMarkers(img, d415.aruco_dict)
for j in range(len(corners)):
rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(corners[j], 0.2, d415.C_r, d415.coeffs_r)
# cv2.aruco.drawAxis(img, d415.C_r, d415.coeffs_r, rvec, tvec, 0.2)
c2k = opt.t_4_4__rvec(rvec.reshape([3, ]), tvec.reshape([3, ]))
if ids[j] == 0:
c2k_0.append(c2k)
elif ids[j] == 1:
c2k_1.append(c2k)
# cv2.imshow("color", img)
# cv2.waitKey()
for i in range(5):
b2k_0 = b2e[i].dot(e2c).dot(c2k_0[i])
# print("b2k_0: ")
# print(b2k_0)
b2k_1 = b2e[i].dot(e2c).dot(c2k_1[i])
# print("b2k_1: ")
# print(b2k_1)
print("1 to 2:")
print(opt.inv(b2k_0).dot(b2k_1))
n = 9 # number of photos
c2w = np.zeros([n, 6])
for i in range(n):
color = cv2.imread(path % i)
corners, ids, _ = cv2.aruco.detectMarkers(color, d415.aruco_dict)
index = ids[0, 0]
size = 0.2
rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(
corners[0], size, d415.C_r, d415.coeffs_r)
c2k = opt.t_4_4__rvec(rvec.reshape([
3,
]), tvec.reshape([
3,
]))
c2w[i] = opt.rvec6__t_4_4(c2k.dot(opt.inv(w2k[index])))
print(c2w)
np.save("c2w", c2w)
# -------------------------
# for marker relative poses
# -------------------------
# color = cv2.imread(path % 5)
# gray = cv2.cvtColor(color, cv2.COLOR_BGR2GRAY)
# param = cv2.aruco.DetectorParameters_create()
# corners, ids, _ = cv2.aruco.detectMarkers(color, d415.aruco_dict)
# # corner refinement
# criteria = (cv2.TERM_CRITERIA_EPS + cv2.TermCriteria_COUNT, 40, 0.001)
# for i in range(len(corners)):
# cv2.cornerSubPix(gray, corners[1], (3, 3), (-1, -1), criteria)
num_data = c2o.shape[0]
# ------------------------
# prepare b2e
# ------------------------
# -------------------------
# solve AX = XB
# A = T2(B_E).inv * T1(B_E)
# B = T2(C_O) * T2(C_O).inv
# -------------------------
A, B = [], []
for i in range(num_data-1):
A.append(opt.inv(b2e[i+1].reshape([4, 4])).dot(b2e[i].reshape([4, 4])))
B.append(c2o[i+1].reshape([4, 4]).dot(opt.inv(c2o[i].reshape([4, 4]))))
x0_mat = np.array([[-0.5, 0.866, 0., 0.020806],
[-0.866, -0.5, 0., 0.010413],
[0., 0., 1., 0.160870],
[0., 0., 0., 1.]])
# x0_mat = np.array([[0., -0.5, 0.866, 0.160870],
# [0., -0.866, -0.5, 0.020806],
# [1., 0., 0., 0.010413],
# [0., 0., 0., 1.]])
x0 = opt.rvec6__t_4_4(x0_mat)
print(x0)
ls = opt.least_squares(residual, x0, jac="3-point")
print(ls.x)
key = cv2.waitKey(1)
if key == ord('q'):
break
cv2.imshow("color", color)
# w2b [4, 4]
w2b = np.loadtxt("..\\..\\data_locate_base\\result.txt").reshape([4, 4])
# b2e [4, 4]
b2e_pos = np.loadtxt("..\\..\\data_locate_base\\b2e_pos.txt") / 1000.
b2e_ori = np.loadtxt("..\\..\\data_locate_base\\b2e_ori.txt")
b2e = opt.t_4_4s__quats(opt.swap_quats(b2e_ori), b2e_pos)[0]
# e2c [4, 4]
c2e = np.load("..\\..\\data_locate_base\\c2e.npy")
e2c = opt.inv(c2e)
points_3d = np.zeros([4, 3])
for i in range(4):
u = corners[i][0]
v = corners[i][1]
point_c = np.ones([4, 1])
point_c[0:3, :] = d415.construct_point(
u, v, 0.1 * depth[int(round(v)), int(round(u))]).reshape([3, 1])
# point_w = w2b.dot(b2e).dot(e2c).dot(point_c)
point_w = point_c
points_3d[i] = point_w[0:3].reshape([
3,
])
np.savetxt("..\\..\\data_locate_base\\wood.txt", points_3d)
# =====================================