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]))
# ---------------
# initial guess
# ---------------
x0 = np.zeros([
12,
])
x0[0:6] = opt.rvec6__t_4_4(
np.array([[0.99486095, 0.10078919, 0.00965603, 0.30825997],
[-0.10085231, 0.99488154, 0.00628804, 1.63810419],
[-0.00897284, -0.00722956, 0.99993361, -0.55456868],
[0., 0., 0., 1.]]))
x0[6:12] = opt.rvec6__t_4_4(
np.array([[-0.99637026, -0.0836713, -0.01566585, 0.62051195],
[0.08371704, -0.99648695, -0.0022858, 1.60664948],
[-0.01541956, -0.003589, 0.99987467, -0.55516513],
[0., 0., 0., 1.]]))
# ------------------------
# cost function: x: [k2b[0]
# k2b[1]]
# ------------------------
def P(k):
x_plot = range(140, len(A_whole))
y0_plot = []
y1_plot = []
y2_plot = []
y3_plot = []
y4_plot = []
y5_plot = []
for num in range(140, len(A_whole)):
A = A_whole[0:num]
B = B_whole[0:num]
x0_mat = np.array([[-0.46491281, 0.88355481, 0.05645332, 0.04465462],
[-0.88535499, -0.4638495, -0.03146717, 0.04170271],
[-0.00161712, -0.06461072, 0.99790923, 0.156906],
[0., 0., 0., 1.]])
x0 = opt.rvec6__t_4_4(x0_mat)
ls = opt.least_squares(residual, x0, jac="3-point")
y0_plot.append(ls.x[0])
y1_plot.append(ls.x[1])
y2_plot.append(ls.x[2])
y3_plot.append(ls.x[3])
y4_plot.append(ls.x[4])
y5_plot.append(ls.x[5])
plt.plot(x_plot, y0_plot)
plt.show()
plt.plot(x_plot, y1_plot)
plt.show()
plt.plot(x_plot, y2_plot)
plt.show()
plt.plot(x_plot, y3_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:
# --------------------------------
print("initial guess of b2w:")
path_img = "..\\data_locate_base\\marker_%d.png"
size = 0.2
img = cv2.imread(path_img % 0)
corners, ids, _ = cv2.aruco.detectMarkers(img, rs.aruco_dict())
rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(corners[0], size, c[0:3,
0:3],
rs.coeffs())
c2k = opt.t_4_4__rvec(rvec.reshape([
3,
]), tvec.reshape([
3,
]))
x0 = opt.rvec6__t_4_4(
opt.inv(e2b[0]).dot(opt.inv(c2e)).dot(c2k).dot(
opt.inv(w2k[opt.id_new(ids[0, 0])])))
print(x0)
# --------------------------------
# get all the marker information
# --------------------------------
corners_all, ids_all = [], []
for i in range(n):
img = cv2.imread(path_img % i)
param = cv2.aruco.DetectorParameters_create()
param.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_APRILTAG
corners, ids, _ = cv2.aruco.detectMarkers(img,
rs.aruco_dict(),
parameters=param)
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)