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)