sf-mx/SwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
d96f3cc9db8ed5e961f1545cb3768844abca54d8
SwissMX/geometry.py

534 lines
18 KiB
Python
Executable File
Raw Blame History

#!/usr/bin/env python
# *-----------------------------------------------------------------------*
# | |
# | Copyright (c) 2022 by Paul Scherrer Institute (http://www.psi.ch) |
# | Based on Zac great first implementation |
# | Author Thierry Zamofing (thierry.zamofing@psi.ch) |
# *-----------------------------------------------------------------------*
'''
coordinate systems, optical center, xray axis, pixel sizes etc.
ppm= pixel per mm
update_pix2pos
modes:
0x01: update_pix2pos
0x02: update_optical_center
'''
import logging
import numpy as np
_log=logging.getLogger(__name__)
class geometry:
def __init__(self):
pass
def find_optical_center(self, p):
# p is an array of
# at zoom out: (p1x,p1y),(p2x,p2y),(p3x,p3y),...
# at zoom in : (p1x,p1y),(p2x,p2y),(p3x,p3y),...
#
# the pixel positions are given in chip pixel coordinates (0/0= top/right)
# and ignore roi and binning
# the returned (cx,cy) is the pixel koordinate that does not change with zoom
# this coordinate represents also the origin of other coordinates
pass
def update_optical_center(self, meas, debug=False):
if debug:
import sys
import matplotlib.pyplot as plt
if sys.gettrace(): # in debuggingmode
plt.interactive(True) #uncomment to debug
print('DEBUG MODE')
fig, ax=plt.subplots()
ax.invert_yaxis()
ax.grid()
ax.axis('equal')
for k, v in meas.items():
v=np.array(v)
# ax.plot(v[:, 0], v[:, 1])
ax.fill(v[:, 0], v[:, 1], fill=False)
numPoints=len(next(iter(meas.values())))
numZoom=len(meas)
M=np.ndarray((numPoints, numZoom, 2), np.float)
K=np.array(tuple(meas.keys()), np.float)
for i, (k, v) in enumerate(meas.items()):
M[:, i, :]=np.array(v)
if debug:
for i in range(numPoints):
ax.plot(M[i, :, 0], M[i, :, 1])
# parametrizex lines:
# (USE ONLY 2 zoom levels (first and last!!!)
# (x,y)=(p0x+a*q, p0y+b*q, q=0 -> (p0x,p0y), q=1 (p1x,p1y)
# -> p1x=p0x+a*1 -> a=p1x-p0x
# -> p1y=p0y+b*1 -> b=p1y-p0y
AB=M[:,-1,:]-M[:,0,:] # parameters a1..an b1..bn
# least square fitting for x,y,q
# x=p0x+a0*q -> x-a0*q=p0x
# y=p0y+b0*q
# x=pnx+an*q
# y=pny+bn*q
# [1 0 -a0] [x] [p0x]
# [.. .. ..] * [y] = [..]
# [1 0 -an] [q] [pnx]
# [0 1 -b0] [p0y]
# [.. .. ..] [..]
# [0 1 -bn] [pny]
# A * aa = y
A=np.ndarray((numPoints*2, 3), np.float)
A[:numPoints,:2]=(1,0)
A[numPoints:,:2]=(0,1)
A[:,2]=-AB.T.ravel()
A=np.asmatrix(A)
y=np.asmatrix(M[:, 0, :].T.ravel()).T
aa=(A.T*A).I*A.T*y
if debug:
# plot least square line fitting
ax.autoscale(False)
P=np.ndarray((2, 2), np.float)
for i in range(numPoints):
q=aa[2]
P[0, :]=M[i, 0, :]
P[1, :]=M[i, 0, :]+q*AB[i,:]
ax.plot(P[:, 0], P[:, 1])
ax.plot(aa[0], aa[1], marker='*',color='r')
plt.show()
self._opt_ctr=np.asarray(aa[:2]).ravel()
_log.debug('optical center:{}'.format(tuple(self._opt_ctr)))
# # line fitting y= ax+b
# # calculate line fitting (least square)
# # y= a*x+b
# # [x1 1] [a] [y1]
# # [.. 1] * [b] = [..]
# # [xn 1] [yn]
# # A * aa = y
# # calculate least square line fitting for each point
# A=np.asmatrix(np.ndarray((numZoom, 2), np.float))
# A[:, 1]=1
# AA=np.ndarray((numPoints, 2), np.float) # fitting results a1..an b1..bn
# for i in range(numPoints):
# A[:, 0]=M[i, :, 0].reshape(-1, 1)
# y=np.asmatrix(M[i, :, 1]).T
# aa=(A.T*A).I*A.T*y
# AA[i]=aa.reshape(2)
# if debug:
# # plot least square line fitting
# ax.autoscale(False)
# rngx=(M[:, :, 0].min(), M[:, :, 0].max())
# Y=np.stack((AA[:, 0]*rngx[0]+AA[:, 1], AA[:, 0]*rngx[1]+AA[:, 1]))
# P=np.ndarray((2, 2), np.float)
# P[:, 0]=rngx
# for i in range(numPoints):
# P[:, 1]=Y[:, i]
# ax.plot(P[:, 0], P[:, 1])
# plt.show()
# # TODO: calculate all cross points of least square line fitting
# # a1*x + b1 = y
# # a2*x + b2 = y
# # [a1 -1] * [x] [-b1]
# # [a2 -1] * [y] = [-b2]
# P=np.ndarray(((numPoints-1)*numPoints//2, 2), np.float)
# k=0
# D=np.ndarray((2, 2), np.float)
# D[:, 1]=-1
# # for i in range(numPoints):
# # for j in range(i+1,numPoints):
# # P[k,:]=
# # k+=1
def interp_zoom(self, zoom):
# this calculates the interpolated pix2pos coordinate transformation matrix
# the returned value is a 2x2 matrix:
# [a b] [mx] [px]
# [ ]*[ ]=[ ]
# [c d] [my] [py]
K, AA=self._lut_pix2pos
pix2pos=self._pix2pos=np.asmatrix(np.ndarray((2, 2), np.float))
pix2pos[0, 0]=np.interp(zoom, K, AA[:, 0, 0])
pix2pos[0, 1]=np.interp(zoom, K, AA[:, 0, 1])
pix2pos[1, 0]=np.interp(zoom, K, AA[:, 1, 0])
pix2pos[1, 1]=np.interp(zoom, K, AA[:, 1, 1])
_log.debug(f'{zoom} -> {pix2pos.ravel()}')
def update_pix2pos(self, meas):
# calculates _lut_pix2pos out of measurements
# meas is a structure as in the sample code
# [pxx pxy]
# [pyx pyy]
#
# [a b] [px] [mx]
# [ ]*[ ]=[ ]
# [c d] [py] [my]
#
# [a*px1 b*py1] [mx1]
# [ ... ... ] [...]
# [a*pxn b*pyn] =[mxn]
# [c*px1 d*py1] [my1]
# [ ... ... ] [...]
# [c*pxn d*pyn] [myn]
#
# [px1 py1 0 0 ] [a] [mx1]
# [... .. 0 0 ] [b] [...]
# [pxn pyn 0 0 ]*[c]=[mxn]
# [ 0 0 px1 py1] [d] [my1]
# [ 0 0 ... .. ] [...]
# [ 0 0 pxn pyn] [myn]
#
# A *aa = y
# https://de.wikipedia.org/wiki/Methode_der_kleinsten_Quadrate
# r= A*aa-y
# mit aa=(A.T*A)^-1*A.T*y
# A=np.zeros((d.shape[0]*2,d.shape[1]))
# A[:d.shape[1]-1,:2]=A[d.shape[1]-1:,2:]=d[:,:2]
#
# y=d[:,2:].T.ravel()
# y=np.asmatrix(y).T
#
# A=np.asmatrix(A)
# aa=(A.T*A).I*A.T*y
# a,b,c,d=aa
# a,b,c,d=np.asarray(aa).ravel()
#
# aa=aa.reshape(2,-1)
_log.debug('raw data\nmeas:{}'.format(meas))
if len(meas)<2:
_log.error('need at least 2 zoom levels:\nmeas:{}'.format(meas))
return
AA=np.ndarray((len(meas), 2, 2), np.float)
K=np.array(tuple(meas.keys()), np.float)
self._lut_pix2pos=(K, AA)
for i, (k, v) in enumerate(meas.items()):
m=np.array(v) # measurements
if m.shape[0]<3:
_log.error('zoom:{}: need at least 3 points per zoom levels:\nmeas:{}'.format(k, v))
return
d=m[1:]-m[0] # distances
A=np.zeros((d.shape[0]*2, d.shape[1]), np.float)
A[:d.shape[0], :2]=A[d.shape[0]:, 2:]=d[:, 2:]
y=d[:, :2].T.ravel()
y=np.asmatrix(y).T
A=np.asmatrix(A)
aa=(A.T*A).I*A.T*y
AA[i]=aa.reshape(2, -1)
# bx_coefs = np.polyfit(nbx[0], nbx[1], 3)
# np.poly1d(bx_coefs)
_log.debug('least square data:\nK:{}\nAA:{}'.format(K, AA))
def autofocus(self):
# cam camera object
# mot motor object
# rng region (min max relative to current position) to seek
# n number of images to take in region
# roi region of interrest to calculate sharpness
# mode mode to calculate sharpness (sum/max-min/hist? of edge detection in roi)
pass
def pix2pos(self, p, zoom=None):
# returns the position m(x,y) in meter relative to the optical center at a given zoom level of the pixel p(x,y)
# if zoom=None, the last zoom value is used
if zoom is not None:
self.interp_zoom(zoom)
return np.asarray(self._pix2pos*np.mat(p).T).ravel()
def pos2pix(self, p, zoom=None):
# returns the pixel p(x,y) of the position m(x,y) in meter relative to the optical center at a given zoom level
# if zoom=None, the last zoom value is used
if zoom is not None:
self.interp_zoom(zoom)
return np.asarray(self._pix2pos.I*np.mat(p).T).ravel()
def optctr2xray(self):
# returns the vector m(x,y) of the optical center to the xray
pass
@staticmethod
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 ]
#[ 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)
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
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
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
# 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)
try:
aa=(A.T*A).I*A.T*y
except np.linalg.LinAlgError as e:
_log.warning(e)
return
aa=aa.A.ravel()
print(f'plane={aa[0]}X+{aa[1]}Y+{aa[2]}')
for p in points:
print(f'{p}->{aa[0]*p[0]+aa[1]*p[1]+aa[2]}')
self._fitPlane=aa
if __name__=="__main__":
import argparse
logging.basicConfig(level=logging.DEBUG, format='%(levelname)s:%(module)s:%(lineno)d:%(funcName)s:%(message)s ')
#(h, t)=os.path.split(sys.argv[0]);cmd='\n '+(t if len(h)>20 else sys.argv[0])+' '
#exampleCmd=('', '-m0xf -v0')
epilog=__doc__ #+'\nExamples:'+''.join(map(lambda s:cmd+s, exampleCmd))+'\n'
parser=argparse.ArgumentParser(epilog=epilog, formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument("-m", "--mode", type=lambda x: int(x,0), help="mode (see bitmasks) default=0x%(default)x", default=0xff)
args=parser.parse_args()
_log.info('Arguments:{}'.format(args.__dict__))
logging.getLogger('matplotlib').setLevel(logging.INFO)
import numpy as np
import matplotlib as mpl
import matplotlib.pyplot as plt
np.set_printoptions(suppress=True)
np.set_printoptions(linewidth=196)
obj=geometry()
if args.mode&0x01:
measure={
1:[(-3.0, -7.6, 116.99110193046, 632.5463827525),
(-2.0, -7.6, 934.07501517856, 600.7926167715),
(-2.0, -7.1, 916.54131238102, 191.0366615002),
(-3.0, -7.1, 103.74668003329, 226.2150231456)],
200:[(-3.1, -7.3, 113.66321353086, 824.9041423107),
(-2.3, -7.3, 1065.97386092697, 792.2851118419),
(-2.3, -6.7, 1033.68452410347, 74.0336610693),
(-3.1, -6.7, 84.62681572700, 116.6832971512)],
400:[(-3.4, -6.7, 155.00053674203, 601.3838942136),
(-3.0, -6.7, 957.95919656052, 573.0827012272),
(-3.2, -6.5, 541.08684037200, 187.9171307943),
(-3.2, -6.8, 564.32152887203, 789.1146957326)],
600:[(-3.3, -6.8, 328.27244399903, 509.5061192017),
(-3.1, -6.8, 992.78996735279, 488.0323963092),
(-3.2, -6.9, 672.03111567934, 832.4122409755),
(-3.2, -6.7, 645.70960116180, 164.2534779331)],
800:[(-3.2, -6.7, 639.52253576449, 53.4455632943),
(-3.2, -6.85, 671.47023245203, 882.6335091391),
(-3.3, -6.75, 105.12470026379, 361.3051859197),
(-3.1, -6.75, 1195.96864609255, 313.1068618673)],
1000:[(-3.25, -6.75, 195.05641095116, 353.3492286375),
(-3.15, -6.75, 1117.27204644084, 314.9636405871),
(-3.2, -6.8, 675.10991143017, 790.3040145281),
(-3.2, -6.72, 638.98580653116, 59.3803912957)]}
measure=\
{1: [(6.4190000000000005, 6.907, 696.2976073449591, 227.76776138682274),
(5.719, 6.807, 410.67447894055806, 284.44919082240665),
(5.719, 5.207, 434.78681995014756, 938.5631353309454),
(6.619, 5.207, 808.7345059175318, 927.4590367789931)],
200: [(6.619, 6.607, 940.264899030901, 187.39774454408854),
(5.519, 6.607, 272.1925062601967, 219.8977706369289),
(5.519, 5.407, 298.31361407848783, 941.97432810785),
(6.619, 5.407, 969.4645435773624, 920.3295993015843)],
400: [(6.719, 6.707, 1099.5956451604673, 46.46696479882394),
(5.719, 6.707, 91.88845211350781, 99.00912182475837),
(5.719, 5.807, 122.49326406274122, 998.8073593766618),
(6.719, 5.807, 1137.5074440945523, 963.4302060882611)],
600: [(6.619, 6.807, 1068.8592966662907, 106.9592377596037),
(6.019, 6.807, 75.47928488084321, 151.1144445833613),
(6.019, 6.307, 103.13064811335389, 977.5525209662414),
(6.619, 6.307, 1104.9406638931132, 939.9675773457886)],
800: [(6.619, 6.307, 1160.5691045813528, 202.93697492698996),
(6.219, 6.307, 65.05964829488164, 253.28864950246646),
(6.219, 6.107, 88.00366601279788, 797.1538427010806),
(6.619, 6.107, 1180.6517090746024, 753.0303703110591)],
1000: [(6.519, 6.267, 1087.8134573150567, 183.90222416155348),
(6.319, 6.267, 160.59872007696185, 229.55456266733404),
(6.319, 6.107, 190.50887289109403, 963.1404158981525),
(6.519, 6.107, 1116.3595117455784, 925.6445737503763)]}
obj.update_pix2pos(measure)
obj.interp_zoom(1)
print(obj._pix2pos)
print(obj._pix2pos.I)
obj.interp_zoom(200)
obj.interp_zoom(100)
if args.mode&0x02:
opt_ctr_meas={ # x,y = 0.02, -4.89
1000:[(1057.4251530483375, 116.10122290395591),
(117.84916300310408, 190.27827474963223),
(184.2181041281829, 963.2812360887852),
(1092.5616512910262, 899.514998537239)],
800:[(888.2494207687248, 203.2917926172947),
(329.96950424600305, 248.83910515411347),
(372.9141132092893, 708.2162858826),
(906.4683457834523, 675.6824912134438)],
600:[(781.5385742538922, 251.44180872764602),
(447.09116505496564, 264.4553265953085),
(471.81684900352445, 554.6567750441825),
(798.4561474818535, 535.1364982426887)],
400:[(722.9777438494109, 286.5783069703348),
(525.1722722609408, 295.68776947769857),
(535.5830865550707, 462.26079818377866),
(729.4845027832422, 450.5486321028824)],
200:[(689.1425973934884, 308.70128734536104),
(565.5141776506945, 307.39993555859473),
(574.6236401580583, 406.30267135282986),
(693.0466527537872, 399.79591241899857)],
1:[(673.5263759522934, 307.39993555859473),
(591.5412133860195, 308.70128734536104),
(595.4452687463182, 376.3715802572061),
(672.2250241655271, 373.7688766836736)]}
opt_ctr_meas=\
{1000: [(943.5814614822486, 271.79551650350834),
(310.28543090843203, 317.87448013362246),
(357.2157641345181, 945.3504169712918),
(982.8856744171983, 907.5691713113356)],
# 800: [(804.3925790602868, 377.45059798190084),
# (428.94991325159833, 401.78484483987137),
# (456.760481089279, 776.3584304036324),
# (829.595906163185, 752.0241835456618)],
# 600: [(725.3062767718826, 435.6789743920446),
# (497.52754628726467, 448.43934430925407),
# (514.6798917766387, 674.3986678435945),
# (740.7483129345815, 659.5001702939031)],
400: [(676.5616510826158, 467.6400729767663),
(539.6440684653923, 477.20915075936176),
(550.068542225442, 612.9457099265683),
(686.4200153677604, 603.8457508612842)],
# 200: [(647.9882475277134, 488.31261219978296),
# (564.236743827639, 494.4770028644908),
# (571.9870755133423, 577.6833939808159),
# (654.4623032630916, 571.5416217015793)],
# 1: [(633.8759316189822, 496.84436992197425),
# (578.3851177374523, 500.9472972316709),
# (582.6205681144841, 559.3272486023482),
# (638.3177769017573, 554.8006203263507)]
}
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)
Reference in New Issue View Git Blame Copy Permalink