Commit 0c465724 authored by Josef Brandt's avatar Josef Brandt

Cleanup coord transfer

Sorry, forgot that in the prev. commit...
parent 7c32cc86
......@@ -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
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