saving document, plane and fiducal fitter

geometry.py

@@ -268,7 +268,7 @@ class geometry:
     pass
 
   @staticmethod
-  def least_square_trf(points):
+  def least_square_trf(points,fid=None,sort=True):
     # inputs are 4 points of a parallelogram
     # this function returns the least square transformation
     #[ px]  [a b c ] [ q ]
@@ -287,35 +287,71 @@ class geometry:
     #
     #          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)
+    # 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)
 
+    if fid is None:
+      fid=np.array(((0,0),(0,1),(1,0),(1,1)))
+    elif type(fid)!=np.ndarray:
+      fid=np.array(fid)
 
     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,:]
+    if sort:
+      # p01 ----------- p11
+      #  |               |
+      #  |               |
+      # p00-------------p10
+      def sort(a):
+        s=a[:,0].argsort()
+        a=a[s,:]
+        s=a[:2,1].argsort()
+        a[:2,:]=a[:2,:][s,:]
+        s=a[2:,1].argsort()
+        a[2:,:]=a[2:,:][s,:]
+        return a
+      y=sort(y)
+      fid=sort(fid)
+
+    A=np.ndarray((len(fid)*2,6))
+    i=0
+    for q,r in fid:
+      A[i]  =(q,r,1,0,0,0)
+      A[i+1]=(0,0,0,q,r,1)
+      i+=2
+
     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):
+  def least_square_plane(self,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
+
+    #  a   b    c
+    # [px1 py1  1 ] [a] [pz1]
+    # [px2 py2  1 ] [b] [pz2]
+    # [... ...  1 ]*[c]=[...]
+    # [pxn pyn  1 ]     [pzn]
+    A=np.ndarray((points.shape[0],3))
+    y=points[:,2]
+    A[:,2]=1
+    A[:,0:2]=points[:,:2]
+    y=np.asmatrix(y.ravel()).T
+    A=np.asmatrix(A)
+    aa=(A.T*A).I*A.T*y
+    aa=aa.A.ravel()
+    for p in points:
+      print(f'{p}->{aa[0]*p[0]+aa[1]*p[1]+aa[2]}')
+    self._fitPlane=aa
 
 
 if __name__=="__main__":
@@ -473,7 +509,7 @@ if __name__=="__main__":
                   [40, 35],
                   [10, 35],
                   [40, 15]])
-    pts=-pts
+    #pts=-pts
 
 
     for p in pts: