| 53 | return c, R, t |
| 54 | |
| 55 | def rigid_transform_3D_batch(A, B): |
| 56 | n, dim = A.shape |
| 57 | centroid_A = np.mean(A, axis=0) |
| 58 | centroid_B = np.mean(B, axis=0) |
| 59 | H = np.dot(np.transpose(A - centroid_A), B - centroid_B) / n |
| 60 | U, s, V = np.linalg.svd(H) |
| 61 | R = np.dot(np.transpose(V), np.transpose(U)) |
| 62 | if np.linalg.det(R) < 0: |
| 63 | s[-1] = -s[-1] |
| 64 | V[2] = -V[2] |
| 65 | R = np.dot(np.transpose(V), np.transpose(U)) |
| 66 | |
| 67 | varP = np.var(A, axis=0).sum() |
| 68 | c = 1 / varP * np.sum(s) |
| 69 | |
| 70 | t = -np.dot(c * R, np.transpose(centroid_A)) + np.transpose(centroid_B) |
| 71 | A2 = np.transpose(np.dot(c * R, np.transpose(A))) + t |
| 72 | return A2 |
| 73 | |
| 74 | def rigid_align(A, B): |
| 75 | c, R, t = rigid_transform_3D(A, B) |