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 0383fec09c31e2478bfd2c34e1bf44b769261cb8..bab622abab955255da51dcf73dcd42bd60fd9a57 100644
--- a/Comparison/src/main/java/cz/fidentis/analyst/icp/EigenvalueDecomposition.java
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/EigenvalueDecomposition.java
@@ -934,21 +934,7 @@ public class EigenvalueDecomposition implements java.io.Serializable {
return res;
}
- /** Return the real parts of the eigenvalues
- @return real(diag(D))
- */
-
- public double[] getRealEigenvalues () {
- return d;
- }
- /** Return the imaginary parts of the eigenvalues
- @return imag(diag(D))
- */
-
- public double[] getImagEigenvalues () {
- return e;
- }
/** Return the block diagonal eigenvalue matrix
@return D
@@ -980,5 +966,4 @@ public class EigenvalueDecomposition implements java.io.Serializable {
D[2][0],D[2][1],D[2][2],D[2][3],
D[3][0],D[3][1],D[3][2],D[3][3]);
}
- private static final long serialVersionUID = 1;
}
\ No newline at end of file
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 5657115f145f4301ccf278826572459af9efc17a..6568bb9a9dff2f8344cd00415a643309b5220564 100644
--- a/Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java
@@ -6,7 +6,6 @@ import cz.fidentis.analyst.mesh.core.MeshPoint;
import cz.fidentis.analyst.visitors.mesh.HausdorffDistance;
import javax.vecmath.Matrix4d;
-import javax.vecmath.Quat4d;
import javax.vecmath.Vector3d;
import java.util.ArrayList;
import java.util.Collections;
@@ -66,7 +65,7 @@ public class Icp {
double prevMeanD = Double.POSITIVE_INFINITY;
- while ((currentIteration < 1) &&
+ while ((currentIteration < maxIteration) &&
(Double.isInfinite(prevMeanD) || Math.abs(prevMeanD - Objects.requireNonNull(transformation).getMeanD() ) > error )){
hausdorffDist.visitMeshFacet(transformedFacet);
@@ -77,9 +76,9 @@ public class Icp {
prevMeanD = transformation.getMeanD();
}
- transformation = computeIcpTransformation1(nearestPoints, distances);
- transformations.add(transformation);
- applyTransformation1(transformation);
+ transformation = computeIcpTransformation(nearestPoints, distances);
+ //transformations.add(transformation);
+ applyTransformation(transformation);
currentIteration ++;
}
@@ -100,161 +99,6 @@ public class Icp {
* PRIVATE METHODS
***********************************************************/
- private Matrix4d computeIcpRotation(List<Vector3d> nearestPoints, Vector3d nearCenter, Vector3d compCenter) {
- List<MeshPoint> comparedPoints = transformedFacet.getVertices();
- double CX = 0;
- double CY = 0;
- double CZ = 0;
-
- double NX = 0;
- double NY = 0;
- double NZ = 0;
-
-
- for (int i = 0; i < comparedPoints.size(); i++) {
- if (nearestPoints.get(i) == null) {
- continue;
- }
- CX += relativeCoordinate(comparedPoints.get(i).getPosition(), compCenter).x;
- CY += relativeCoordinate(comparedPoints.get(i).getPosition(), compCenter).y;
- CZ += relativeCoordinate(comparedPoints.get(i).getPosition(), compCenter).z;
-
- NX += relativeCoordinate(nearestPoints.get(i), nearCenter).x;
- NY += relativeCoordinate(nearestPoints.get(i), nearCenter).y;
- NZ += relativeCoordinate(nearestPoints.get(i), nearCenter).z;
- }
-
- double Sxx = CX * NX;
- double Sxy = CX * NY;
- double Sxz = CX * NZ;
-
- double Syx = CY * NX;
- double Syy = CY * NY;
- double Syz = CY * NZ;
-
- double Szx = CZ * NX;
- double Szy = CZ * NY;
- double Szz = CZ * NZ;
-
- return new Matrix4d(Sxx + Syy + Szz, Syz -Szy, -Sxz + Szx, Sxy + Syz,
- -Szy + Syz, Sxx - Szz - Syy, Sxy + Syx, Sxz -Szx,
- Szx -Sxz, Syx +Sxy, Syy-Szz-Sxx, Syz + Szy,
- -Syx + Sxy, Szx + Sxz, Szy +Syz, Szz - Syy -Sxx );
- }
-
- private Matrix4d computeQuatMatrix(Quat4d q){
- double qq = q.w* q.w+ q.x+ q.x+ q.y* q.y+ q.z* q.z;
- return new Matrix4d(qq, 0, 0, 0,
- 0, (q.w*q.w + q.x *q.x - q.y * q.y - q.z * q.z), 2*(q.x * q.y - q.w *q.z), 2*(q.x * q.z + q.w *q.y),
- 0, 2*(q.y * q.x + q.w *q.z),(q.w*q.w - q.x *q.x + q.y * q.y - q.z * q.z), 2*(q.y * q.z - q.w *q.x),
- 0, 2*(q.z * q.x - q.w *q.y), 2*(q.z * q.y + q.w *q.x), (q.w*q.w - q.x *q.x - q.y * q.y + q.z * q.z));
- }
-
- /**
- * Compute transformation parameters( translation, rotation and scale factor).
- * Based on http://www.sci.utah.edu/~shireen/pdfs/tutorials/Elhabian_ICP09.pdf
- * (p. 90 .. 92)
- *
- * @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.
-
- */
- private IcpTransformation computeIcpTransformation2(List<Vector3d> nearestPoints, List<Double> distances) {
- List<MeshPoint>comparedPoints = transformedFacet.getVertices();
- double meanD = 0;
- double countOfNotNullPoints = 0;
-
- List<Vector3d>centers = computeCenterBothFacets(nearestPoints, comparedPoints);
- Vector3d mainCenter = centers.get(0);
- Vector3d comparedCenter = centers.get(1);
-
- //computing rotation
- Matrix4d rotMatrix = computeIcpRotation(nearestPoints,centers.get(0), centers.get(1));
-
- EigenvalueDecomposition matrix = new EigenvalueDecomposition(rotMatrix);
- 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 rotMatrix)
- for (int i = 0; i < 4; i++) {
- if (eigD.getElement(max, max) <= eigD.getElement(i, i)) {
- max = i;
- }
- }
- Quat4d q = new Quat4d(eigM.getElement(1, max), eigM.getElement(2, max),
- eigM.getElement(3, max),eigM.getElement(0, max));
- q.normalize();
-
- Matrix4d relativeNMatrix, relativeCMatrix;
- Matrix4d numerator = new Matrix4d();
- Matrix4d denominator = new Matrix4d();
-
- Matrix4d sumNumerator = new Matrix4d();
- Matrix4d sumDenominator = new Matrix4d();
-
- Matrix4d quatMatrix = computeQuatMatrix(q);
- Matrix4d quatMatrixMulRelativeC = new Matrix4d();
-
- /*
- * computing scale
- * s = Sum(rmi*((Q(.transpose)*Q)*rci)) / Sum(rci * rci)
- * Sum - i = 0 .. comparedPoints.size()
- * rmi - relative coordinate of nearest neighbour (i = index)
- * rci - relative coordinate of compared point (i = index)
- * Q(.transpose)*Q - quatMatrix
- * */
-
- for(int i = 0; i < comparedPoints.size(); i++) {
- if (nearestPoints.get(i) == null) {
- continue;
- }
-
- meanD += distances.get(i);
- countOfNotNullPoints ++;
-
- Vector3d relativeC = relativeCoordinate(comparedPoints.get(i).getPosition(),comparedCenter);
- Vector3d relativeN = relativeCoordinate(nearestPoints.get(i),mainCenter);
-
- relativeNMatrix = pointToMatrix(relativeN);
- relativeCMatrix = pointToMatrix(relativeC);
-
- quatMatrixMulRelativeC.mul(quatMatrix, relativeCMatrix);
- numerator.mul(relativeNMatrix, quatMatrixMulRelativeC);
-
- denominator.mul(relativeCMatrix, relativeCMatrix);
-
- sumNumerator.add(numerator);
- sumDenominator.add(denominator);
- }
- // x, y, z should be same because it is uniform scale
- double scaleFactor = sumNumerator.getElement(0,0)/ sumDenominator.getElement(0,0);
-
- /* computing translation
- * t = mc - s*(Q(.transpose)*Q)*cc
- * mc - mainCenter
- * cc - comparedCenter
- * s - scaleFactor
- * Q(.transpose)*Q - quatMatrix
- * */
- Matrix4d translationMatrix = new Matrix4d();
- Matrix4d mainCenterMatrix = pointToMatrix(mainCenter);
- Matrix4d comparedCenterMatrix = pointToMatrix(comparedCenter);
-
- Matrix4d quatMatrixMulComparedCenterAndScale = new Matrix4d();
- quatMatrixMulComparedCenterAndScale.mul(quatMatrix, comparedCenterMatrix);
- quatMatrixMulComparedCenterAndScale.mul(scaleFactor);
- translationMatrix.sub(mainCenterMatrix, quatMatrixMulComparedCenterAndScale);
- Vector3d translationOffset = new Vector3d(translationMatrix.getElement(0,0),
- translationMatrix.getElement(0,1), translationMatrix.getElement(0,2));
-
- meanD /= countOfNotNullPoints;
-
- return new IcpTransformation(translationOffset, q, scaleFactor, meanD);
- }
-
-
/**
* Compute transformation parameters( translation, rotation and scale factor).
* Based on old Fidentis implementation
@@ -264,12 +108,8 @@ public class Icp {
* @return Return Icp transformation which is represented by computed parameters.
*/
- private IcpTransformation computeIcpTransformation1(List<Vector3d> nearestPoints, List<Double> distances) {
+ private IcpTransformation computeIcpTransformation(List<Vector3d> nearestPoints, List<Double> distances) {
List<MeshPoint>comparedPoints = transformedFacet.getVertices();
- double x, y, z;
- double meanX = 0;
- double meanY = 0;
- double meanZ = 0;
double meanD = 0;
double countOfNotNullPoints = 0;
@@ -285,16 +125,8 @@ public class Icp {
continue;
}
// START computing Translation coordinates
- x = nearestPoints.get(i).x - comparedPoints.get(i).getPosition().x;
- y = nearestPoints.get(i).y - comparedPoints.get(i).getPosition().y;
- z = nearestPoints.get(i).z - comparedPoints.get(i).getPosition().z;
-
- meanX += x;
- meanY += y;
- meanZ += z;
meanD += distances.get(i);
-
Vector3d relativeC = relativeCoordinate(comparedPoints.get(i).getPosition(),comparedCenter);
Vector3d relativeN = relativeCoordinate(nearestPoints.get(i),mainCenter);
@@ -306,13 +138,9 @@ public class Icp {
}
meanD /= countOfNotNullPoints;
- meanX /= countOfNotNullPoints;
- meanY /= countOfNotNullPoints;
- meanZ /= countOfNotNullPoints;
- Vector3d translation = new Vector3d(meanX, meanY, meanZ);
- // Vector3d translation = new Vector3d(mainCenter.x - comparedCenter.x,
- // mainCenter.y - comparedCenter.y, mainCenter.z -comparedCenter.z);
+ Vector3d translation = new Vector3d(mainCenter.x - comparedCenter.x,
+ mainCenter.y - comparedCenter.y, mainCenter.z -comparedCenter.z);
// END computing translation parameter
@@ -328,10 +156,10 @@ public class Icp {
max = i;
}
}
- Quat4d q = new Quat4d(eigM.getElement(1, max), eigM.getElement(2, max),
- eigM.getElement(3, max),eigM.getElement(0, max));
+ Quaternion q = new Quaternion(eigM.getElement(0, max), eigM.getElement(1, max), eigM.getElement(2, max),
+ eigM.getElement(3, max));
- q.normalize();
+ q = q.normalize();
// END computing rotation parameter
//computing SCALE parameter
@@ -340,8 +168,7 @@ public class Icp {
double sxDown = 0;
if (scale) {
- Matrix4d rotationMatrix = new Matrix4d();
- rotationMatrix.set(q);
+ Matrix4d rotationMatrix = q.toMatrix();
Matrix4d matrixPoint, matrixPointCompare;
@@ -369,7 +196,6 @@ public class Icp {
} // end computing scale parameter
-
return new IcpTransformation(translation, q, scaleFactor, meanD);
}
@@ -378,65 +204,34 @@ public class Icp {
*
* @param transformation Computed transformation.
*/
- private void applyTransformation1(IcpTransformation transformation) {
+ private void applyTransformation(IcpTransformation transformation) {
Vector3d meshPointPosition;
- Quat4d rotationCopy = new Quat4d();
- Quat4d conjugateRotation = new Quat4d();
+ Quaternion rotationCopy;
- Quat4d rotation = new Quat4d();
+ Quaternion rotation;
for (MeshPoint comparedPoint : transformedFacet.getVertices()) {
meshPointPosition = comparedPoint.getPosition();
- Quat4d point = new Quat4d(meshPointPosition.x, meshPointPosition.y, meshPointPosition.z, 1);
+ Quaternion point = new Quaternion(1, meshPointPosition.x, meshPointPosition.y, meshPointPosition.z);
- if (transformedFacet.getVertices().size() > 1) {
- conjugateRotation.conjugate(transformation.getRotation());
- rotationCopy.mul(point, conjugateRotation);
- rotation.mul(transformation.getRotation(), rotationCopy);
+ if (transformedFacet.getVertices().size() > 1) {
+ rotationCopy = Quaternion.multiply(point, transformation.getRotation().getConjugate());
+ rotation = Quaternion.multiply(transformation.getRotation(), rotationCopy);
} else {
rotation = point;
}
if(scale && !Double.isNaN(transformation.getScaleFactor())) {
- meshPointPosition.set(rotation.x * transformation.getScaleFactor() + transformation.getTranslation().x,
- rotation.y * transformation.getScaleFactor() + transformation.getTranslation().y,
- rotation.z * transformation.getScaleFactor() + transformation.getTranslation().z);
+ meshPointPosition.set(rotation.getX() * transformation.getScaleFactor() + transformation.getTranslation().x,
+ rotation.getY() * transformation.getScaleFactor() + transformation.getTranslation().y,
+ rotation.getZ() * transformation.getScaleFactor() + transformation.getTranslation().z);
} else {
- meshPointPosition.set(rotation.x + transformation.getTranslation().x,
- rotation.y + transformation.getTranslation().y ,
- rotation.z + transformation.getTranslation().z);
+ meshPointPosition.set(rotation.getX() + transformation.getTranslation().x,
+ rotation.getY() + transformation.getTranslation().y ,
+ rotation.getZ() + transformation.getTranslation().z);
}
}
}
- /**
- * Apply computed transformation to compared facet.
- *
- * @param transformation Computed transformation.
- */
- private void applyTransformation2(IcpTransformation transformation) {
- /*
- * Mi = s*R*Ci + t
- * Mi - model/main facet (i - index)
- * s - scale
- * R - rotation
- * Ci - compared facet (i - index)
- * t - translation
- */
- Vector3d meshPointPosition;
- Quat4d rotatedPoint = new Quat4d();
-
- for (MeshPoint comparedPoint : transformedFacet.getVertices()) {
- meshPointPosition = comparedPoint.getPosition();
-
- Quat4d point = new Quat4d(meshPointPosition.x, meshPointPosition.y, meshPointPosition.z, 1);
- rotatedPoint.mul(transformation.getRotation(), point);
-
- meshPointPosition.set(rotatedPoint.x * transformation.getScaleFactor() + transformation.getTranslation().x,
- rotatedPoint.y * transformation.getScaleFactor() + transformation.getTranslation().y ,
- rotatedPoint.z * transformation.getScaleFactor() + transformation.getTranslation().z);
-
- }
- }
/**
@@ -505,8 +300,6 @@ public class Icp {
p.y, -p.z, 0, p.x,
p.z, p.y, -p.x,0 );
}
-
-
/**
* Compute sum matrix of given point. Given point is point from main facet.
*
diff --git a/Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformation.java b/Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformation.java
index 103958d7ffbd16432d7f338cef7f28933ffa0e44..e98ebc23c81735516722474c712758c329ca67fa 100644
--- a/Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformation.java
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformation.java
@@ -1,6 +1,5 @@
package cz.fidentis.analyst.icp;
-import javax.vecmath.Quat4d;
import javax.vecmath.Vector3d;
/**
@@ -10,7 +9,7 @@ import javax.vecmath.Vector3d;
*/
public class IcpTransformation {
- private final Quat4d rotation;
+ private final Quaternion rotation;
private final double scaleFactor;
private final Vector3d translation;
private final double meanD;
@@ -25,7 +24,7 @@ public class IcpTransformation {
* In case there is no scale the value is 0.
* @param meanD MeanD represents mean distance between objects.
*/
- public IcpTransformation(Vector3d translation, Quat4d rotation, double scaleFactor,double meanD) {
+ public IcpTransformation(Vector3d translation, Quaternion rotation, double scaleFactor, double meanD) {
this.translation = translation;
this.scaleFactor = scaleFactor;
this.rotation = rotation;
@@ -36,7 +35,7 @@ public class IcpTransformation {
* Getter for rotation.
* @return Return rotation parameter.
*/
- public Quat4d getRotation() {
+ public Quaternion getRotation() {
return rotation;
}
diff --git a/Comparison/src/main/java/cz/fidentis/analyst/icp/Quaternion.java b/Comparison/src/main/java/cz/fidentis/analyst/icp/Quaternion.java
new file mode 100644
index 0000000000000000000000000000000000000000..47b7c0eec4c58bbf64280fb24bc1adc333147fa0
--- /dev/null
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/Quaternion.java
@@ -0,0 +1,147 @@
+package cz.fidentis.analyst.icp;
+
+import javax.vecmath.Matrix4d;
+
+/**
+ * This class represents quaternions.
+ *
+ * @author Maria Kocurekova
+ */
+public final class Quaternion {
+ public final double w;
+ private final double x;
+ private final double y;
+ private final double z;
+
+ /**
+ * Builds a quaternion from its components.
+ *
+ * @param w Scalar component.
+ * @param x First vector component.
+ * @param y Second vector component.
+ * @param z Third vector component.
+ */
+ public Quaternion( final double w,
+ final double x,
+ final double y,
+ final double z) {
+ this.w = w;
+ this.x = x;
+ this.y = y;
+ this.z = z;
+ }
+
+ /**
+ * 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.
+ */
+ public double getZ(){
+ return z;
+ }
+
+ /**
+ * Gets the first component of the quaternion (scalar part).
+ *
+ * @return the scalar part.
+ */
+ public double getW(){
+ return w;
+ }
+
+ /**
+ * Returns the conjugate quaternion of the instance.
+ *
+ * @return the conjugate quaternion
+ */
+ public Quaternion getConjugate() {
+ return new Quaternion(w, -x, -y, -z);
+ }
+
+ /**
+ * Returns the Hamilton product of two quaternions.
+ *
+ * @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);
+ }
+
+ /**
+ * Convert a Quaternion to a rotation matrix
+ *
+ * @return a rotation matrix (4x4)
+ */
+ public Matrix4d toMatrix(){
+ double xx = x * x;
+ double xy = x * y;
+ double xz = x * z;
+ double xw = x * w;
+ double yy = y * y;
+ double yz = y * z;
+ double yw = y * w;
+ double zz = z * z;
+ double zw = z * w;
+
+ return new Matrix4d(1 - 2 * ( yy + zz ),2 * ( xy + zw), 2 * ( xz - yw),0,
+ 2 * ( xy - zw ), 1 - 2 * ( xx + zz ), 2 * ( yz + xw ), 0,
+ 2 * ( xz + yw ), 2 * ( yz - xw ), 1 - 2 * ( xx + yy ), 0,
+ 0, 0,0, 1);
+ }
+
+}