| 47 | [0,0,0,1]], dtype=np.float) |
| 48 | |
| 49 | def compute_error_in_q(args, dl, model, device, results, batch_size=1): |
| 50 | use_SVD=True # Turn on for Direct-PN and Direct-PN+U reported result, despite it makes minuscule differences |
| 51 | time_spent = [] |
| 52 | predict_pose_list = [] |
| 53 | gt_pose_list = [] |
| 54 | ang_error_list = [] |
| 55 | i = 0 |
| 56 | for batch in dl: |
| 57 | if args.NeRFH: |
| 58 | data, pose, img_idx = batch |
| 59 | else: |
| 60 | data, pose = batch |
| 61 | data = data.to(device) # input |
| 62 | pose = pose.reshape((batch_size,3,4)).numpy() # label |
| 63 | |
| 64 | if use_SVD: |
| 65 | # using SVD to make sure predict rotation is normalized rotation matrix |
| 66 | with torch.no_grad(): |
| 67 | _, predict_pose = model(data) |
| 68 | R_torch = predict_pose.reshape((batch_size, 3, 4))[:,:3,:3] # debug |
| 69 | predict_pose = predict_pose.reshape((batch_size, 3, 4)).cpu().numpy() |
| 70 | |
| 71 | R = predict_pose[:,:3,:3] |
| 72 | res = R@np.linalg.inv(R) |
| 73 | # print('R@np.linalg.inv(R):', res) |
| 74 | |
| 75 | u,s,v=torch.svd(R_torch) |
| 76 | Rs = torch.matmul(u, v.transpose(-2,-1)) |
| 77 | predict_pose[:,:3,:3] = Rs[:,:3,:3].cpu().numpy() |
| 78 | else: |
| 79 | start_time = time.time() |
| 80 | # inference NN |
| 81 | with torch.no_grad(): |
| 82 | predict_pose = model(data) |
| 83 | predict_pose = predict_pose.reshape((batch_size, 3, 4)).cpu().numpy() |
| 84 | time_spent.append(time.time() - start_time) |
| 85 | |
| 86 | pose_q = transforms.matrix_to_quaternion(torch.Tensor(pose[:,:3,:3]))#.cpu().numpy() # gnd truth in quaternion |
| 87 | pose_x = pose[:, :3, 3] # gnd truth position |
| 88 | predicted_q = transforms.matrix_to_quaternion(torch.Tensor(predict_pose[:,:3,:3]))#.cpu().numpy() # predict in quaternion |
| 89 | predicted_x = predict_pose[:, :3, 3] # predict position |
| 90 | pose_q = pose_q.squeeze() |
| 91 | pose_x = pose_x.squeeze() |
| 92 | predicted_q = predicted_q.squeeze() |
| 93 | predicted_x = predicted_x.squeeze() |
| 94 | |
| 95 | #Compute Individual Sample Error |
| 96 | q1 = pose_q / torch.linalg.norm(pose_q) |
| 97 | q2 = predicted_q / torch.linalg.norm(predicted_q) |
| 98 | d = torch.abs(torch.sum(torch.matmul(q1,q2))) |
| 99 | d = torch.clamp(d, -1., 1.) # acos can only input [-1~1] |
| 100 | theta = (2 * torch.acos(d) * 180/math.pi).numpy() |
| 101 | error_x = torch.linalg.norm(torch.Tensor(pose_x-predicted_x)).numpy() |
| 102 | results[i,:] = [error_x, theta] |
| 103 | #print ('Iteration: {} Error XYZ (m): {} Error Q (degrees): {}'.format(i, error_x, theta)) |
| 104 | |
| 105 | # save results for visualization |
| 106 | predict_pose_list.append(predicted_x) |