Initial checkpoint of affine3d transformation pkg. (WIP)

relies on numpy for underlying rep, and transformations.py (here)
for geometry.

Author: dbadb

BuildRaspi.md

@@ -450,7 +450,9 @@ ff02::2        ip6-allrouters
 
 ``` bash
 sudo python3 -m pip install picamera
-sudo python3 -m pip install pynetworktables
+# following may be present in frcvision, but shouldn't hurt
+sudo python3 -m pip install numpy
+sudo python3 -m pip install pynetworktables 
 ```
 
 ### install node and extensions

mathutils/__init__.py

@@ -0,0 +1,106 @@
+import numpy as np
+import math
+import transformations as xform
+from quaternion import *
+from affine2d import *
+
+class Affine3d(object):
+    """We represent a 3D affine transformation as a 4x4 matrix."
+
+    Lots of places to learn about affine transformations like this:    
+
+    http://graphics.cs.cmu.edu/nsp/course/15-462/Spring04/slides/04-transform.pdf
+
+    Examples
+    >>> a = Affine3d()
+    >>> b = Affine3d.fromRotation(30, [1, 0, 0])
+    >>> c = Affine3d.fromTranslation(.5, 1.5, 2.5)
+    >>> d = Affine3d.fromQuaternion(1,2,3,4)
+    >>> e = Affine3d.fromQuaternion([1,2,3,4])
+    >>> e.equals(d)
+    True
+    >>> f = Affine3d.fromQuaternion(Quaternion())
+    >>> a.equals(f)
+    True
+    >>> f = Affine3d.concatenate(b, c, d)
+    >>> pts = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]], dtype=np.float32)
+    >>> npts = f.transformPoints(pts)
+    >>> len(npts) == len(pts)
+    True
+    
+    """
+
+    @staticmethod
+    def fromIdentity():
+        return Affine3d(xform.identity_matrix())
+    
+    @staticmethod
+    def fromTranslation(x, y, z):
+        "return Affine3d representing translation of x, y and z"
+        return Affine3d(xform.translation_matrix([x, y, z]))
+
+    @staticmethod
+    def fromRotation(angle, dir):
+        "return Affine3d representing rotation of angle (in degrees) around dir"
+        return Affine3d(xform.rotation_matrix(math.radians(angle), dir))
+
+    @staticmethod
+    def fromQuaternion(*q):
+        """return Affine3d representing rotation via quaternion.
+        See Quaternion for a variety of methods to describe rotations.
+        """
+        if(len(q) == 1):
+            q = q[0]
+        if isinstance(q, Quaternion):
+            q = q.q # member of Quaternion
+        return Affine3d(xform.quaternion_matrix(q))
+
+    @staticmethod
+    def concatenate(*objs):
+        "return Affine3d representing concatenation of Affine3ds"
+        mats = [x.matrix for x in objs]
+        return Affine3d(xform.concatenate_matrices(*mats))
+    
+    # XXX: add more factories (scale, reflection, shear)
+
+    def __init__(self, initmat=None):
+        if initmat is None:
+            self.matrix = xform.identity_matrix()
+        else:
+            self.matrix = initmat
+    
+    def __repr__(self):
+        "for interactive prompt inspection"
+        return self.matrix.__repr__()
+
+    def __str__(self):
+        "for print"
+        return self.matrix.__str__()
+    
+    def equals(self, other):
+        return np.allclose(self.matrix, other.matrix)
+    
+    def transformPoints(self, pts):
+        """return an array of points transformed by my transformation matrix.
+        pts is assumed to be an array of triples.
+        """
+        result = np.array(pts, dtype=np.float32)
+        i = 0
+        for p3 in pts:
+            # p3 to p4 back to p3 (so dot works)
+            p4 = p3.tolist() + [1]
+            npt = self.matrix.dot(p4)[:3] 
+            result[i] = npt # npt type s np.array, should we issue tolist()?
+            i+=1
+        return result
+
+    def asInverse(self):
+        return Affine3d(xform.inverse_matrix(self.matrix))
+    
+    def asAffine2d(self):
+        """return a 2d representation of 
+        """
+    
+if __name__ == "__main__":
+    import doctest
+    doctest.testmod()

mathutils/affine2d.py

@@ -0,0 +1,77 @@
+import transformations as xform
+import numpy as np
+import math
+
+class Quaternion:
+    """Robust representation of 3D rotation
+
+    Quaternions w+ix+jy+kz are represented as [w, x, y, z].
+
+    Examples:
+    >>> a = Quaternion()
+    >>> np.allclose(np.identity(4), a.asMatrix())
+    True
+    >>> b = Quaternion.fromAxisAngle(45, [0, 0, 1])
+    >>> m = b.asMatrix()
+    >>> q1 = Quaternion.fromMatrix(np.identity(4))
+    >>> a.equals(q1)
+    True
+    >>> q2 = Quaternion.fromAngles(45, 0, 0)
+    >>> q3 = Quaternion.fromAngles(0, 45, 0)
+    >>> q4 = Quaternion.fromAngles(0, 0, 45)
+    """
+
+    @staticmethod
+    def fromMatrix(matrix, isprecise=False):
+        return Quaternion(xform.quaternion_from_matrix(matrix, isprecise))
+
+    @staticmethod
+    def fromAxisAngle(angle, axis):
+        """ return quaternion for rotation (degrees) about axis 
+        """
+        return Quaternion(xform.quaternion_about_axis(math.radians(angle), axis))
+    
+    @staticmethod
+    def fromAngles(ai, aj, ak, axes='sxyz'):
+        m = xform.euler_matrix(math.radians(ai), 
+                               math.radians(aj), 
+                               math.radians(ak), 
+                               axes)
+        q = xform.quaternion_from_matrix(m) 
+        return Quaternion(q)
+
+    def __init__(self, initq=None):
+        if initq is None:
+            self.q = np.array([1,0,0,0], dtype=np.float32) # produces identity matrix
+        else:
+            self.q = initq
+
+    def __repr__(self):
+        "for interactive prompt inspection"
+        return self.q.__repr__()
+
+    def __str__(self):
+        "for print"
+        return self.q.__str__()
+    
+    def asMatrix(self):
+        """ return a numpy array, to convert to Affine3d, use Affine3d.fromQuaternion
+        """
+        return xform.quaternion_matrix(self.q)
+
+    def equals(self, other):
+        return xform.is_same_quaternion(self.q, other.q)
+    
+    def multiply(self, other):
+        """
+        >>> q1 = Quaternion(4, 1, -2, 3)
+        >>> q2 = Quaternion(8, -5, 6, 7)
+        >>> q3 = q1.multiply(q2)
+        >>> q3.equals(Quaternion(28, -44, -14, 48))
+        True
+        """
+        return xform.quaternion_multiply(self.q, other.q)
+
+if __name__ == "__main__":
+    import doctest
+    doctest.testmod()