Tanoda
KabschSolver.cs
Go to the documentation of this file.
1/******************************************************************************
2 * Copyright (C) Ultraleap, Inc. 2011-2020. *
3 * *
4 * Use subject to the terms of the Apache License 2.0 available at *
5 * http://www.apache.org/licenses/LICENSE-2.0, or another agreement *
6 * between Ultraleap and you, your company or other organization. *
7 ******************************************************************************/
8
9using UnityEngine;
10using System.Collections.Generic;
11
12namespace Leap.Unity.Interaction {
13
14 public class KabschSolver {
15 Vector3[] QuatBasis = new Vector3[3];
16 Vector3[] DataCovariance = new Vector3[3];
17 public Vector3 translation = Vector3.zero;
18 public Quaternion OptimalRotation = Quaternion.identity;
19 public float scaleRatio = 1f;
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);
24
25 //Calculate the centroid offset and construct the centroid-shifted point matrices
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];
30 }
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;
36 }
37 translation = refCentroid - inCentroid;
38
39 //Calculate the scale ratio
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;
45 }
46 scaleRatio = (refScale / inScale);
47 } else { scaleRatio = 1f; }
48
49 if (inPoints.Count != 1) {
50 //Calculate the covariance matrix, is a 3x3 Matrix and Calculate the optimal rotation
51 extractRotation(TransposeMult(inPts, refPts, DataCovariance), ref OptimalRotation, optimalRotationIterations);
52 } else {
53 OptimalRotation = Quaternion.identity;
54 }
55
56 return
57 Matrix4x4.TRS(refCentroid, Quaternion.identity, Vector3.one * scaleRatio) *
58 Matrix4x4.TRS(Vector3.zero, OptimalRotation, Vector3.one) *
59 Matrix4x4.TRS(-inCentroid, Quaternion.identity, Vector3.one);
60 }
61
62 //https://animation.rwth-aachen.de/media/papers/2016-MIG-StableRotation.pdf
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));
72
73 float w = omega.magnitude;
74 if (w < 0.000000001f)
75 break;
76 q = Quaternion.AngleAxis(w * Mathf.Rad2Deg, omega / w) * q;
77 q = Quaternion.Lerp(q, q, 0f); //Normalizes the Quaternion; critical for error suppression
78 }
79 }
80
81 //Calculate Covariance Matrices --------------------------------------------------
82 Vector3[] TransposeMult(List<Vector3> vec1, List<Vector3> vec2, Vector3[] covariance) {
83 for (int i = 0; i < 3; i++) { //i is the row in this matrix
84 covariance[i] = Vector3.zero;
85 for (int j = 0; j < 3; j++) {//j is the column in the other matrix
86 for (int k = 0; k < vec1.Count; k++) {//k is the column in this matrix
87 covariance[i][j] += vec1[k][i] * vec2[k][j];
88 }
89 }
90 }
91 return covariance;
92 }
93
94 public static Vector3[] MatrixFromQuaternion(Quaternion q, Vector3[] covariance) {
95 covariance[0] = q * Vector3.right;
96 covariance[1] = q * Vector3.up;
97 covariance[2] = q * Vector3.forward;
98 return covariance;
99 }
100 }
101
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));
107 }
108 }
109}
static Vector3[] MatrixFromQuaternion(Quaternion q, Vector3[] covariance)
Definition: KabschSolver.cs:94
Matrix4x4 SolveKabsch(List< Vector3 > inPoints, List< Vector3 > refPoints, int optimalRotationIterations=9, bool solveScale=false)
Definition: KabschSolver.cs:20