calculate s, R, t so that: gt = R * s * est + t method can be: sim3, se3, posyaw, none; n_aligned: -1 means using all the frames
(p_es, p_gt, q_es, q_gt, method, n_aligned=-1)
| 118 | |
| 119 | # a general interface |
| 120 | def alignTrajectory(p_es, p_gt, q_es, q_gt, method, n_aligned=-1): |
| 121 | ''' |
| 122 | calculate s, R, t so that: |
| 123 | gt = R * s * est + t |
| 124 | method can be: sim3, se3, posyaw, none; |
| 125 | n_aligned: -1 means using all the frames |
| 126 | ''' |
| 127 | assert p_es.shape[1] == 3 |
| 128 | assert p_gt.shape[1] == 3 |
| 129 | assert q_es.shape[1] == 4 |
| 130 | assert q_gt.shape[1] == 4 |
| 131 | |
| 132 | s = 1 |
| 133 | R = None |
| 134 | t = None |
| 135 | if method == 'sim3': |
| 136 | assert n_aligned >= 2 or n_aligned == -1, "sim3 uses at least 2 frames" |
| 137 | s, R, t = alignSIM3(p_es, p_gt, q_es, q_gt, n_aligned) |
| 138 | elif method == 'se3': |
| 139 | R, t = alignSE3(p_es, p_gt, q_es, q_gt, n_aligned) |
| 140 | elif method == 'posyaw': |
| 141 | R, t = alignPositionYaw(p_es, p_gt, q_es, q_gt, n_aligned) |
| 142 | elif method == 'none': |
| 143 | R = np.identity(3) |
| 144 | t = np.zeros((3, )) |
| 145 | else: |
| 146 | assert False, 'unknown alignment method' |
| 147 | |
| 148 | return s, R, t |
| 149 | |
| 150 | |
| 151 | if __name__ == '__main__': |
no test coverage detected