def getRTfromH(H):
rot = H[0:3,0:3]
trans = H[0:3,3]
q = quat.rot_to_qua(rot)
rot_vec = quat.log_qua(q)
return rot_vec, trans
def get_cparams(cur_T, f, k1, k2):
# extract rotation and translation information from T matrix
cur_R, cur_t = cur_T[:3, :3], cur_T[:3, 3]
# convert rotation matrix into quaternion and then to the rotation vector
cur_q = quat.rot_to_qua(cur_R)
cur_rot_vec = quat.log_qua(cur_q)[1:]
# construct camera parameters which is size 9 vector
cam_param = np.array([cur_rot_vec[0], cur_rot_vec[1], cur_rot_vec[2], \
cur_t[0], cur_t[1], cur_t[2], \
f, k1, k2])
return cam_param
def rpy2quat(r, p, y):
rot = np.dot(
np.array([[m.cos(y), -m.sin(y), 0.], [m.sin(y), m.cos(y), 0.],
[0., 0., 1.]]),
np.dot(
np.array([[m.cos(p), 0, m.sin(p)], [0, 1., 0],
[-m.sin(p), 0., m.cos(p)]]),
np.array([
[1., 0, 0],
[0, m.cos(r), -m.sin(r)],
[0., m.sin(r), m.cos(r)],
])))
q = quat.rot_to_qua(rot)
return q
def LinearPnP(K=None,
points_3d=None,
camera_indices=None,
points_indices=None,
points_2d=None,
camera_params=None):
terminal_T = max(camera_indices)
n_obser = points_2d.shape[0]
A = np.zeros((n_obser * 2, 13))
for i in range(0, 2 * n_obser, 2):
p3d = points_3d[points_indices[int(i / 2)], :]
p2d = points_2d[int(i / 2), :]
A[i, 0] = camera_indices[int(i / 2)]
A[i, 1:5] = np.array([p3d[0], p3d[1], p3d[2], 1])
A[i, 9:] = np.array(
[-p2d[0] * p3d[0], -p2d[0] * p3d[1], -p2d[0] * p3d[2], -p2d[0]])
A[i + 1, 0] = camera_indices[int(i / 2)]
A[i + 1, 5:9] = np.array([p3d[0], p3d[1], p3d[2], 1])
A[i + 1, 9:] = np.array(
[-p2d[1] * p3d[0], -p2d[1] * p3d[1], -p2d[1] * p3d[2], -p2d[1]])
for t in range(terminal_T + 1):
current = (A[:, 0] == t)
cur_A = A[current, 1:]
[u, s, v] = np.linalg.svd(cur_A)
P = v[11, :]
P_m = np.array([[P[0], P[1], P[2], P[3]], [P[4], P[5], P[6], P[7]],
[P[8], P[9], P[10], P[11]]])
RT = np.einsum('ij,jk->ik', np.linalg.inv(K), P_m)
R, T = RT[:, :3], RT[:, 3]
quat_cur = quat.rot_to_qua(R)
rot_vec = quat.log_qua(quat_cur.reshape(-1, 4))
cam_ind = camera_indices[t]
camera_params[cam_ind, :6] = np.array(
[rot_vec[0], rot_vec[1], rot_vec[2], T[0], T[1], T[2]])
return camera_params
def homo2qp(T):
q = quat.rot_to_qua(T[:3, :3])
pos = T[:3, 3]
return q, pos