def rot3d(deg_x, deg_y, deg_z):
"""Returns the 3D rotation matrix in ZYX (i.e. yaw-pitch-roll) order."""
Rx = rotx3d(np.deg2rad(deg_x))
Ry = roty3d(np.deg2rad(deg_y))
Rz = rotz3d(np.deg2rad(deg_z))
R = prjutils.matmul(Rx, prjutils.matmul(Ry, Rz))
return R
def _show_progress(self, H):
if self.full_reference_image is None:
_logger.error('Full template image was not set, cannot show progress!')
return
pts = np.zeros((3, 4), dtype=float)
pts[:, 0] = np.array([0, 0, 1], dtype=float)
pts[:, 1] = np.array([self.width, 0, 1], dtype=float)
pts[:, 2] = np.array([self.width, self.height, 1], dtype=float)
pts[:, 3] = np.array([0, self.height, 1], dtype=float)
ref_pts = pts.copy()
ref_pts = prj.apply_projection(self.H0, ref_pts)
if self.H_gt is not None:
P = prj.matmul(self.H0, prj.matmul(np.linalg.inv(H), prj.matmul(np.linalg.inv(self.H0), prj.matmul(self.H_gt, self.H0))))
pts = prj.apply_projection(P, pts)
vis = self.full_reference_image.copy()
for i in range(4):
pt1 = (int(ref_pts[0, i]), int(ref_pts[1, i]))
pt2 = (int(ref_pts[0, (i+1) % 4]), int(ref_pts[1, (i+1) % 4]))
vis = cv2.line(vis, pt1, pt2, (0, 255, 0), 3)
if self.H_gt is not None:
pt1 = (int(pts[0, i]), int(pts[1, i]))
pt2 = (int(pts[0, (i+1) % 4]), int(pts[1, (i+1) % 4]))
vis = cv2.line(vis, pt1, pt2, (255, 0, 255), 2)
imvis.imshow(vis, title='Progress', wait_ms=10)
def _warp_current_image(self, img, H):
res = cv2.warpPerspective(img, prj.matmul(self.H0, H), (self.width, self.height),
flags=cv2.INTER_LINEAR + cv2.WARP_INVERSE_MAP,
borderMode=cv2.BORDER_CONSTANT, borderValue=(0, 0, 0))
if self.verbose:
imvis.imshow(res, 'Current Warp', wait_ms=10)
return res
def _update_warp(self, params, H):
A = np.zeros((3, 3), dtype=float)
for i in range(8):
A += (params[i] * self.sl3_bases[i])
G = np.zeros((3, 3), dtype=float)
A_i = np.eye(3, dtype=float)
factor_i = 1.0
for i in range(9):
G += (1.0 / factor_i) * A_i
A_i = prj.matmul(A_i, A)
factor_i *= (i+1.0)
if self.method == Method.IC:
H = prj.matmul(H, np.linalg.inv(G))
elif self.method in [Method.FC, Method.ESM]:
H = prj.matmul(H, G)
return H
def pose_from_apriltag(detection,
detector,
K,
tag_size_mm,
return_reprojection_error=False):
# Camera params: (fx, fy, cx, cy)
camera_params = (K[0, 0], K[1, 1], K[0, 2], K[1, 2])
pose, initial_reprj_err, final_reprj_err = detector.detection_pose(
detection, camera_params, tag_size_mm)
# We want the tag's/world z-axis pointing up, so R = R_tag * Rx(180)
Mx180 = np.row_stack((np.column_stack(
(rotx3d180(), np.array([0., 0., 0.]).reshape(
(3, 1)))), np.array([0., 0., 0., 1.])))
if return_reprojection_error:
return prjutils.matmul(pose, Mx180), final_reprj_err
else:
return prjutils.matmul(pose, Mx180)
def _compute_Jacobian(self, dxdy, ref_dxdy=None):
# Sped up from 70ms (loop version) to 0.8/0.9ms
dim_g3d = (self.width*self.height, 1, 2)
if self.method == Method.ESM:
assert ref_dxdy is not None
grad = ((dxdy + ref_dxdy) / 2.0).reshape(dim_g3d)
elif self.method in [Method.FC, Method.IC]:
grad = dxdy.reshape(dim_g3d)
else:
raise NotImplementedError()
J3d = prj.matmul(grad, self.JwJg)
J = J3d.reshape((self.height * self.width, self.JwJg.shape[2]))
return J
def _compute_update_params(self, hessian, J, residuals):
params = np.zeros((8, 1), dtype=float)
hessian_inv = np.linalg.inv(hessian)
for v in range(self.height):
for u in range(self.width):
idx = u + v*self.width
J_row = J[idx, :].reshape((1, -1))
if self.method == Method.ESM:
params += -1 * np.transpose(J_row) * residuals[idx, 0]
else:
params += np.transpose(J_row) * residuals[idx, 0]
params = prj.matmul(hessian_inv, params)
return params
def align_depth_to_color(depth, K_color, K_depth, Rt_stereo, width_color,
height_color):
target_width = 400
target_height = 400
sx = target_width / width_color
sy = target_height / height_color
width_color = target_width
height_color = target_height
print(depth.shape)
K_c = K_color.copy()
K_c[0, 0] = K_c[0, 0] * sx
K_c[0, 1] = K_c[0, 1] * sx
K_c[0, 2] = K_c[0, 2] * sx
K_c[1, 1] = K_c[1, 1] * sy
K_c[1, 2] = K_c[1, 2] * sy
#### Depth image to 3D (reference frame: stereo camera, not necessarily the actual common world frame of the multicam setup)
height_d, width_d = depth.shape[:2]
xx_d, yy_d = np.meshgrid(np.arange(0, width_d), np.arange(0, height_d))
pixels_d = np.row_stack((xx_d[:].reshape((1, -1)), yy_d[:].reshape(
(1, -1)), np.ones((1, height_d * width_d), dtype=xx_d.dtype)))
valid = depth > 0
pixels_d = pixels_d[:, valid.flatten()]
depth_valid = depth[valid]
pts_norm_d = prjutils.apply_transformation(np.linalg.inv(K_depth),
pixels_d)
pts_cam_d = prjutils.shift_points_along_viewing_rays(
pts_norm_d, depth_valid.reshape((1, -1)))
pts_3d = prjutils.matmul(Rt_stereo[0], pts_cam_d) + Rt_stereo[1]
#### Project into color image
pixels_c = prjutils.apply_projection(K_c, pts_3d)
# print('Depth2color pixels (shape, min, max)', pixels_c.shape, np.min(pixels_c, axis=1), np.max(pixels_c, axis=1))
x_c = pixels_c[0, :].astype(np.int64)
y_c = pixels_c[1, :].astype(np.int64)
valid_x = np.logical_and(x_c >= 0, x_c < width_color)
valid_y = np.logical_and(y_c >= 0, y_c < height_color)
valid = np.logical_and(valid_x, valid_y)
x_c = x_c[valid]
y_c = y_c[valid]
values = pts_3d[2, valid]
aligned = np.zeros((height_color, width_color), dtype=depth.dtype)
aligned[y_c, x_c] = values
return aligned
def _compute_Hessian(self, J):
# pu.tic('hess-vec')
Hess = prj.matmul(np.transpose(J), J)
# pu.toc('hess-vec')
# Sped up from ~100ms (loop version) to 0.4ms
# pu.tic('hess-loop')
# num_params = len(self.sl3_bases)
# Hessian = np.zeros((num_params, num_params), dtype=float)
# for r in range(J.shape[0]):
# row = J[r, :].reshape((1, -1))
# Hessian += prj.matmul(np.transpose(row), row)
# pu.toc('hess-loop')
# for r in range(8):
# for c in range(8):
# if Hess[r,c] != Hessian[r,c]:
# print(f'HESSIAN DIFFERS AT {r},{c}: {Hess[r,c]-Hessian[r,c]} {Hess[r,c]} vs {Hessian[r,c]}')
return Hess
def get_transformation(self, cam_id, tag_id1, tag_id2):
if tag_id1 == tag_id2:
if (cam_id, tag_id1) in self.camera_transformations:
return self.camera_transformations[(cam_id, tag_id1)]
else:
return None
path = self.__dijkstra(tag_id1, tag_id2)
if not path:
return None
# Backtrack:
# Note that the path is reversed, e.g. tag 2 => tag 5 => tag 0 (world reference frame)
# So, we need to compute: M_t2->cam * M_t5->t2 * M_t0->t5
if (cam_id, tag_id1) not in self.camera_transformations:
return None
M = self.camera_transformations[(cam_id, tag_id1)]
for i in range(len(path) - 1):
tfrom = path[i]
tto = path[i + 1]
Mt = self.tag_transformations[(tto, tfrom)]
M = prjutils.matmul(M, Mt)
# print(' CAM {}, PATH {} => {}'.format(cam_id, tfrom, tto))
return M
def _compute_JwJg(self):
# Sped up from 170ms to 6ms
# pu.tic('loop')
# self.JwJg_list = list()
# for v in range(self.height):
# for u in range(self.width):
# # Eq.(63)
# Jw = np.array([
# [u, v, 1, 0, 0, 0, -u*u, -u*v, -u],
# [0, 0, 0, u, v, 1, -u*v, -v*v, -v]],
# dtype=float)
# # Shapes: [2x8] = [2x9] * [9x8]
# JwJg = prj.matmul(Jw, self.Jg)
# self.JwJg_list.append(JwJg)
# pu.toc('loop')
# pu.tic('3d')
# jwjg = np.zeros(self.height*self.width, 2, 8)
u, v = np.meshgrid(np.arange(0, self.width), np.arange(0, self.height))
u = u.reshape((-1, ))
v = v.reshape((-1, ))
jw = np.zeros((self.height*self.width, 2, 9), dtype=float)
jw[:, 0, 0] = u
jw[:, 0, 1] = v
jw[:, 0, 2] = 1
jw[:, 0, 6] = -np.multiply(u, u)
jw[:, 0, 7] = -np.multiply(u, v)
jw[:, 0, 8] = -u
jw[:, 1, 3] = u
jw[:, 1, 4] = v
jw[:, 1, 5] = 1
jw[:, 1, 6] = -np.multiply(u, v)
jw[:, 1, 7] = -np.multiply(v, v)
jw[:, 1, 8] = -v
jgshape = self.Jg.shape
jg = self.Jg.reshape((1, *jgshape))
self.JwJg = prj.matmul(jw, jg)
def between_tag_transformation(M1, M2):
"""Computes the transformation from tag 1's coordinate system to tag 2's coord. sys, i.e. [M_inverse tag2 to cam] * [M tag1 to cam]"""
return prjutils.matmul(np.linalg.inv(M2), M1)