Decompose/rotMatToAxisAngle funcitons were broken. Far more complicated than I

was hoping.

fsl/utils/transform.py

@@ -123,34 +123,109 @@ def compose(scales, offsets, rotations, origin=None):
 
 def decompose(xform):
     """Decomposes the given transformation matrix into separate offsets,
-    scales, and rotations.
+    scales, and rotations, according to the algorithm described in:
 
-    .. note:: Shears are not yet supported, and may never be supported.
-    """
+    Spencer W. Thomas, Decomposing a matrix into simple transformations, pp
+    320-323 in *Graphics Gems II*, James Arvo (editor), Academic Press, 1991,
+    ISBN: 0120644819.
 
-    offsets = xform[:3, 3]
-    scales  = [np.sqrt(np.sum(xform[:3, 0] ** 2)),
-               np.sqrt(np.sum(xform[:3, 1] ** 2)),
-               np.sqrt(np.sum(xform[:3, 2] ** 2))]
-    
-    rotmat         = np.copy(xform[:3, :3])
-    rotmat[:, 0] /= scales[0]
-    rotmat[:, 1] /= scales[1]
-    rotmat[:, 2] /= scales[2]
+    It is assumed that the given transform has no perspective components. Any
+    shears in the affine are discarded.
 
-    rots = rotMatToAxisAngles(rotmat)
+    :arg xform: A ``(4, 4)`` affine transformation matrix.
 
-    return scales, offsets, rots
+    :returns: The following:
+                - A sequence of three scales
+                - A sequence of three translations
+                - A sequence of three rotations, in radians
+    """
+ 
+    # The inline comments in the code below are taken verbatim from
+    # the referenced article, [except for notes in square brackets].
+    
+    # The next step is to extract the translations. This is trivial;
+    # we find t_x = M_{4,1}, t_y = M_{4,2}, and t_z = M_{4,3}. At this
+    # point we are left with a 3*3 matrix M' = M_{1..3,1..3}.
+    xform        = xform.T
+    translations = xform[ 3, :3]
+    xform        = xform[:3, :3]
+
+    M1 = xform[0]
+    M2 = xform[1]
+    M3 = xform[2]
+
+    # The process of finding the scaling factors and shear parameters
+    # is interleaved. First, find s_x = |M'_1|. 
+    sx = np.sqrt(np.dot(M1, M1))
+
+    # Then, compute an initial value for the xy shear factor,
+    # s_xy = M'_1 * M'_2. (this is too large by the y scaling factor).
+    sxy = np.dot(M1, M2)
+
+    # The second row of the matrix is made orthogonal to the first by
+    # setting M'_2 = M'_2 - s_xy * M'_1.
+    M2 = M2 - sxy * M1
+
+    # Then the y scaling factor, s_y, is the length of the modified
+    # second row.
+    sy = np.sqrt(np.dot(M2, M2))
+
+    # The second row is normalized, and s_xy is divided by s_y to
+    # get its final value.
+    M2  = M2  / sy
+    sxy = sxy / sy
+
+    # The xz and yz shear factors are computed as in the preceding,
+    sxz = np.dot(M1, M3)
+    syz = np.dot(M2, M3)
+
+    # the third row is made orthogonal to the first two rows,
+    M3 = M3 - sxz * M1 - syz * M2
+
+    # the z scaling factor is computed,
+    sz = np.sqrt(np.dot(M3, M3))
+
+    # the third row is normalized, and the xz and yz shear factors are
+    # rescaled.
+    M3  = M3  / sz
+    sxz = sxz / sz
+    syz = sxz / sz
+
+    # The resulting matrix now is a pure rotation matrix, except that it 
+    # might still include a scale factor of -1. If the determinant of the
+    # matrix is -1, negate the matrix and all three scaling factors. Call
+    # the resulting matrix R.
+    #
+    # [We do things different here - if the rotation matrix has negative
+    #  determinant, the flip is encoded in the x scaling factor.]
+    R = np.array([M1, M2, M3])
+    if linalg.det(R) < 0:
+        R[0] = -R[0]
+        sx   = -sx
+
+    # Finally, we need to decompose the rotation matrix into a sequence
+    # of rotations about the x, y, and z axes. [This is done in the
+    # rotMatToAxisAngles function]
+    rx, ry, rz = rotMatToAxisAngles(R.T)
+
+    return [sx, sy, sz], translations, [rx, ry, rz]
 
 
 def rotMatToAxisAngles(rotmat):
     """Given a ``(3, 3)`` rotation matrix, decomposes the rotations into
     an angle in radians about each axis.
     """
-    xrot = np.arctan2(rotmat[2, 1], rotmat[2, 2])
-    yrot = np.sqrt(   rotmat[2, 1] ** 2 + rotmat[2, 2] ** 2)
-    yrot = np.arctan2(rotmat[2, 0], yrot)
-    zrot = np.arctan2(rotmat[1, 0], rotmat[0, 0])
+
+    yrot = np.sqrt(rotmat[0, 0] ** 2 + rotmat[1, 0] ** 2)
+
+    if np.isclose(yrot, 0):
+        xrot = np.arctan2(-rotmat[1, 2], rotmat[1, 1])
+        yrot = np.arctan2(-rotmat[2, 0], yrot)
+        zrot = 0
+    else:
+        xrot = np.arctan2( rotmat[2, 1], rotmat[2, 2])
+        yrot = np.arctan2(-rotmat[2, 0], yrot)
+        zrot = np.arctan2( rotmat[1, 0], rotmat[0, 0]) 
 
     return [xrot, yrot, zrot]