Calculates the rotation matrices for a batch of rotation vectors Parameters ---------- rot_vecs: numpy.ndarray Nx3 array of N axis-angle vectors Returns ------- R: numpy.ndarray Nx3x3 The rotation matrices for the given axis-an
(rot_vecs, epsilon=1e-8)
| 1122 | |
| 1123 | |
| 1124 | def batch_rodrigues_numpy(rot_vecs, epsilon=1e-8): |
| 1125 | ''' Calculates the rotation matrices for a batch of rotation vectors |
| 1126 | Parameters |
| 1127 | ---------- |
| 1128 | rot_vecs: numpy.ndarray Nx3 |
| 1129 | array of N axis-angle vectors |
| 1130 | Returns |
| 1131 | ------- |
| 1132 | R: numpy.ndarray Nx3x3 |
| 1133 | The rotation matrices for the given axis-angle parameters |
| 1134 | ''' |
| 1135 | |
| 1136 | batch_size = rot_vecs.shape[0] |
| 1137 | |
| 1138 | angle = np.linalg.norm(rot_vecs + 1e-8, axis=1, keepdims=True) |
| 1139 | rot_dir = rot_vecs / angle |
| 1140 | |
| 1141 | cos = np.cos(angle)[:, None, :] |
| 1142 | sin = np.sin(angle)[:, None, :] |
| 1143 | |
| 1144 | # Bx1 arrays |
| 1145 | rx, ry, rz = np.split(rot_dir, 3, axis=1) |
| 1146 | K = np.zeros((batch_size, 3, 3)) |
| 1147 | zeros = np.zeros((batch_size, 1)) |
| 1148 | |
| 1149 | K = np.concatenate([zeros, -rz, ry, rz, zeros, -rx, -ry, rx, zeros], axis=1) \ |
| 1150 | .reshape((batch_size, 3, 3)) |
| 1151 | |
| 1152 | ident = np.eye(3)[None, :, :] |
| 1153 | rot_mat = ident + sin * K + (1 - cos) * np.einsum('bij,bjk->bik', K, K) |
| 1154 | return rot_mat |
| 1155 | |
| 1156 | |
| 1157 | def shuffle_joints(joints, joint_pairs): |