Created
August 31, 2016 02:59
-
-
Save Harakan/b75d3635a1b7a047e1dc2bdc2d844530 to your computer and use it in GitHub Desktop.
Transformer shifts center to 0 as Z approaches 0
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment