Commit 7c32cc86 authored by Josef Brandt's avatar Josef Brandt

Coordinate Transform standalone

parent 514de030
"""
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
"""
Parity = np.mat(np.diag([-1. if invertX else 1.,
-1. if invertY else 1.,
-1. if invertZ else 1.]))
srcCenter = srcPoints.mean(axis=0) # pc
dstCenter = dstPoints.mean(axis=0) # zpc
srcPointsCentered: np.ndarray = srcPoints - srcCenter
dstPointsCentered: np.ndarray = dstPoints - dstCenter
# points -= pc[np.newaxis, :]
# zpoints -= zpc[np.newaxis, :]
copyDstPoints: np.ndarray = dstPoints.copy()
copyDstPointsCentered: np.ndarray = dstPointsCentered.copy()
# ppoints = points[:, :].copy()
# def err(angles_shift):
# T = (getRotMat(angles_shift[:3]) * Parity).A
# return (np.dot(srcPointsCentered, T) + angles_shift[np.newaxis, 3:] - copyDstPoints).ravel()
#
# angle = np.zeros(3)
# opt = least_squares(err, np.concatenate((angle, np.zeros(3))),
# bounds=(np.array([-np.pi / 4] * 3 + [-np.inf] * 3),
# np.array([np.pi / 4] * 3 + [np.inf] * 3)),
# method='dogbox')
# permbest = opt
# # pointsbest = ppoints
#
# optangles = permbest.x[:3]
# shift = permbest.x[3:]
# def error(angleShift: np.ndarray, _srcPoints: np.ndarray, _knownDstPoints: np.ndarray) -> np.ndarray:
# """
# :param angleShift: shape(6) array, first three elements: angles in xyz (radians), then shift in xyz
# :param _srcPoints: shape(N, 3) array of source points
# :param _knownDstPoints: shape(N, 3) array of destination points
# """
# R: np.matrix = getRotMat(angleShift[:3])
# _dstPoints: np.ndarray = applyTransformToPoints(R, angleShift[3:], _srcPoints)
# return (_dstPoints - _knownDstPoints).ravel()
# optFun = lambda x: error(x, srcPoints, dstPoints)
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
return rotMat, shift, accept
\ No newline at end of file
import unittest
import numpy as np
from ..coordTransform import getRotMat, getTransform, applyTransformToPoints
class TestCoordinateTransform(unittest.TestCase):
def test_coordTransform(self):
srcPoints: np.ndarray = np.array([[0, 100, 0], [100, 100, -20], [70, 30, 10]], dtype=np.float)
shifts: list = [(2, 5, 10), (-3, 5, 2), (-20, 20, 7)]
angles: np.ndarray = np.array([-10, 0, 10])
maxError: float = 0.0
for shift in shifts:
for angleX in angles:
for angleY in angles:
for angleZ in angles:
curAngles = np.array([angleX, angleY, angleZ])
rotmat: np.matrix = getRotMat(np.deg2rad(curAngles))
dstPoints: np.ndarray = applyTransformToPoints(rotmat, shift, srcPoints)
optRotmat, optShift, accept = getTransform(srcPoints, dstPoints)
calcDstPoints: np.ndarray = applyTransformToPoints(optRotmat, optShift, srcPoints)
errors: np.ndarray = np.linalg.norm(calcDstPoints-dstPoints, axis=1)
for error in errors:
if error > maxError:
maxError = error
self.assertAlmostEqual(maxError, 0.0)
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment