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 e6c0547da27d5e30a2e59c4484980a5c1fc93379..865c584bd18aeb04f30fe6391e86dfb714803bd0 100644
--- a/Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java
@@ -9,6 +9,7 @@ import javax.vecmath.Quat4d;
import javax.vecmath.Vector3d;
import java.util.ArrayList;
import java.util.List;
+import java.util.Objects;
public class Icp {
@@ -23,8 +24,8 @@ public class Icp {
double prevMeanD = Double.POSITIVE_INFINITY; // mean distance of previous transformation
- while ((currentIteration <= maxIteration) &&
- (Double.isInfinite(prevMeanD) || Math.abs(prevMeanD - transformation.getMeanD()) > e )){
+ while ((currentIteration < maxIteration) &&
+ (Double.isInfinite(prevMeanD) || Math.abs(prevMeanD - Objects.requireNonNull(transformation).getMeanD() ) > e )){
hausdorffDist.visitMeshFacet(comparedFacet);
List<Double> distances = hausdorffDist.getDistances().get(comparedFacet);
@@ -45,8 +46,8 @@ public class Icp {
/**
*
- * @param nearestPoints
- * @param comparedPoints
+ * @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.
*/
@@ -95,6 +96,7 @@ public class Icp {
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,
@@ -108,6 +110,7 @@ public class Icp {
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;
@@ -115,7 +118,7 @@ public class Icp {
double meanZ = 0;
double meanD = 0;
- int countOfNotNullPoints = 0;
+ double countOfNotNullPoints = 0;
List<Vector3d>centers = computeCenterBothFacets(nearestPoints, comparedPoints);
Vector3d mainCenter = centers.get(0);
@@ -124,13 +127,9 @@ public class Icp {
Matrix4d sumMatrix = new Matrix4d();
Matrix4d multipleMatrix = new Matrix4d();
- // List<Vector3d> near = new ArrayList<>(comparedPoints.size());
- List<Vector3d> compared = new ArrayList<>(comparedPoints.size());
-
-
for(int i = 0; i < comparedPoints.size(); i++) {
- if (comparedPoints.get(i) == null){
+ if (nearestPoints.get(i) == null){
continue;
}
@@ -143,24 +142,13 @@ public class Icp {
meanY += y;
meanZ += z;
meanD += distances.get(i);
- //near.add(nearestPoints.get(i));
- compared.add(comparedPoints.get(i).getPosition());
- // END computing Translation coordinates
// START computing Rotation parameters
- // set the value of multipleMatrix to transpose of sumMatrix of relative coordinates compared point
- multipleMatrix.transpose(sumMatrixComp(relativeCoordinate(comparedPoints.get(i).getPosition(),comparedCenter)));
-
- // set the value of multipleMatrix to the result of multiplying itself with sumMatrix of relative coordinates main point
- multipleMatrix.mul(sumMatrixMain(relativeCoordinate(nearestPoints.get(i), mainCenter)));
-
- // set the value of sumMatrix to sum of itself and multipleMatrix
+ //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);
- // END computing Rotation parameters
-
-
- // START computing Scale parameters
countOfNotNullPoints ++;
@@ -170,29 +158,12 @@ public class Icp {
meanX /= countOfNotNullPoints;
meanY /= countOfNotNullPoints;
meanZ /= countOfNotNullPoints;
- // end computing translation parameter
-/*
- //computing ROTATION parameter
- Vector3d mainCenter = computeCenterV(nearestPoints);
- Vector3d comparedCenter = computeCenterV(compared);
-
- List<Vector3d> relativeM = relativeCord(nearestPoints, mainCenter);
- List<Vector3d> relativeC = relativeCord(compared, comparedCenter);
-
-
-
- for(int i = 0; i < relativeC.size(); i++) {
- multipleMatrix.transpose(sumMatrixComp(relativeC.get(i)));
- multipleMatrix.mul(sumMatrixMain(relativeM.get(i)));
- sumMatrix.add(multipleMatrix);
- }
- */
-
+ // END computing translation parameter
Quat4d rotation = new Quat4d();
- sumMatrix.setRotation(rotation);
+ rotation.set(sumMatrix);
rotation.normalize();
- // end computing rotation parameter
+ // END computing rotation parameter
//computing SCALE parameter
@@ -229,25 +200,40 @@ public class Icp {
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);
+ }
+
public void applyTransformation(List<MeshPoint> comparedPoints, IcpTransformation transformation, boolean scale) {
Vector3d meshPointPosition;
- Quat4d rotationCopy = new Quat4d();
+ Quat4d rotationCopy ;
Quat4d conjugateRotation = new Quat4d();
+
for (MeshPoint comparedPoint : comparedPoints) {
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.mul(point,conjugateRotation);
- rotationCopy.mul(transformation.getRotation());
+ conjugateRotation.conjugate(transformation.getRotation());
+ rotationCopy = multiply(point, conjugateRotation);
+ rotationCopy = multiply(transformation.getRotation(), rotationCopy);
+
} else {
rotationCopy = point;
}
@@ -257,10 +243,11 @@ public class Icp {
rotationCopy.y * transformation.getScaleFactor() + transformation.getTranslation().y,
rotationCopy.z * transformation.getScaleFactor() + transformation.getTranslation().z);
} else {
- meshPointPosition.set(rotationCopy.x+ transformation.getTranslation().x,
- rotationCopy.y + transformation.getTranslation().y,
+ meshPointPosition.set(rotationCopy.x+ transformation.getTranslation().x ,
+ rotationCopy.y + transformation.getTranslation().y ,
rotationCopy.z + transformation.getTranslation().z);
}
+
}
}