def creatCalibMatrix():
"""Generate and return symbolic expression of 4 x 4 affine rotation matrix from US probe reference frame to US image reference frame.
Returns
-------
prTi : sympy.matrices.matrices.MutableMatrix
The matrix expression.
syms : list
List of ``sympy.core.symbol.Symbol`` symbol objects used to generate the expression.
"""
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]\
))
syms = [x1, y1, z1, alpha1, beta1, gamma1]
return prTi, syms
def createCalibEquations():
"""Generate and return symbolic calibration equations (1) in [Ref2]_.
Returns
-------
Pph : sympy.matrices.matrices.MutableMatrix
3 x 1 matrix containing symbolic equations (1) in [Ref2]_.
J : sympy.matrices.matrices.MutableMatrix*)
3 x 14 matrix representing the Jacobian of equations ``Pph``.
prTi : sympy.matrices.matrices.MutableMatrix*)
4 x 4 affine rotation matrix from US probe reference frame to US image reference frame.
syms : dict
Dictionary of where keys are variable names and values are ``sympy.core.symbol.Symbol`` objects.
These symbols were used to create equations in ``Pph``, ``J``, ``prTi``.
variables : list
14-elem list of variable names (see ``process.Process.calibrateProbe()``).
mus : list
14-elem list of varables measurement units.
"""
# Pi
sx = Symbol('sx')
sy = Symbol('sy')
u = Symbol('u')
v = Symbol('v')
Pi = Matrix(([sx * u],\
[sy * v],\
[0],\
[1]\
))
# prTi
prTi, syms = creatCalibMatrix()
[x1, y1, z1, alpha1, beta1, gamma1] = syms
# wTpr
#wTpr = Matrix(MatrixSymbol('wTpr', 4, 4))
wTpr = MatrixOfMatrixSymbol('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]\
))
# 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 = list(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
# Set path for markers coordinates file
p.setKineFiles(('test_baloon_2ang.c3d',))
# Calculate pose from US probe to laboratory reference frame
p.calculatePoseForUSProbe(mkrList=('Rigid_Body_1-Marker_1','Rigid_Body_1-Marker_2','Rigid_Body_1-Marker_3','Rigid_Body_1-Marker_4'))
# Calculate pose from US images to laboratory reference frame
p.calculatePoseForUSImages()
# Reorient global reference frame to be approximately aligned with US scans direction
from sympy import Matrix, Symbol, cos as c, sin as s
alpha = Symbol('alpha')
beta = Symbol('beta')
T1 = Matrix(([1,0,0,0],
[0,c(alpha),s(alpha),0],
[0,-s(alpha),c(alpha),0],
[0,0,0,1]
))
T = T1.evalf(subs={'alpha':np.deg2rad(-10.)})
T = np.array(T).astype(np.float)
# Set time frames for images that can be cointaned in the voxel array
p.setValidFramesForVoxelArray(voxFrames='auto')
# Calculate convenient pose for the voxel array
p.calculateConvPose(T)
# Calculate scale factors
#fxyz = 'auto_bounded_parallel_scans'
fxyz = (1,10,1)