# -*- coding: utf-8 -*-
"""
.. module:: calib
:synopsis: Module for US probe calibration (equations creation, solution, solution quality assessment)
"""
import numpy as np
from sympy import Matrix, Symbol, MatrixSymbol
from sympy import cos as c, sin as s
from scipy.optimize import root
from sympy.utilities.lambdify import lambdify
[docs]def createCalibEquations():
"""Generate and return symbolic calibration equations (1) in [Ref2]_.
:return: list of following variables:
Pph (*sympy.Matrix*) – 3 x 1 matrix containing symbolic equations (1) in [Ref2]_.
J (*sympy.Matrix*) – 3 x 14 matrix representing the Jacobian of equations ``Pph``
:rtype: tuple
"""
# Pi
sx = Symbol('sx')
sy = Symbol('sy')
u = Symbol('u')
v = Symbol('v')
Pi = Matrix(([sx * u],\
[sy * v],\
[0],\
[1]\
))
# prTi
x1 = Symbol('x1')
y1 = Symbol('y1')
z1 = Symbol('z1')
alpha1 = Symbol('alpha1')
beta1 = Symbol('beta1')
gamma1 = Symbol('gamma1')
prTi = Matrix(([c(alpha1)*c(beta1), c(alpha1)*s(beta1)*s(gamma1)-s(alpha1)*c(gamma1), c(alpha1)*s(beta1)*c(gamma1)+s(alpha1)*s(gamma1), x1],\
[s(alpha1)*c(beta1), s(alpha1)*s(beta1)*s(gamma1)+c(alpha1)*c(gamma1), s(alpha1)*s(beta1)*c(gamma1)-c(alpha1)*s(gamma1), y1],\
[-s(beta1), c(beta1)*s(gamma1), c(beta1)*c(gamma1), z1],\
[0, 0, 0, 1]\
))
# prTi = Matrix(([c(alpha1)*c(beta1), c(alpha1)*s(beta1)*s(gamma1)+s(alpha1)*c(gamma1), -c(alpha1)*s(beta1)*c(gamma1)+s(alpha1)*s(gamma1), x1],\
# [-s(alpha1)*c(beta1), -s(alpha1)*s(beta1)*s(gamma1)+c(alpha1)*c(gamma1), s(alpha1)*s(beta1)*c(gamma1)+c(alpha1)*s(gamma1), y1],\
# [s(beta1), -c(beta1)*s(gamma1), c(beta1)*c(gamma1), z1],\
# [0, 0, 0, 1]\
# ))
# wTpr
wTpr = Matrix(MatrixSymbol('wTpr', 4, 4))
wTpr[3, 0:4] = np.array([0,0,0,1])
# phTw
x2 = Symbol('x2')
y2 = Symbol('y2')
z2 = Symbol('z2')
alpha2 = Symbol('alpha2')
beta2 = Symbol('beta2')
gamma2 = Symbol('gamma2')
phTw = Matrix(([c(alpha2)*c(beta2), c(alpha2)*s(beta2)*s(gamma2)-s(alpha2)*c(gamma2), c(alpha2)*s(beta2)*c(gamma2)+s(alpha2)*s(gamma2), x2],\
[s(alpha2)*c(beta2), s(alpha2)*s(beta2)*s(gamma2)+c(alpha2)*c(gamma2), s(alpha2)*s(beta2)*c(gamma2)-c(alpha2)*s(gamma2), y2],\
[-s(beta2), c(beta2)*s(gamma2), c(beta2)*c(gamma2), z2],\
[0, 0, 0, 1]\
))
# phTw = Matrix(([c(alpha2)*c(beta2), c(alpha2)*s(beta2)*s(gamma2)+s(alpha2)*c(gamma2), -c(alpha2)*s(beta2)*c(gamma2)+s(alpha2)*s(gamma2), x2],\
# [-s(alpha2)*c(beta2), -s(alpha2)*s(beta2)*s(gamma2)+c(alpha2)*c(gamma2), s(alpha2)*s(beta2)*c(gamma2)+c(alpha2)*s(gamma2), y2],\
# [s(beta2), -c(beta2)*s(gamma2), c(beta2)*c(gamma2), z2],\
# [0, 0, 0, 1]\
# ))
# Calculate full equations
Pph = phTw * wTpr * prTi * Pi
Pph = Pph[0:3,:]
# Calculate full Jacobians
x = Matrix([sx, sy, x1, y1, z1, alpha1, beta1, gamma1, x2, y2, z2, alpha2, beta2, gamma2])
J = Pph.jacobian(x)
# Create symbols dictionary
syms = {}
for expr in Pph:
atoms = sorted(expr.atoms(Symbol))
for i in xrange(0, len(atoms)):
syms[atoms[i].name] = atoms[i]
# Create list of variables and measurements units
variables = ['sx', 'sy', 'x1', 'y1', 'z1', 'alpha1', 'beta1', 'gamma1', 'x2', 'y2', 'z2', 'alpha2', 'beta2', 'gamma2']
mus = ['mm/px', 'mm/px', 'mm', 'mm', 'mm', 'rad', 'rad', 'rad', 'mm', 'mm', 'mm', 'rad', 'rad', 'rad']
# Return data
return Pph, J, prTi, syms, variables, mus
[docs]def solveEquations(eq, J, syms, variables, init, Rpr, Tpr, features):
# Check variables number vs equations number
if len(features) * len(features[features.keys()[0]]) < len(variables):
raise Exception('The number of equations must be breater than the number of variables')
# Lambdify expressions
xLam = []
for k in xrange(0,len(variables)):
xLam.append(syms[variables[k]])
xLam.append(syms['wTpr_00'])
xLam.append(syms['wTpr_01'])
xLam.append(syms['wTpr_02'])
xLam.append(syms['wTpr_03'])
xLam.append(syms['wTpr_10'])
xLam.append(syms['wTpr_11'])
xLam.append(syms['wTpr_12'])
xLam.append(syms['wTpr_13'])
xLam.append(syms['wTpr_20'])
xLam.append(syms['wTpr_21'])
xLam.append(syms['wTpr_22'])
xLam.append(syms['wTpr_23'])
xLam.append(syms['u'])
xLam.append(syms['v'])
eqLam = lambdify(xLam, eq)
JLam = lambdify(xLam, J)
# Solve the equations
global jac
jac = None
def funcSolver(x, syms, variables, eq, J, Rpr, Tpr, points):
frames = points.keys()
f = []
df = []
for i in xrange(0,len(frames)):
#for i in xrange(1,100):
#for i in xrange(1,len(frames)):
args = []
for k in xrange(0,len(variables)):
args.append(x[k])
# Get time frame
fr = int(frames[i])
# Substitute wTpr (constant)
args.append(Rpr[fr,0,0])
args.append(Rpr[fr,0,1])
args.append(Rpr[fr,0,2])
args.append(Tpr[fr,0])
args.append(Rpr[fr,1,0])
args.append(Rpr[fr,1,1])
args.append(Rpr[fr,1,2])
args.append(Tpr[fr,1])
args.append(Rpr[fr,2,0])
args.append(Rpr[fr,2,1])
args.append(Rpr[fr,2,2])
args.append(Tpr[fr,2])
for j in xrange(0,len(points[frames[i]])):
# Substitute u, v (constant)
args.append(points[frames[i]][j][0])
args.append(points[frames[i]][j][1])
# Evaluate equation
fNew = eqLam(*args)
dfNew = np.array(JLam(*args))[0].tolist()
del args[-2:]
f.append(fNew)
df.append(dfNew)
# Save Jacobian for later use
global jac
jac = np.array(df)
# Return equtions and jacobian
print 'Solving {0} equations ...'.format(len(f))
return f, df
sol = root(funcSolver, init, jac=True, method='lm', args=(syms,variables,eq,J,Rpr,Tpr,features))
# Calculate problem condition number
U, s, V = np.linalg.svd(jac)
kond = int(s[0] / s[-1])
return sol, kond
[docs]def calculateRP(T, sx, sy, points):
# Calculate points in the global reference frame
pointsGl = np.array((4,len(points)))
frames = points.keys()
for i in xrange(0, len(points)):
fr = int(frames[i])
point = np.zeros((4,))
point[0] = points[frames[i]][0][0] * sx
point[1] = points[frames[i]][0][1] * sy
point[2:4] = (0, 1)
pointsGl[:,i] = np.dot(T[fr,:,:],point)
pointsGl = pointsGl[0:3,:]
# Calculate RP
meanPoint = np.mean(pointsGl, axis=1)
pointsNorm = np.linalg.norm(pointsGl-meanPoint, axis=0)
RP = np.mean(pointsNorm)
return RP
[docs]def calculateDA(T, sx, sy, points, L):
# Calculate points in the global reference frame
pointsGl1 = np.array((4,len(points)))
pointsGl2 = np.array((4,len(points)))
frames = points.keys()
for i in xrange(0, len(points)):
if len(points[frames[i]]) < 2:
continue
fr = int(frames[i])
point1 = np.zeros((4,))
point1[0] = points[frames[i]][0][0] * sx
point1[1] = points[frames[i]][0][1] * sy
point1[2:4] = (0, 1)
pointsGl1[:,i] = np.dot(T[fr,:,:],point1)
point2 = np.zeros((4,))
point2[0] = points[frames[i]][1][0] * sx
point2[1] = points[frames[i]][1][1] * sy
point2[2:4] = (0, 1)
pointsGl2[:,i] = np.dot(T[fr,:,:],point2)
pointsGl1 = pointsGl1[0:3,:]
pointsGl2 = pointsGl2[0:3,:]
# Calculate DA
dist = np.linalg.norm(pointsGl1-pointsGl2, axis=0)
DA = dist - L
return DA