#36 - compute rotation parameter

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);
             }
+
         }
     }