(args, dl, model, device, results, batch_size=1)
| 77 | plt.savefig(fname, dpi=50) |
| 78 | |
| 79 | def compute_error_in_q(args, dl, model, device, results, batch_size=1): |
| 80 | use_SVD=True # Turn on for Direct-PN and Direct-PN+U reported result, despite it makes minuscule differences |
| 81 | time_spent = [] |
| 82 | predict_pose_list = [] |
| 83 | gt_pose_list = [] |
| 84 | ang_error_list = [] |
| 85 | pose_result_raw = [] |
| 86 | pose_GT = [] |
| 87 | i = 0 |
| 88 | |
| 89 | for batch in dl: |
| 90 | if args.NeRFH: |
| 91 | data, pose, img_idx = batch |
| 92 | else: |
| 93 | data, pose = batch |
| 94 | data = data.to(device) # input |
| 95 | pose = pose.reshape((batch_size,3,4)).numpy() # label |
| 96 | |
| 97 | if args.preprocess_ImgNet: |
| 98 | data = preprocess_data(data, device) |
| 99 | |
| 100 | if use_SVD: |
| 101 | # using SVD to make sure predict rotation is normalized rotation matrix |
| 102 | with torch.no_grad(): |
| 103 | if args.featuremetric: |
| 104 | _, predict_pose = model(data) |
| 105 | else: |
| 106 | predict_pose = model(data) |
| 107 | |
| 108 | R_torch = predict_pose.reshape((batch_size, 3, 4))[:,:3,:3] # debug |
| 109 | predict_pose = predict_pose.reshape((batch_size, 3, 4)).cpu().numpy() |
| 110 | |
| 111 | R = predict_pose[:,:3,:3] |
| 112 | res = R@np.linalg.inv(R) |
| 113 | # print('R@np.linalg.inv(R):', res) |
| 114 | |
| 115 | u,s,v=torch.svd(R_torch) |
| 116 | Rs = torch.matmul(u, v.transpose(-2,-1)) |
| 117 | predict_pose[:,:3,:3] = Rs[:,:3,:3].cpu().numpy() |
| 118 | else: |
| 119 | start_time = time.time() |
| 120 | # inference NN |
| 121 | with torch.no_grad(): |
| 122 | predict_pose = model(data) |
| 123 | predict_pose = predict_pose.reshape((batch_size, 3, 4)).cpu().numpy() |
| 124 | time_spent.append(time.time() - start_time) |
| 125 | |
| 126 | pose_q = transforms.matrix_to_quaternion(torch.Tensor(pose[:,:3,:3]))#.cpu().numpy() # gnd truth in quaternion |
| 127 | pose_x = pose[:, :3, 3] # gnd truth position |
| 128 | predicted_q = transforms.matrix_to_quaternion(torch.Tensor(predict_pose[:,:3,:3]))#.cpu().numpy() # predict in quaternion |
| 129 | predicted_x = predict_pose[:, :3, 3] # predict position |
| 130 | pose_q = pose_q.squeeze() |
| 131 | pose_x = pose_x.squeeze() |
| 132 | predicted_q = predicted_q.squeeze() |
| 133 | predicted_x = predicted_x.squeeze() |
| 134 | |
| 135 | #Compute Individual Sample Error |
| 136 | q1 = pose_q / torch.linalg.norm(pose_q) |
no test coverage detected