Source code for PyOptoUS.calib

# -*- 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