10using System.Collections.Generic;
15 Vector3[] QuatBasis =
new Vector3[3];
16 Vector3[] DataCovariance =
new Vector3[3];
20 public Matrix4x4
SolveKabsch(List<Vector3> inPoints, List<Vector3> refPoints,
21 int optimalRotationIterations = 9,
bool solveScale =
false) {
22 if (inPoints.Count != refPoints.Count) {
return Matrix4x4.identity; }
23 List<Vector3> inPts =
new List<Vector3>(inPoints), refPts =
new List<Vector3>(refPoints);
26 Vector3 inCentroid = Vector3.zero; Vector3 refCentroid = Vector3.zero;
27 for (
int i = 0; i < inPts.Count; i++) {
28 inCentroid += inPts[i];
29 refCentroid += refPts[i];
31 inCentroid /= inPts.Count;
32 refCentroid /= refPts.Count;
33 for (
int i = 0; i < inPts.Count; i++) {
34 inPts[i] -= inCentroid;
35 refPts[i] -= refCentroid;
40 if (solveScale && inPoints.Count > 1) {
41 float inScale = 0f, refScale = 0f;
42 for (
int i = 0; i < inPoints.Count; i++) {
43 inScale += (
new Vector3(inPoints[i].x, inPoints[i].y, inPoints[i].z) - inCentroid).magnitude;
44 refScale += (
new Vector3(refPoints[i].x, refPoints[i].y, refPoints[i].z) - refCentroid).magnitude;
49 if (inPoints.Count != 1) {
51 extractRotation(TransposeMult(inPts, refPts, DataCovariance), ref
OptimalRotation, optimalRotationIterations);
57 Matrix4x4.TRS(refCentroid, Quaternion.identity, Vector3.one *
scaleRatio) *
59 Matrix4x4.TRS(-inCentroid, Quaternion.identity, Vector3.one);
63 void extractRotation(Vector3[] A, ref Quaternion q,
int optimalRotationIterations = 9) {
64 for (
int iter = 0; iter < optimalRotationIterations; iter++) {
65 q.FillMatrixFromQuaternion(ref QuatBasis);
66 Vector3 omega = (Vector3.Cross(QuatBasis[0], A[0]) +
67 Vector3.Cross(QuatBasis[1], A[1]) +
68 Vector3.Cross(QuatBasis[2], A[2])) *
69 (1f / Mathf.Abs(Vector3.Dot(QuatBasis[0], A[0]) +
70 Vector3.Dot(QuatBasis[1], A[1]) +
71 Vector3.Dot(QuatBasis[2], A[2]) + 0.000000001f));
73 float w = omega.magnitude;
76 q = Quaternion.AngleAxis(w * Mathf.Rad2Deg, omega / w) * q;
77 q = Quaternion.Lerp(q, q, 0f);
82 Vector3[] TransposeMult(List<Vector3> vec1, List<Vector3> vec2, Vector3[] covariance) {
83 for (
int i = 0; i < 3; i++) {
84 covariance[i] = Vector3.zero;
85 for (
int j = 0; j < 3; j++) {
86 for (
int k = 0; k < vec1.Count; k++) {
87 covariance[i][j] += vec1[k][i] * vec2[k][j];
95 covariance[0] = q * Vector3.right;
96 covariance[1] = q * Vector3.up;
97 covariance[2] = q * Vector3.forward;
102 public static class FromMatrixExtension {
103 public static Vector3 GetVector3(
this Matrix4x4 m) {
return m.GetColumn(3); }
104 public static Quaternion GetQuaternion(
this Matrix4x4 m) {
105 if (m.GetColumn(2) == m.GetColumn(1)) {
return Quaternion.identity; }
106 return Quaternion.LookRotation(m.GetColumn(2), m.GetColumn(1));
static Vector3[] MatrixFromQuaternion(Quaternion q, Vector3[] covariance)
Quaternion OptimalRotation
Matrix4x4 SolveKabsch(List< Vector3 > inPoints, List< Vector3 > refPoints, int optimalRotationIterations=9, bool solveScale=false)