From 7291810f5a9b27605699f8dfc1fbab1b18b1ccf5 Mon Sep 17 00:00:00 2001
From: Radek Oslejsek <oslejsek@fi.muni.cz>
Date: Sun, 2 May 2021 13:18:22 +0200
Subject: [PATCH] Final refactoring
---
.../analyst/icp/EigenvalueDecomposition.java | 133 +++++++-----------
.../java/cz/fidentis/analyst/icp/Icp.java | 2 +-
.../cz/fidentis/analyst/icp/Quaternion.java | 3 +-
.../analyst/tests/EfficiencyTests.java | 2 +-
4 files changed, 55 insertions(+), 85 deletions(-)
diff --git a/Comparison/src/main/java/cz/fidentis/analyst/icp/EigenvalueDecomposition.java b/Comparison/src/main/java/cz/fidentis/analyst/icp/EigenvalueDecomposition.java
index 2f75f97a..4a09774d 100644
--- a/Comparison/src/main/java/cz/fidentis/analyst/icp/EigenvalueDecomposition.java
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/EigenvalueDecomposition.java
@@ -427,7 +427,6 @@ public class EigenvalueDecomposition implements java.io.Serializable {
// Fortran subroutine in EISPACK.
// Initialize
-
int nn = this.n;
int n = nn-1;
int low = 0;
@@ -437,25 +436,13 @@ public class EigenvalueDecomposition implements java.io.Serializable {
double p=0,q=0,r=0,s=0,z=0,t,w,x,y;
// Store roots isolated by balanc and compute matrix norm
-
- double norm = 0.0;
- for (int i = 0; i < nn; i++) {
- if (i < low | i > high) {
- d[i] = arrayH[i][i];
- e[i] = 0.0;
- }
- for (int j = Math.max(i-1,0); j < nn; j++) {
- norm = norm + Math.abs(arrayH[i][j]);
- }
- }
-
+ double norm = storeRootsGetNorm(nn, low, high);
+
// Outer loop over eigenvalue index
-
int iter = 0;
while (n >= low) {
// Look for single small sub-diagonal element
-
int l = n;
while (l > low) {
s = Math.abs(arrayH[l-1][l-1]) + Math.abs(arrayH[l][l]);
@@ -469,18 +456,13 @@ public class EigenvalueDecomposition implements java.io.Serializable {
}
// Check for convergence
- // One root found
-
- if (l == n) {
+ if (l == n) { // One root found
arrayH[n][n] = arrayH[n][n] + exshift;
d[n] = arrayH[n][n];
e[n] = 0.0;
n--;
iter = 0;
-
- // Two roots found
-
- } else if (l == n-1) {
+ } else if (l == n-1) { // Two roots found
w = arrayH[n][n-1] * arrayH[n-1][n];
p = (arrayH[n-1][n-1] - arrayH[n][n]) / 2.0;
q = p * p + w;
@@ -489,14 +471,8 @@ public class EigenvalueDecomposition implements java.io.Serializable {
arrayH[n-1][n-1] = arrayH[n-1][n-1] + exshift;
x = arrayH[n][n];
- // Real pair
-
- if (q >= 0) {
- if (p >= 0) {
- z = p + z;
- } else {
- z = p - z;
- }
+ if (q >= 0) { // Real pair
+ z = (p >= 0) ? p + z : p - z;
d[n-1] = x + z;
d[n] = d[n-1];
if (z != 0.0) {
@@ -512,33 +488,24 @@ public class EigenvalueDecomposition implements java.io.Serializable {
p = p / r;
q = q / r;
- // Row modification
-
- for (int j = n-1; j < nn; j++) {
+ for (int j = n-1; j < nn; j++) { // Row modification
z = arrayH[n-1][j];
arrayH[n-1][j] = q * z + p * arrayH[n][j];
arrayH[n][j] = q * arrayH[n][j] - p * z;
}
- // Column modification
-
- for (int i = 0; i <= n; i++) {
+ for (int i = 0; i <= n; i++) { // Column modification
z = arrayH[i][n-1];
arrayH[i][n-1] = q * z + p * arrayH[i][n];
arrayH[i][n] = q * arrayH[i][n] - p * z;
}
- // Accumulate transformations
-
- for (int i = low; i <= high; i++) {
+ for (int i = low; i <= high; i++) { // Accumulate transformations
z = arrayV[i][n-1];
arrayV[i][n-1] = q * z + p * arrayV[i][n];
arrayV[i][n] = q * arrayV[i][n] - p * z;
}
-
- // Complex pair
-
- } else {
+ } else { // Complex pair
d[n-1] = x + p;
d[n] = x + p;
e[n-1] = z;
@@ -546,13 +513,8 @@ public class EigenvalueDecomposition implements java.io.Serializable {
}
n = n - 2;
iter = 0;
-
- // No convergence yet
-
- } else {
-
+ } else { // No convergence yet
// Form shift
-
x = arrayH[n][n];
y = 0.0;
w = 0.0;
@@ -561,9 +523,7 @@ public class EigenvalueDecomposition implements java.io.Serializable {
w = arrayH[n][n-1] * arrayH[n-1][n];
}
- // Wilkinson's original ad hoc shift
-
- if (iter == 10) {
+ if (iter == 10) { // Wilkinson's original ad hoc shift
exshift += x;
for (int i = low; i <= n; i++) {
arrayH[i][i] -= x;
@@ -573,9 +533,7 @@ public class EigenvalueDecomposition implements java.io.Serializable {
w = -0.4375 * s * s;
}
- // MATLAB's new ad hoc shift
-
- if (iter == 30) {
+ if (iter == 30) { // MATLAB's new ad hoc shift
s = (y - x) / 2.0;
s = s * s + w;
if (s > 0) {
@@ -595,7 +553,6 @@ public class EigenvalueDecomposition implements java.io.Serializable {
iter = iter + 1; // (Could check iteration count here.)
// Look for two consecutive small sub-diagonal elements
-
int m = n-2;
while (m >= l) {
z = arrayH[m][m];
@@ -619,16 +576,9 @@ public class EigenvalueDecomposition implements java.io.Serializable {
m--;
}
- for (int i = m+2; i <= n; i++) {
- arrayH[i][i-2] = 0.0;
- if (i > m+2) {
- arrayH[i][i-3] = 0.0;
- }
- }
+ updateH(m);
// Double QR step involving rows l:n and columns m:n
-
-
for (int k = m; k <= n-1; k++) {
boolean notlast = (k != n-1);
if (k != m) {
@@ -660,9 +610,8 @@ public class EigenvalueDecomposition implements java.io.Serializable {
z = r / s;
q = q / p;
r = r / p;
-
+
// Row modification
-
for (int j = k; j < nn; j++) {
p = arrayH[k][j] + q * arrayH[k+1][j];
if (notlast) {
@@ -674,7 +623,6 @@ public class EigenvalueDecomposition implements java.io.Serializable {
}
// Column modification
-
for (int i = 0; i <= Math.min(n,k+3); i++) {
p = x * arrayH[i][k] + y * arrayH[i][k+1];
if (notlast) {
@@ -686,7 +634,6 @@ public class EigenvalueDecomposition implements java.io.Serializable {
}
// Accumulate transformations
-
for (int i = low; i <= high; i++) {
p = x * arrayV[i][k] + y * arrayV[i][k+1];
if (notlast) {
@@ -701,12 +648,38 @@ public class EigenvalueDecomposition implements java.io.Serializable {
} // check convergence
} // while (n >= low)
- // Backsubstitute to find vectors of upper triangular form
-
- if (norm == 0.0) {
- return;
+ if (norm != 0.0) {
+ backSubstitute(nn, eps, norm); // Backsubstitute to find vectors of upper triangular form
+ backTransform(nn, low, high); // Back transformation to get eigenvectors of original matrix
+ }
+ }
+
+ private void updateH(int m) {
+ for (int i = m+2; i <= n; i++) {
+ arrayH[i][i-2] = 0.0;
+ if (i > m+2) {
+ arrayH[i][i-3] = 0.0;
+ }
}
-
+ }
+
+ private double storeRootsGetNorm(int nn, int low, int high) {
+ double norm = 0.0;
+ for (int i = 0; i < nn; i++) {
+ if (i < low | i > high) {
+ d[i] = arrayH[i][i];
+ e[i] = 0.0;
+ }
+ for (int j = Math.max(i-1,0); j < nn; j++) {
+ norm = norm + Math.abs(arrayH[i][j]);
+ }
+ }
+ return norm;
+ }
+
+ private void backSubstitute(int nn, double eps, double norm) {
+ double p=0,q=0,r=0,s=0,z=0,t,w,x,y;
+
for (n = nn-1; n >= 0; n--) {
p = d[n];
q = e[n];
@@ -835,10 +808,10 @@ public class EigenvalueDecomposition implements java.io.Serializable {
}
}
}
-
- // Vectors of isolated roots
-
- for (int i = 0; i < nn; i++) {
+ }
+
+ private void backTransform(int nn, int low, int high) {
+ for (int i = 0; i < nn; i++) { // Vectors of isolated roots
if (i < low | i > high) {
for (int j = i; j < nn; j++) {
arrayV[i][j] = arrayH[i][j];
@@ -846,11 +819,9 @@ public class EigenvalueDecomposition implements java.io.Serializable {
}
}
- // Back transformation to get eigenvectors of original matrix
-
- for (int j = nn-1; j >= low; j--) {
+ for (int j = nn-1; j >= low; j--) { // Back transformation to get eigenvectors of original matrix
for (int i = low; i <= high; i++) {
- z = 0.0;
+ double z = 0.0;
for (int k = low; k <= Math.min(j,high); k++) {
z = z + arrayV[i][k] * arrayH[k][j];
}
diff --git a/Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java b/Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java
index 4a682da7..33588ae4 100644
--- a/Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java
@@ -168,7 +168,7 @@ public class Icp {
Matrix4d matrixPointCompare = pointToMatrix(relativeCoordinate(comparedPoints.get(i).getPosition(),comparedCenter));
matrixPointCompare.mul(rotationMatrix);
-
+
matrixPoint.mulTransposeLeft(matrixPoint, matrixPointCompare);
matrixPointCompare.mulTransposeLeft(matrixPointCompare, matrixPointCompare);
diff --git a/Comparison/src/main/java/cz/fidentis/analyst/icp/Quaternion.java b/Comparison/src/main/java/cz/fidentis/analyst/icp/Quaternion.java
index 11aede9c..0e7c603a 100644
--- a/Comparison/src/main/java/cz/fidentis/analyst/icp/Quaternion.java
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/Quaternion.java
@@ -2,11 +2,10 @@ package cz.fidentis.analyst.icp;
import javax.vecmath.Matrix4d;
import javax.vecmath.Quat4d;
-import javax.vecmath.Vector3d;
/**
* This class represents quaternions.
- * Primary, it fixes the error in the {@code Quat4d} constructor, which normalizes
+ * Primary, it fixes the error (?) in the {@code Quat4d} constructor, which normalizes
* the given coordinates for some reason. Moreover, this extension
* introduces useful methods that accelerate the ICP calculation a bit.
*
diff --git a/GUI/src/main/java/cz/fidentis/analyst/tests/EfficiencyTests.java b/GUI/src/main/java/cz/fidentis/analyst/tests/EfficiencyTests.java
index 09fce379..ec473c14 100644
--- a/GUI/src/main/java/cz/fidentis/analyst/tests/EfficiencyTests.java
+++ b/GUI/src/main/java/cz/fidentis/analyst/tests/EfficiencyTests.java
@@ -149,7 +149,7 @@ public class EfficiencyTests {
private static void printIcp(HumanFace face1, HumanFace face2) throws IOException {
long startTime = System.currentTimeMillis();
Icp icp = new Icp();
- icp.setParameters(100, false, 0.001);
+ icp.setParameters(10, false, 0.05);
icp.getTransformedFacet(face1.getMeshModel().getFacets().get(0), face2.getMeshModel().getFacets().get(0));
long endTime = System.currentTimeMillis();
System.out.println("ICP:\t " +(endTime-startTime) + " msec, " + (icp.getTransformations().size()+1) + " iterations");
--
GitLab