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 70bb83c56e380c01b0e51aea8dff0b35df7da15b..fdf25dabd989cb1b065f83dbd6cad4ffeaa369e6 100644
--- a/Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java
@@ -8,26 +8,45 @@ import javax.vecmath.Matrix4d;
import javax.vecmath.Quat4d;
import javax.vecmath.Vector3d;
import java.util.ArrayList;
+import java.util.LinkedList;
import java.util.List;
import java.util.Objects;
+/**
+ * Icp represents class for computing the ICP (Iterative closest point) algorithm.
+ * It is use for minimize the distance between two MeshFacet.
+ * This class does not have parameterized constructor, for running ICP algorithm use public method initIcp.
+ *
+ * @author Maria Kocurekova
+ */
public class Icp {
- public List<IcpTransformation> initIcp(MeshFacet mainFacet, MeshFacet comparedFacet, int maxIteration, boolean scale, double e){
+ private final List<IcpTransformation> transformations = new LinkedList<>();
+ /**
+ * InitIcp represents running of the ICP algorithm. It use methods for computing and applying transformations.
+ * It is running until count of iterations is lower than maxIteration or until we reach lower distance
+ * between two objects than allowed error.
+ *
+ * @param mainFacet MainFacet represents fix faced.
+ * We want to align computedFacet according to MainFacet.
+ * @param comparedFacet Facet we want to be aligned.
+ * @param maxIteration Max count of running iterations
+ * (it includes computing new transformation and applying it).
+ * @param scale In case there is scale, scale factor is also computed.
+ * @param e Max error which is allowed.
+ * @return Return comparedFacet with new coordinates.
+ */
+ public MeshFacet initIcp(MeshFacet mainFacet, MeshFacet comparedFacet, int maxIteration, boolean scale, double e){
HausdorffDistance hausdorffDist = new HausdorffDistance(mainFacet, HausdorffDistance.Strategy.POINT_TO_POINT, false, false);
- List<IcpTransformation> trans = new ArrayList<>(maxIteration);
-
int currentIteration = 0;
IcpTransformation transformation = null;
- double prevMeanD = Double.POSITIVE_INFINITY; // mean distance of previous transformation
+ double prevMeanD = Double.POSITIVE_INFINITY;
while ((currentIteration < maxIteration) &&
(Double.isInfinite(prevMeanD) || Math.abs(prevMeanD - Objects.requireNonNull(transformation).getMeanD() ) > e )){
- /* while ((currentIteration < maxIteration) &&
- (Double.isInfinite(prevMeanD) || (Objects.requireNonNull(transformation).getMeanD() ) > e )){*/
hausdorffDist.visitMeshFacet(comparedFacet);
List<Double> distances = hausdorffDist.getDistances().get(comparedFacet);
@@ -38,103 +57,42 @@ public class Icp {
}
transformation = computeIcpTransformation(nearestPoints, distances, comparedFacet.getVertices(),scale);
- trans.add(transformation);
+ transformations.add(transformation);
applyTransformation(comparedFacet.getVertices(), transformation, scale);
currentIteration ++;
}
- return trans;
+ return comparedFacet;
}
/**
+ * Compute transformation parameters( translation, rotation and scale factor).
*
- * @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.
+ * @param nearestPoints List of nearest points computed by Hausdorff distance.
+ * @param distances list of distances computed by Hausdorff distance
+ * @param comparedPoints List of points of compared facet.
+ * List of points which transformation we are compute according list of nearest points.
+ * @param scale In case there is scale, scale factor is also computed.
+ * @return Return Icp transformation which is represented by computed parameters.
*/
- private List<Vector3d> computeCenterBothFacets(List<Vector3d> nearestPoints, List<MeshPoint> comparedPoints) {
- double xN = 0;
- double yN = 0;
- double zN = 0;
-
- double xC = 0;
- double yC = 0;
- double zC = 0;
-
- int countOfNotNullPoints = 0;
- List<Vector3d> result = new ArrayList<>(2);
-
- for (int i = 0; i < nearestPoints.size(); i++) {
-
- if(nearestPoints.get(i) == null){
- continue;
- }
-
- xN += nearestPoints.get(i).x;
- yN += nearestPoints.get(i).y;
- zN += nearestPoints.get(i).z;
-
- xC += comparedPoints.get(i).getPosition().x;
- yC += comparedPoints.get(i).getPosition().y;
- zC += comparedPoints.get(i).getPosition().z;
-
- countOfNotNullPoints ++;
- }
- result.add(new Vector3d(xN/countOfNotNullPoints,yN/countOfNotNullPoints,zN/countOfNotNullPoints));
- result.add(new Vector3d(xC/countOfNotNullPoints,yC/countOfNotNullPoints,zC/countOfNotNullPoints));
- return result;
- }
-
-
- private Vector3d relativeCoordinate(Vector3d p, Vector3d center) {
- return new Vector3d(p.x - center.x,p.y - center.y, p.z - center.z);
- }
-
- private Matrix4d sumMatrixComp(Vector3d 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 );
- }
-
-
- private Matrix4d sumMatrixMain(Vector3d 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);
- }
-
- public Matrix4d pointToMatrix(Vector3d p){
- return new Matrix4d(p.x, p.y, p.z, 1,
- 0, 0, 0, 0, 0, 0,
- 0,0, 0, 0, 0, 0);
- }
-
-
public IcpTransformation computeIcpTransformation(List<Vector3d> nearestPoints, List<Double> distances, List<MeshPoint>comparedPoints, boolean scale) {
double x, y, z;
double meanX = 0;
double meanY = 0;
double meanZ = 0;
double meanD = 0;
-
double countOfNotNullPoints = 0;
List<Vector3d>centers = computeCenterBothFacets(nearestPoints, comparedPoints);
Vector3d mainCenter = centers.get(0);
Vector3d 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){
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;
@@ -146,14 +104,12 @@ public class Icp {
meanD += distances.get(i);
// START computing Rotation parameters
-
//multipleMatrix =
//= (transpose (sumMatrix of relative coordinates of compared point)) x (sumMatrix of relative coordinates of nearest point)
multipleMatrix.mulTransposeLeft(sumMatrixComp(relativeCoordinate(comparedPoints.get(i).getPosition(),comparedCenter)), sumMatrixMain(relativeCoordinate(nearestPoints.get(i),mainCenter)));
sumMatrix.add(multipleMatrix);
countOfNotNullPoints ++;
-
}
meanD /= countOfNotNullPoints;
@@ -162,27 +118,11 @@ public class Icp {
meanZ /= countOfNotNullPoints;
// END computing translation parameter
- /* EigenvalueDecomposition matrix = new EigenvalueDecomposition(sumMatrix);
- Matrix4d eigD = matrix.getD();
- Matrix4d eigM = matrix.getV();
-
- int max = 0;
- for (int i = 0; i < 4; i++) {
- if (eigD.getElement(max, max) <= eigD.getElement(i, i)) {
- max = i;
- }
- }*/
-
Quat4d rotation = new Quat4d();
rotation.set(sumMatrix);
- /* Quat4d rotationE = new Quat4d(eigM.getElement(1, max), eigM.getElement(2, max),
- eigM.getElement(3, max),eigM.getElement(0, max));
- rotationE.normalize();*/
-
rotation.normalize();
// END computing rotation parameter
-
//computing SCALE parameter
double sxUp = 0;
double scaleFactor = 0;
@@ -214,30 +154,19 @@ public class Icp {
} // end computing scale parameter
- return new IcpTransformation(new Vector3d(meanX, meanY, meanZ), rotation, scaleFactor, meanD, null);
- }
-
-
-
- private Vector3d multiplyVectorByNumber(Vector3d vector, double number) {
- return new Vector3d(vector.x * number, vector.y * number, vector.z * number);
- }
-
-
- public Quat4d multiply(Quat4d q1, Quat4d q2) {
-
- double x, y, z, w;
-
- w = q1.w * q2.w- q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
- x = q1.x * q2.w + q1.w * q2.x + q1.y * q2.z - q1.z * q2.y;
- y = q1.y * q2.w + q1.w * q2.y + q1.z * q2.x - q1.x * q2.z;
- z = q1.z * q2.w + q1.w * q2.z + q1.x * q2.y - q1.y * q2.x;
- return new Quat4d(x, y, z, w);
+ return new IcpTransformation(new Vector3d(meanX, meanY, meanZ), rotation, scaleFactor, meanD);
}
+ /**
+ * Apply computed transformation to compared facet.
+ *
+ * @param comparedPoints List of points on which we want to apply transformation.
+ * @param transformation Computed transformation.
+ * @param scale In case there is a scale we use scale factor for computing new coordinate of point.
+ */
public void applyTransformation(List<MeshPoint> comparedPoints, IcpTransformation transformation, boolean scale) {
Vector3d meshPointPosition;
- Quat4d rotationCopy ;
+ Quat4d rotationCopy = new Quat4d();
Quat4d conjugateRotation = new Quat4d();
@@ -245,24 +174,20 @@ public class Icp {
meshPointPosition = comparedPoint.getPosition();
Quat4d point = new Quat4d(meshPointPosition.x, meshPointPosition.y, meshPointPosition.z, 1);
- //point.normalize();
- if (comparedPoints.size() > 1) {
- conjugateRotation.conjugate(transformation.getRotation());
- rotationCopy = multiply(point, conjugateRotation);
- rotationCopy = multiply(transformation.getRotation(), rotationCopy);
+ if (comparedPoints.size() > 1) {
+ conjugateRotation.conjugate(transformation.getRotation());
+ rotationCopy.mul(point, conjugateRotation);
+ rotationCopy.mul(transformation.getRotation(), rotationCopy);
} else {
rotationCopy = point;
}
if(scale && !Double.isNaN(transformation.getScaleFactor())) {
- meshPointPosition.set(rotationCopy.x * transformation.getScaleFactor() + transformation.getTranslation().x,
- rotationCopy.y * transformation.getScaleFactor() + transformation.getTranslation().y,
- rotationCopy.z * transformation.getScaleFactor() + transformation.getTranslation().z);
+ meshPointPosition.set(rotationCopy.x * transformation.getScaleFactor() + transformation.getTranslation().x + meshPointPosition.x,
+ rotationCopy.y * transformation.getScaleFactor() + transformation.getTranslation().y + meshPointPosition.y,
+ rotationCopy.z * transformation.getScaleFactor() + transformation.getTranslation().z + meshPointPosition.z);
} else {
- /* meshPointPosition.set(rotationCopy.x+ transformation.getTranslation().x ,
- rotationCopy.y + transformation.getTranslation().y ,
- rotationCopy.z + transformation.getTranslation().z);*/
meshPointPosition.set(rotationCopy.x+ transformation.getTranslation().x + meshPointPosition.x,
rotationCopy.y + transformation.getTranslation().y + meshPointPosition.y,
rotationCopy.z + transformation.getTranslation().z + meshPointPosition.z);
@@ -271,41 +196,109 @@ public class Icp {
}
}
- public IcpTransformation computeFinalTransformation(List<IcpTransformation> transformations, boolean scale) {
- double scaleFactor = 1;
- Vector3d translation = new Vector3d();
- Quat4d rotation = new Quat4d();
+ /**
+ * Getter for all transformations.
+ * @return List of IcpTransformation.
+ */
+ public List<IcpTransformation> getTransformations() {
+ return transformations;
+ }
- for(int i = 0; i < transformations.size(); i++) {
- IcpTransformation trans = transformations.get(i);
+ /***********************************************************
+ * PRIVATE METHODS
+ ***********************************************************/
- if(scale){
- scaleFactor = trans.getScaleFactor();
- }
+ /**
+ * 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<Vector3d> computeCenterBothFacets(List<Vector3d> nearestPoints, List<MeshPoint> comparedPoints) {
+ double xN = 0;
+ double yN = 0;
+ double zN = 0;
- if(i == 0){
- translation = trans.getTranslation();
- }
+ double xC = 0;
+ double yC = 0;
+ double zC = 0;
- if(i > 0 && scale) {
- translation = multiplyVectorByNumber(translation, scaleFactor);
- }
- Quat4d conjugateRotation = trans.getRotation();
- Quat4d tmp = new Quat4d(translation.x, translation.y, translation.z, 1 );
+ int countOfNotNullPoints = 0;
+ List<Vector3d> result = new ArrayList<>(2);
+
+ for (int i = 0; i < nearestPoints.size(); i++) {
- tmp.mul(trans.getRotation());
- conjugateRotation.conjugate();
- tmp.mul(conjugateRotation);
+ if(nearestPoints.get(i) == null){
+ continue;
+ }
- translation.x = tmp.x + trans.getTranslation().x;
- translation.y = tmp.y + trans.getTranslation().y;
- translation.z = tmp.z + trans.getTranslation().z;
+ xN += nearestPoints.get(i).x;
+ yN += nearestPoints.get(i).y;
+ zN += nearestPoints.get(i).z;
- rotation.mul(trans.getRotation());
+ xC += comparedPoints.get(i).getPosition().x;
+ yC += comparedPoints.get(i).getPosition().y;
+ zC += comparedPoints.get(i).getPosition().z;
+ countOfNotNullPoints ++;
}
+ result.add(new Vector3d(xN/countOfNotNullPoints,yN/countOfNotNullPoints,zN/countOfNotNullPoints));
+ result.add(new Vector3d(xC/countOfNotNullPoints,yC/countOfNotNullPoints,zC/countOfNotNullPoints));
+ return result;
+ }
- return new IcpTransformation(translation,rotation, scaleFactor, 0, null);
+ /**
+ * 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 Vector3d relativeCoordinate(Vector3d p, Vector3d center) {
+ return new Vector3d(p.x - center.x,p.y - center.y, p.z - center.z);
+ }
+
+ /**
+ * Compute sum matrix of given point. Given point represents compared point.
+ *
+ * @param p Compared point
+ * @return Sum matrix of given point
+ */
+ private Matrix4d sumMatrixComp(Vector3d 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 );
+ }
+
+
+ /**
+ * 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(Vector3d 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.
+ */
+ private Matrix4d pointToMatrix(Vector3d p){
+ return new Matrix4d(p.x, p.y, p.z, 1,
+ 0, 0, 0, 0, 0, 0,
+ 0,0, 0, 0, 0, 0);
}
-}
+}
\ No newline at end of file
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 c46898aeb34ef2f17d19dabddf78da62bf965285..103958d7ffbd16432d7f338cef7f28933ffa0e44 100644
--- a/Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformation.java
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformation.java
@@ -1,41 +1,65 @@
package cz.fidentis.analyst.icp;
-import javax.vecmath.Matrix4d;
import javax.vecmath.Quat4d;
import javax.vecmath.Vector3d;
+/**
+ * IcpTransformation class is holding computed data for transformation.
+ *
+ * @author Maria Kocurekova
+ */
public class IcpTransformation {
- private Quat4d rotation;
- private double scaleFactor;
- private Vector3d translation;
- private Matrix4d transformationMatrix;
- private double meanD;
+ private final Quat4d rotation;
+ private final double scaleFactor;
+ private final Vector3d translation;
+ private final double meanD;
- public IcpTransformation(Vector3d translation, Quat4d rotation, double scaleFactor,double meanD , Matrix4d transformationMatrix) {
+ /**
+ * Constructor for IcpTransformation.
+ *
+ * @param translation Translation represents translation of ComputedFacet on x, y, z axis.
+ * @param rotation Rotation is represented by Quaternion
+ * (x, y, z coordinate and scalar component).
+ * @param scaleFactor ScaleFactor represents scale between two objects.
+ * 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) {
this.translation = translation;
this.scaleFactor = scaleFactor;
this.rotation = rotation;
this.meanD = meanD;
- this.transformationMatrix = transformationMatrix;
}
+ /**
+ * Getter for rotation.
+ * @return Return rotation parameter.
+ */
public Quat4d getRotation() {
return rotation;
}
+ /**
+ * Getter for scale.
+ * @return Return scale factor.
+ */
public double getScaleFactor() {
return scaleFactor;
}
+ /**
+ * Getter for translation.
+ * @return Return translation parameter.
+ */
public Vector3d getTranslation() {
return translation;
}
- public Matrix4d getTransformationMatrix() {
- return transformationMatrix;
- }
-
+ /**
+ * Getter for mean distance.
+ * @return Return mean distance parameter.
+ */
public double getMeanD() {
return meanD;
}