Commit 6d68768a authored by Radek Ošlejšek's avatar Radek Ošlejšek
Browse files

Merge branch '296-fix-icp' into 'master' 

Resolve "Fix ICP"

Closes #296

See merge request grp-fidentis/analyst2!321
parents 4b5a0ab6 a1e0b63f

GeometryEngines/src/main/java/cz/fidentis/analyst/engines/icp/IcpTransformation.java

+17 −10
Original line number Diff line number Diff line
@@ -3,6 +3,7 @@ package cz.fidentis.analyst.engines.icp; 
import cz.fidentis.analyst.math.Quaternion;



import javax.vecmath.Point3d;

import javax.vecmath.Tuple3d;

import javax.vecmath.Vector3d;



/**
@@ -15,8 +16,11 @@ import javax.vecmath.Vector3d; 
 *                 (x, y, z coordinate and scalar component).

 * @param scaleFactor ScaleFactor represents scale between two objects.

 *                    In case there is no scale the value is 1.

 * @param rotCenter Center of rotation of the transformed object. It is the center of the static object

 *                  towards which the transform object is to be registered.

 *                  If {@code null}, then the center of the origin is taken into account.

 */

public record IcpTransformation(Vector3d translation, Quaternion rotation, double scaleFactor) {

public record IcpTransformation(Vector3d translation, Quaternion rotation, double scaleFactor, Tuple3d rotCenter) {



    /**

     * Apply transformation to given 3D point.
@@ -30,21 +34,24 @@ public record IcpTransformation(Vector3d translation, Quaternion rotation, doubl 
            return null;

        }



        Quaternion rotQuat = new Quaternion(point.x, point.y, point.z, 1);

        Quaternion rotQuat = (rotCenter == null)

                ? new Quaternion(point.x, point.y, point.z, 1)

                : new Quaternion(point.x - rotCenter.x, point.y - rotCenter.y, point.z - rotCenter.z, 1);

        Quaternion rotationCopy = Quaternion.multiply(rotQuat, rotation.getConjugate());

        rotQuat = Quaternion.multiply(rotation, rotationCopy);



        Tuple3d tr = (rotCenter == null) ? new Point3d() : rotCenter;

        if(scale && !Double.isNaN(scaleFactor)) {

            return new Point3d(

                    rotQuat.x * scaleFactor + translation.x,

                    rotQuat.y * scaleFactor + translation.y,

                    rotQuat.z * scaleFactor + translation.z

                    rotQuat.x * scaleFactor + translation.x + tr.x,

                    rotQuat.y * scaleFactor + translation.y + tr.y,

                    rotQuat.z * scaleFactor + translation.z + tr.z

            );

        } else {

            return new Point3d(

                    rotQuat.x + translation.x,

                    rotQuat.y + translation.y ,

                    rotQuat.z + translation.z

                    rotQuat.x + translation.x + tr.x,

                    rotQuat.y + translation.y + tr.y,

                    rotQuat.z + translation.z + tr.z

            );

        }

    }

GeometryEngines/src/main/java/cz/fidentis/analyst/engines/icp/impl/IcpVisitorImpl.java

+2 −2
Original line number Diff line number Diff line
@@ -314,7 +314,7 @@ public class IcpVisitorImpl implements IcpVisitor { 
     * This method also computes and stores ICP minimization objective 

     * <strong>BEFORE the transformation</strong>!

     *

     * @param hdVisitor A visitor with computed closest points to the {@code transformedFacet}

     * @param hdVisitor A visitor with computed the closest points to the {@code transformedFacet}

     * @param transformedFacet Facet to be superimposed 

     * @return Return an ICP transformation

     */
@@ -369,7 +369,7 @@ public class IcpVisitorImpl implements IcpVisitor { 
            scaleFactor = (sxDown == 0.0) ? 0.0 : sxUp / sxDown;

        }



        return new IcpTransformation(translation, q, scaleFactor);

        return new IcpTransformation(translation, q, scaleFactor, mainCenter);

    }

    

    /**