Cleanup coord transfer

Sorry, forgot that in the prev. commit...

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