| 35 | |
| 36 | |
| 37 | def rigid_transform_3D(A, B): |
| 38 | n, dim = A.shape |
| 39 | centroid_A = np.mean(A, axis=0) |
| 40 | centroid_B = np.mean(B, axis=0) |
| 41 | H = np.dot(np.transpose(A - centroid_A), B - centroid_B) / n |
| 42 | U, s, V = np.linalg.svd(H) |
| 43 | R = np.dot(np.transpose(V), np.transpose(U)) |
| 44 | if np.linalg.det(R) < 0: |
| 45 | s[-1] = -s[-1] |
| 46 | V[2] = -V[2] |
| 47 | R = np.dot(np.transpose(V), np.transpose(U)) |
| 48 | |
| 49 | varP = np.var(A, axis=0).sum() |
| 50 | c = 1 / varP * np.sum(s) |
| 51 | |
| 52 | t = -np.dot(c * R, np.transpose(centroid_A)) + np.transpose(centroid_B) |
| 53 | return c, R, t |
| 54 | |
| 55 | def rigid_transform_3D_batch(A, B): |
| 56 | n, dim = A.shape |