def step_fn(step, inputs):
# Forward pass and loss
with torch.no_grad():
loss, data = utils.forward_pass(model, loss_fn, inputs)
print("loss %f" % loss.item())
print(data.keys())
print(data["pose"].shape)
for i in range(4):
print(list(data["pose"][i, 0, :].cpu().detach().numpy()))
print(list(data["pose"][i, 1, :].cpu().detach().numpy()))
print("--")
depth_img = viz.tensor2depthimg(
torch.cat((*data["depth"][0][:, 0], ), dim=0))
tgt_img = viz.tensor2img(torch.cat((*data["tgt"], ), dim=1))
img = np.concatenate((tgt_img, depth_img), axis=1)
warp_imgs = []
diff_imgs = []
for warp, diff in zip(data["warp"], data["diff"]):
warp = restack(restack(warp, 1, -1), 0, -2)
diff = restack(restack(diff, 1, -1), 0, -2)
warp_imgs.append(viz.tensor2img(warp))
diff_imgs.append(viz.tensor2diffimg(diff))
world = inverse_warp.depth_to_3d_points(data["depth"][0], data["K"])
points = world[0, :].view(3, -1).transpose(
1, 0).cpu().detach().numpy().astype(np.float64)
colors = (data["tgt"][0, :].view(3, -1).transpose(
1, 0).cpu().detach().numpy().astype(np.float64) + 1) / 2
point_cloud.points = o3d.open3d.Vector3dVector(points)
point_cloud.colors = o3d.open3d.Vector3dVector(colors)
vis.add_geometry(point_cloud)
loop = True
while loop:
key = cv2.waitKey(10)
if key == 27:
exit()
elif key != -1:
loop = False
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
cv2.imshow("target and depth", img)
for i, (warp, diff) in enumerate(zip(warp_imgs, diff_imgs)):
cv2.imshow("warp scale: %d" % i, warp)
cv2.imshow("diff scale: %d" % i, diff)
def main():
dataset = SequenceDataset(sys.argv[1])
vis = o3d.Visualizer()
vis.create_window()
point_cloud = o3d.geometry.PointCloud()
vis.add_geometry(point_cloud)
for i in range(0, len(dataset)):
tgt, refs, K, Kinv, sparse, dense, tgt_pose, ref_pose = dataset[i]
print(tgt_pose)
exit()
pose_b = pose.unsqueeze(0)
dense_b = dense.unsqueeze(0).unsqueeze(0)
ref_b = refs[1].unsqueeze(0)
K_b = K.unsqueeze(0)
warp, valid, world, projpixel, grid = inverse_warp.inverse_warp(
ref_b, dense_b, pose_b, K_b)
# Vizualisation
colors = (ref_b[0, :].view(3, -1).transpose(1, 0) + 1) / 2
points = world[0, :3].view(3, -1).transpose(1, 0)
point_cloud.points = o3d.open3d.Vector3dVector(points)
point_cloud.colors = o3d.open3d.Vector3dVector(colors)
vis.add_geometry(point_cloud)
img = torch.cat((refs[0], tgt, refs[1]), dim=1)
while cv2.waitKey(10) != 27:
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
cv2.imshow("img", viz.tensor2img(img))
cv2.imshow("dense", viz.tensor2depthimg(dense))
cv2.imshow("warp", viz.tensor2img(warp.squeeze(0)))
def simple_inspector(data):
img = data["img"]
warp = data["warp"]
homography = data["homography"]
cv2.imshow("img", viz.tensor2img(img))
cv2.imshow("warp", viz.tensor2img(warp))
def sfm_inspector(data):
global K
K = utils.torch_to_numpy(data["K"])
img = torch.cat((data["refs"][0], data["tgt"], data["refs"][1]), dim=1)
cv2.imshow("img", viz.tensor2img(img))
cv2.imshow("gt_sparse", viz.tensor2depthimg(data["gt_sparse"]))
torch.linspace(-H/2,H/2,H)), dim=2) \
.transpose(0,1) \
.to(img.device) \
.contiguous()
warped_grid = homo_warp_grid(grid, torch.inverse(homography))
warped_grid[:,:,:,0] /= (W/2)
warped_grid[:,:,:,1] /= (H/2)
warped_img = F.grid_sample(img, warped_grid, mode="bilinear", align_corners=True)
if not has_batch:
warped_img = warped_img.squeeze(0)
return warped_img
if __name__ == "__main__":
img = torch.tensor(np.random.rand(4,3,320,460), dtype=torch.float32)
while True:
homography = random_homographies(img.shape[0], rotation=np.pi/8, translation=50, scale=0.1, sheer=0.1, projective=0.001)
warp = warp_image(img, homography)
img_stack = torch.cat((*img,), dim=2)
warp_stack = torch.cat((*warp,), dim=2)
cv2.imshow("img", viz.tensor2img(img_stack))
cv2.imshow("warp", viz.tensor2img(warp_stack))
key = cv2.waitKey(0)
if key == 27:
break
def _step_fn(self, step, inputs):
# Forward pass and loss
with torch.no_grad():
loss, data = utils.forward_pass(self.model, self.loss_fn, inputs)
print(f"loss {loss.item():.3f}")
for i in range(data["pose"].shape[1]):
pose = list(data["pose"][0, i, :].cpu().detach().numpy())
#print("pose %d -> x: %.6f, y: %.6f, z: %.6f, rx: %.6f, ry: %.6f, rz: %.6f" % (i, *pose))
poses = data["pose"]
T0 = utils.torch_to_numpy(
geometry.to_homog_matrix(geometry.pose_vec2mat(
poses[:, 1])).squeeze(0))
T1 = np.identity(4)
T1[:3, 3] = 0
T2 = utils.torch_to_numpy(
geometry.to_homog_matrix(geometry.pose_vec2mat(
poses[:, 0])).squeeze(0))
T_gt = utils.torch_to_numpy(data["T"].squeeze(0))
T0_gt = T_gt[0]
T1_gt = np.identity(4)
T1_gt[:3, 3] = 0
T2_gt = T_gt[1]
Ta, Tb, Tc = T0.copy(), T1.copy(), T2.copy()
Ta_gt, Tb_gt, Tc_gt = T0_gt.copy(), T1_gt.copy(), T2_gt.copy()
# Trajectory
if self.prev_tgt_i != data["tgt_i"] - 1 or self.scale is None:
self.positions = [] # New sequence!
self.positions_gt = []
self.scale = np.linalg.norm(Tc_gt[:3, -1] - Ta_gt[:3, -1]
) / np.linalg.norm(Tc[:3, -1] - Ta[:3, -1])
self.prev_tgt_i = data["tgt_i"]
Ta_gt[:3, -1] /= self.scale
Tc_gt[:3, -1] /= self.scale
print(Tc_gt)
print(Tc)
if len(self.positions) == 0:
self.positions = [Ta, Tb, Tc]
self.positions_gt = [Ta_gt, Tb_gt, Tc_gt]
else:
inv = np.linalg.pinv(self.positions[-1])
self.positions = [inv @ T for T in self.positions]
self.positions.append(Tc)
inv_gt = np.linalg.pinv(self.positions_gt[-1])
self.positions_gt = [inv_gt @ T_gt for T_gt in self.positions_gt]
self.positions_gt.append(Tc_gt)
# Debug images
depth_img = viz.tensor2depthimg(data["depth"][0][0, 0])
tgt_img = viz.tensor2img(data["tgt"][0])
img = np.concatenate((tgt_img, depth_img), axis=1)
tgtrefs = viz.tensor2img(
torch.cat((data["refs"][0, 0], data["tgt"][0], data["refs"][0, 1]),
dim=1))
points, colors = to_points_3d(data["tgt"][0], data["depth"][0],
data["K"])
loop = True
while loop:
key = cv2.waitKey(10)
if key == 27 or self.renderer.should_quit():
exit()
elif key != -1:
loop = False
cv2.imshow("target and depth", img)
cv2.imshow("target and refs", tgtrefs)
self.renderer.clear_screen()
self.renderer.draw_points(points, colors)
line = [T[:3, 3] for T in self.positions]
line_gt = [T[:3, 3] for T in self.positions_gt]
self.renderer.draw_line(line, color=(1., 0., 0.))
self.renderer.draw_line(line_gt, color=(0., 1., 0.))
#self.renderer.draw_cameras([T0], color=(1.,0.,0.))
#self.renderer.draw_cameras([T1], color=(0.,1.,0.))
#self.renderer.draw_cameras([T2], color=(0.,0.,1.))
self.renderer.finish_frame()
def _debug_step(self, loss, data):
while True:
print(f"b = {self.b}")
self.prel1 = utils.torch_to_numpy(
data["A"]["Prel"][self.b].transpose(0, 1))
self.prel2 = utils.torch_to_numpy(
data["B"]["Prel"][self.b].transpose(0, 1))
self.prelflat = np.concatenate(
(self.prel1.flatten(), self.prel2.flatten()))
self.img = viz.tensor2img(data["img"][self.b])
self.warp = viz.tensor2img(data["warp"][self.b])
self.AF = data["A"]["F"]
self.BF = data["B"]["F"]
self.B = self.BF.shape[0]
self.des1 = utils.torch_to_numpy(self.AF[self.b].transpose(0, 1))
self.des2 = utils.torch_to_numpy(self.BF[self.b].transpose(0, 1))
self.s1 = utils.torch_to_numpy(data["A"]["S"][self.b])
self.s2 = utils.torch_to_numpy(data["B"]["S"][self.b])
self.p1 = utils.torch_to_numpy(data["A"]["P"][self.b].transpose(
0, 1))
self.p2 = utils.torch_to_numpy(data["B"]["P"][self.b].transpose(
0, 1))
self.img_matches = []
self.inliers = []
if True: # cv2 match descriptor
bf = cv2.BFMatcher.create(cv2.NORM_L2, crossCheck=True)
matches = bf.match(self.des1, self.des2)
#matches = sorted(matches, key = lambda x: -x.distance)
print(self.des1.shape, len(matches))
#matches = matches[:20]
kp1 = [cv2.KeyPoint(xy[0], xy[1], 2) for xy in self.p1]
kp2 = [cv2.KeyPoint(xy[0], xy[1], 2) for xy in self.p2]
self.img_matches.append(
viz.draw_text(
"CV2 BFMatcher",
cv2.drawMatches(self.img,
kp1,
self.warp,
kp2,
matches,
flags=2,
outImg=None)))
self._compute_debug(loss, data)
self.img_matches = np.concatenate(
[img for img in self.img_matches])
plt.ion()
fig = plt.figure(1)
plt.clf()
while True:
key = cv2.waitKey(10)
if key == 27: # esc
print("exit")
exit()
elif key == 119: # w
self.b = min(self.b + 1, self.B - 1)
break
elif key == 115: # s
self.b = max(self.b - 1, 0)
break
elif key == 32: # space
print("next")
return
cv2.imshow("matches", self.img_matches)
plt.hist(self.prelflat, 200, (0., 1.), color=(0, 0, 1))
#plt.hist(inliers, 10, (0.,1.), color=(0,0,1))
fig.canvas.flush_events()
if self.load_gt:
sparse = np.load(paths[0][:-4] + ".npy").astype(np.float32)
dense = np.load(paths[0][:-4] + "_dense.npy").astype(
np.float32)
tgt_pose = self.poses[i][tgt]
ref_pose = [self.poses[i][tgt - 1], self.poses[i][tgt + 1]]
data = {
**data, "gt_sparse": torch.tensor(sparse).unsqueeze(0),
"gt_dense": torch.tensor(dense).unsqueeze(0),
"gt_pose": torch.tensor(0)
}
return data
if __name__ == '__main__':
dataset = SequenceDataset(sys.argv[1])
for i in range(0, len(dataset)):
data = dataset[i]
print(data["gt_sparse"].shape)
#print(relative_transform(tgt_pose, ref_pose[0]))
#print(relative_transform(tgt_pose, ref_pose[1]))
#print("---")
img = torch.cat((data["refs"][0], data["tgt"], data["refs"][1]), dim=1)
cv2.imshow("img", viz.tensor2img(img))
cv2.imshow("gt_sparse", viz.tensor2depthimg(data["gt_sparse"]))
cv2.imshow("gt_dense", viz.tensor2depthimg(data["gt_dense"]))
key = cv2.waitKey(0)
if key == 27:
break