# -*- coding: iso-8859-7 -*-
from p_gnum import (array,matrixmultiply,transpose,solve_linear_equations
LinAlgError, zeros, Float)
from math import sqrt,cos,sin,fabs,atan2,pi
from var import dpt
import copy
class SimilarTransformation:
"""Keeps the rotation, translation and scale.
It is generally 3dimensional, but it also works for the 2dimensional case
if cu[2]=0 and gon[0]=gon[1]=0."""
def __init__(self, cu=(0.0,0.0,0.0), gon=(0.0,0.0,0.0), am=1.0):
"Create the object; first rotation around (0,0,0), then scale around (0,0,0), then translation."
self.cu = array(cu)
self.gon = array(gon)
self.am = am
self.rxyz = matwfk(gon)
self.a = cu[0], cu[1], am*cos(gon[2]), am*sin(gon[2])
def clone(self):
"Return a distinct copy of self."
# return SimilarTransformation(self.cu, self.gon, self.am)
return copy.deepcopy(self) # This is faster than the commented method
def invert(self):
"""Returns a new transformation which is the inversion of self.
[A'] = [CU] + m [R] [A] => m [R] [A] = [A'] - [CU] =>
[A] = (1/m) [RI] ([A'] - [CU]) =>
[A] = -(1/m) [RI] [CU] + (1/m) [RI] [A']
[A] = [CU'] + m' [R'] [A']
[RI] is the inverse of [R], which is also the transpose.
The angles are found by the rotation matrix
"""
tra = SimilarTransformation(am=1.0/self.am)
tra.rxyz = transpose(self.rxyz)
# om = atan2(tra.rxyz[1,2], tra.rxyz[2,2])
# kap = atan2(tra.rxyz[0,1], tra.rxyz[0,0])
# s = sin(om)
# c = cos(om)
# if fabs(s) > fabs(c): phi = atan2(-tra.rxyz[0,2], tra.rxyz[1,2]/s) # Avoid zero division
# else: phi = atan2(-tra.rxyz[0,2], tra.rxyz[2,2]/c) # Avoid zero division
# tra.gon = array((om, phi, kap))
tra.gon = array(tra.calcwfk())
tra.cu = -tra.am * matrixmultiply(tra.rxyz, self.cu)
tra.a = tra.cu[0], tra.cu[1], tra.am*cos(tra.gon[2]), tra.am*sin(tra.gon[2])
return tra
def calcwfk(tra):
"Compute the omega,phi,kapa from the rotation matrix."
om = atan2(tra.rxyz[1,2], tra.rxyz[2,2])
kap = atan2(tra.rxyz[0,1], tra.rxyz[0,0])
s = sin(om)
c = cos(om)
if fabs(s) > fabs(c): phi = atan2(-tra.rxyz[0,2], tra.rxyz[1,2]/s) # Avoid zero division
else: phi = atan2(-tra.rxyz[0,2], tra.rxyz[2,2]/c) # Avoid zero division
return dpt(om), dpt(phi), dpt(kap)
def calc(self, cp):
"Transform the coordinates of a 3d point."
return self.cu + self.am * matrixmultiply(self.rxyz, array(cp))
def calc2d(self, cp):
"Transform the coordinates of a 2d point."
xr, yr, zr = cp; a = self.a
xx = a[0] + a[2]*xr + a[3]*yr
yy = a[1] + a[2]*yr - a[3]*xr
zz = self.cu[2] + self.am*zr
return xx, yy, zz
def setRotcenter(self, rotcenter):
"""Arrange the coefficients so that we have rotation with respect to rotcenter.
This menas that we rotate and scale around rotcenter and then we translate.
Note that this is NOT the same transformation as before.
Alternatively we could have a constructor which accepts the angles, the scale
the center of rotation AND the translation, and produces the same Transformation
object that setRotcenter() does."""
rotcenter = array(rotcenter)
self.cu = -self.am * matrixmultiply(self.rxyz, rotcenter) + rotcenter + self.cu
self.a = self.cu[0], self.cu[1], self.am*cos(self.gon[2]), self.am*sin(self.gon[2])
def chain(self, other):
"Chain this transformation and another; first current then the other."
self.cu = other.am * matrixmultiply(other.rxyz, self.cu) + other.cu
self.rxyz = matrixmultiply(other.rxyz, self.rxyz)
self.am *= other.am
# self.gon += other.gon
# self.gon = dpt(self.gon) # This does not work
self.gon = array(self.calcwfk())
self.a = self.cu[0], self.cu[1], self.am*cos(self.gon[2]), self.am*sin(self.gon[2])
def read(self, fr, skipmatrix=True):
"""Reads the coefficients from an opened text file.
It is the reponsibility of the caller to catch any exceptions."""
cu = [0.0, 0.0, 0.0]
gon = [0.0, 0.0, 0.0]
cu[0], cu[1], cu[2] = read1(fr)
gon[0], gon[1], gon[2] = read1(fr)
(am,) = read1(fr)
self.__init__(cu, gon, am)
if not skipmatrix:
r = zeros((3, 3), Float)
# r = [[0.0, 0.0, 0.0],
# [0.0, 0.0, 0.0],
# [0.0, 0.0, 0.0],
# ]
# for r1 in r:
# r1[0], r1[1], r1[2] = read1(fr)
for i in xrange(3):
r[i, :] = read1(fr)
self.rxyz, r = r, self.rxyz
for i in xrange(3):
for j in xrange(3):
if fabs(r[i,j]-self.rxyz[i,j]) > 1.0e-6:
print "Warning: The Similar transformation, which was read, is not consistent."
print "gon =", self.gon
print "should be=", self.calcwfk()
return
def write(self, fw):
"""Write the projection parameters to an ascii file.
[A'] = [CU] + m [R] [A] => m [R] [A] = [A'] - [CU] =>
It is generally 3dimensional, but it also works for the 2dimensional case
if cu[2]=0 and gon[0]=gon[1]=0."""
fw.write("""\
#Similar transformation
# [X] initial [x] Transformed
# [A] = [Y] coordinates [A'] = [y] coordinates
# [Z] [z]
#
# [A'] = [CU] + m [R] [A]
#
# [DX]
# [CU] = [DY] m = scale [R] = rotation matrix
# [DZ]
#
# [omega]
# [gon] = [phi]
# [kappa]
""")
fw.write("\n#CU:\n%27.20e %27.20e %27.20e\n" % tuple(self.cu))
fw.write("\n#gon:\n%27.20e %27.20e %27.20e\n" % tuple(self.gon))
fw.write("\n#am:\n%27.20e\n" % self.am)
fw.write("\n#R:\n")
for r in self.rxyz:
fw.write("%27.20e %27.20e %27.20e\n" % tuple(r))
def similarReadt(fd):
"Read the transformation parameters from a Datlin instance, and return the object."
fd.datCom("TRANSLATION")
cu = [fd.datFloat()]
cu.append(fd.datFloat())
cu.append(fd.datFloat())
fd.datCom("ROTATION")
gon = [fd.datFloat()]
gon.append(fd.datFloat())
gon.append(fd.datFloat())
fd.datCom("SCALE")
am = fd.datFloat()
return SimilarTransformation(cu, gon, am)
def similar2lsm(fots):
"Find translation rotation, scale (similarity) in 2d using least square."
A, B = [], []
for xg,yg,zg,xr,yr,zr,xyok,zok in fots:
A.append([1.0*xyok, 0.0, xr*xyok, yr*xyok])
B.append(xg*xyok)
A.append([0.0, 1.0*xyok, yr*xyok, -xr*xyok])
B.append(yg*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 solution failed: ", why
return None, 1e30, 1e30
am = sqrt( a[2]**2 + a[3]**2 )
gon= [ 0.0, 0.0, dpt(atan2(a[3], a[2])) ]
dz = 0.0
for xg,yg,zg,xr,yr,zr,xyok,zok in fots: dz += zg - am*zr
n = len(fots); dz /= n
cp = a[0], a[1], dz
tra = SimilarTransformation(cp, gon, am)
er, erh = er2d(fots, tra)
return tra, er, erh
def er2d(fots, tra):
"Compute 2 dimensional error after transformation."
er = erh = 0.0
n = 0.0
for xg,yg,zg,xr,yr,zr,xyok,zok in fots:
xx, yy, zz = tra.calc2d((xr, yr, zr))
xyok **= 2
er += xyok*((xx-xg)**2 + (yy-yg)**2)
erh += xyok*((zz-zg)**2)
n += xyok
er = sqrt(er/n); erh = sqrt(erh/n)
return er, erh
def similar3lsm(fots):
"Find translation, rotation, scale in 3d using least square."
tra, er, erh = similar2lsm(fots) # First aproximation from 2d
if tra == None: return tra, er, erh # Failed
tra = elt3(tra, fots) # Iterative solutions
if tra == None: return tra, 1e30, 1e30
er, erh = er3d(fots, tra) # Compute error
return tra, er, erh
def er3d(fots, tra):
"Calculates the mean error of xy and h in the 3d solution."
er = erh = n = nh = 0
for xg,yg,zg,xr,yr,zr,xyok,zok in fots:
xyok **= 2; zok **= 2
# print "control: xyok, zok=", xyok, zok
ct = tra.calc((xr, yr, zr))
er += xyok * ((xg-ct[0])**2 + (yg-ct[1])**2)
erh += zok * ((zg-ct[2])**2)
n += xyok; nh += zok
er = sqrt(er/n); erh = sqrt(erh/nh)
er = (er + erh/3.0) # xy error = 1/3 of z error
# er = (er + erh*2.0/3.0) # xy error = 2/3 of z error
return er, erh
def elt3(tra, fots):
"""Find translation, rotation, scale in 3d using least square.
The rotation angles are computed with iterations.
The translations and the scale are computed analytically."""
#-----Iterations
errtol = 1.0e-6
three = range(3)
# print "cu=", tra.cu
# print "gon=", tra.gon
# print "am=", tra.am
gon = array(tra.gon)
am = tra.am
for iter in xrange(1,10):
A, B = formEqs3d(gon, am, fots)
AT = transpose(A)
A = matrixmultiply(AT, A)
B = matrixmultiply(AT, B)
try:
bt = solve_linear_equations(A, B)
except LinAlgError, why:
# print "Match2 solution failed: ", why
return None
#---------Add the corrections to the angles
dfmax = 0
for i in three: gon[i] = dpt(gon[i] + bt[3+i])
am = bt[6]
# print "cu=", bt[0:3]
# print "gon=", tra.gon
# print "am=", tra.am
if max(fabs(bt[3]), fabs(bt[4]), fabs(bt[5])) <= errtol: break
# instabil(dfm, iter)
if iter >= 10: print "Warning max iters in elt3!"
return SimilarTransformation(bt[:3], gon, am)
def formEqs3d(gon, am, fots):
"Forms the equation for the next iteration of computation of angles in 3d."
tw, tf, tk, tm = matgon(gon, am)
n = len(fots)
A = zeros((3*n, 7), Float)
B = zeros(3*n, Float)
i = 0
for xg,yg,zg,xr,yr,zr,xyok,zok in fots:
#------- , , ,
A[i, 0] = xyok
A[i+1, 1] = xyok
A[i+2, 2] = zok
cp = array([xr*xyok,yr*xyok,zr*xyok], Float)
j = i + 3
A[i:j, 3] = matrixmultiply(tw, cp)
A[i:j, 4] = matrixmultiply(tf, cp)
A[i:j, 5] = matrixmultiply(tk, cp)
A[i:j, 6] = matrixmultiply(tm, cp)
A[j-1, 3:7] = (zok/xyok) * A[j-1, 3:7]
B[i:j] = xg*xyok, yg*xyok, zg*zok
i = j
# f = 7*"%15.4f" + "=%15.4f"
# for k in xrange(len(B)):
# print f % (tuple(A[k, :]) + (B[k], ))
return A, B
def matgon(gon, am):
" , , ."
cosw = cos( gon[0] )
sinw = sin( gon[0] )
cosf = cos( gon[1] )
sinf = sin( gon[1] )
cosk = cos( gon[2] )
sink = sin( gon[2] )
#----- ---------------------------------------------
rx, drx = mhtstr(cosw, sinw, 1, 2, 0)
ry, dry = mhtstr(cosf, -sinf, 0, 2, 1)
dry = -dry
rz, drz = mhtstr(cosk, sink, 0, 1, 2)
#----- --------------------------------
t1 = matrixmultiply(drx, ry)
tw = am * matrixmultiply(t1, rz)
#----- --------------------------------
t1 = matrixmultiply(rx, dry)
tf = am * matrixmultiply(t1, rz)
#----- --------------------------------
t1 = matrixmultiply(rx, ry)
tk = am * matrixmultiply(t1, drz)
#----- -------------------------------
t1 = matrixmultiply(rx, ry)
tm = matrixmultiply(t1, rz)
return tw, tf, tk, tm
def mhtstr (cosf, sinf, i, j, k):
"Rotation matrix for an angle (, , )."
ry = zeros((3, 3), Float)
ry[i,i] = cosf
ry[i,j] = sinf
ry[j,i] = -sinf
ry[j,j] = cosf
ry[k,k] = 1.0
dry = zeros((3, 3), Float)
dry[i,i] = -sinf
dry[i,j] = cosf
dry[j,i] = -cosf
dry[j,j] = -sinf
return ry, dry
def matwfk(gon):
"""Computes the rotation matrix of w,f,k.
: " , . 104
**
**:
cos cos cos sin -sin
sin sin cos - cos sin sin sin sin + cos cos sin cos
cos sin cos + sin sin cos sin sin - sin cos cos cos
"""
cosw = cos(gon[0])
sinw = sin(gon[0])
cosf = cos(gon[1])
sinf = sin(gon[1])
cosk = cos(gon[2])
sink = sin(gon[2])
rxyz, t1 = mhtstr(cosw, sinw, 1, 2, 0) # = rx
te, t1 = mhtstr(cosf, -sinf, 0, 2, 1) # = ry
tt = matrixmultiply(rxyz, te) # = rxy
te, t1 = mhtstr(cosk, sink, 0, 1, 2) # = rz
return matrixmultiply(tt, te) # = rxyz
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
dl = dl.split() # Avoid comments at the end of the line
for i, dl1 in enumerate(dl):
if dl1[:1] == "#":
del dl[i:]
break
dl[i] = float(dl1)
return dl
def testinvert():
"Tests that the invert produces good results."
from math import asin,atan2
am = 5.0
gon = pi/10, pi/8, pi/6
cu = [1000.0, 5000.0, 100.0]
tra = SimilarTransformation(cu, gon, am)
# print tra.rxyz
# om = atan2(tra.rxyz[1,2], tra.rxyz[2,2])
# kap = atan2(tra.rxyz[0,1], tra.rxyz[0,0])
# s = sin(om)
# c = cos(om)
# if fabs(s) > fabs(c): phi = atan2(-tra.rxyz[0,2], tra.rxyz[1,2]/s)) # Avoid zero division
# else: phi = atan2(-tra.rxyz[0,2], tra.rxyz[2,2]/c)) # Avoid zero division
# print gon
# print om, phi, kap
tri = tra.invert()
print matrixmultiply(tra.rxyz, tri.rxyz)
print "3d transformation"
a = (23546.32, 78987.45, 98.78)
print a
b = tra.calc(a)
print b
print tri.calc(b)
print "2d transformation"
am = 5.0
gon = 0.0, 0.0, pi/6
cu = [1000.0, 5000.0, 100.0]
tra = SimilarTransformation(cu, gon, am)
tri = tra.invert()
print "Normal gon=", tra.gon
print "Invert gon=", tri.gon
print a
b = tra.calc2d(a)
print b
print tri.calc2d(b)
def testreadwrite():
am = 5.0
gon = pi/10, pi/8, pi/6
cu = [1000.0, 5000.0, 100.0]
tra = SimilarTransformation(cu, gon, am)
fw = open("q1.sim", "w")
tra.write(fw)
fw.close()
fr = open("q1.sim")
tra1 = SimilarTransformation()
tra1.read(fr, skipmatrix=False)
fr.close()
def testchain():
print "testchain:"
cu = 2.08816307754466804909e+05, 3.94416790482127433643e+06, -4.70765397048860961604e+02
gon = 2.91895764477823858873e-02, 6.78397358848321918590e-02, 3.29709158818852765549e-01
am = 1.29641809095613846914e-01
gon = gon1 = pi, pi/3, pi/2
tra = SimilarTransformation(cu, gon, am)
cu = 5.52953827138223801740e+05, 4.41078384706697985530e+06, -1.80966772568628584850e+04
gon = 2.43959115522558001032e-01, 5.96481558881933704441e+00, 5.10397238383233187164e+00
gon = pi*0.3, pi*0.4, pi*0.5
am = 2.69451701714143940225e-02
tra1 = SimilarTransformation(cu, gon, am)
tra.chain(tra1)
fw = open("q1.sim", "w")
tra.write(fw)
fw.close()
fr = open("q1.sim")
tra3 = SimilarTransformation()
tra3.read(fr, skipmatrix=False)
fr.close()
print "gon1 :", gon1
print "gon2 :", gon
print "sum of gon :", gon1[0]+gon[0],gon1[1]+gon[1],gon1[2]+gon[2]
print "but correct:", tra.gon
def testreadinvert():
fr = open("testreadinvert.cof")
tra = SimilarTransformation()
print "reading tra"
tra.read(fr, skipmatrix=False)
tri = SimilarTransformation()
print "reading trap"
tri.read(fr, skipmatrix=False)
fr.close()
print matrixmultiply(tra.rxyz, tri.rxyz)
print "3d transformation"
a = (23546.32, 78987.45, 98.78)
print a
b = tra.calc(a)
print b
print tri.calc(b)
print "2d transformation"
am = 5.0
gon = 0.0, 0.0, pi/6
cu = [1000.0, 5000.0, 100.0]
tra = SimilarTransformation(cu, gon, am)
tri = tra.invert()
print "Normal gon=", tra.gon
print "Invert gon=", tri.gon
print a
b = tra.calc2d(a)
print b
print tri.calc2d(b)
if __name__ == "__main__":
# testinvert()
# testreadwrite()
# testreadinvert()
testchain()
|