[#38] feat: calculation of vertices rotation

Comparison/pom.xml

@@ -93,6 +93,13 @@
             <version>5.6.0</version>
             <scope>test</scope>
         </dependency>
+        <!-- http://ejml.org/wiki/index.php?title=Main_Page -->
+        <dependency>
+            <groupId>org.ejml</groupId>
+            <artifactId>ejml-all</artifactId>
+            <version>0.41</version>
+        </dependency>
+
     </dependencies>
     <properties>
         <project.build.sourceEncoding>UTF-8</project.build.sourceEncoding>

Comparison/src/main/java/cz/fidentis/analyst/procrustes/ProcrustesAnalysis.java

@@ -3,17 +3,20 @@ package cz.fidentis.analyst.procrustes;
 import cz.fidentis.analyst.feature.FeaturePoint;
 import cz.fidentis.analyst.procrustes.exceptions.ProcrustesAnalysisException;
 import cz.fidentis.analyst.procrustes.utils.ProcrustesAnalysisUtils;
+import org.ejml.simple.SimpleMatrix;
+import org.ejml.simple.SimpleSVD;
 
 import java.util.List;
 import javax.vecmath.Vector3f;
 
+
 /**
  * @author Jakub Kolman
  */
 public class ProcrustesAnalysis {
 
-    private List<FeaturePoint> orderedFpList1;
-    private List<FeaturePoint> orderedFpList2;
+    private List<FeaturePoint> orderedFeaturePointList1;
+    private List<FeaturePoint> orderedFeaturePointList2;
 
     private ProcrustesAnalysisFaceModel featurePointModel1;
     private ProcrustesAnalysisFaceModel featurePointModel2;
@@ -33,14 +36,20 @@ public class ProcrustesAnalysis {
         if (fpList1.size() != fpList2.size()) {
             throw new ProcrustesAnalysisException("Lists of feature points do not have the same size");
         }
+
+        orderedFeaturePointList1 = ProcrustesAnalysisUtils.sortListByFeaturePointType(fpList1);
+        orderedFeaturePointList2 = ProcrustesAnalysisUtils.sortListByFeaturePointType(fpList2);
+
         if (!featurePointTypesEquivalence(fpList1, fpList2)) {
             throw new ProcrustesAnalysisException("Lists of feature points do not have the same feature point types");
         }
 
+        // Might be sufficient to send as parameter orderedFpList instead of fpList1 and continue to work
+        // only with faceModels in rest of the class
         featurePointModel1 = new ProcrustesAnalysisFaceModel(fpList1, vertices1);
         featurePointModel2 = new ProcrustesAnalysisFaceModel(fpList2, vertices2);
 
-        centroid = ProcrustesAnalysisUtils.findCenteroidOfFeaturePoints(orderedFpList1);
+        centroid = ProcrustesAnalysisUtils.findCenteroidOfFeaturePoints(orderedFeaturePointList1);
 
         superImpose(featurePointModel1, featurePointModel2);
 
@@ -59,6 +68,51 @@ public class ProcrustesAnalysis {
         // moves vertices of face models
         featurePointModel1.setVertices(moveVertices(centroid, featurePointModel1.getVertices()));
         featurePointModel1.setVertices(moveVertices(centroid, featurePointModel2.getVertices()));
+
+        rotate(featurePointModel1, featurePointModel2);
+    }
+
+    /**
+     * By rotation of matrices solves orthogonal procrustes problem
+     *
+     * @param featurePointModel1
+     * @param featurePointModel2
+     */
+    private void rotate(ProcrustesAnalysisFaceModel featurePointModel1,
+                        ProcrustesAnalysisFaceModel featurePointModel2) {
+        // There is no reason trying to rotate less than 3 elements
+        if (this.orderedFeaturePointList1.size() < 3) {
+            return;
+        }
+        SimpleMatrix primaryMatrix = ProcrustesAnalysisUtils.createFeaturePointMatrix(orderedFeaturePointList2);
+        SimpleMatrix transposedMatrix = ProcrustesAnalysisUtils.createFeaturePointMatrix(
+                orderedFeaturePointList1).transpose();
+
+        SimpleMatrix multipliedMatrix = primaryMatrix.mult(transposedMatrix);
+        SimpleSVD<SimpleMatrix> singularValueDecomposition = multipliedMatrix.svd();
+        SimpleMatrix transposedU = singularValueDecomposition.getU().transpose();
+        SimpleMatrix r = singularValueDecomposition.getV().mult(transposedU);
+        primaryMatrix = primaryMatrix.mult(r);
+
+        featurePointModel2.setFeaturePointsMap(
+                ProcrustesAnalysisUtils.createFeaturePointMapFromMatrix(primaryMatrix, featurePointModel2));
+
+        rotateVertices(featurePointModel2, primaryMatrix);
+    }
+
+    /**
+     * Rotates every vertex of model by given matrix
+     *
+     * @param model
+     * @param matrix
+     */
+    private void rotateVertices(ProcrustesAnalysisFaceModel model, SimpleMatrix matrix) {
+        if (model.getVertices() != null) {
+            int i = 0;
+            for (Vector3f v : model.getVertices()) {
+                v.set(ProcrustesAnalysisUtils.rotateVertex(v, matrix));
+            }
+        }
     }
 
     /**
@@ -92,10 +146,10 @@ public class ProcrustesAnalysis {
      * @return
      */
     private boolean featurePointTypesEquivalence(List<FeaturePoint> fpList1, List<FeaturePoint> fpList2) {
-        orderedFpList1 = ProcrustesAnalysisUtils.sortListByFeaturePointType(fpList1);
-        orderedFpList2 = ProcrustesAnalysisUtils.sortListByFeaturePointType(fpList2);
+        orderedFeaturePointList1 = ProcrustesAnalysisUtils.sortListByFeaturePointType(fpList1);
+        orderedFeaturePointList2 = ProcrustesAnalysisUtils.sortListByFeaturePointType(fpList2);
 
-        return ProcrustesAnalysisUtils.checkfeaturePointsType(orderedFpList1, orderedFpList2);
+        return ProcrustesAnalysisUtils.checkfeaturePointsType(orderedFeaturePointList1, orderedFeaturePointList2);
     }
 
     /**
@@ -109,8 +163,8 @@ public class ProcrustesAnalysis {
      * @return ratioValue
      */
     private double calculateScalingValue() {
-        double[] distancesOfList1 = ProcrustesAnalysisUtils.calculateMeanDistancesFromOrigin(orderedFpList1);
-        double[] distancesOfList2 = ProcrustesAnalysisUtils.calculateMeanDistancesFromOrigin(orderedFpList2);
+        double[] distancesOfList1 = ProcrustesAnalysisUtils.calculateMeanDistancesFromOrigin(orderedFeaturePointList1);
+        double[] distancesOfList2 = ProcrustesAnalysisUtils.calculateMeanDistancesFromOrigin(orderedFeaturePointList2);
 
         double[] ratioArray = new double[distancesOfList1.length];
         double ratioValue = 0;

Comparison/src/main/java/cz/fidentis/analyst/procrustes/ProcrustesAnalysisFaceModel.java

@@ -18,12 +18,14 @@ import java.util.List;
  */
 public class ProcrustesAnalysisFaceModel {
     
-    private final HashMap<Integer, FeaturePoint> featurePointsMap;
+    private HashMap<Integer, FeaturePoint> featurePointsMap;
     private List<Vector3f> vertices;
+    private final HashMap<Integer, Integer> featurePointTypeCorrespondence;
 
     public ProcrustesAnalysisFaceModel(List<FeaturePoint> fpList, List<Vector3f> vertices) {
         this.featurePointsMap = ProcrustesAnalysisUtils.generateFeaturePointHashMap(fpList);
         this.vertices = vertices;
+        this.featurePointTypeCorrespondence = createFeaturePointTypeCorrespondence(fpList);
     }
 
     public void setVertices(List<Vector3f> vertices) {
@@ -33,4 +35,37 @@ public class ProcrustesAnalysisFaceModel {
     public List<Vector3f> getVertices() {
         return vertices;
     }
+
+    public void setFeaturePointsMap(HashMap<Integer, FeaturePoint> featurePointsMap) {
+        this.featurePointsMap = featurePointsMap;
+    }
+
+    public HashMap<Integer, FeaturePoint> getFeaturePointsMap() {
+        return featurePointsMap;
+    }
+
+    public HashMap<Integer, Integer> getFeaturePointTypeCorrespondence() {
+        return featurePointTypeCorrespondence;
+    }
+
+    /**
+     * Creates corresponding key value pair of matrix row index and a feature point type value.
+     * It is used in {@link cz.fidentis.analyst.procrustes.utils.ProcrustesAnalysisUtils#createFeaturePointMapFromMatrix}
+     * so we can get back type of feature point after working with matrices where the type is not stored,
+     * but instead are used matrix indexes.
+     *
+     * @param fpList
+     * @return
+     */
+    private HashMap<Integer, Integer> createFeaturePointTypeCorrespondence(List<FeaturePoint> fpList) {
+        HashMap<Integer, Integer> map = new HashMap<>();
+        for (int i = 0; i < fpList.size(); i++) {
+            map.put(i, fpList.get(i).getFeaturePointType().getType());
+        }
+        return map;
+    }
+
+//    public List<FeaturePoint> getFeaturePoints() {
+//        return new List<FeaturePoint>(this.featurePointsMap.values());
+//    }
 }

Comparison/src/main/java/cz/fidentis/analyst/procrustes/utils/ProcrustesAnalysisUtils.java

@@ -6,14 +6,19 @@
 package cz.fidentis.analyst.procrustes.utils;
 
 import cz.fidentis.analyst.feature.FeaturePoint;
+import cz.fidentis.analyst.feature.provider.FeaturePointTypeProvider;
+import cz.fidentis.analyst.procrustes.ProcrustesAnalysisFaceModel;
+import org.ejml.data.Matrix;
+import org.ejml.simple.SimpleMatrix;
 
+import javax.vecmath.Matrix3d;
+import javax.vecmath.Matrix3f;
 import javax.vecmath.Vector3f;
 import java.util.Collections;
 import java.util.HashMap;
 import java.util.List;
 
 /**
- *
  * @author Jakub Kolman
  */
 public class ProcrustesAnalysisUtils {
@@ -27,11 +32,16 @@ public class ProcrustesAnalysisUtils {
     public static List<FeaturePoint> sortListByFeaturePointType(List<FeaturePoint> featurePointList) {
         Collections.sort(
                 featurePointList, (FeaturePoint fp1, FeaturePoint fp2)
-                -> fp1.getFeaturePointType().getType() - fp2.getFeaturePointType().getType() // Ascending
+                        -> fp1.getFeaturePointType().getType() - fp2.getFeaturePointType().getType() // Ascending
         );
         return featurePointList;
     }
 
+    /**
+     * Creates hash map with a key feature point type and value feature point
+     * @param featurePointList
+     * @return
+     */
     public static HashMap<Integer, FeaturePoint> generateFeaturePointHashMap(List<FeaturePoint> featurePointList) {
         HashMap<Integer, FeaturePoint> map = new HashMap<>();
         for (FeaturePoint fp : featurePointList) {
@@ -44,12 +54,12 @@ public class ProcrustesAnalysisUtils {
 
     /**
      * Checks if two feature point lists have the same types of feature points.
-     *
+     * <p>
      * To use this method you need to supply ordered lists by Feature Point type
      * as parameters. Otherwise even if two lists contain the same featurepoints
      * it will return false.
-     *
-     * Use sort function {@link #sortByFeaturePointType} on feature point lists
+     * <p>
+     * Use sort function {@link #sortListByFeaturePointType} on feature point lists
      * two get correct results.
      *
      * @param featurePointList1
@@ -99,30 +109,89 @@ public class ProcrustesAnalysisUtils {
         return new Vector3f(x / featurePointList.size(), y / featurePointList.size(), z / featurePointList.size());
     }
 
-//    /**
-//     * Calculates distance of one feature point from another
-//    
-//     * @param fp1
-//     * @param fp2
-//     * @return 
-//     */
-//    public static double calculateDistanceOfTwoPoints(FeaturePoint fp1, FeaturePoint fp2) {
-//        return Math.sqrt(
-//                (Math.pow(fp1.getX(), 2) - Math.pow(fp2.getX(), 2))
-//                + (Math.pow(fp1.getY(), 2) - Math.pow(fp2.getY(), 2))
-//                + (Math.pow(fp1.getZ(), 2) - Math.pow(fp2.getZ(), 2)));
-//    }
+    /**
+     * Calculates distance of one feature point from another
+     *
+     * @param fp1
+     * @param fp2
+     * @return distance
+     */
+    public static double calculateDistanceOfTwoPoints(FeaturePoint fp1, FeaturePoint fp2) {
+        return Math.sqrt(
+                (Math.pow(fp1.getX(), 2) - Math.pow(fp2.getX(), 2))
+                        + (Math.pow(fp1.getY(), 2) - Math.pow(fp2.getY(), 2))
+                        + (Math.pow(fp1.getZ(), 2) - Math.pow(fp2.getZ(), 2)));
+    }
+
     /**
      * Calculates distance of single feature point from origin (0,0,0)
      *
      * @param fp
-     * @return
+     * @return distance
      */
     public static double calculatePointDistanceFromOrigin(FeaturePoint fp) {
         return Math.sqrt(
                 Math.pow(fp.getX(), 2)
-                + Math.pow(fp.getY(), 2)
-                + Math.pow(fp.getZ(), 2));
+                        + Math.pow(fp.getY(), 2)
+                        + Math.pow(fp.getZ(), 2));
     }
 
+    /**
+     * Creates matrix from given feature point list
+     *
+     * @param featurePointList
+     * @return matrix
+     */
+    public static SimpleMatrix createFeaturePointMatrix(List<FeaturePoint> featurePointList) {
+        SimpleMatrix matrix = new SimpleMatrix(featurePointList.size(), 3);
+
+        for (int i = 0; i < featurePointList.size(); i++) {
+            matrix.set(i, 0, featurePointList.get(i).getX());
+            matrix.set(i, 1, featurePointList.get(i).getY());
+            matrix.set(i, 2, featurePointList.get(i).getZ());
+        }
+        return matrix;
+    }
+
+    /**
+     * Creates feature point map HashMap with key FeaturePoint.type and value FeaturePoint back from matrix.
+     *
+     * @param matrix
+     * @param model
+     * @return
+     */
+    public static HashMap<Integer, FeaturePoint> createFeaturePointMapFromMatrix(SimpleMatrix matrix, ProcrustesAnalysisFaceModel model) {
+        HashMap<Integer, FeaturePoint> map = new HashMap<>();
+        for (int i = 0; i < matrix.numRows(); i++) {
+            FeaturePoint featurePoint = new FeaturePoint(
+                    matrix.get(i, 0),
+                    matrix.get(i, 1),
+                    matrix.get(i, 2),
+                    FeaturePointTypeProvider.getInstance().getFeaturePointTypeById(
+                            model.getFeaturePointTypeCorrespondence().get(i)));
+            map.put(model.getFeaturePointTypeCorrespondence().get(i), featurePoint);
+        }
+        return map;
+    }
+
+    /**
+     * Rotates vertex by using matrix multiplication of rotation matrix and vertex position.
+     *
+     * @param v
+     * @param matrix
+     * @return rotetedVertex coordinates [x, y, z]
+     */
+    public static float[] rotateVertex(Vector3f v, SimpleMatrix matrix) {
+        float x = (v.x * matrix.getIndex(0, 0))
+                + (v.y * matrix.getIndex(1, 0))
+                + (v.z * matrix.getIndex(2, 0));
+        float y = (v.x * matrix.getIndex(0, 1))
+                + (v.y * matrix.getIndex(1, 1))
+                + (v.z * matrix.getIndex(2, 1));
+        float z = (v.x * matrix.getIndex(0, 2))
+                + (v.y * matrix.getIndex(1, 2))
+                + (v.z * matrix.getIndex(2, 2));
+        float[] rotatedVertex = new float[]{x, y, z};
+        return rotatedVertex;
+    }
 }

MeshModel/src/main/java/cz/fidentis/analyst/feature/FeaturePointType.java

@@ -8,7 +8,8 @@ import java.io.Serializable;
  * @author Jakub Kolman
  */
 public class FeaturePointType implements Serializable {
-    private final int type;
+
+    private final int type;         // sometimes refered to as ID (such as in class @FeaturePointTypeProvider)
     private final String name;
     private final String info;
     private final String code;

MeshModel/src/main/java/cz/fidentis/analyst/feature/provider/FeaturePointTypeProvider.java

@@ -74,8 +74,9 @@ public final class FeaturePointTypeProvider {
 
     /**
      * get single feature point type by its id
+     * id here represents int value that is defined as FeaturePointType (int) property type.
      *
-     * @param id
+     * @param id (type)
      * @return
      */
     public FeaturePointType getFeaturePointTypeById(int id) {