coordTransform.py 3.8 KB
Newer Older
Josef Brandt's avatar
Josef Brandt committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51
"""
GEPARD - Gepard-Enabled PARticle Detection
Copyright (C) 2018  Lars Bittrich and Josef Brandt, Leibniz-Institut für
Polymerforschung Dresden e. V. <bittrich-lars@ipfdd.de>

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program, see COPYING.
If not, see <https://www.gnu.org/licenses/>.
"""
import numpy as np
from scipy.optimize import least_squares


def getRotMat(angles: np.ndarray) -> np.matrix:
    """
    :param angles: np.array shape(3), rot in x, y, z in radians
    """
    c1, s1 = np.cos(angles[0]), np.sin(angles[0])
    c2, s2 = np.cos(angles[1]), np.sin(angles[1])
    c3, s3 = np.cos(angles[2]), np.sin(angles[2])
    return np.mat([[c1*c3 - s1*c2*s3, -c1*s3 - s1*c2*c3, s1*s2],
                   [s1*c3 + c1*c2*s3, -s1*s3 + c1*c2*c3, -c1*s2],
                   [s1*s3, s2*c3, c2]]).T


def applyTransformToPoints(rotMat: np.matrix, shift: np.ndarray, srcPoints: np.ndarray) -> np.ndarray:
    center: np.ndarray = np.mean(srcPoints, axis=0)
    pointsCentered: np.ndarray = srcPoints-center
    pointsRot: np.matrix = np.dot(pointsCentered, rotMat).A
    return pointsRot + center + shift


def getTransform(srcPoints: np.ndarray, dstPoints: np.ndarray, invertX: bool = False, invertY: bool = False,
                 invertZ: bool = False):
    """
    Calculates transforms to convert from SOURCE to DESTINATION Coordinate System
    :param srcPoints: np.ndarray (Nx3) of N points in 3D (x, y, z) --> SOURCE Coordinate System
    :param dstPoints: np.ndarray (Nx3) of N points in 3D (x, y, z) --> DESTINATION Coordinate System
    :param invertX: bool, wether or not to flip the x-axis
    :param invertY: bool, wether or not to flip the y-axis
    :param invertZ: bool, wether or not to flip the z-axis
Josef Brandt's avatar
Josef Brandt committed
52 53 54
    :return rotationMatrix: np.matrix (3x3) for rotation
    :return shift: np.array shape(3), shift in x, y, z
    :return accept: bool: Whether the result was accepted.
Josef Brandt's avatar
Josef Brandt committed
55 56 57 58 59 60
    """

    Parity = np.mat(np.diag([-1. if invertX else 1.,
                             -1. if invertY else 1.,
                             -1. if invertZ else 1.]))

Josef Brandt's avatar
Josef Brandt committed
61 62
    srcCenter: np.ndarray = srcPoints.mean(axis=0)
    dstCenter: np.ndarray = dstPoints.mean(axis=0)
Josef Brandt's avatar
Josef Brandt committed
63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86
    srcPointsCentered: np.ndarray = srcPoints - srcCenter
    dstPointsCentered: np.ndarray = dstPoints - dstCenter

    def angleErrors(_angles: np.ndarray, _srcCentered: np.ndarray, _dstCentered: np.ndarray) -> np.ndarray:
        """
        :param _angles: shape (3) array of radian angles in x, y, z
        :param _srcCentered: shape (N, 3) array of centered source points
        :param _dstCentered: shape (N, 3) array of centered destination points
        """
        R = (getRotMat(_angles) * Parity).A
        return (np.dot(_srcCentered, R) - _dstCentered).ravel()

    angleOptFun = lambda x: angleErrors(x, srcPointsCentered, dstPointsCentered)
    opt = least_squares(angleOptFun, np.zeros(3), bounds=(np.array([-np.pi/4]*3), np.array([np.pi/4]*3)),
                        method='dogbox')
    optangles = opt.x
    rotMat: np.matrix = getRotMat(optangles) * Parity
    shift: np.ndarray = dstCenter - srcCenter
    errors: np.ndarray = applyTransformToPoints(rotMat, shift, srcPoints) - dstPoints
    d = np.linalg.norm(errors, axis=1)
    accept = True
    if np.any(d > 1.0):
        print(f'Transformation residuals are large:{d}')
        accept = False
Josef Brandt's avatar
Josef Brandt committed
87
    return rotMat, shift, accept