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 c027f4128c81484870af11803d7f7229081ae466..4a682da76bec22cb01897d081433dde9a0ebd51a 100644
--- a/Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java
@@ -74,12 +74,12 @@ public class Icp {
List<Double> distances = hausdorffDist.getDistances().get(transformedFacet);
List<Point3d> nearestPoints = hausdorffDist.getNearestPoints().get(transformedFacet);
- if(transformation != null){
+ if (transformation != null) {
prevMeanD = transformation.getMeanD();
}
transformation = computeIcpTransformation(nearestPoints, distances);
- //transformations.add(transformation);
+ transformations.add(transformation);
applyTransformation(transformation);
currentIteration ++;
}
@@ -103,11 +103,11 @@ public class Icp {
/**
* Compute transformation parameters( translation, rotation and scale factor).
- * Based on old Fidentis implementation
+ * Based on old FIDENTIS implementation
*
* @param nearestPoints List of nearest points computed by Hausdorff distance.
* @param distances list of distances computed by Hausdorff distance
- * @return Return Icp transformation which is represented by computed parameters.
+ * @return Return ICP transformation which is represented by computed parameters.
*/
private IcpTransformation computeIcpTransformation(List<Point3d> nearestPoints, List<Double> distances) {
@@ -148,24 +148,8 @@ public class Icp {
// END computing translation parameter
- EigenvalueDecomposition matrix = new EigenvalueDecomposition(sumMatrix);
- Matrix4d eigD = matrix.getD();
- Matrix4d eigM = matrix.getV();
- int max = 0;
-
- //Finding quaternion q representing the rotation
- // (it is the eigenvector corresponding to the largest eigenvalue of sumMatrix)
- for (int i = 0; i < 4; i++) {
- if (eigD.getElement(max, max) <= eigD.getElement(i, i)) {
- max = i;
- }
- }
- Quaternion q = new Quaternion(
- eigM.getElement(0, max),
- eigM.getElement(1, max),
- eigM.getElement(2, max),
- eigM.getElement(3, max));
- q = q.normalize();
+ Quaternion q = new Quaternion(new EigenvalueDecomposition(sumMatrix));
+ q.normalize();
// END computing rotation parameter
//computing SCALE parameter
@@ -175,27 +159,21 @@ public class Icp {
if (scale) {
Matrix4d rotationMatrix = q.toMatrix();
-
- Matrix4d matrixPoint, matrixPointCompare;
-
for (int i = 0; i < nearestPoints.size(); i++) {
if(nearestPoints.get(i) == null) {
continue;
}
- matrixPoint = pointToMatrix(relativeCoordinate(nearestPoints.get(i), mainCenter));
- matrixPointCompare = pointToMatrix(relativeCoordinate(comparedPoints.get(i).getPosition(),comparedCenter));
+ Matrix4d matrixPoint = pointToMatrix(relativeCoordinate(nearestPoints.get(i), mainCenter));
+ Matrix4d matrixPointCompare = pointToMatrix(relativeCoordinate(comparedPoints.get(i).getPosition(),comparedCenter));
matrixPointCompare.mul(rotationMatrix);
- Matrix4d tmp1 = new Matrix4d();
- Matrix4d tmp2 = new Matrix4d();
+ matrixPoint.mulTransposeLeft(matrixPoint, matrixPointCompare);
+ matrixPointCompare.mulTransposeLeft(matrixPointCompare, matrixPointCompare);
- tmp1.mulTransposeLeft(matrixPoint, matrixPointCompare);
- tmp2.mulTransposeLeft(matrixPointCompare, matrixPointCompare);
-
- sxUp += tmp1.getElement(0, 0);
- sxDown += tmp2.getElement(0,0);
+ sxUp += matrixPoint.getElement(0, 0);
+ sxDown += matrixPointCompare.getElement(0,0);
}
scaleFactor = sxUp / sxDown;
@@ -212,30 +190,25 @@ public class Icp {
*/
private void applyTransformation(IcpTransformation transformation) {
Point3d meshPointPosition;
- Quaternion rotationCopy;
-
- Quaternion rotation;
for (MeshPoint comparedPoint : transformedFacet.getVertices()) {
meshPointPosition = comparedPoint.getPosition();
- Quaternion point = new Quaternion(1, meshPointPosition.x, meshPointPosition.y, meshPointPosition.z);
-
+ Quaternion rotation = new Quaternion(meshPointPosition.x, meshPointPosition.y, meshPointPosition.z, 1);
+
if (transformedFacet.getVertices().size() > 1) {
- rotationCopy = Quaternion.multiply(point, transformation.getRotation().getConjugate());
+ Quaternion rotationCopy = Quaternion.multiply(rotation, transformation.getRotation().getConjugate());
rotation = Quaternion.multiply(transformation.getRotation(), rotationCopy);
- } else {
- rotation = point;
}
if(scale && !Double.isNaN(transformation.getScaleFactor())) {
meshPointPosition.set(
- rotation.getX() * transformation.getScaleFactor() + transformation.getTranslation().x,
- rotation.getY() * transformation.getScaleFactor() + transformation.getTranslation().y,
- rotation.getZ() * transformation.getScaleFactor() + transformation.getTranslation().z);
+ rotation.x * transformation.getScaleFactor() + transformation.getTranslation().x,
+ rotation.y * transformation.getScaleFactor() + transformation.getTranslation().y,
+ rotation.z * transformation.getScaleFactor() + transformation.getTranslation().z);
} else {
meshPointPosition.set(
- rotation.getX() + transformation.getTranslation().x,
- rotation.getY() + transformation.getTranslation().y ,
- rotation.getZ() + transformation.getTranslation().z);
+ rotation.x + transformation.getTranslation().x,
+ rotation.y + transformation.getTranslation().y ,
+ rotation.z + transformation.getTranslation().z);
}
}
}
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 c8d8c24dc163993e6d7f099e0812067e6cd64f27..11aede9cb40b154c48e0fb000d0b50c75edc88a1 100644
--- a/Comparison/src/main/java/cz/fidentis/analyst/icp/Quaternion.java
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/Quaternion.java
@@ -1,18 +1,18 @@
package cz.fidentis.analyst.icp;
import javax.vecmath.Matrix4d;
+import javax.vecmath.Quat4d;
+import javax.vecmath.Vector3d;
/**
- * This class represents quaternions.
+ * This class represents quaternions.
+ * 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.
*
* @author Maria Kocurekova
*/
-public final class Quaternion {
-
- private final double w;
- private final double x;
- private final double y;
- private final double z;
+public final class Quaternion extends Quat4d {
/**
* Builds a quaternion from its components.
@@ -22,102 +22,70 @@ public final class Quaternion {
* @param y Second vector component.
* @param z Third vector component.
*/
- public Quaternion(double w, double x, double y, double z) {
- this.w = w;
+ public Quaternion(double x, double y, double z, double w) {
this.x = x;
this.y = y;
this.z = z;
+ this.w = w;
}
-
- /**
- * Gets the second component of the quaternion (first component
- * of the vector part).
- *
- * @return the first component of the vector part.
- */
- public double getX(){
- return x;
- }
-
- /**
- * Gets the third component of the quaternion (second component
- * of the vector part).
- *
- * @return the second component of the vector part.
- */
- public double getY(){
- return y;
- }
-
+
/**
- * Gets the fourth component of the quaternion (third component
- * of the vector part).
- *
- * @return the third component of the vector part.
+ * Builds a quaternion from eigenvalue decomposition.
+ *
+ * @param ev eigenvalue decomposition.
*/
- public double getZ(){
- return z;
- }
+ public Quaternion(EigenvalueDecomposition ev) {
+ Matrix4d eigD = ev.getD();
+ Matrix4d eigM = ev.getV();
+ int max = 0;
- /**
- * Gets the first component of the quaternion (scalar part).
- *
- * @return the scalar part.
- */
- public double getW(){
- return w;
+ //Finding quaternion q representing the rotation
+ // (it is the eigenvector corresponding to the largest eigenvalue of sumMatrix)
+ for (int i = 0; i < 4; i++) {
+ if (eigD.getElement(max, max) <= eigD.getElement(i, i)) {
+ max = i;
+ }
+ }
+
+ this.x = eigM.getElement(1, max);
+ this.y = eigM.getElement(2, max);
+ this.z = eigM.getElement(3, max);
+ this.w = eigM.getElement(0, max);
}
/**
* Returns the conjugate quaternion of the instance.
+ * It method is equivalent to calling the {@link #conjugate} method,
+ * but it is more effective and readable when the origin quaternion has to be preserved.
*
* @return the conjugate quaternion
*/
public Quaternion getConjugate() {
- return new Quaternion(w, -x, -y, -z);
+ return new Quaternion(-x, -y, -z, w);
}
/**
* Returns the Hamilton product of two quaternions.
+ * It is equivalent to {@code q1.mul(q2)} of {@code Quat4d}.
+ * But calling this method it is faster and more readable if the first quaternion
+ * has to be preserved (and then cloned first).
*
* @param q1 First quaternion.
* @param q2 Second quaternion.
* @return the Hamilton product of two quaternions
*/
public static Quaternion multiply(final Quaternion q1, final Quaternion q2) {
- // Components of the first quaternion.
- final double q1w = q1.getW();
- final double q1x = q1.getX();
- final double q1y = q1.getY();
- final double q1z = q1.getZ();
-
- // Components of the second quaternion.
- final double q2w = q2.getW();
- final double q2x = q2.getX();
- final double q2y = q2.getY();
- final double q2z = q2.getZ();
-
- // Components of the product.
- final double w = q1w * q2w - q1x * q2x - q1y * q2y - q1z * q2z;
- final double x = q1w * q2x + q1x * q2w + q1y * q2z - q1z * q2y;
- final double y = q1w * q2y - q1x * q2z + q1y * q2w + q1z * q2x;
- final double z = q1w * q2z + q1x * q2y - q1y * q2x + q1z * q2w;
-
- return new Quaternion(w, x, y, z);
- }
-
- /**
- * Computes the normalized quaternion
- *
- * @return a normalized quaternion
- */
- public Quaternion normalize() {
- double norm = Math.sqrt(w * w + x * x + y * y + z * z);
- return new Quaternion(w / norm, x / norm, y / norm, z / norm);
+ final double w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
+ final double x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
+ final double y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
+ final double z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
+ return new Quaternion(x, y, z, w);
}
/**
- * Convert a Quaternion to a rotation matrix
+ * Convert a Quaternion to a rotation matrix.
+ * It is equivalent to but bit faster than calling
+ * {@code new Matrix4d(quternion, new Vector3d(0,0,0), 1);}
*
* @return a rotation matrix (4x4)
*/
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 59a693005f81601459f3cdf22b41b52a873edc64..09fce379447f18a48d9fe0b5e9a930576a63b3fc 100644
--- a/GUI/src/main/java/cz/fidentis/analyst/tests/EfficiencyTests.java
+++ b/GUI/src/main/java/cz/fidentis/analyst/tests/EfficiencyTests.java
@@ -40,17 +40,14 @@ public class EfficiencyTests {
*/
public static void main(String[] args) throws IOException, ClassNotFoundException, Exception {
face1 = printFaceLoad(faceFile4);
- face2 = printFaceLoad(faceFile4);
+ face2 = printFaceLoad(faceFile2);
face1.getMeshModel().simplifyModel();
face2.getMeshModel().simplifyModel();
- for (int i = 0; i < 10; i++) {
- //HumanFace face = HumanFacePrivilegedCache.instance().loadFace(faceFile4);
- //face.getMeshModel().simplifyModel();
- //System.out.println(i+".\t" + HumanFacePrivilegedCache.instance());
- printFaceDump(face1);
- }
+ //for (int i = 0; i < 10; i++) {
+ // printFaceDump(face1);
+ //}
boolean relativeDist = false;
boolean printDetails = false;
@@ -58,9 +55,9 @@ public class EfficiencyTests {
//face1.getMeshModel().compute(new GaussianCurvature()); // initialize everything, then measure
- System.out.println("ICP:");
- Icp icp = new Icp();
- icp.getTransformedFacet(face1.getMeshModel().getFacets().get(0), face2.getMeshModel().getFacets().get(0));
+ System.out.println();
+ System.out.println("ICP calculation:");
+ printIcp(face1, face2);
System.out.println("Symmetry plane calculation:");
printSymmetryPlane(face1, true, SignificantPoints.CurvatureAlg.MEAN);
@@ -149,6 +146,15 @@ public class EfficiencyTests {
return face;
}
+ 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.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");
+ }
+
public static String formatSize(long v) {
if (v < 1024) return v + " B";
int z = (63 - Long.numberOfLeadingZeros(v)) / 10;