towards least_square_trf

geometry.py

@@ -267,6 +267,56 @@ class geometry:
     # returns the vector m(x,y) of the optical center to the xray
     pass
 
+  @staticmethod
+  def least_square_trf(points):
+    # inputs are 4 points of a parallelogram
+    # this function returns the least square transformation
+    #[ px]  [a b c ] [ q ]
+    #[ py] =[d e f ]*[ r ]
+    #       [0 0 1 ] [ 1 ]
+    # with (q,r) in [0,0],[0,1],[1,0],[1,1]
+    #  a   b    c   d    e    f
+    # [q00 r00  1   0    0    0 ] [a] [px00]
+    # [0   0    0   q00 r00   1 ] [b] [py00]
+    # [q01 r01  1   0    0    0 ]*[c]=[px01]
+    # [0   0    0   q01 r01   1 ] [d] [py01]
+    # [q10 r10  1   0    0    0 ] [e] [px10]
+    # [0   0    0   q10 r10   1 ] [f] [py10]
+    # [q11 r11  1   0    0    0 ]     [px11]
+    # [0   0    0   q11 r11   1 ]     [py11]
+    #
+    #          A        *aa = y
+
+    A=np.array((
+      (0,0,1,0,0,0),
+      (0,0,0,0,0,1),
+      (0,1,1,0,0,0),
+      (0,0,0,0,1,1),
+      (1,0,1,0,0,0),
+      (0,0,0,1,0,1),
+      (1,1,1,0,0,0),
+      (0,0,0,1,1,1)), np.float)
+
+
+    y=points # sort points p00 p01 p10 p11
+    s=y[:,1].argsort()
+    y=y[s,:]
+    s=y[:2,0].argsort()
+    y[:2,:]=y[:2,:][s,:]
+    s=y[2:,0].argsort()
+    y[2:,:]=y[2:,:][s,:]
+    y=np.asmatrix(y.ravel()).T
+    A=np.asmatrix(A)
+    aa=(A.T*A).I*A.T*y
+    aa=aa.reshape((2,3))
+    return aa
+
+  @staticmethod
+  def least_square_plane(points):
+    #inputs are multiple (x,y,z) points
+    # this function returns the parameters of least square fitted plane x=x,y=y,z=ax+bx+c
+    pass
+
 
 if __name__=="__main__":
   import argparse
@@ -408,4 +458,35 @@ if __name__=="__main__":
 
 
     obj.update_optical_center(opt_ctr_meas, True)
+  if args.mode&0x04:
+    pts=np.array([[ -633.75367631,  -561.87640769],
+       [ -636.87852469,  -258.90639846],
+       [-1098.22395446,  -351.56498398],
+       [-1096.62487933,  -694.59151602]])
 
+    pts=np.array([[-699.81571798, -700.30880259],
+                  [-700.33262571, -500.3524493],
+                  [-1000.63771992, -501.08112667],
+                  [-999.69062995, -701.32290775]])
+
+    pts=np.array([[10,15],
+                  [40, 35],
+                  [10, 35],
+                  [40, 15]])
+    pts=-pts
+
+
+    for p in pts:
+      print(p)
+
+    trf=geometry.least_square_trf(pts)
+    for p in ((0,0),(0,1),(1,0),(1,1)):
+      fit=(trf*np.matrix((p[0],p[1],1)).T).A.ravel()
+      print(p,fit)
+
+    if args.mode&0x08:
+      pts=np.array([[3.95620203, 3.17424935, 3.1415],
+             [3.92067666, 2.43961212, 3.1415],
+             [2.80894834, 2.71536886, 3.1415],
+             [2.84446241, 3.54734879, 3.1415]])
+      plane=geometry.least_square_plane(pts)