def show_epipolar_rui(x1, x2, img1_rgb, img2_rgb, F_gt, im_shape):
N_points = x1.shape[0]
x1_homo = utils_misc.homo_np(x1)
x2_homo = utils_misc.homo_np(x2)
right_P = np.matmul(F_gt, x1_homo.T)
right_epipolar_x = np.tile(np.array([[0], [1]]), N_points) * im_shape[1]
# Using the eqn of line: ax+by+c=0; y = (-c-ax)/b, http://ai.stanford.edu/~mitul/cs223b/draw_epipolar.m
right_epipolar_y = (-right_P[2:3, :] -
right_P[0:1, :] * right_epipolar_x) / right_P[1:2, :]
colors = np.random.rand(x2.shape[0])
plt.figure(figsize=(30, 8))
plt.subplot(121)
plt.imshow(img1_rgb,
cmap=None if len(img1_rgb.shape) == 3 else plt.get_cmap('gray'))
plt.scatter(x1[:, 0], x1[:, 1], s=50, c=colors, edgecolors='w')
plt.subplot(122)
# plt.figure(figsize=(30, 8))
plt.imshow(img2_rgb,
cmap=None if len(img2_rgb.shape) == 3 else plt.get_cmap('gray'))
plt.plot(right_epipolar_x, right_epipolar_y)
plt.scatter(x2[:, 0], x2[:, 1], s=50, c=colors, edgecolors='w')
plt.xlim(0, im_shape[1] - 1)
plt.ylim(im_shape[0] - 1, 0)
plt.show()
def show_epipolar_normalized(x1, x2, img1_rgb, img2_rgb, F_gt, im_shape):
N_points = x1.shape[0]
x1_homo = utils_misc.homo_np(x1)
x2_homo = utils_misc.homo_np(x2)
right_P = np.matmul(F_gt, x1_homo.T)
right_epipolar_x = np.tile(np.array([[-1.], [1.]]), N_points) * im_shape[1]
# Using the eqn of line: ax+by+c=0; y = (-c-ax)/b, http://ai.stanford.edu/~mitul/cs223b/draw_epipolar.m
right_epipolar_y = (-right_P[2:3, :] -
right_P[0:1, :] * right_epipolar_x) / right_P[1:2, :]
colors = np.random.rand(x2.shape[0])
plt.figure(figsize=(30, 8))
plt.subplot(121)
# plt.imshow(img1_rgb)
# plt.scatter(x1[:, 0]*f+W/2., x1[:, 1]*f+H/2., s=50, c=colors, edgecolors='w')
plt.scatter(x1[:, 0], x1[:, 1], s=50, c=colors, edgecolors='w')
plt.xlim(-im_shape[1], im_shape[1])
plt.ylim(im_shape[0], -im_shape[0])
plt.gca().set_aspect('equal', adjustable='box')
plt.subplot(122)
# plt.imshow(img2_rgb)
plt.plot(right_epipolar_x, right_epipolar_y)
plt.scatter(x2[:, 0], x2[:, 1], s=50, c=colors, edgecolors='w')
# plt.axis('equal')
plt.xlim(-im_shape[1], im_shape[1])
plt.ylim(im_shape[0], -im_shape[0])
plt.gca().set_aspect('equal', adjustable='box')
plt.show()
def epi_distance_np(F, X, Y, if_homo=False):
# Not squared. https://arxiv.org/pdf/1706.07886.pdf
if not if_homo:
X = utils_misc.homo_np(X)
Y = utils_misc.homo_np(Y)
if len(X.shape) == 2:
nominator = np.abs(np.diag(Y @ F @ X.T))
Fx1 = F @ X.T
Fx2 = F.T @ Y.T
denom_recp_Y_to_FX = 1. / np.sqrt(Fx1[0]**2 + Fx1[1]**2)
denom_recp_X_to_FY = 1. / np.sqrt(Fx2[0]**2 + Fx2[1]**2)
else:
nominator = np.abs(
np.diagonal(np.transpose(Y @ F @ X, (1, 2)), axis=1, axis2=2))
Fx1 = F @ np.transpose(X, (1, 2))
Fx2 = np.transpose(F, (1, 2)) @ np.transpose(Y, (1, 2))
denom_recp_Y_to_FX = 1. / np.sqrt(Fx1[:, 0]**2 + Fx1[:, 1]**2)
denom_recp_X_to_FY = 1. / np.sqrt(Fx2[:, 0]**2 + Fx2[:, 1]**2)
# print(nominator.size(), denom.size())
dist1 = nominator * denom_recp_Y_to_FX
dist2 = nominator * denom_recp_X_to_FY
dist3 = nominator * (denom_recp_Y_to_FX + denom_recp_X_to_FY)
# return (dist1+dist2)/2., dist1, dist2
return dist3, dist1, dist2
def rectify_all(self, visualize=False):
# for each frame, get the visible points on front view with identity left camera, as well as indexes of points on both left/right images
print('Rectifying...')
self.val_idxes_list = []
self.X_rect_list = []
for i in range(self.N_frames):
print(i, self.N_frames)
velo = list(self.dataset.velo)[i] # [N, 4]
velo = velo[:, :3]
velo_reproj = utils_misc.homo_np(velo)
val_idxes, X_rect = self.rectify(velo_reproj,
self.dataset_rgb[i][0],
self.dataset_rgb[i][1],
visualize=((i % 100 == 0)
& visualize))
self.val_idxes_list.append(val_idxes)
self.X_rect_list.append(X_rect)
print('Finished rectifying all frames.')
return self.val_idxes_list, self.X_rect_list
def show_demo(self):
velo_reproj_list = []
for i in range(self.N_frames):
velo = list(self.dataset.velo)[i] # [N, 4]
# project the points to the camera
velo = velo[:, :3]
velo_reproj = utils_misc.homo_np(velo)
velo_reproj_list.append(velo_reproj)
for cam_iter, cam in enumerate(['leftRGB', 'rightRGB']):
P_rect = self.P_rects[
cam] # P_rect_0[0-3]: 3x4 projection matrix after rectification; the reprojection matrix in MV3D
P_velo2im = np.dot(np.dot(P_rect, self.R_cam2rect),
self.velo2cam) # 4*3
velo_pts_im = np.dot(P_velo2im, velo_reproj.T).T # [*, 3]
velo_pts_im[:, :2] = velo_pts_im[:, :2] / velo_pts_im[:, 2][
..., np.newaxis]
# check if in bounds
# use minus 1 to get the exact same value as KITTI matlab code
velo_pts_im[:, 0] = np.round(velo_pts_im[:, 0]) - 1
velo_pts_im[:, 1] = np.round(velo_pts_im[:, 1]) - 1
val_inds = (velo_pts_im[:, 0] >= 0) & (velo_pts_im[:, 1] >= 0)
val_inds = val_inds & (velo_pts_im[:, 0] <
self.im_shape[1]) & (velo_pts_im[:, 1] <
self.im_shape[0])
velo_pts_im = velo_pts_im[val_inds, :]
if i == 0:
print(
'Demo: Showing left/right data (unfiltered and unrectified) of the first frame.'
)
plt.figure(figsize=(30, 8))
plt.imshow(self.dataset_rgb[i][cam_iter])
plt.scatter(velo_pts_im[:, 0].astype(np.int),
velo_pts_im[:, 1].astype(np.int),
s=2,
c=1. / velo_pts_im[:, 2])
plt.xlim(0, self.im_shape[1] - 1)
plt.ylim(self.im_shape[0] - 1, 0)
plt.title(cam)
plt.show()
def reproj_and_scatter(Rt,
X_rect,
im_rgb,
kitti_two_frame_loader=None,
visualize=True,
title_appendix='',
param_list=[],
set_lim=False,
debug=True,
s=10):
if kitti_two_frame_loader is None:
if debug:
print('Reading from input list of param_list=[K, im_shape].')
K = param_list[0]
im_shape = param_list[1]
else:
K = kitti_two_frame_loader.K
im_shape = kitti_two_frame_loader.im_shape
x1_homo = np.matmul(K, np.matmul(Rt, utils_misc.homo_np(X_rect).T)).T
x1 = x1_homo[:, 0:2] / x1_homo[:, 2:3]
if visualize:
plt.figure(figsize=(30, 8))
cmap = None if len(
np.array(im_rgb).shape) == 3 else plt.get_cmap('gray')
plt.imshow(im_rgb, cmap=cmap)
val_inds = scatter_xy(
x1,
x1_homo[:, 2],
im_shape,
'Reprojection to cam 2 with rectified X and camera_' +
title_appendix,
new_figure=False,
set_lim=set_lim,
s=s)
else:
val_inds = utils_misc.within(x1[:, 0], x1[:, 1], im_shape[1],
im_shape[0])
return val_inds, x1
def show_epipolar_rui_gtEst(x1,
x2,
img1_rgb,
img2_rgb,
F_gt,
F_est,
im_shape,
title_append='',
emphasis_idx=[],
label_text=False,
weights=None,
if_show=True,
linewidth=1.0):
N_points = x1.shape[0]
x1_homo = utils_misc.homo_np(x1)
x2_homo = utils_misc.homo_np(x2)
right_P = np.matmul(F_gt, x1_homo.T)
right_epipolar_x = np.tile(np.array([[0], [1]]), N_points) * im_shape[1]
# Using the eqn of line: ax+by+c=0; y = (-c-ax)/b, http://ai.stanford.edu/~mitul/cs223b/draw_epipolar.m
right_epipolar_y = (-right_P[2:3, :] -
right_P[0:1, :] * right_epipolar_x) / right_P[1:2, :]
# colors = get_spaced_colors(x2.shape[0])
# colors = np.random.random((x2.shape[0], 3))
# plt.figure(figsize=(60, 8))
plt.figure(figsize=(30, 4))
plt.imshow(img2_rgb,
cmap=None if len(img2_rgb.shape) == 3 else plt.get_cmap('gray'))
plt.plot(right_epipolar_x,
right_epipolar_y,
'b',
linewidth=linewidth,
zorder=1)
if weights is None:
print(f"x2: {x2.shape}")
plt.scatter(x2[:, 0], x2[:, 1], s=50, c='r', edgecolors='w', zorder=2)
else:
plt.scatter(x2[:, 0],
x2[:, 1],
s=weights * 10000,
c='r',
edgecolors='w',
zorder=2)
if emphasis_idx:
for idx in emphasis_idx:
plt.scatter(x2[idx, 0],
x2[idx, 1],
s=80,
color='y',
edgecolors='w')
if label_text:
for idx in range(N_points):
plt.text(x2[idx, 0],
x2[idx, 1] - 10,
str(idx),
fontsize=20,
fontweight='extra bold',
color='w')
right_P = np.matmul(F_est, x1_homo.T)
right_epipolar_x = np.tile(np.array([[0], [1]]), N_points) * im_shape[1]
right_epipolar_y = (-right_P[2:3, :] -
right_P[0:1, :] * right_epipolar_x) / right_P[1:2, :]
plt.plot(right_epipolar_x,
right_epipolar_y,
'g',
linewidth=linewidth,
zorder=1) # 'r'
plt.xlim(0, im_shape[1] - 1)
plt.ylim(im_shape[0] - 1, 0)
# plt.title('Blue lines for GT F; Red lines for est. F. -- '+title_append)
plt.title(f"{title_append}")
if if_show:
plt.show()
def draw_corr_widths_and_epi(F_gt,
im1,
im2,
x1,
x2,
linewidth,
title='',
rescale=True,
scale=1.):
# im1 = img1_rgb
# im2 = img2_rgb
# x1 = x1_sample
# x2 = x2_sample
im_shape = im1.shape
assert im1.shape == im2.shape, 'Shape mismatch between im1 and im2! @draw_corr()'
x2_copy = x2.copy()
x2_copy[:, 0] = x2_copy[:, 0] + im_shape[1]
# lines2 = cv2.computeCorrespondEpilines(x1.reshape(-1,1,2).astype(int), 1,F_gt)
# lines2 = lines2.reshape(-1,3)
# im2, _, colors = drawlines(np.array(im2).copy(), np.array(im1).copy(), lines2, x2.astype(int), x1.astype(int), width=2)
im12 = np.hstack((im1, im2))
plt.figure(figsize=(60, 8))
plt.imshow(im12)
for i in range(x1.shape[0]):
if rescale:
width = 5 if linewidth[i] < 2 else 10
else:
width = linewidth[i] * scale
p = plt.plot(np.vstack((x1[i, 0], x2_copy[i, 0])),
np.vstack((x1[i, 1], x2_copy[i, 1])),
linewidth=width,
marker='o',
markersize=8)
print(p[0].get_color())
N_points = x1.shape[0]
x1_homo = utils_misc.homo_np(x1)
x2_homo = utils_misc.homo_np(x2)
right_P = np.matmul(F_gt, x1_homo.T)
right_epipolar_x = np.tile(np.array([[0], [1]]),
N_points) * im_shape[1]
# Using the eqn of line: ax+by+c=0; y = (-c-ax)/b, http://ai.stanford.edu/~mitul/cs223b/draw_epipolar.m
right_epipolar_y = (-right_P[2:3, :] - right_P[0:1, :] *
right_epipolar_x) / right_P[1:2, :]
colors = np.random.rand(x2.shape[0])
# plt.figure(figsize=(30, 8))
# plt.subplot(121)
# plt.imshow(img1_rgb)
# plt.scatter(x1[:, 0], x1[:, 1], s=50, c=colors, edgecolors='w')
# plt.subplot(122)
# # plt.figure(figsize=(30, 8))
# plt.imshow(img2_rgb)
plt.plot(right_epipolar_x + im_shape[1], right_epipolar_y)
# plt.scatter(x2[:, 0], x2[:, 1], s=50, c=colors, edgecolors='w')
plt.xlim(0, im_shape[1] * 2 - 1)
plt.ylim(im_shape[0] - 1, 0)
# plt.show()
plt.title(title, {'fontsize': 40})
plt.show()