(R_est,t_est,R_gt,t_gt)
| 39 | return np.arccos(cosvalue) |
| 40 | |
| 41 | def ATE_TEASER(R_est,t_est,R_gt,t_gt): |
| 42 | # R_est: 3x3N |
| 43 | # t_est: 3xN |
| 44 | # R_gt: 3x3N |
| 45 | # t_gt: 3xN |
| 46 | # return: ATE |
| 47 | N = int(R_est.shape[1]/3) |
| 48 | assert R_est.shape == R_gt.shape |
| 49 | assert t_est.shape == t_gt.shape |
| 50 | assert N == t_est.shape[1] |
| 51 | |
| 52 | # find global transformation |
| 53 | dof = 3 |
| 54 | MeasurementNoiseStd = 0.1 |
| 55 | epsilon_square = chi2.ppf(0.9999, dof) * (MeasurementNoiseStd ** 2) |
| 56 | epsilon = np.sqrt(epsilon_square) |
| 57 | |
| 58 | solver_params = teaserpp_python.RobustRegistrationSolver.Params() |
| 59 | solver_params.cbar2 = 1 |
| 60 | solver_params.noise_bound = 1 |
| 61 | solver_params.estimate_scaling = False |
| 62 | solver_params.rotation_estimation_algorithm = teaserpp_python.RobustRegistrationSolver.ROTATION_ESTIMATION_ALGORITHM.GNC_TLS |
| 63 | solver_params.rotation_gnc_factor = 1.4 |
| 64 | solver_params.rotation_max_iterations = 100 |
| 65 | solver_params.rotation_cost_threshold = 1e-12 |
| 66 | |
| 67 | t_cam_gt = np.zeros((3, N)) |
| 68 | t_cam_est = np.zeros((3, N)) |
| 69 | for i in range(N): |
| 70 | t_cam_gt[:, i] = R_gt[:, 3 * i:3 * i + 3].T @ (-t_gt[:, i]) |
| 71 | t_cam_est[:, i] = R_est[:, 3 * i:3 * i + 3].T @ (-t_est[:, i]) |
| 72 | |
| 73 | src = t_cam_est |
| 74 | dst = t_cam_gt |
| 75 | |
| 76 | # estimate scale |
| 77 | dst_avg = trim_mean(dst, proportiontocut=0.05, axis=1) |
| 78 | src_avg = trim_mean(src, proportiontocut=0.05, axis=1) |
| 79 | dst_dis = np.linalg.norm(dst - dst_avg.reshape(3, 1), axis=0) |
| 80 | src_dis = np.linalg.norm(src - src_avg.reshape(3, 1), axis=0) |
| 81 | # delete 10% outliers |
| 82 | index = src_dis < np.percentile(src_dis, 90) |
| 83 | src = src[:, index] |
| 84 | dst = dst[:, index] |
| 85 | dst_avg = np.mean(dst, axis=1) |
| 86 | src_avg = np.mean(src, axis=1) |
| 87 | |
| 88 | scale1 = np.mean(np.linalg.norm(dst - dst_avg.reshape(3, 1), axis=0)) |
| 89 | scale2 = np.mean(np.linalg.norm(src - src_avg.reshape(3, 1), axis=0)) |
| 90 | |
| 91 | src = src / np.mean(np.linalg.norm(src - src_avg.reshape(3, 1), axis=0)) |
| 92 | dst = dst / np.mean(np.linalg.norm(dst - dst_avg.reshape(3, 1), axis=0)) |
| 93 | |
| 94 | # randomly choose 5000 |
| 95 | if src.shape[1] > 5000: |
| 96 | idx = np.random.choice(src.shape[1], 5000, replace=False) |
| 97 | src = src[:, idx] |
| 98 | dst = dst[:, idx] |
nothing calls this directly
no outgoing calls
no test coverage detected