#36 compute transformation parameters

Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java

+package cz.fidentis.analyst.icp;
+
+import cz.fidentis.analyst.mesh.core.MeshFacet;
+import cz.fidentis.analyst.mesh.core.MeshPoint;
+import cz.fidentis.analyst.visitors.mesh.HausdorffDistance;
+
+import javax.vecmath.Matrix4d;
+import javax.vecmath.Quat4d;
+import javax.vecmath.Vector3d;
+import java.util.ArrayList;
+import java.util.List;
+
+public class Icp {
+
+    public List<IcpTransformation> 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
+
+        while ((currentIteration <= maxIteration) &&
+                (Double.isInfinite(prevMeanD) ||  Math.abs(prevMeanD - transformation.getMeanD()) > e )){
+
+            hausdorffDist.visitMeshFacet(comparedFacet);
+            List<Double> distances = hausdorffDist.getDistances().get(comparedFacet);
+            List<Vector3d> nearestPoints = hausdorffDist.getNearestPoints().get(comparedFacet);
+
+            if(transformation != null){
+                prevMeanD = transformation.getMeanD();
+            }
+
+            transformation = computeIcpTransformation(nearestPoints, distances, comparedFacet.getVertices(),scale);
+            trans.add(transformation);
+            applyTransformation(comparedFacet.getVertices(), transformation, scale);
+            currentIteration ++;
+        }
+
+        return trans;
+    }
+
+    /**
+     *
+     * @param nearestPoints
+     * @param comparedPoints
+     * @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;
+
+        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;
+
+        int 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();
+
+        // 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){
+                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;
+            z = nearestPoints.get(i).z - comparedPoints.get(i).getPosition().z;
+
+            meanX += x;
+            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
+            sumMatrix.add(multipleMatrix);
+            // END computing Rotation parameters
+
+
+            // START computing Scale parameters
+
+            countOfNotNullPoints ++;
+
+        }
+
+        meanD /= countOfNotNullPoints;
+        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);
+        }
+        */
+
+
+        Quat4d rotation = new Quat4d();
+        sumMatrix.setRotation(rotation);
+        rotation.normalize();
+        // end computing rotation parameter
+
+
+        //computing SCALE parameter
+        double sxUp = 0;
+        double scaleFactor = 0;
+        double sxDown = 0;
+
+        if (scale) {
+            Matrix4d rotationMatrix = new Matrix4d();
+            rotationMatrix.set(rotation);
+            Matrix4d matrixPoint, matrixPointCompare;
+
+            for (int i = 0; i < nearestPoints.size(); i++) {
+                if(nearestPoints.get(i) == null) {
+                    continue;
+                }
+                matrixPoint = pointToMatrix(relativeCoordinate(nearestPoints.get(i), mainCenter));
+                matrixPointCompare = pointToMatrix(relativeCoordinate(comparedPoints.get(i).getPosition(),comparedCenter));
+                matrixPointCompare.mul(rotationMatrix);
+
+                matrixPoint.transpose();
+                matrixPoint.mul(matrixPointCompare);
+                sxUp += matrixPoint.getElement(0, 0);
+
+                matrixPointCompare.transpose();
+                matrixPointCompare.mul(matrixPointCompare);
+                sxDown += matrixPointCompare.getElement(0,0);
+            }
+
+            scaleFactor = sxUp / sxDown;
+
+        } // 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 void applyTransformation(List<MeshPoint> comparedPoints, IcpTransformation transformation, boolean scale) {
+        Vector3d meshPointPosition;
+        Quat4d rotationCopy = new Quat4d();
+        Quat4d conjugateRotation = new Quat4d();
+
+        for (MeshPoint comparedPoint : comparedPoints) {
+            meshPointPosition = comparedPoint.getPosition();
+
+            Quat4d point = new Quat4d(meshPointPosition.x, meshPointPosition.y, meshPointPosition.z, 1);
+
+            if (comparedPoints.size() > 1) {
+                conjugateRotation.conjugate(transformation.getRotation());
+                rotationCopy.mul(point,conjugateRotation);
+                rotationCopy.mul(transformation.getRotation());
+            } 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);
+            } else {
+                meshPointPosition.set(rotationCopy.x+ transformation.getTranslation().x,
+                        rotationCopy.y + transformation.getTranslation().y,
+                        rotationCopy.z + transformation.getTranslation().z);
+            }
+        }
+    }
+
+    public IcpTransformation computeFinalTransformation(List<IcpTransformation> transformations, boolean scale) {
+        double scaleFactor = 1;
+        Vector3d translation =  new Vector3d();
+        Quat4d rotation = new Quat4d();
+
+        for(int i = 0; i < transformations.size(); i++) {
+            IcpTransformation trans = transformations.get(i);
+
+            if(scale){
+                scaleFactor = trans.getScaleFactor();
+            }
+
+            if(i == 0){
+                translation = trans.getTranslation();
+            }
+
+            if(i > 0 && scale) {
+                translation = multiplyVectorByNumber(translation, scaleFactor);
+            }
+            Quat4d conjugateRotation = trans.getRotation();
+            Quat4d tmp = new Quat4d(translation.x, translation.y, translation.z, 1 );
+
+            tmp.mul(trans.getRotation());
+            conjugateRotation.conjugate();
+            tmp.mul(conjugateRotation);
+
+            translation.x = tmp.x + trans.getTranslation().x;
+            translation.y = tmp.y + trans.getTranslation().y;
+            translation.z = tmp.z + trans.getTranslation().z;
+
+            rotation.mul(trans.getRotation());
+
+        }
+
+        return new IcpTransformation(translation,rotation, scaleFactor, 0, null);
+    }
+
+}

Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformation.java

+package cz.fidentis.analyst.icp;
+
+import javax.vecmath.Matrix4d;
+import javax.vecmath.Quat4d;
+import javax.vecmath.Vector3d;
+
+public class IcpTransformation {
+
+    private Quat4d rotation;
+    private double scaleFactor;
+    private Vector3d translation;
+    private Matrix4d transformationMatrix;
+    private double meanD;
+
+    public IcpTransformation(Vector3d translation, Quat4d rotation, double scaleFactor,double meanD , Matrix4d transformationMatrix) {
+        this.translation = translation;
+        this.scaleFactor = scaleFactor;
+        this.rotation = rotation;
+        this.meanD = meanD;
+        this.transformationMatrix = transformationMatrix;
+    }
+
+    public Quat4d getRotation() {
+        return rotation;
+    }
+
+    public double getScaleFactor() {
+        return scaleFactor;
+    }
+
+    public Vector3d getTranslation() {
+        return translation;
+    }
+
+    public Matrix4d getTransformationMatrix() {
+        return transformationMatrix;
+    }
+
+    public double getMeanD() {
+        return meanD;
+    }
+}