from math import sqrt,hypot
from p_gnum import array,matrixmultiply,transpose,solve_linear_equations,LinAlgError
from p_ggen import iterby2
class __Projection:
"Abstract class for projection objects."
icodp = None
name = None
def copy(self):
"Return an independent copy of self."
return self.__class__(self.L)
def relative(self, fots):
"Find mean and subtract constant term; normalize pixel coordinates to the length of world coordinates."
assert len(fots) > 1, "Only 1 point??!!"
Xsum = Ysum = Zsum = xrsum = yrsum = 0.0
Xmin = fots[0][0]
Ymin = fots[0][1]
Zmin = fots[0][2]
xrmin = fots[0][3]
yrmin = fots[0][4]
for X,Y,Z,xr,yr,_,_,_ in fots:
Xsum += X
Ysum += Y
xrsum += xr
yrsum += yr
if X < Xmin: Xmin = X
if Y < Ymin: Ymin = Y
if Z < Zmin: Zmin = Z
if xr < xrmin: xrmin = xr
if yr < yrmin: yrmin = yr
n = len(fots)
Xsum /= n
Ysum /= n
Zsum /= n
xrsum /= n
yrsum /= n
am = 1.0
D = hypot(Xsum-Xmin, Ysum-Ymin)
dr = hypot(xrsum-xrmin, yrsum-yrmin)
if dr > 0.0 and D > 0.0: am = D/dr
fotsr = [(X-Xmin, Y-Ymin, Z-Zmin, (x-xrmin)*am,(y-yrmin)*am,zr,xyok,zok) for (X,Y,Z,x,y,zr,xyok,zok) in fots]
return fotsr, [Xmin, Ymin, Zmin], [xrmin, yrmin, am]
def er_nodes(self, fots):
"Compute the 2d error of the projection."
er = erh = 0.0
n = 0.0
for xg,yg,zg,xr,yr,zr,xyok,zok in fots:
xyok **= 2
xx, yy, zz = self.project((xg, yg, zg))
er += xyok*((xx-xr)**2 + (yy-yr)**2)
n += xyok
er = sqrt(er/n); erh = sqrt(erh/n)
dis = [hypot(ga[3]-ra[3], ga[4]-ra[4]) for (ga, ra) in iterby2(fots)]
discom = sum(dis)
return er, erh, discom
def er(self, fots):
"""Compute the 2d error of the projection.
We use trapezoidal integration to take into account the distance between the nodes.
gdiscom is the length of the 3d-line common to both curves.
discom is the length of the 2d-line common to both curves.
The first value that is returned is the error in projected (2d) coordinates.
The second value that is returned is the error in 3d coordinates.
"""
gdiscom = sum([sqrt((ga[0]-ra[0])**2+(ga[1]-ra[1])**2+(ga[2]-ra[2])**2) for (ga, ra) in iterby2(fots)])
dis = [hypot(ga[3]-ra[3], ga[4]-ra[4]) for (ga, ra) in iterby2(fots)]
discom = sum(dis)
# if discom/gps1.length() < 0.66: continue # Curves are not near enough (or bad prealignment)
r = []
for xg,yg,zg,xr,yr,zr,xyok,zok in fots:
xyok **= 2
xx, yy, zz = self.project((xg, yg, zg))
r.append(xyok*((xx-xr)**2 + (yy-yr)**2))
er = 0.0
for i in xrange(len(fots)-1):
er += (r[i]+r[i+1])*0.5*dis[i]
er = sqrt(er/discom)
return er, er*gdiscom/discom, discom
def readIcod(self, fr):
"Reads and checks the code, if it matches the object, or it it matches polynomial1 projection."
dl1 = read1(fr)
ic = int(dl1)
if ic == self.icodp or ic == 0: return ic
raise ValueError, "Projection code in file is wrong" # Accept polynomial as a first approximation
def readCoefs(self, fr, n):
"Reads n real coefficients."
L = []
for i in xrange(n):
dl1 = read1(fr)
dl1 = dl1.replace("d", "e").replace("D", "e") # Allow for Fortran generated file
L.append(float(dl1))
return L
###############################################################################
###############################################################################
class DLTProjection(__Projection):
"This class provides the machinery for the DLT projection."
icodp = 1
name = "Direct Linear Transform"
def __init__(self, L=None):
"Initialize the object with known coeffcients."
if (L == None):
self.L = [None,
1.0, 0.0, 0.0, 0.0, # Coefs for xr
0.0, 1.0, 0.0, 0.0, # Coefs for yr
0.0, 0.0, 0.0, # Coefs for denominator
0.0, 0.0, 0.0, # Xmin, Ymin, Zmin
0.0, 0.0, 1.0 # xpmin, ypmin, am
]
else:
assert len(L) == 18, "There should exactly 18 coefficients for the DLT Projection"
self.L = L[:]
def project(self, c3d):
"Projects 3d point with polynomial projection."
X, Y, Z = c3d[:3]
L = self.L
X -= L[12]; Y -= L[13]; Z -= L[14]
par = L[9]*X + L[10]*Y + L[11]*Z + 1
xp = (L[1]*X + L[2]*Y + L[3]*Z + L[4]) / par
yp = (L[5]*X + L[6]*Y + L[7]*Z + L[8]) / par
return xp/L[17]+L[15], yp/L[17]+L[16], c3d[2] # The last coordinate (z) is not used
def lsm23(self, fots):
"Find polynomial coefficients using least square."
fotsr, con, dcp = self.relative(fots)
A, B = [], []
for (X,Y,Z,x,y,zr,xyok,zok) in fotsr:
#FIXME ??????????WEIGHTS??????????????
A.append((-X, -Y, -Z, -1, 0, 0, 0, 0, x*X, x*Y, x*Z))
B.append(-x)
A.append((0, 0, 0, 0, -X, -Y, -Z, -1, y*X, y*Y, y*Z))
B.append(-y)
A = array(A)
B = array(B)
AT = transpose(A)
A = matrixmultiply(AT, A)
B = matrixmultiply(AT, B)
try: a = solve_linear_equations(A, B)
except LinAlgError, why:
# print "Match2 DLT solution failed: ", why
return None, None, None
self.L = list(a)
self.L.insert(0, None)
self.L.extend(con)
self.L.extend(dcp)
return self.er(fots)
def read(self, fr, skipicod=False):
"""Reads the coefficients from an opened text file.
It is the reponsibility of the caller to catch any exceptions."""
if skipicod: ic = self.icodp
else: ic = self.readIcod(fr)
if ic == self.icodp:
self.L = self.readCoefs(fr, 17)
self.L.insert(0, None)
else: # Read coefficients from polynomial1 projection
L = self.readCoefs(fr, 8)
self.L = [None,
L[0], L[1], L[2], L[3], # Coefs for xr
L[4], L[5], L[6], L[7], # Coefs for yr
0.0, 0.0, 0.0, # Coefs for denominator
0.0, 0.0, 0.0, # Xmin, Ymin, Zmin
0.0, 0.0, 1.0 # xpmin, ypmin, am
]
def write(self, fw):
"Write the projection parameters to an ascii file."
fw.write("""\
#DLT projection
#
# L1 X' + L2 Y' + L3 Z' + L4
# x' = -----------------------------
# L9 X' + L10 Y' + L11 Z' + 1
#
# L5 X' + L6 Y' + L7 Z' + L8
# y' = -----------------------------
# L9 X' + L10 Y' + L11 Z' + 1
#
# x = x/mu + xmin
# y = y/mu + ymin
# X' = X - Xmin
# Y' = Y - Ymin
# Z' = Z - Zmin
#
""")
fw.write("\n%2d # ThanCad Projection code\n\n" % self.icodp)
for i in xrange(1, 5): fw.write("%27.20e # L%d\n" % (self.L[i], i))
fw.write("\n")
for i in xrange(5, 9): fw.write("%27.20e # L%d\n" % (self.L[i], i))
fw.write("\n")
for i in xrange(9, 12): fw.write("%27.20e # L%d\n" % (self.L[i], i))
fw.write("\n")
fw.write("%27.20e # Xmin\n" % self.L[12])
fw.write("%27.20e # Ymin\n" % self.L[13])
fw.write("%27.20e # Zmin\n" % self.L[14])
fw.write("\n")
fw.write("%27.20e # xmin\n" % self.L[15])
fw.write("%27.20e # xmin\n" % self.L[16])
fw.write("%27.20e # mu\n" % self.L[17])
###############################################################################
###############################################################################
class Rational1Projection(__Projection):
"This class provides the machinery for the 1st order rational polynomial projection."
icodp = 2
name = "1st order rational"
def __init__(self, L=None):
"Initialize the object with known coeffcients."
if (L == None):
self.L = [1.0, 0.0, 0.0, 0.0, # Coefs for xr
0.0, 0.0, 0.0, # Coefs for denominator xr
0.0, 1.0, 0.0, 0.0, # Coefs for yr
0.0, 0.0, 0.0, # Coefs for denominator yr
0.0, 0.0, 0.0, # Xmin, Ymin, Zmin
0.0, 0.0, 1.0 # xpmin, ypmin, am
]
else:
assert len(L) == 20, "There should exactly 20 coefficients for the Rational1 Projection"
self.L = L[:]
def project(self, c3d):
"Projects 3d point with polynomial projection."
X, Y, Z = c3d[:3]
L = self.L
X -= L[14]; Y -= L[15]; Z -= L[16]
par = L[4]*X + L[5]*Y + L[6]*Z + 1
xp = (L[0]*X + L[1]*Y + L[2]*Z + L[3]) / par
par = L[11]*X + L[12]*Y + L[13]*Z + 1
yp = (L[7]*X + L[8]*Y + L[9]*Z + L[10]) / par
return xp/L[19]+L[17], yp/L[19]+L[18], c3d[2] # The last coordinate (z) is not used
def lsm23(self, fots):
"Find polynomial coefficients using least square."
fotsr, con, dcp = self.relative(fots)
self.L = []
for i in 0, 1:
A, B = [], []
for (X,Y,Z,x,y,zr,xyok,zok) in fotsr:
#FIXME ??????????WEIGHTS??????????????
if i == 0: cx = x
else: cx = y
A.append((-X, -Y, -Z, -1, cx*X, cx*Y, cx*Z))
B.append(-cx)
A = array(A)
B = array(B)
AT = transpose(A)
A = matrixmultiply(AT, A)
B = matrixmultiply(AT, B)
try: a = solve_linear_equations(A, B)
except LinAlgError, why:
# print "Match2 Rational 1 solution failed: ", why
return None, None, None
self.L.extend(list(a))
self.L.extend(con)
self.L.extend(dcp)
return self.er(fots)
def read(self, fr, skipicod=False):
"""Reads the coefficients from an opened text file.
It is the reponsibility of the caller to catch any exceptions."""
if skipicod: ic = self.icodp
else: ic = self.readIcod(fr)
if ic == self.icodp:
self.L = self.readCoefs(fr, 20)
else: # Read coefficients from polynomial1 projection
L = self.readCoefs(fr, 8)
self.L = [L[0], L[1], L[2], L[3], # Coefs for xr
0.0, 0.0, 0.0, # Coefs for denominator xr
L[4], L[5], L[6], L[7], # Coefs for yr
0.0, 0.0, 0.0, # Coefs for denominator yr
0.0, 0.0, 0.0, # Xmin, Ymin, Zmin
0.0, 0.0, 1.0 # xpmin, ypmin, am
]
def write(self, fw):
"Write the projection parameters to an ascii file."
fw.write("""\
#Rational projection of first order
#
# L1 X' + L2 Y' + L3 Z' + L4
# x' = ----------------------------
# L5 X' + L6 Y' + L7 Z' + 1
#
# L8 X' + L9 Y' + L10 Z' + L11
# y' = ------------------------------
# L12 X' + L13 Y' + L14 Z' + 1
#
# x = x/mu + xmin
# y = y/mu + ymin
# X' = X - Xmin
# Y' = Y - Ymin
# Z' = Z - Zmin
#
""")
fw.write("\n%2d # ThanCad Projection code\n\n" % self.icodp)
for i in xrange(0, 4): fw.write("%27.20e #L%d\n" % (self.L[i], i+1))
fw.write("\n")
for i in xrange(4, 7): fw.write("%27.20e #L%d\n" % (self.L[i], i+1))
fw.write("\n")
for i in xrange(7, 11): fw.write("%27.20e #L%d\n" % (self.L[i], i+1))
fw.write("\n")
for i in xrange(11, 14): fw.write("%27.20e #L%d\n" % (self.L[i], i+1))
fw.write("\n")
fw.write("%27.20e #Xmin\n" % self.L[14])
fw.write("%27.20e #Ymin\n" % self.L[15])
fw.write("%27.20e #Zmin\n" % self.L[16])
fw.write("\n")
fw.write("%27.20e #xmin\n" % self.L[17])
fw.write("%27.20e #xmin\n" % self.L[18])
fw.write("%27.20e #mu\n" % self.L[19])
###############################################################################
###############################################################################
class Rational2Projection(__Projection):
"This class provides the machinery for the 2nd order rational polynomial projection."
icodp = 4
name = "2nd order rational"
def __init__(self, L=None):
"Initialize the object with known coeffcients."
if (L == None):
self.L = [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, # Coefs for xr
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, # Coefs for denominator xr
0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, # Coefs for yr
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, # Coefs for denominator yr
0.0, 0.0, 0.0, # Xmin, Ymin, Zmin
0.0, 0.0, 1.0 # xpmin, ypmin, am
]
else:
assert len(L) == 36, "There should exactly 36 coefficients for the Rational2 Projection"
self.L = L[:]
def project(self, c3d):
"Projects 3d point with polynomial projection."
X, Y, Z = c3d[:3]
L = self.L
X -= L[30]; Y -= L[31]; Z -= L[32]
par = L[8]*X + L[9]*Y + L[10]*Z + 1 + L[11]*X**2 + L[12]*Y**2 + L[13]*Z**2 + L[14]*X*Y
xp = ( L[0]*X + L[1]*Y + L[2]*Z + L[3] + L[4]*X**2 + L[5]*Y**2 + L[6]*Z**2 + L[7]*X*Y) / par
par = L[23]*X + L[24]*Y + L[25]*Z + 1 + L[26]*X**2 + L[27]*Y**2 + L[28]*Z**2 + L[29]*X*Y
yp = (L[15]*X + L[16]*Y + L[17]*Z + L[18] + L[19]*X**2 + L[20]*Y**2 + L[21]*Z**2 + L[22]*X*Y) / par
return xp/L[35]+L[33], yp/L[35]+L[34], c3d[2] # The last coordinate (z) is not used
def lsm23(self, fots):
"Find polynomial coefficients using least square."
fotsr, con, dcp = self.relative(fots)
self.L = []
for i in 0, 1:
A, B = [], []
for (X,Y,Z,x,y,zr,xyok,zok) in fotsr:
#FIXME ??????????WEIGHTS??????????????
if i == 0: cx = x
else: cx = y
A.append((-X, -Y, -Z, -1, -X**2, -Y**2, -Z**2, -X*Y, cx*X, cx*Y, cx*Z, cx*X**2, cx*Y**2, cx*Z**2, cx*X*Y))
B.append(-cx)
A = array(A)
B = array(B)
AT = transpose(A)
A = matrixmultiply(AT, A)
B = matrixmultiply(AT, B)
try: a = solve_linear_equations(A, B)
except LinAlgError, why:
# print "Match2 Rational 1 solution failed: ", why
return None, None, None
self.L.extend(list(a))
self.L.extend(con)
self.L.extend(dcp)
return self.er(fots)
def read(self, fr, skipicod=False):
"""Reads the coefficients from an opened text file.
It is the reponsibility of the caller to catch any exceptions."""
if skipicod: ic = self.icodp
else: ic = self.readIcod(fr)
if ic == self.icodp:
self.L = self.readCoefs(fr, 36)
else: # Read coefficients from polynomial1 projection
L = self.readCoefs(fr, 8)
self.L = [L[0], L[1], L[2], L[3], 0.0, 0.0, 0.0, 0.0, # Coefs for xr
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, # Coefs for denominator xr
L[4], L[5], L[6], L[7], 0.0, 0.0, 0.0, 0.0, # Coefs for yr
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, # Coefs for denominator yr
0.0, 0.0, 0.0, # Xmin, Ymin, Zmin
0.0, 0.0, 1.0 # xpmin, ypmin, am
]
def write(self, fw):
"Write the projection parameters to an ascii file."
fw.write("""\
#Rational projection of second order
#
# L0 X' + L1 Y' + L2 Z' + L3 + L4 X^2 + L5 Y^2 + L6 Z^2 + L7 X Y
# x' = --------------------------------------------------------------------
# L8 X' + L9 Y' + L10 Z' + 1 + L11 X^2 + L12 Y^2 + L13 Z^2 + L14 X Y
#
# L15 X' + L16 Y' + L17 Z' + L18 + L19 X^2 + L20 Y^2 + L21 Z^2 + L22 X Y
# y' = ----------------------------------------------------------------------------
# L23 X' + L24 Y' + L25 Z' + 1 + L26 X^2 + L27 Y^2 + L28 Z^2 + L29 X Y
#
# x = x/mu + xmin
# y = y/mu + ymin
# X' = X - Xmin
# Y' = Y - Ymin
# Z' = Z - Zmin
#
""")
fw.write("\n%2d # ThanCad Projection code\n\n" % self.icodp)
for i in xrange(0, 8): fw.write("%27.20e #L%d\n" % (self.L[i], i+1))
fw.write("\n")
for i in xrange(8, 15): fw.write("%27.20e #L%d\n" % (self.L[i], i+1))
fw.write("\n")
for i in xrange(15, 23): fw.write("%27.20e #L%d\n" % (self.L[i], i+1))
fw.write("\n")
for i in xrange(23, 30): fw.write("%27.20e #L%d\n" % (self.L[i], i+1))
fw.write("\n")
fw.write("%27.20e #Xmin\n" % self.L[30])
fw.write("%27.20e #Ymin\n" % self.L[31])
fw.write("%27.20e #Zmin\n" % self.L[32])
fw.write("\n")
fw.write("%27.20e #xmin\n" % self.L[33])
fw.write("%27.20e #xmin\n" % self.L[34])
fw.write("%27.20e #mu\n" % self.L[35])
###############################################################################
###############################################################################
class Rational15Projection(__Projection):
"This class provides the machinery for the 2nd order mominmator, first order denominator polynomial projection."
icodp = 5
name = "2nd and 1st order rational"
def __init__(self, L=None):
"Initialize the object with known coeffcients."
if (L == None):
self.L = [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, # Coefs for xr
0.0, 0.0, 0.0, # Coefs for denominator xr
0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, # Coefs for yr
0.0, 0.0, 0.0, # Coefs for denominator yr
0.0, 0.0, 0.0, # Xmin, Ymin, Zmin
0.0, 0.0, 1.0 # xpmin, ypmin, am
]
else:
assert len(L) == 28, "There should exactly 28 coefficients for the Rational 1.5 Projection"
self.L = L[:]
def project(self, c3d):
"Projects 3d point with polynomial projection."
X, Y, Z = c3d[:3]
L = self.L
X -= L[22]; Y -= L[23]; Z -= L[24]
par = L[8]*X + L[9]*Y + L[10]*Z + 1
xp = ( L[0]*X + L[1]*Y + L[2]*Z + L[3] + L[4]*X**2 + L[5]*Y**2 + L[6]*Z**2 + L[7]*X*Y) / par
par = L[19]*X + L[20]*Y + L[21]*Z + 1
yp = (L[11]*X + L[12]*Y + L[13]*Z + L[14] + L[15]*X**2 + L[16]*Y**2 + L[17]*Z**2 + L[18]*X*Y) / par
return xp/L[27]+L[25], yp/L[27]+L[26], c3d[2] # The last coordinate (z) is not used
def lsm23(self, fots):
"Find polynomial coefficients using least square."
fotsr, con, dcp = self.relative(fots)
self.L = []
for i in 0, 1:
A, B = [], []
for (X,Y,Z,x,y,zr,xyok,zok) in fotsr:
#FIXME ??????????WEIGHTS??????????????
if i == 0: cx = x
else: cx = y
A.append((-X, -Y, -Z, -1, -X**2, -Y**2, -Z**2, -X*Y, cx*X, cx*Y, cx*Z))
B.append(-cx)
A = array(A)
B = array(B)
AT = transpose(A)
A = matrixmultiply(AT, A)
B = matrixmultiply(AT, B)
try: a = solve_linear_equations(A, B)
except LinAlgError, why:
# print "Match2 Rational 1 solution failed: ", why
return None, None, None
self.L.extend(list(a))
self.L.extend(con)
self.L.extend(dcp)
return self.er(fots)
def read(self, fr, skipicod=False):
"""Reads the coefficients from an opened text file.
It is the reponsibility of the caller to catch any exceptions."""
if skipicod: ic = self.icodp
else: ic = self.readIcod(fr)
if ic == self.icodp:
self.L = self.readCoefs(fr, 28)
else: # Read coefficients from polynomial1 projection
L = self.readCoefs(fr, 8)
self.L = [L[0], L[1], L[2], L[3], 0.0, 0.0, 0.0, 0.0, # Coefs for xr
0.0, 0.0, 0.0, # Coefs for denominator xr
L[4], L[5], L[6], L[7], 0.0, 0.0, 0.0, 0.0, # Coefs for yr
0.0, 0.0, 0.0, # Coefs for denominator yr
0.0, 0.0, 0.0, # Xmin, Ymin, Zmin
0.0, 0.0, 1.0 # xpmin, ypmin, am
]
def write(self, fw):
"Write the projection parameters to an ascii file."
fw.write("""\
#Rational projection of second order
#
# L0 X' + L1 Y' + L2 Z' + L3 + L4 X^2 + L5 Y^2 + L6 Z^2 + L7 X Y
# x' = --------------------------------------------------------------------
# L8 X' + L9 Y' + L10 Z' + 1
#
# L11 X' + L12 Y' + L13 Z' + L14 + L15 X^2 + L16 Y^2 + L17 Z^2 + L18 X Y
# y' = ----------------------------------------------------------------------------
# L19 X' + L20 Y' + L21 Z' + 1
#
# x = x/mu + xmin
# y = y/mu + ymin
# X' = X - Xmin
# Y' = Y - Ymin
# Z' = Z - Zmin
#
""")
fw.write("\n%2d # ThanCad Projection code\n\n" % self.icodp)
for i in xrange(0, 8): fw.write("%27.20e #L%d\n" % (self.L[i], i+1))
fw.write("\n")
for i in xrange(8, 11): fw.write("%27.20e #L%d\n" % (self.L[i], i+1))
fw.write("\n")
for i in xrange(11, 19): fw.write("%27.20e #L%d\n" % (self.L[i], i+1))
fw.write("\n")
for i in xrange(19, 22): fw.write("%27.20e #L%d\n" % (self.L[i], i+1))
fw.write("\n")
fw.write("%27.20e #Xmin\n" % self.L[22])
fw.write("%27.20e #Ymin\n" % self.L[23])
fw.write("%27.20e #Zmin\n" % self.L[24])
fw.write("\n")
fw.write("%27.20e #xmin\n" % self.L[25])
fw.write("%27.20e #xmin\n" % self.L[26])
fw.write("%27.20e #mu\n" % self.L[27])
###############################################################################
###############################################################################
class Polynomial1Projection(__Projection):
"This class provides the machinery for the polynomial projection of 1st degree."
icodp = 0
name = "1st order polynomial"
def __init__(self, L=None):
"Initialize the object with known coeffcients."
if (L == None):
self.L = [1.0, 0.0, 0.0, 0.0, # Coefs for xr
0.0, 1.0, 0.0, 0.0, # Coefs for yr
]
else:
assert len(L) == 8, "There should exactly 8 coefficients for the Polynomial Projection of 1st order"
self.L = L[:]
def project(self, c3d):
"Projects 3d point with polynomial projection."
x, y, z = c3d[:3]
L = self.L
xp = L[0]*x+L[1]*y+L[2]*z+L[3]
yp = L[4]*x+L[5]*y+L[6]*z+L[7]
return xp, yp, z # The last coordinate (z) is not used
def lsm23(self, fots):
"Find polynomial coefficients using least square."
L = []
for i in 0, 1:
A, B = [], []
for xg,yg,zg,xr,yr,zr,xyok,zok in fots:
A.append([xg*xyok, yg*xyok, zg*xyok, xyok])
if i == 0: B.append(xr*xyok)
else : B.append(yr*xyok)
A = array(A)
B = array(B)
AT = transpose(A)
A = matrixmultiply(AT, A)
B = matrixmultiply(AT, B)
try: a = solve_linear_equations(A, B)
except LinAlgError, why:
# print "Match2 polynomial solution failed: ", why
return None, None, None
L.extend(list(a))
self.L = L
return self.er(fots)
def read(self, fr, skipicod=False):
"""Reads the coefficients from an opened text file.
It is the reponsibility of the caller to catch any exceptions."""
if skipicod: ic = self.icodp
else: ic = self.readIcod(fr)
assert ic == self.icodp, "Well it IS a polynomial1 projection!"
self.L = self.readCoefs(fr, 8)
def write(self, fw):
"Write the projection coefficnets to a text file."
fw.write("#First order polynomial projection\n")
fw.write("# x = L0 X + L1 Y + L2 Z + L3\n")
fw.write("# y = L4 X + L5 Y + L6 Z + L7\n")
fw.write("\n%2d # ThanCad Projection code\n\n" % self.icodp)
for i in xrange(4): fw.write("%27.20e # L%d\n" % (self.L[i], i))
fw.write("\n")
for i in xrange(4, 8): fw.write("%27.20e # L%d\n" % (self.L[i], i))
###############################################################################
###############################################################################
class Polynomial2Projection(__Projection):
"This class provides the machinery for the polynomial projection of 2nd degree."
icodp = 3
name = "2nd order polynomial"
def __init__(self, L=None):
"Initialize the object with known coeffcients."
if (L == None):
self.L = [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, # Coefs for xr
0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 # Coefs for yr
]
else:
assert len(L) == 16, "The reshould exactly 8 coefficients for the Polynomial Projection of 2nd order"
self.L = L[:]
def project(self, c3d):
"Projects 3d point with polynomial projection."
x, y, z = c3d[:3]
L = self.L
xp = L[0]*x+L[1]*y+L[2] *z+L[3] +L[4] *x**2+L[5] *y**2+L[6] *z**2+L[7] *x*y
yp = L[8]*x+L[9]*y+L[10]*z+L[11]+L[12]*x**2+L[13]*y**2+L[14]*z**2+L[15]*x*y
return xp, yp, z # The last coordinate (z) is not used
def lsm23(self, fots):
"Find polynomial coefficients using least square."
L = []
for i in 0, 1:
A, B = [], []
for xg,yg,zg,xr,yr,zr,xyok,zok in fots:
A.append([xg, yg, zg, 1.0, xg**2, yg**2, zg**2, xg*yg])
if i == 0: B.append(xr)
else : B.append(yr)
A = array(A)
B = array(B)
AT = transpose(A)
A = matrixmultiply(AT, A)
B = matrixmultiply(AT, B)
try: a = solve_linear_equations(A, B)
except LinAlgError, why:
# print "Match2 polynomial solution failed: ", why
return None, None, None
L.extend(list(a))
self.L = L
return self.er(fots)
def read(self, fr, skipicod=False):
"""Reads the coefficients from an opened text file.
It is the reponsibility of the caller to catch any exceptions."""
if skipicod: ic = self.icodp
else: ic = self.readIcod(fr)
if ic == self.icodp:
self.L = self.readCoefs(fr, 16)
else: # Read coefficients from polynomial1 projection
L = self.readCoefs(fr, 8)
self.L = [L[0], L[1], L[2], L[3], 0.0, 0.0, 0.0, 0.0, # Coefs for xr
L[4], L[5], L[6], L[7], 0.0, 0.0, 0.0, 0.0 # Coefs for yr
]
def write(self, fw):
"Write the projection coefficnets to a text file."
fw.write("#Second order polynomial projection\n")
fw.write("# x = L0 X + L1 Y + L2 Z + L3 + L4 X^2 + L5 Y^2 + L6 Z^2 + L7 X Y\n")
fw.write("# y = L8 X + L9 Y + L10 Z + L11 + L12 X^2 + L13 Y^2 + L14 Z^2 + L15 X Y\n")
fw.write("\n%2d # ThanCad Projection code\n\n" % self.icodp)
for i in xrange(8): fw.write("%27.20e # L%d\n" % (self.L[i], i))
fw.write("\n")
for i in xrange(8, 16): fw.write("%27.20e # L%d\n" % (self.L[i], i))
###############################################################################
###############################################################################
def Projection(icod):
"Select projection class according to integer indes."
if icod == 0: return Polynomial1Projection
elif icod == 1: return DLTProjection
elif icod == 2: return Rational1Projection
elif icod == 3: return Polynomial2Projection
elif icod == 4: return Rational2Projection
elif icod == 5: return Rational15Projection
assert False, "No projection with integer index %d" % (icod,)
def read1(fr):
"Reads a non-blank non-comment line; it is the responsibility of the caller to handle exceptions."
while True:
dl = fr.next().strip()
if len(dl) > 0 and dl[0] != "#": break # Comment lines
dl1 = dl.split()[0] # Avoid comments at the end of the line
return dl1
def readProj(fr):
"Reads a projection form file and return Projection object."
dl1 = read1(fr)
ic = int(dl1)
p = Projection(ic)()
p.read(fr, skipicod=True)
return p
if __name__ == "__main__":
p = Polynomial2Projection()
p.write(open("q1.cof", "w"))
|