Function ATE_LEASTSQUARE

utils/error.py:213–249

(R_est,t_est,R_gt,t_gt)

def ATE_LEASTSQUARE(R_est,t_est,R_gt,t_gt):
    # R_est: 3x3N
    # t_est: 3xN
    # R_gt: 3x3N
    # t_gt: 3xN
    # return: ATE
    N = int(R_est.shape[1]/3)
    assert R_est.shape == R_gt.shape
    assert t_est.shape == t_gt.shape
    assert N == t_est.shape[1]

    # find global transformation
    # find transformation
    rotations_target = [R_gt[:, 3 * i:3 * (i + 1)] @ R_est[:, 3 * i:3 * (i + 1)].T for i in range(N)]
    R = average_rotation_from_3x3n(rotations_target)
    rotation_error = [np.linalg.norm(R @ R_est[:, 3 * i:3 * (i + 1)] - R_gt[:, 3 * i:3 * (i + 1)]) for i in range(N)]
    print('rotation error:',np.mean(rotation_error))

    target = R @ t_est 
    target_avg = np.mean(target, axis=1)
    target = target - target_avg.reshape(3, 1)


    # find scale
    t_gt_avg = np.mean(t_gt, axis=1)
    cov_t_gt = np.mean(np.linalg.norm(t_gt - t_gt_avg.reshape(3, 1), axis=0))
    cov_t_est = np.mean(np.linalg.norm(target, axis=0))
    s = cov_t_gt / cov_t_est

    target = s * target

    # find translation
    t = t_gt - target
    t_avg = np.mean(t, axis=1)

    # (R * t_est - target_avg) * s + t_avg
    return s, R, t_avg.reshape(3, 1) - target_avg.reshape(3 , 1) * s

if __name__ == "__main__":
    N = 5