From 0c465724883b4536ab354e7c7ba167e7adfbff28 Mon Sep 17 00:00:00 2001 From: Josef Brandt Date: Tue, 1 Sep 2020 14:12:36 +0200 Subject: [PATCH] Cleanup coord transfer Sorry, forgot that in the prev. commit... --- coordTransform.py | 45 ++++++--------------------------------------- 1 file changed, 6 insertions(+), 39 deletions(-) diff --git a/coordTransform.py b/coordTransform.py index f4099de..f002448 100644 --- a/coordTransform.py +++ b/coordTransform.py @@ -49,53 +49,20 @@ def getTransform(srcPoints: np.ndarray, dstPoints: np.ndarray, invertX: bool = F :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 + :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. """ 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 - + srcCenter: np.ndarray = srcPoints.mean(axis=0) + dstCenter: np.ndarray = dstPoints.mean(axis=0) 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 @@ -117,4 +84,4 @@ def getTransform(srcPoints: np.ndarray, dstPoints: np.ndarray, invertX: bool = F 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 + return rotMat, shift, accept -- GitLab