Transformer shifts center to 0 as Z approaches 0

transform.py

import numpy as np

class LinearAlgebraTransformer():
    def __init__(self, upper_height, lower_points, upper_points):
        '''Given upper_height as the offset to upper points,
            <upper|lower>_points as a zippered array of (deflection,distance),
            where each <distance|deflection> are given as (x,y)
            
            This class creates a linear transform matrix that you can transform any size of inputs into.
            
            This transform works by creating a linear transform matrix
            from the matrix P (Positions from calibration) of size n=8:
            [x0 y0 z0]
            [x1 y1 z1] = P
            [  ....  ]
            [xn yn zn]

            And the matched matrix D (Deflections as a percentage of "max")
            [dx0 dy0]
            [dx1 dy1] = D
            [  ...  ]
            [dxn dyn]

            To solve the linear transforms to solve for the "Transform" matrix T:
            P.T=D
            with the sizes of: [8x3].[3x2]=[8x2]

            *NOTE: due to matrix magics, you can't just do a simple inverse
            * to solve the magic matrix equation of T=(P_left^-1).D
            *
            * So we take the left inverse, and I chose this cause it made the dimenisons work.
            *  - That's literally the only reason
            *  - The pseudo inverse in numpy seems to work too, so that's a nice bi-directional solution
            * 
            * Given a matrix A of size [5x3]
            * Left inverse solves the equation of (A-1).A = I  [3x3]
            * Right inverse solves the equation of A.(A-1) = I [5x5]
            * See the difference in sizes, that's important to making the math work.
            *** leftInverse(A) = inverse(A_t*A)*A_t

            Use the function deflections=transform(np.array([(x,y,z),(xn,yn,zn)])) to use the magic
            - Output size matches input size for an array of #rows=n

            The stupid inputs was because I didn't want to fix upstream. Feel free to fix that
            '''

        self._upper_height = upper_height
        self._lower_points = lower_points
        self._transform = self._create_transform(lower_points,upper_points,upper_height)
        self._cache = {}


    def _create_transform(self,lower_points,upper_points,upper_height):
        '''Creates self._transform, you feed it xyz and defliections - this creates the linear transform matrix for use in self.transform()'''

        upper_distances = [distance for (deflection, distance) in upper_points.items()]
        upper_deflections = [deflection for (deflection, distance) in upper_points.items()]
        lower_distances = [distance for (deflection, distance) in lower_points.items()]
        lower_deflections = [deflection for (deflection, distance) in lower_points.items()]

        #List of Tupples, Tupples contain each row, [colum_index][row_index]
        #[(0,)]*4 means 4 rows of 1 index each 4X1 matrix
        #[(1,1),(2,2)] is an array 2X2 with 1's on top and 2's on the bottom 
        #np.concatenate axis=1 is colum concatenate, axis=0 is concatenating a new row
        lower_3d_distances = np.concatenate((lower_distances, [(0,)]*4), axis=1)
        upper_3d_distances = np.concatenate((upper_distances, [(upper_height,)]*4), axis=1)


        distances_3d = np.concatenate((lower_3d_distances, upper_3d_distances), axis=0)
        print "####### P matrix ############"
        print distances_3d

        distances_3d_inv = self._left_inverse(distances_3d)
        print "####### P^-1 (left) matrix ############"
        print distances_3d_inv

        #linalg's pseudo inverse does the same as left inverse but by solving the problem of least squares (or something)
        #other_distances_3d_inv = np.linalg.pinv(distances_3d)

        deflections = np.concatenate((lower_deflections,upper_deflections), axis=0)
        print "####### D matrix ############"
        print deflections
        transform = np.dot(distances_3d_inv, deflections)
        print "####### T matrix ############"
        print transform
        return transform

    def _left_inverse(self,matrix):
        ''' leftInverse(A) = inverse(A_t*A)*A_t'''

        transposed_matrix = np.transpose(matrix)
        sq_helper = np.dot(transposed_matrix,matrix)
        inverse_helper = np.linalg.inv(sq_helper)
        left_inverse = np.dot(inverse_helper,transposed_matrix)
        return left_inverse

    def transform(self,xyz_point):

        deflections = np.dot(xyz_point,self._transform)
        return deflections
        
        
        if __name__ == "__main__":
    #adhock debug:
    height = 50.0 

    #(deflection,distance)
    lower_points_real = { 
            (0.6807058823529412, 0.31967914438502676): (-20.0, 20.0),
            (0.6759144, 0.6752727): (20.0, 20.0),
            (0.2768556149732621, 0.6595294117647058): (20.0, -20.0),
            (0.2850695187165776, 0.3080427807486631): (-20.0, -20.0)
            }
    upper_points_real = { 
            (0.7645561497326203, 0.7457754010695187): (20.0, 20.0),
            (0.7635294117647059, 0.2402139037433155): (-20.0, 20.0),
            (0.1868449197860963, 0.22481283422459894): (-20.0, -20.0),
            (0.1680213903743315, 0.7210695187165775): (20.0, -20.0)
            }

    lower_points = { 
            (0.90, 0.90): (20.0, 20.0),
            (0.90, 0.10): (-20.0, 20.0),
            (0.10, 0.90): (20.0, -20.0),
            (0.10, 0.10): (-20.0, -20.0)
            }
    upper_points = { 
            (0.99, 0.99): (20.0, 20.0),
            (0.99, 0.01): (-20.0, 20.0),
            (0.01, 0.01): (-20.0, -20.0),
            (0.01, 0.99): (20.0, -20.0)
            }

    print "Upper points:"
    print upper_points

    print "Lower Points:"
    print lower_points
    
    example_xyz = (0.0,0.0,0.0)

    print "LinTransformerMade"
    LinTransformer=LinearAlgebraTransformer(height ,lower_points_real ,upper_points_real)
    LinTransformer=LinearAlgebraTransformer(height ,lower_points, upper_points)
    deflections = LinTransformer.transform(example_xyz)
    print "Deflections at 0mm centered: {0}".format(deflections)

    example_xyz = (0.0,0.0,10.0)
    deflections = LinTransformer.transform(example_xyz)
    print "Deflections after 10mm centered: {0}".format(deflections)

    example_xyz = (0.0,0.0,50.0)
    deflections = LinTransformer.transform(example_xyz)
    print "Deflections after 50mm centered: {0}".format(deflections)