def residual(x):
global corners_all, ids_all, count_marker
c2w = []
for i in range(N):
c2w.append(opt.t_4_4__rvec(x[i*6: i*6+3], x[i*6+3: i*6+6]))
w2k = []
w2k.append(np.eye(4))
for i in range(m-1):
w2k.append(opt.t_4_4__rvec(x[6*N+i*6: 6*N+i*6+3], x[6*N+i*6+3: 6*N+i*6+6]))
# residual
res = np.zeros([count_marker*4*2, ])
marker_index = 0
# res = []
# i-th photo j-th marker k-th corner
for i in range(len(corners_all)):
corners, ids = corners_all[i], ids_all[i]
for j in range(len(corners)):
# if ids[j, 0] != i and ids[j, 0] != i+1:
# continue
for k in range(4):
corner = d415.C_ext_r.dot(c2w[i]).dot(w2k[ids[j, 0]]).dot(rs.P(0.2, k))
res[marker_index*8+k*2+0] = (corner[0][0] / corner[2][0] - corners[j][0, k][0])
res[marker_index*8+k*2+1] = (corner[1][0] / corner[2][0] - corners[j][0, k][1])
marker_index += 1
return res
def residual(x):
global e2b, c2e, d415, p
# 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)
def residual(x):
global A, B
res = np.zeros([16, ])
x_mat = opt.t_4_4__rvec(x[0:3], x[3:6])
for i in range(len(A)):
res += np.power( A[i].dot(x_mat) - x_mat.dot(B[i]) , 2).reshape([16, ])
return res
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]))
w2k[i].reshape([16, 1]))
# --------------------------------
# initial guess of b2w [6, ] * 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
import numpy as np
import libs.lib_rs as rs
import cv2
import libs.lib_optimization as opt
b2e_pos = np.loadtxt("..\\data_calib\\20200617_b2e_pos.txt") / 1000.
b2e_ori = np.loadtxt("..\\data_calib\\20200617_b2e_ori.txt")
b2e = opt.t_4_4s__quats(opt.swap_quats(b2e_ori), b2e_pos)
e2c_rvec = np.loadtxt("..\\data_calib\\20200612_result\\final_result.txt")
e2c = opt.t_4_4__rvec(e2c_rvec[0:3], e2c_rvec[3:6])
# ---------------
# 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)
import numpy as np
import libs.lib_rs as rs
import cv2
import libs.lib_optimization as opt
path = "..\\data_2D\\20200628_marker\\marker_%d.png"
d415 = rs.D415()
# ------------------------
# initial guesses
# ------------------------
w2k = [np.eye(4)] * 3
w2k[0] = np.eye(4)
w2k[1] = opt.t_4_4__rvec(
np.array([1.07888743e-02, -3.80953256e-02, -1.59157464e+00]),
np.array([-9.08505140e-03, -4.23919083e-01, -1.34970647e-04]))
w2k[2] = opt.t_4_4__rvec(
np.array([-2.29909471e-02, 1.96608912e-02, -1.56549568e+00]),
np.array([6.33976050e-04, 3.89877312e-01, -5.58933275e-03]))
np.save("w2k", opt.rvecs6__t_4_4s(w2k[1:])) # exclude first one
# -------------------------
# for camera poses
# -------------------------
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)