diff --git a/Comparison/src/main/java/cz/fidentis/analyst/face/HumanFaceUtils.java b/Comparison/src/main/java/cz/fidentis/analyst/face/HumanFaceUtils.java
index 27d4396722094d37c5a4fcfce3ca00863b7789ac..3c91d5b5524ed0554cb6874e52b9a99e4fadcb29 100644
--- a/Comparison/src/main/java/cz/fidentis/analyst/face/HumanFaceUtils.java
+++ b/Comparison/src/main/java/cz/fidentis/analyst/face/HumanFaceUtils.java
@@ -319,9 +319,9 @@ public class HumanFaceUtils {
}
point.set(
- (rotQuat.x + translation.x) * scale,
- (rotQuat.y + translation.y) * scale,
- (rotQuat.z + translation.z) * scale
+ rotQuat.x * scale + translation.x,
+ rotQuat.y * scale + translation.y,
+ rotQuat.z * scale + translation.z
);
}
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 363ee1e8b7666b1fffcd1cf8def6965f391d20f7..cc7386c20b2f462ce7f369ebb92bfdfe70af0abd 100644
--- a/Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformation.java
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformation.java
@@ -82,9 +82,9 @@ public class IcpTransformation {
if(scale && !Double.isNaN(getScaleFactor())) {
return new Point3d(
- (rotQuat.x + getTranslation().x) * getScaleFactor(),
- (rotQuat.y + getTranslation().y) * getScaleFactor(),
- (rotQuat.z + getTranslation().z) * getScaleFactor()
+ rotQuat.x * getScaleFactor() + getTranslation().x,
+ rotQuat.y * getScaleFactor() + getTranslation().y,
+ rotQuat.z * getScaleFactor() + getTranslation().z
);
} else {
return new Point3d(
diff --git a/Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformer.java b/Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformer.java
index 5c47b7e9700ba9317bcb13a7e5bc8dbd81a1dbbc..345c62e138d4bde06ccadbb97b1d1eaae3a4c860 100644
--- a/Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformer.java
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformer.java
@@ -17,7 +17,10 @@ import java.util.Objects;
import java.util.Set;
import javax.vecmath.Matrix4d;
import javax.vecmath.Point3d;
+import javax.vecmath.Tuple3d;
+import javax.vecmath.Tuple4d;
import javax.vecmath.Vector3d;
+import javax.vecmath.Vector4d;
/**
* Visitor applying the Iterative Closest Point (ICP) algorithm to minimize
@@ -32,9 +35,19 @@ import javax.vecmath.Vector3d;
* Sequential inspection of multiple facets using a single visitor's instance
* is possible.
* </p>
+ * <p>
+ * This implementation is based on
+ * <pre>
+ * Ferkova, Z. Comparison and Analysis of Multiple 3D Shapes [online].
+ * Brno, 2016 [cit. 2022-02-08].
+ * Available from: <a href="https://is.muni.cz/th/wx40f/">https://is.muni.cz/th/wx40f/</a>.
+ * Master's thesis. Masaryk University, Faculty of Informatics.
+ * </pre>
+ * </p>
*
* @author Maria Kocurekova
* @author Radek Oslejsek
+ * @author Zuzana Ferkova
*/
public class IcpTransformer extends MeshVisitor {
@@ -267,7 +280,7 @@ public class IcpTransformer extends MeshVisitor {
prevMeanD = transformation.getMeanD();
}
- transformation = computeIcpTransformation(nearestPoints, distances, reducedFacet);
+ transformation = computeIcpTransformation(nearestPoints, distances, reducedFacet.getVertices());
transformations.get(transformedFacet).add(transformation);
applyTransformation(transformedFacet, transformation);
currentIteration ++;
@@ -277,84 +290,80 @@ public class IcpTransformer extends MeshVisitor {
/***********************************************************
* PRIVATE METHODS
***********************************************************/
-
+
/**
- * Compute transformation parameters( translation, rotation and scale factor).
+ * Compute transformation parameters (translation, rotation, and scale factor).
* 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.
-
*/
- private IcpTransformation computeIcpTransformation(List<Point3d> nearestPoints, List<Double> distances, MeshFacet transformedFacet) {
- List<MeshPoint>comparedPoints = transformedFacet.getVertices();
+ private IcpTransformation computeIcpTransformation(List<Point3d> nearestPoints, List<Double> distances, List<MeshPoint>trPoints) {
+ //
+ // Compute translation and mean distance from centroids:
+ //
double meanD = 0;
- double countOfNotNullPoints = 0;
-
- List<Point3d>centers = computeCenterBothFacets(nearestPoints, comparedPoints);
- Point3d mainCenter = centers.get(0);
- Point3d comparedCenter = centers.get(1);
-
- Matrix4d sumMatrix = new Matrix4d();
- Matrix4d multipleMatrix = new Matrix4d();
-
- for(int i = 0; i < comparedPoints.size(); i++) {
- if (nearestPoints.get(i) == null || !Doubles.isFinite(distances.get(i))) {
- continue;
+ Point3d mainCenter = new Point3d(0, 0, 0);
+ Point3d trCenter = new Point3d(0, 0, 0);
+ int countOfNotNullPoints = 0;
+ for (int i = 0; i < nearestPoints.size(); i++) {
+ if (nearestPoints.get(i) != null && Doubles.isFinite(distances.get(i))) {
+ mainCenter.add(nearestPoints.get(i));
+ trCenter.add(trPoints.get(i).getPosition());
+ countOfNotNullPoints++;
+ meanD += distances.get(i);
}
- // START computing Translation coordinates
- meanD += distances.get(i);
-
- Point3d relativeC = relativeCoordinate(comparedPoints.get(i).getPosition(),comparedCenter);
- Point3d relativeN = relativeCoordinate(nearestPoints.get(i),mainCenter);
-
- // START computing Rotation parameters
- multipleMatrix.mulTransposeLeft(sumMatrixComp(relativeC), sumMatrixMain(relativeN));
- sumMatrix.add(multipleMatrix);
-
- countOfNotNullPoints ++;
}
-
- meanD /= countOfNotNullPoints;
+ mainCenter.scale(1.0 / countOfNotNullPoints);
+ trCenter.scale(1.0 / countOfNotNullPoints);
Vector3d translation = new Vector3d(
- mainCenter.x - comparedCenter.x,
- mainCenter.y - comparedCenter.y,
- mainCenter.z - comparedCenter.z);
-
- // END computing translation parameter
+ mainCenter.x - trCenter.x,
+ mainCenter.y - trCenter.y,
+ mainCenter.z - trCenter.z
+ );
+
+ meanD /= countOfNotNullPoints;
- Quaternion q = new Quaternion(new EigenvalueDecomposition(sumMatrix));
+ //
+ // Compute rotation:
+ //
+ Matrix4d sumMat = new Matrix4d();
+ for (int i = 0; i < trPoints.size(); i++) {
+ if (nearestPoints.get(i) != null && Doubles.isFinite(distances.get(i))) {
+ //Matrix4d tmpMat = sumMatrixComp(relativeCoord4d(trPoints.get(i).getPosition(), trCenter));
+ //tmpMat.mul(sumMatrixMain(relativeCoord4d(nearestPoints.get(i), mainCenter)));
+ //sumMat.add(tmpMat);
+ sumMat.add(multMat(
+ relativeCoord4d(trPoints.get(i).getPosition(), trCenter),
+ relativeCoord4d(nearestPoints.get(i), mainCenter)
+ ));
+ }
+ }
+
+ Quaternion q = new Quaternion(new EigenvalueDecomposition(sumMat));
q.normalize();
- // END computing rotation parameter
- //computing SCALE parameter
+ //
+ // compute scale
+ //
double scaleFactor = 1.0;
if (scale) {
double sxUp = 0;
double sxDown = 0;
- Matrix4d rotationMatrix = q.toMatrix();
+ Matrix4d rotMat = q.toMatrix();
for (int i = 0; i < nearestPoints.size(); i++) {
- if (nearestPoints.get(i) == null || !Doubles.isFinite(distances.get(i))) {
- continue;
+ if (nearestPoints.get(i) != null && Doubles.isFinite(distances.get(i))) {
+ Vector4d relC = relativeCoord4d(nearestPoints.get(i), mainCenter);
+ Vector4d relA = relativeCoord4d(trPoints.get(i).getPosition(), trCenter);
+ rotMat.transform(relA);
+ sxUp += relC.dot(relA);
+ sxDown += relA.dot(relA);
}
-
- Matrix4d matrixPoint = pointToMatrix(relativeCoordinate(nearestPoints.get(i), mainCenter));
- Matrix4d matrixPointCompare = pointToMatrix(relativeCoordinate(comparedPoints.get(i).getPosition(), comparedCenter));
-
- matrixPointCompare.mul(rotationMatrix);
-
- matrixPoint.mulTransposeLeft(matrixPoint, matrixPointCompare);
- matrixPointCompare.mulTransposeLeft(matrixPointCompare, matrixPointCompare);
-
- sxUp += matrixPoint.getElement(0, 0);
- sxDown += matrixPointCompare.getElement(0,0);
}
-
scaleFactor = sxUp / sxDown;
-
- } // end computing scale parameter
+ }
return new IcpTransformation(translation, q, scaleFactor, meanD);
}
@@ -366,55 +375,15 @@ public class IcpTransformer extends MeshVisitor {
* @param transformation Computed transformation.
*/
private void applyTransformation(MeshFacet transformedFacet, IcpTransformation transformation) {
- transformedFacet.getVertices().parallelStream().forEach(
- p -> {
+ transformedFacet.getVertices().parallelStream()
+ .forEach(p -> {
p.setPosition(transformation.transformPoint(p.getPosition(), scale));
}
);
}
- /**
- * Compute center of both given objects which are represents by list of Vector3d.
- * Return list of two centers points.
- *
- * @param nearestPoints list of the nearest neighbours
- * @param comparedPoints list fo compared points
- * @return List with two points. The first one represents center of main facet
- * and the second one represents center of compared facet.
- */
- private List<Point3d> computeCenterBothFacets(List<Point3d> nearestPoints, List<MeshPoint> comparedPoints) {
- //assert(nearestPoints.size() == comparedPoints.size());
- Point3d n = new Point3d(0, 0, 0);
- Point3d c = new Point3d(0, 0, 0);
- int countOfNotNullPoints = 0;
- List<Point3d> result = new ArrayList<>(2);
-
- for (int i = 0; i < nearestPoints.size(); i++) {
- if (nearestPoints.get(i) == null) {
- continue;
- }
- n.add(nearestPoints.get(i));
- c.add(comparedPoints.get(i).getPosition());
- countOfNotNullPoints ++;
- }
-
- n.scale(1.0/countOfNotNullPoints);
- c.scale(1.0/countOfNotNullPoints);
- result.add(n);
- result.add(c);
- return result;
- }
-
- /**
- * Compute relative coordinate of given point according to given center.
- * Relative coordinates represents distance from the center of the mesh to vertex.
- *
- * @param p Point of which relative coordinates we want to be computed.
- * @param center Vector3d which represents center of object.
- * @return Vector3d which represents coordinates of new point according to center.
- */
- private Point3d relativeCoordinate(Point3d p, Point3d center) {
- return new Point3d(p.x - center.x, p.y - center.y, p.z - center.z);
+ private Vector4d relativeCoord4d(Point3d p, Point3d center) {
+ return new Vector4d(p.x - center.x, p.y - center.y, p.z - center.z, 1.0);
}
/**
@@ -423,40 +392,49 @@ public class IcpTransformer extends MeshVisitor {
* @param p Compared point
* @return Sum matrix of given point
*/
- private Matrix4d sumMatrixComp(Point3d p) {
+ @Deprecated
+ private Matrix4d sumMatrixComp(Tuple4d p) {
return new Matrix4d(
- 0, -p.x, -p.y, -p.z,
- p.x, 0, p.z, -p.y,
- p.y, -p.z, 0, p.x,
- p.z, p.y, -p.x, 0);
+ 0, p.x, p.y, p.z,
+ -p.x, 0, -p.z, p.y,
+ -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.
*
* @param p Compared point
* @return Sum matrix of given point
*/
- private Matrix4d sumMatrixMain(Point3d p) {
+ @Deprecated
+ private Matrix4d sumMatrixMain(Tuple4d p) {
return new Matrix4d(
0, -p.x, -p.y, -p.z,
p.x, 0, -p.z, p.y,
p.y, p.z, 0, -p.x,
p.z, -p.y, p.x, 0);
}
-
+
/**
- * Convert point (Vector3d) to matrix(Matrix4d).
- *
- * @param p Point, we want to be converted.
- * @return Matrix of point.
+ * Fast implementation of <pre>sumMatrixComp(a).mult(sumMatrixMain(b));</pre>
*/
- private Matrix4d pointToMatrix(Point3d p){
+ private Matrix4d multMat(Tuple4d a, Tuple4d b) {
+ double axbx = a.x * b.x;
+ double axby = a.x * b.y;
+ double axbz = a.x * b.z;
+ double aybx = a.y * b.x;
+ double ayby = a.y * b.y;
+ double aybz = a.y * b.z;
+ double azbx = a.z * b.x;
+ double azby = a.z * b.y;
+ double azbz = a.z * b.z;
+
return new Matrix4d(
- p.x, p.y, p.z, 1,
- 0, 0, 0, 0,
- 0, 0, 0, 0,
- 0, 0, 0, 0);
+ axbx+ayby+azbz, aybz-azby, -axbz+azbx, axby-aybx,
+ -azby+aybz, axbx-azbz-ayby, axby+aybx, axbz+azbx,
+ azbx-axbz, aybx+axby, ayby-azbz-axbx, aybz+azby,
+ -aybx+axby, azbx+axbz, azby+aybz, azbz-ayby-axbx
+ );
}
-
-
}
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 0e7c603ab5340d1dad0256d891a85a399f1893fe..f99bfb4b9ee3d76b05acab57303ebf7bed7f6791 100644
--- a/Comparison/src/main/java/cz/fidentis/analyst/icp/Quaternion.java
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/Quaternion.java
@@ -99,12 +99,21 @@ public final class Quaternion extends Quat4d {
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
+ );
+
+ /*
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
);
+ */
}
}