Align two trajectories using the method of Horn (closed-form). Args: model -- first trajectory (3xn) data -- second trajectory (3xn) Returns: rot -- rotation matrix (3x3) trans -- translation vector (3x1) trans_error -- translational error per point
(model, data)
| 21 | loss_fn_alex = LearnedPerceptualImagePatchSimilarity(net_type='alex', normalize=True).cuda() |
| 22 | |
| 23 | def align(model, data): |
| 24 | """Align two trajectories using the method of Horn (closed-form). |
| 25 | |
| 26 | Args: |
| 27 | model -- first trajectory (3xn) |
| 28 | data -- second trajectory (3xn) |
| 29 | |
| 30 | Returns: |
| 31 | rot -- rotation matrix (3x3) |
| 32 | trans -- translation vector (3x1) |
| 33 | trans_error -- translational error per point (1xn) |
| 34 | |
| 35 | """ |
| 36 | np.set_printoptions(precision=3, suppress=True) |
| 37 | model_zerocentered = model - model.mean(1).reshape((3,-1)) |
| 38 | data_zerocentered = data - data.mean(1).reshape((3,-1)) |
| 39 | |
| 40 | W = np.zeros((3, 3)) |
| 41 | for column in range(model.shape[1]): |
| 42 | W += np.outer(model_zerocentered[:, |
| 43 | column], data_zerocentered[:, column]) |
| 44 | U, d, Vh = np.linalg.linalg.svd(W.transpose()) |
| 45 | S = np.matrix(np.identity(3)) |
| 46 | if (np.linalg.det(U) * np.linalg.det(Vh) < 0): |
| 47 | S[2, 2] = -1 |
| 48 | rot = U*S*Vh |
| 49 | trans = data.mean(1).reshape((3,-1)) - rot * model.mean(1).reshape((3,-1)) |
| 50 | |
| 51 | model_aligned = rot * model + trans |
| 52 | alignment_error = model_aligned - data |
| 53 | |
| 54 | trans_error = np.sqrt(np.sum(np.multiply( |
| 55 | alignment_error, alignment_error), 0)).A[0] |
| 56 | |
| 57 | return rot, trans, trans_error |
| 58 | |
| 59 | |
| 60 | def evaluate_ate(gt_traj, est_traj): |