"""
.. module:: kine
:synopsis: helper module for kinematics
"""
import numpy as np
import btk
#from scipy.interpolate import interp1d
from scipy.interpolate import InterpolatedUnivariateSpline
[docs]def readC3D(fileName, sections, opts={}):
"""Read C3D file.
Parameters
----------
fileName : str
Full path of the C3D file.
sections : list
List of strings indicating which section to read.
It can contain the following: 'markers'.
opts : dict
Options dictionary that can contain the following keys:
- setMarkersZeroValuesToNaN: if true, marker corrdinates exactly
matching 0 will be replace with NaNs (e.g. Optitrack systems).
Default is false.
Returns
-------
dict
Collection of read data. It contains, as keys, the items contained
in ``sections``:
- markers: this is a dictionary where each key is a point label, and each
value is a N x 3 np.ndarray of 3D coordinates (in *mm*), where N is the
number of time frames.
"""
# Open C3D pointer
reader = btk.btkAcquisitionFileReader()
reader.SetFilename(fileName)
reader.Update()
acq = reader.GetOutput()
# Initialize data structure
data = {}
# Read markers
if 'markers' in sections:
# Convert points unit to mm
pointUnit = acq.GetPointUnit()
if pointUnit == 'mm':
scaleFactor = 1.
elif pointUnit == 'm':
scaleFactor = 1000.
else:
raise Exception('Point unit not recognized')
# Get relevant marker data (N x 3)
markers = {}
coll = acq.GetPoints()
for i in xrange(coll.GetItemNumber()):
point = coll.GetItem(i)
label = point.GetLabel()
marker = point.GetValues() * scaleFactor
if 'setMarkersZeroValuesToNaN' in opts and opts['setMarkersZeroValuesToNaN']:
marker[marker==0.] = np.nan # replace 0. with np.nan
markers[label] = marker
data['markers'] = markers
# Return data
return data
[docs]def writeC3D(fileName, data, copyFromFile=None):
"""Write to C3D file.
Parameters
----------
fileName : str
Full path of the C3D file.
data : dict
Data dictionary that can contain the following keys:
- markers: this is marker-related data. This dictionary contains:
- data: dictionary where each key is a point label, and each
value is a N x 3 np.ndarray of 3D coordinates (in *mm*), where N is
the number of time frames. This field is always necessary.
- framesNumber: number of data points per marker.
This field is necessary when creating files from scratch.
- unit: string indicating the markers measurement unit. Available
strings are 'mm' and 'm'.
This field is necessary when creating files from scratch.
- freq: number indicating the markers acquisition frequency.
This field is necessary when creating files from scratch.
copyFromFile : str
If None, it creates a new file from scratch.
If str indicating the path of an existing C3D file, it adds/owerwrite data copied from that file.
"""
if copyFromFile is not None:
# Open C3D pointer
reader = btk.btkAcquisitionFileReader()
reader.SetFilename(copyFromFile)
reader.Update()
acq = reader.GetOutput()
if 'markers' in data:
nMarkerFrames = acq.GetPointFrameNumber()
pointUnit = acq.GetPointUnit()
else:
# Create new acquisition
acq = btk.btkAcquisition()
if 'markers' in data:
nMarkerFrames = data['markers']['framesNumber']
acq.Init(0, nMarkerFrames)
pointUnit = data['markers']['unit']
acq.SetPointUnit(pointUnit)
pointFreq = data['markers']['freq']
acq.SetPointFrequency(pointFreq)
if 'markers' in data:
# Write marker data
markers = data['markers']['data']
for m in markers:
newMarker = btk.btkPoint(m, nMarkerFrames)
if pointUnit == 'm':
markerData = markers[m] / 1000.
elif pointUnit == 'mm':
markerData = markers[m].copy()
newMarker.SetValues(markerData)
acq.AppendPoint(newMarker)
# Write to C3D
writer = btk.btkAcquisitionFileWriter()
writer.SetInput(acq)
writer.SetFilename(fileName)
writer.Update()
[docs]def markersClusterFun(mkrs, mkrList):
"""Default function for calculating a roto-translation matrix from a cluster
of markers to laboratory reference frame. It is based on the global position
for the markers only, and there is not assumption of rigid body.
The reference frame is defined as:
- X versor from mkrList[-2] to mkrList[-1]
- Z cross-product between X and versor from mkrList[-2] to mkrList[-3]
- Y cross product between Z and X
- Origin: mkrList[-2]
Parameters
----------
mkrs : dict
Dictionary where each key is a maker name and each value
is a N x 3 np.ndarray of 3D coordinates, where N is the number of time frames.
mkrList : list
List of marker names, whenever the names order is important.
Returns
-------
R : np.ndarray
N x 3 x 3 rotation matrix.
T : np.ndarray
N x 3 translation vector.
"""
# Define markers to use
#M1 = mkrs[mkrList[-4]]
M2 = mkrs[mkrList[-3]]
M3 = mkrs[mkrList[-2]]
M4 = mkrs[mkrList[-1]]
# Create versors
X = getVersor(M4 - M3)
Z = getVersor(np.cross(X, M2 - M3))
Y = getVersor(np.cross(Z, X))
# Create rotation matrix from probe reference frame to laboratory reference frame
R = np.array((X.T, Y.T, Z.T)) # 3 x 3 x N
R = np.transpose(R, (2,1,0)) # N x 3 x 3
# Create position vector
T = M3.copy()
# Return data
return R, T
[docs]def changeMarkersReferenceFrame(mkrs, Rfull):
"""Express markers in another reference frame.
Parameters
----------
mkrs : dict
Dictionary where each key is a maker name and each value
is a N x 3 np.ndarray of 3D coordinates, where N is the number of time frames.
Rfull : np.ndarray
N x 4 x 4 affine matrix from current refence frame
to new reference frame, for N frames.
Returns
-------
dict
Same structure as ``mkrs``, but with new coordinates.
"""
# Calculate marker coordinates in local reference frame
mkrsNew = {}
mkrList = mkrs.keys()
for m in mkrList:
M = mkrs[m][:] # copy
if len(M.shape) == 1:
N = Rfull.shape[0]
M = np.tile(M, (N,1))
M = np.hstack((M, np.ones((M.shape[0],1))))[:,:,None]
mkrsNew[m] = dot3(Rfull, M)[:,0:3,0]
return mkrsNew
[docs]def rigidBodySVDFun(mkrs, mkrList, args):
"""Function for calculating the optimal roto-translation matrix from a rigid
cluster of markers to laboratory reference frame. The computation, by using
SVD, minimizes the RMSE between the markers inthe laboratory reference frame
and the position of the markers in the local reference frame.
See ``rigidBodyTransformation()`` for more details.
Parameters
----------
mkrs : dict
Dictionary where each key is a marker name and each value
is a N x 3 np.ndarray of 3D coordinates, where N is the number of time frames.
mkrList : list
List of marker names used in the SVD.
args : mixed
Additional arguments:
- 'mkrsLoc': dictionary where keys are marker names and values are 3-elem
np.arrays indicating the coordinates in the local reference frame.
Returns
-------
R : np.ndarray
N x 3 x 3 rotation matrix.
T : np.ndarray
N x 3 translation vector.
"""
# Extract coordinates of markers in rigid local reference frame
mkrsLoc = args['mkrsLoc']
# Create Nmarkers x 3 matrix for local coordinates
x = np.array([mkrsLoc[m].tolist() for m in mkrList])
# Loop for each time frame
Nf = mkrs[mkrList[0]].shape[0]
R = np.zeros((Nf, 3, 3))
T = np.zeros((Nf, 3))
for i in xrange(0,Nf):
# Create Nmarkers x 3 matrix for global coordinates
y = np.array([mkrs[m][i,:].tolist() for m in mkrList])
# Calculate number of visible markers
idxNan = np.isnan([y[:,0]])[0]
Nv = y.shape[0] - idxNan.sum()
# Check minimum markers number
if Nv >= 3:
# Calculate optimal roto-translation matrix
Ri, Ti, ei = rigidBodyTransformation(x[~idxNan,:], y[~idxNan,:])
else:
# Set data to nan
Ri = np.empty((3,3)) * np.nan
Ti = np.empty((3,)) * np.nan
ei = np.empty((y.shape[0],)) * np.nan
print 'Only %d markers are visible for frame %d. Data will be set to nan' % (Nv, i)
# Calculate RSME
RMSE = np.sqrt(np.sum(ei))
iMax = np.argmax(ei)
eMax = np.max(ei)
print 'RMSE for rigid pose estimation for frame %d: %.5f mm. Max distance for %s: %.5f mm' % (i, RMSE, mkrList[iMax], eMax)
# Insert into roto-translation matrix
R[i,:,:] = Ri
T[i,:] = Ti
# Return data
return R, T
[docs]def rigidBodyTransformation(x, y):
"""Estimate or rigid rotation and translation between x and y in such a way
that y = Rx + t + e is optimal in a least square optimal. Details of the
algorithm can be found here:
- Arun et al. (1987)
- Woltring (1992)
- Soderkvist & Wedin (1993)
- Challis (1995)
Parameters
----------
x : np.ndarray
Nm x 3 array containing coordinates for Nm points in
the local rigid reference frame.
y : np.ndarray
Nm x 3 array containing coordinates for Nm points in
the global reference frame.
Returns
-------
R : np.ndarray
3 x 3 estimated rotation matrix.
t : np.ndarray
3-elem translation t.
e : np.ndarray
Nm-elem estimated error e.
"""
# Get markers number
Nmarkers = x.shape[0]
# Calculation of the cross-dispersion matrix C
xmean = np.mean(x, axis=0)
ymean = np.mean(y, axis=0)
A = x - np.dot(np.ones((Nmarkers,3)), np.diag(xmean))
B = y - np.dot(np.ones((Nmarkers,3)), np.diag(ymean))
C = np.dot(B.T, A) / Nmarkers
# Singular value decomposition of C
U, S, V = np.linalg.svd(C)
tol = 0.00002
s = S#np.diag(S)
Srank = np.sum(s > tol)
if Srank < 2:
raise Exception('Markers are probably colinear aligned')
if Srank < 3:
# All markers in one plane
# Calculate cross product, i.e. normal vector
U[:,2] = np.cross(U[:,0], U[:,1])
V[:,2] = np.cross(V[:,0], V[:,1])
# Calculation of R, t and e
D = np.round(np.linalg.det(np.dot(U, V.T))) # if D=-1 correction is needed:
R = np.dot(np.dot(U, np.diag([1,1,D])), V)
t = (ymean.T - np.dot(R, xmean).T).T # vectors are internal in the [x y z] format
#t=t'; # patch to accomodate external format
# Calculate estimation error
yestimated = np.dot(x, R.T) + np.dot(np.ones((Nmarkers,3)), np.diag(t)) # vectors are internal in the [x y z] format
dy = yestimated - y
e = np.sqrt(np.sum(dy**2, axis=1)).squeeze()
return R, t, e
[docs]def pca(D):
"""Run Principal Component Analysis on data matrix. It performs SVD
decomposition on data covariance matrix.
Parameters
----------
D : np.ndarray
Nv x No matrix, where Nv is the number of variables
and No the number of observations.
Returns
-------
list
U, s as out of SVD (``see np.linalg.svd``)
"""
cov = np.cov(D)
U, s, V = np.linalg.svd(cov)
return U, s
[docs]def dot2(a, b):
"""Compute K matrix products between a M x N array and a K x N x P
array in a vectorized way.
Parameters
----------
a : np.ndarray
M x N array
b : np.ndarrayK x N x P array
np.ndarray
Returns
-------
np.ndarray
K x M x P array
"""
return np.transpose(np.dot(np.transpose(b,(0,2,1)),a.T),(0,2,1))
[docs]def vdot2(a, b):
"""Compute dot product in a vectorized way.
Parameters
----------
a : np.ndarray
K x 3 array
b : np.ndarray
K x 3 array
Returns
-------
np.ndarray
K-elems array
"""
r = np.sum(a * b, axis=1)
return r
[docs]def dot3(a, b):
"""Compute K matrix products between a K x M x N array and K x N x P
array in a vectorized way.
Parameters
----------
a : np.ndarray
K x M x N array
b : np.ndarray
K x N x P array
Returns
-------
np.ndarray
K x M x P array
"""
return np.einsum('kmn,knp->kmp', a, b)
[docs]def getVersor(a):
"""Calculate versors of an array.
Parameters
----------
a : np.ndarray
N x 3 array
Returns
-------
np.ndarray
N x 3 array of versors coordinates
"""
#norm = np.sqrt(np.sum(np.multiply(np.mat(a),np.mat(a)),axis=1))
norm = np.sqrt(np.sum(a**2, axis=1))
r = a / (norm[:,None])
return r
[docs]def interpSignals(x, xNew, D, kSpline=1):
"""Interpolate data array, with extrapolation. Data can contain NaNs.
The gaps will not be filled.
Parameters
----------
D : np.ndarray
N x M data array to interpolate (interpolation is column-wise).
x : np.ndarray
axis of the original data, with length N.
xNew : np.ndarray
New axis for the interpolation, with length P.
kSpline : mixed
See ``k`` in ``scipy.interpolate.InterpolatedUnivariateSpline()``.
Returns
-------
np.ndarray
P x M interpolated array
"""
R = np.zeros((xNew.shape[0],D.shape[1])) * np.nan
for i in xrange(0, D.shape[1]):
idx = ~np.isnan(D[:,i])
fIdx = InterpolatedUnivariateSpline(x, idx, k=1)
f = InterpolatedUnivariateSpline(x[idx], D[idx,i], k=kSpline)
#idxNew = np.round(fIdx(xNew)).astype(np.bool)
idxNew = fIdx(xNew) >= 0.9
R[idxNew,i] = f(xNew[idxNew])
return R
[docs]def resampleMarker(M, x=None, origFreq=None, origX=None, step=None):
"""Resample marker data.
The function first tries to see if the new time scale ``x`` and ``origFreq``
(to create the old scale) or ``origX`` are available. If not, the
resampling will take a frame each ``step`` frames.
Parameters
----------
M : np.ndarray
N x 3 marker data array to resample
x : np.ndarray
The new time scale (in *s*) on which to peform the resampling.
origFreq : double
Frequency (in *Hz*) to recreate the old time scale.
origX : np.ndarray
The old time scale (in *s*).
step : int
Number of frames to skip when performing resampling not based on ``x``.
Returns
-------
Mout : np.ndarray
M x 3 resampled marker data
ind : np.ndarray
Indices of ``x`` intersecting time vector of the original ``M``.
"""
if x is not None and (origFreq is not None or origX is not None):
if origFreq is not None:
N = M.shape[0]
dt = 1. / origFreq
x1 = np.linspace(0, (N-1)*dt, num=N)
else:
x1 = origX.copy()
x2 = x.copy()
# f = interp1d(x1, M, axis=0)
# M2 = f(x2)
M2 = interpSignals(x1, x2, M)
elif step is not None:
N = M.shape[0]
x1 = np.linspace(0, N-1, num=N)
x2 = np.arange(0, N-1, step)
# f = interp1d(x1, M, axis=0)
# M2 = f(x2)
M2 = interpSignals(x1, x2, M)
else:
raise Exception('Impossible to resample')
ind = np.nonzero((x2 >= x1[0]) & (x2 <= x1[-1]))[0]
return M2, ind
[docs]def resampleMarkers(M, **kwargs):
"""Resample markers data.
Parameters
----------
M : dict
Dictionary where keys are markers names and values are np.ndarray
N x 3 marker data array to resample.
**kwargs
See ``resampleMarker()``.
Returns
-------
resM : dict
Resampled marker data
ind : np.ndarray
See ``resampleMarker()``.
"""
resM = {}
for m in M:
resM[m], ind = resampleMarker(M[m], **kwargs)
return resM, ind
[docs]def composeRotoTranslMatrix(R, T):
"""Create affine roto-translation matrix from rotation matrix and translation vector.
Parameters
----------
R : np.ndarray
N x 3 x 3 rotation matrix, for N frames.
T : np.ndarray
N x 3 translation vector, for N frames.
Returns
-------
np.ndarray
N x 4 x 4 affine matrix.
"""
Nf = R.shape[0]
Rfull = np.concatenate((R, np.reshape(T,(Nf,3,1))), axis=2)
b = np.tile(np.array((0,0,0,1)), (Nf,1))
Rfull = np.concatenate((Rfull, np.reshape(b,(Nf,1,4))), axis=1)
return Rfull
[docs]def decomposeRotoTranslMatrix(Rfull):
"""Extract rotation matrix and translation vector from affine roto-translation matrix.
Parameters
----------
Rfull : np.ndarray
N x 4 x 4 affine matrix, for N frames.
Returns
-------
R : np.ndarray
N x 3 x 3 rotation matrix and second
T : np.ndarray
N x 3 translation vector, for N frames.
"""
R = Rfull[:,0:3,0:3]
T = Rfull[:,0:3,3]
return R, T
[docs]def inv2(R):
"""Behaves like np.linalg.inv for multiple matrices, but does not raise
exceptions if a matrix contains nans and it is not invertible.
Parameters
----------
R : np.ndarray
N x M x M series of matrices to invert.
Returns
-------
np.ndarray
N x M x M array.
"""
Rinv = np.zeros(R.shape) * np.nan
idx = np.delete(np.arange(R.shape[0]), np.nonzero(np.isnan(R))[0])
Rinv[idx,:,:] = np.linalg.inv(R[idx,:,:])
return Rinv
[docs]def createClusterTemplate(markers, mkrList, timeWin='all_no_nan'):
"""Create cluster template data from existing markers data.
Parameters
----------
markers : dict
Dictionary of point 3D coordinates. Keys are points names
values are np.ndarray N x 3 representing 3D coordinates in the global
reference frame, where N is the number of time frames.
mkrList : list
List of marker names to be used for the template.
timeWin : mixed
Represents which time frames to select for template creation.
If str, it can be:
- 'all_no_nan': all time frames apart from those where marker data is nan.
If list, it must contain two values containing first and last frame for the
time window to search into. Only non-nans will be used.
If single value, it indicates the frame to use.
Returns
-------
dict
Dictionary where keys are marker names and values are 3-elem
np.arrays indicating the coordinates in the cluster reference frame.
"""
# Calculate roto-translation-matrix from local to laboratory reference frame
R, T = markersClusterFun(markers, mkrList)
# Invert roto-translation matrix
Rfull = composeRotoTranslMatrix(R, T)
Rfull = inv2(Rfull)
# Express markers in the local rigid probe reference frame
markersLoc = changeMarkersReferenceFrame(markers, Rfull)
# Calculate reference frame with SVD
if timeWin == 'all_no_nan':
markersLoc = {m: np.nanmean(markersLoc[m], axis=0) for m in mkrList}
elif isinstance(timeWin, (list,tuple)):
i1 = timeWin[0]
i2 = timeWin[1]
markersLoc = {m: np.nanmean(markersLoc[m][i1:i2,:], axis=0) for m in mkrList}
elif isinstance(timeWin, (int,float)):
markersLoc = {m: markersLoc[m][timeWin,:] for m in mkrList}
return markersLoc
[docs]def collinearNPointsStylusFun(P, args):
"""Tip reconstruction function for M collinear points.
Parameters
----------
P : dict
Dictionary of point 3D coordinates. Keys are points names
values are np.ndarray N x 3 representing 3D coordinates (in *mm*)
in the global reference frame, where N is the number of time frames.
args : dict
Dictionary with the floowing keys:
- 'markers': list of marker names to be used.
- 'dist': dictionary of distances between points and tip. Keys must be
present in the 'markers' list, values are distances (in *mm*).
Returns
-------
np.ndarray
N x 3 array representing 3D coordinates of the reconstructed tip
(in *mm*).
"""
# Get distances from stylus tip to each marker
dist = args['dist']
# Get markers
markers = args['markers']
# Difference between extreme markers
existingMarkerIdxs = []
for i in xrange(0, len(markers)):
if markers[i] in P:
existingMarkerIdxs.append(i)
if len(existingMarkerIdxs) < 2:
raise Exception('At least 2 collinear pointer markers must be visible')
m1 = existingMarkerIdxs[0]
m2 = existingMarkerIdxs[-1]
E = P[markers[m1]] - P[markers[m2]]
Nf = P[markers[m1]].shape[0]
# Normalize distance
N = E / np.linalg.norm(E, axis=1)[:,None]
# Calculate N tips, with each distance
X = np.zeros((Nf,0))
Y = np.zeros((Nf,0))
Z = np.zeros((Nf,0))
for i in xrange(0, len(markers)):
if i not in existingMarkerIdxs:
continue
tip = P[markers[i]] + dist[i] * N
X = np.hstack((X, tip[:,0:1]))
Y = np.hstack((Y, tip[:,1:2]))
Z = np.hstack((Z, tip[:,2:3]))
# Average the tips
X = np.nanmean(X, axis=1)[:,None]
Y = np.nanmean(Y, axis=1)[:,None]
Z = np.nanmean(Z, axis=1)[:,None]
tip = np.hstack((X,Y,Z))
return tip
[docs]class Stylus:
"""Helper class for reconstructing stylus tip using source points rigidly connected to stylus.
"""
def __init__(self, P=None, fun=None, args=None):
"""Constructor
"""
self.P = P
self.tipFun = fun
self.tipFunArgs = args
[docs] def setPointsData(self, P):
"""Set source points 3D coordinates.
Parameters
----------
P : dict
Dictionary of point 3D coordinates. Keys are points names,
values are np.ndarray N x 3, where N is the number of time frames.
"""
self.P = P
[docs] def setTipReconstructionFunction(self, fun):
"""Set the function for tip reconstruction from source points.
Parameters
----------
fun : fun
Function taking as input arguments ``P`` and, if not None, ``args``.
It must return a N x 3 np.ndarray representing 3D coordinates of
the reconstructed tip.
"""
self.tipFun = fun
[docs] def setTipReconstructionFunctionArgs(self, args):
"""Set additional arguments for tip reconstruction function.
Parameters
----------
args : mixed
Argument passed to ``fun``.
"""
self.tipFunArgs = args
[docs] def reconstructTip(self):
"""Perform tip reconstruction.
"""
if self.tipFunArgs is None:
self.tip = self.tipFun(self.P)
else:
self.tip = self.tipFun(self.P, self.tipFunArgs)
[docs] def getTipData(self):
"""Get tipa data.
Returns
-------
np.ndarray
N x 3 array representing 3D coordinates of the reconstructed tip.
"""
return self.tip
[docs]def calculateStylusTipInCluster(stylus, markers, clusterMkrList, clusterArgs):
"""Helper function for:
- markers cluster pose estimation (by SVD)
- reconstruction of the stylus tip in the cluster reference frame.
Parameters
----------
markers : dict
See ``mkrs`` in ``rigidBodySVDFun()``.
clusterMkrList : list
See ``mkrList`` in ``rigidBodySVDFun()``.
clusterArgs : mixed
See ``args`` in ``rigidBodySVDFun()``.
Returns
-------
np.ndarray
N x 3 array representing 3D coordinates of the reconstructed tip
(in *mm*) in the cluster reference frame, where N is the number of
time frames.
"""
# Calculate reference frame
R, T = rigidBodySVDFun(markers, clusterMkrList, args=clusterArgs)
# Invert roto-translation matrix
gRl = composeRotoTranslMatrix(R, T)
lRg = inv2(gRl)
# Reconstruct stylus tip
stylus.setPointsData(markers)
stylus.reconstructTip()
tip = stylus.getTipData()
# Average on the time frames
tip = np.nanmean(tip, axis=0)
# Add tip to available markers
markers['Tip'] = tip
# Express tip in the local rigid cluster reference frame
tipLoc = changeMarkersReferenceFrame(markers, lRg)['Tip']
return tipLoc
[docs]def shankPoseISB(mkrs, s='R'):
"""Calculate roto-translation matrix from shank (ISB conventions) to
laboratory reference frame.
Parameters
----------
mkrs : dict
Markers data. Keys are marker names, values are np.ndarray N x 3,
where N is the number of time frames. Used names are:
- 'MM': medial malleolus
- 'LM': lateral melleolus
- 'HF': head of fibula
- 'TT': tibial tuberosity
s : {'R', 'L'}
Anatomical side.
Returns
-------
R : np.ndarray
N x 3 x 3 rotation matrix.
T : np.ndarray
N x 3 translation vector.
References
----------
Leardini A, Benedetti MG, Berti L, Bettinelli D, Nativo R, Giannini S.
Rear-foot, mid-foot and fore-foot motion during the stance phase of gait.
Gait Posture. 2007 Mar;25(3):453-62. Epub 2006 Sep 11. PubMed PMID: 16965916.
"""
# Define markers to use
MM = mkrs['MM']
LM = mkrs['LM']
HF = mkrs['HF']
TT = mkrs['TT']
# Create versors
IM = (LM + MM) / 2
Osha = IM.copy()
if s == 'R':
XshaTemp = getVersor(np.cross(Osha - LM, HF - LM))
else:
XshaTemp = -getVersor(np.cross(Osha - LM, HF - LM))
# Ysha = getVersor((TT - Osha) - np.multiply(XshaTemp,vdot2(TT - Osha, XshaTemp)))
Ysha = getVersor((TT - Osha) - XshaTemp * vdot2(TT - Osha, XshaTemp)[:,None])
Zsha = getVersor(np.cross(XshaTemp, Ysha))
Xsha = getVersor(np.cross(Ysha, Zsha))
# Create rotation matrix from shank reference frame to laboratory reference frame
R = np.array((Xsha.T, Ysha.T, Zsha.T)) # 3 x 3 x N
R = np.transpose(R, (2,1,0)) # N x 3 x 3
# Return data
return R, Osha
[docs]def shankPoseISBWithClusterSVD(mkrs, clusterMkrList, args):
"""Calculate roto-translation matrix from shank (ISB conventions) to
laboratory reference frame, using rigid segment-connected cluster of
technical markers.
Parameters
----------
mkrs : dict
Technical markers data. Keys are marker names, values are np.ndarray
N x 3, where N is the number of time frames.
clusterMkrList : list
List of technical marker names to use.
args : mixed
Additional arguments:
- 'mkrsLoc': dictionary where keys are marker names and values are
3-elem np.arrays indicating the coordinates in the local reference
frame. Both technical and anatomical markers are needed here. For
For anatomical landmark names, see ``shankPoseISB()``.
- 'side': anatomical side, 'R' or 'L'.
Returns
-------
R : np.ndarray
N x 3 x 3 rotation matrix.
T : np.ndarray
N x 3 translation vector.
mkrsSeg : dict
Anatomical markers data in the laboratory reference frame.
"""
# Get roto-translation matrix from cluster to laboratory reference frame
R, T = rigidBodySVDFun(mkrs, clusterMkrList, args)
gRl = composeRotoTranslMatrix(R, T)
# Get markers in local reference frame
mkrsLoc = {m: args['mkrsLoc'][m] for m in ['LM','MM','HF','TT']}
# Express markers in the global reference frame
mkrsSeg = changeMarkersReferenceFrame(mkrsLoc, gRl)
# Calculate roto-translation matrix from shank to laboratory reference frame
R, T = shankPoseISB(mkrsSeg, s=args['side'])
return R, T, mkrsSeg
[docs]def calcaneusPose(mkrs, s='R'):
"""Calculate roto-translation matrix from calcaneous to
laboratory reference frame.
Parameters
----------
mkrs : dict
Markers data. Keys are marker names, values are np.ndarray N x 3,
where N is the number of time frames. Used names are:
- 'CA': calcalneous
- 'PT': lateral apex of the peroneal tubercle
- 'ST': most medial apex of the sustentaculum tali
s : {'R', 'L'}
Anatomical side.
Returns
-------
R : np.ndarray
N x 3 x 3 rotation matrix.
T : np.ndarray
N x 3 translation vector.
References
----------
Leardini A, Benedetti MG, Berti L, Bettinelli D, Nativo R, Giannini S.
Rear-foot, mid-foot and fore-foot motion during the stance phase of gait.
Gait Posture. 2007 Mar;25(3):453-62. Epub 2006 Sep 11. PubMed PMID: 16965916.
"""
# Define markers to use
CA = mkrs['CA']
PT = mkrs['PT']
ST = mkrs['ST']
# Create versors
IC = (ST + PT) / 2
Ocal = CA.copy()
Xcal = getVersor(IC - Ocal)
if s == 'R':
YcalTemp = getVersor(np.cross(Xcal, ST - Ocal))
else:
YcalTemp = -getVersor(np.cross(Xcal, ST - Ocal))
Zcal = getVersor(np.cross(Xcal, YcalTemp))
Ycal = getVersor(np.cross(Zcal, Xcal))
# Create rotation matrix from foot reference frame to laboratory reference frame
R = np.array((Xcal.T, Ycal.T, Zcal.T)) # 3 x 3 x N
R = np.transpose(R, (2,1,0)) # N x 3 x 3
# Return data
return R, Ocal
[docs]def calcaneusPoseWithClusterSVD(mkrs, clusterMkrList, args):
"""Calculate roto-translation matrix from calcaneous to
laboratory reference frame, using rigid segment-connected cluster of
technical markers.
Parameters
----------
mkrs : dict
Technical markers data. Keys are marker names, values are np.ndarray
N x 3, where N is the number of time frames.
clusterMkrList : list
List of technical marker names to use.
args : mixed
Additional arguments:
- 'mkrsLoc': dictionary where keys are marker names and values are
3-elem np.arrays indicating the coordinates in the local reference
frame. Both technical and anatomical markers are needed here. For
For anatomical landmark names, see ``calcaneusPoseISB()``.
- 'side': anatomical side, 'R' or 'L'.
Returns
-------
R : np.ndarray
N x 3 x 3 rotation matrix.
T : np.ndarray
N x 3 translation vector.
mkrsSeg : dict
Anatomical markers data in the laboratory reference frame.
"""
# Get roto-translation matrix from cluster to laboratory reference frame
R, T = rigidBodySVDFun(mkrs, clusterMkrList, args)
gRl = composeRotoTranslMatrix(R, T)
# Get markers in local reference frame
mkrsLoc = {m: args['mkrsLoc'][m] for m in ['CA','PT','ST']}
# Express markers in the global reference frame
mkrsSeg = changeMarkersReferenceFrame(mkrsLoc, gRl)
# Calculate roto-translation matrix from foot to laboratory reference frame
R, T = calcaneusPose(mkrsSeg, s=args['side'])
return R, T, mkrsSeg
[docs]def ges(Rvect):
"""Calculate Groot & Suntay anatomical joint angles from proximal and distal
segment rotation matrices. Angles are related to flexion-extension (FE) axis
of the proximal segment, internal-external (IE) axis of the distal segment,
ab-adduction (AA) floating axis.
Parameters
----------
Rvect : np.ndarray
18-elem vector representing row-flattened version of proximal and
distal segment rotation matrix from global reference frame to segment.
Returns
-------
list
List of Groot & Suntay angles (FE, AA, EI).
References
----------
Grood et Suntay, A joint coordinate system for the clinical description of
three- dimensional motion: application to the knee.
J Biomech. Engng 1983 105: 136-144
"""
R1v = Rvect[0:9]
R2v = Rvect[9:18]
e2=np.cross(R2v[3:6],R1v[6:9]) # e2 = e3 x e1
#---- i/e rotation ----
e2zd=np.dot(e2,-R2v[6:9])
e2xd=np.dot(e2,R2v[0:3])
IE=-np.arctan2(e2zd,e2xd)
#---- flexion ----
e2yp=np.dot(e2,R1v[3:6])
e2xp=np.dot(e2,R1v[0:3])
FE=np.arctan2(e2yp,e2xp)
#---- ab-adduction ----
bet=np.dot(R2v[3:6],R1v[6:9])
AA=np.arccos(bet)-np.pi/2
res = FE, AA, IE
return res
[docs]def R2zxy(Rvect):
"""Convert joint rotation matrix to ZXY Euler sequence.
Parameters
----------
Rvect : np.ndarray
A 9-elements array representing concatenated rows of the joint
rotation matrix.
Returns
-------
list
A list of 3 angle values.
"""
row1 = Rvect[0:3]
row2 = Rvect[3:6]
row3 = Rvect[6:9]
R = np.matrix([row1,row2,row3]) # 3 x 3 joint rotation matrix
x1 = np.arcsin(R[2,1])
sy =-R[2,0]/np.cos(x1)
cy = R[2,2]/np.cos(x1)
y1 = np.arctan2(sy,cy)
sz =-R[0,1]/np.cos(x1)
cz = R[1,1]/np.cos(x1)
z1 = np.arctan2(sz,cz)
if x1 >= 0:
x2 = np.pi - x1
else:
x2 = -np.pi - x1
sy =-R[2,0]/np.cos(x2)
cy = R[2,2]/np.cos(x2)
y2 = np.arctan2(sy,cy)
sz =-R[0,1]/np.cos(x2)
cz = R[1,1]/np.cos(x2)
z2 = np.arctan2(sz,cz)
if -np.pi/2 <= x1 and x1 <= np.pi/2:
yAngle=y1
zAngle=z1
xAngle=x1
else:
yAngle=y2
zAngle=z2
xAngle=x2
res = zAngle, xAngle, yAngle
return res
[docs]def getJointAngles(R1, R2, R2anglesFun=R2zxy):
"""Calculate 3 joint angles between 2 rigid bodies.
Parameters
----------
R1 : np.ndarray
N x 3 x 3 rotation matrices from rigid body to global reference frame for body 1 (N time frames).
R2 : np.ndarray
N x 3 x 3 rotation matrices from rigid body to global reference frame for body 2.
R2anglesFun : func
Function converting from joint rotation matrix to angles (see ``R2zxy()``).
Returns
-------
np.ndarray
N x 3 matrix of angles (in *deg*)
"""
# METHOD 1
N, dim1, dim2 = R1.shape
Rj = dot3(inv2(R1), R2)
Rjv = np.squeeze(np.reshape(Rj,(N,9)))
angles = np.apply_along_axis(R2anglesFun, 1, Rjv)
# NETHOD 2: ges (Source: Leardini, IOR)
# R1v = np.squeeze(np.reshape(inv2(R1),(N,9)))
# R2v = np.squeeze(np.reshape(inv2(R2),(N,9)))
# Rvect = np.hstack((R1v,R2v))
# angles = np.apply_along_axis(ges, 1, Rvect)
# Correct for gimbal-lock
#angles = correctGimbal(angles)
return np.rad2deg(angles)