(R_est,t_est,R_gt,t_gt)
| 125 | return scale1 / scale2 , solution.rotation, scale1 * solution.translation.reshape(3,1) |
| 126 | |
| 127 | def ATE_TEASER_C2W(R_est,t_est,R_gt,t_gt): |
| 128 | # R_est: 3x3N |
| 129 | # t_est: 3xN |
| 130 | # R_gt: 3x3N |
| 131 | # t_gt: 3xN |
| 132 | # return: ATE |
| 133 | N = int(R_est.shape[1]/3) |
| 134 | assert R_est.shape == R_gt.shape |
| 135 | assert t_est.shape == t_gt.shape |
| 136 | assert N == t_est.shape[1] |
| 137 | |
| 138 | # find global transformation |
| 139 | dof = 3 |
| 140 | MeasurementNoiseStd = 0.1 |
| 141 | epsilon_square = chi2.ppf(0.9999, dof) * (MeasurementNoiseStd ** 2) |
| 142 | epsilon = np.sqrt(epsilon_square) |
| 143 | |
| 144 | solver_params = teaserpp_python.RobustRegistrationSolver.Params() |
| 145 | solver_params.cbar2 = 1 |
| 146 | solver_params.noise_bound = 0.1 |
| 147 | solver_params.estimate_scaling = False |
| 148 | solver_params.rotation_estimation_algorithm = teaserpp_python.RobustRegistrationSolver.ROTATION_ESTIMATION_ALGORITHM.GNC_TLS |
| 149 | solver_params.rotation_gnc_factor = 1.4 |
| 150 | solver_params.rotation_max_iterations = 100 |
| 151 | solver_params.rotation_cost_threshold = 1e-12 |
| 152 | |
| 153 | t_cam_gt = np.zeros((3, N)) |
| 154 | t_cam_est = np.zeros((3, N)) |
| 155 | for i in range(N): |
| 156 | t_cam_gt[:, i] = R_gt[:, 3 * i:3 * i + 3].T @ (-t_gt[:, i]) |
| 157 | t_cam_est[:, i] = t_est[:, i] |
| 158 | |
| 159 | src = t_cam_est |
| 160 | dst = t_cam_gt |
| 161 | |
| 162 | # estimate scale |
| 163 | dst_avg = trim_mean(dst, proportiontocut=0.05, axis=1) |
| 164 | src_avg = trim_mean(src, proportiontocut=0.05, axis=1) |
| 165 | dst_dis = np.linalg.norm(dst - dst_avg.reshape(3, 1), axis=0) |
| 166 | src_dis = np.linalg.norm(src - src_avg.reshape(3, 1), axis=0) |
| 167 | # delete 10% outliers |
| 168 | index = (src_dis < np.percentile(src_dis, 90)) & (dst_dis < np.percentile(dst_dis, 90)) |
| 169 | src = src[:, index] |
| 170 | dst = dst[:, index] |
| 171 | dst_avg = np.mean(dst, axis=1) |
| 172 | src_avg = np.mean(src, axis=1) |
| 173 | |
| 174 | scale1 = np.mean(np.linalg.norm(dst - dst_avg.reshape(3, 1), axis=0)) |
| 175 | scale2 = np.mean(np.linalg.norm(src - src_avg.reshape(3, 1), axis=0)) |
| 176 | |
| 177 | src = src / scale2 |
| 178 | dst = dst / scale1 |
| 179 | # randomly choose 5000 |
| 180 | if src.shape[1] > 5000: |
| 181 | idx = np.random.choice(src.shape[1], 5000, replace=False) |
| 182 | src = src[:, idx] |
| 183 | dst = dst[:, idx] |
| 184 |
no outgoing calls
no test coverage detected