def _calibrateAnatomicalSegment(self,
aquiStatic,
segName,
dictAnatomic,
frameInit,
frameEnd,
options=None):
# calibration of technical Frames
for segName in dictAnatomic:
segPicked = self.getSegment(segName)
tf = segPicked.getReferential("TF")
nd1 = str(dictAnatomic[segName]['labels'][0])
pt1 = tf.static.getNode_byLabel(nd1).m_global
nd2 = str(dictAnatomic[segName]['labels'][1])
pt2 = tf.static.getNode_byLabel(nd2).m_global
nd3 = str(dictAnatomic[segName]['labels'][2])
pt3 = tf.static.getNode_byLabel(nd3).m_global
ndO = str(dictAnatomic[segName]['labels'][3])
ptO = tf.static.getNode_byLabel(ndO).m_global
a1 = (pt2 - pt1)
a1 = np.nan_to_num(np.divide(a1, np.linalg.norm(a1)))
v = (pt3 - pt1)
v = np.nan_to_num(np.divide(v, np.linalg.norm(v)))
a2 = np.cross(a1, v)
a2 = np.nan_to_num(np.divide(a2, np.linalg.norm(a2)))
x, y, z, R = frame.setFrameData(a1, a2,
dictAnatomic[segName]['sequence'])
segPicked.anatomicalFrame.static.m_axisX = x # work on the last TF in the list : thus index -1
segPicked.anatomicalFrame.static.m_axisY = y
segPicked.anatomicalFrame.static.m_axisZ = z
segPicked.anatomicalFrame.static.setRotation(R)
segPicked.anatomicalFrame.static.setTranslation(ptO)
# --- relative rotation Technical Anatomical
tf.setRelativeMatrixAnatomic(
np.dot(tf.static.getRotation().T,
segPicked.anatomicalFrame.static.getRotation()))
# add tracking markers as node
for label in segPicked.m_markerLabels:
globalPosition = aquiStatic.GetPoint(
str(label)).GetValues()[frameInit:frameEnd, :].mean(axis=0)
segPicked.anatomicalFrame.static.addNode(label,
globalPosition,
positionType="Global")
def _rightForeFoot_anatomicalCalibrate(self, aquiStatic, dictAnatomic,
frameInit, frameEnd):
seg = self.getSegment("Right Forefoot")
# --- Construction of the anatomical Referential
pt1 = aquiStatic.GetPoint(
str(dictAnatomic["Right Forefoot"]['labels'][0])).GetValues()[
frameInit:frameEnd, :].mean(axis=0)
pt2 = aquiStatic.GetPoint(
str(dictAnatomic["Right Forefoot"]['labels'][1])).GetValues()[
frameInit:frameEnd, :].mean(axis=0)
if dictAnatomic["Right Forefoot"]['labels'][2] is not None:
pt3 = aquiStatic.GetPoint(
str(dictAnatomic["Right Forefoot"]['labels'][2])).GetValues()[
frameInit:frameEnd, :].mean(axis=0) # not used
v = (pt3 - pt1)
else:
v = 100 * np.array([0.0, 0.0, 1.0])
ptOrigin = aquiStatic.GetPoint(
str(dictAnatomic["Right Forefoot"]['labels'][3])).GetValues()[
frameInit:frameEnd, :].mean(axis=0)
a1 = (pt2 - pt1)
a1 = a1 / np.linalg.norm(a1)
v = v / np.linalg.norm(v)
a2 = np.cross(a1, v)
a2 = a2 / np.linalg.norm(a2)
x, y, z, R = cfr.setFrameData(
a1, a2, dictAnatomic["Right Forefoot"]['sequence'])
seg.anatomicalFrame.static.m_axisX = x
seg.anatomicalFrame.static.m_axisY = y
seg.anatomicalFrame.static.m_axisZ = z
seg.anatomicalFrame.static.setRotation(R)
seg.anatomicalFrame.static.setTranslation(ptOrigin)
# --- relative rotation Technical Anatomical
tf = seg.getReferential("TF")
tf.setRelativeMatrixAnatomic(
np.dot(tf.static.getRotation().T,
seg.anatomicalFrame.static.getRotation()))
# --- node manager
for label in seg.m_markerLabels:
globalPosition = aquiStatic.GetPoint(
str(label)).GetValues()[frameInit:frameEnd, :].mean(axis=0)
seg.anatomicalFrame.static.addNode(label,
globalPosition,
positionType="Global")
def _calibrateTechnicalSegment(self,
aquiStatic,
segName,
dictRef,
frameInit,
frameEnd,
options=None):
segPicked = self.getSegment(segName)
for tfName in dictRef[segName]: # TF name
pt1 = aquiStatic.GetPoint(
str(dictRef[segName][tfName]['labels'][0])).GetValues()[
frameInit:frameEnd, :].mean(axis=0)
pt2 = aquiStatic.GetPoint(
str(dictRef[segName][tfName]['labels'][1])).GetValues()[
frameInit:frameEnd, :].mean(axis=0)
pt3 = aquiStatic.GetPoint(
str(dictRef[segName][tfName]['labels'][2])).GetValues()[
frameInit:frameEnd, :].mean(axis=0)
ptOrigin = aquiStatic.GetPoint(
str(dictRef[segName][tfName]['labels'][3])).GetValues()[
frameInit:frameEnd, :].mean(axis=0)
a1 = (pt2 - pt1)
a1 = np.nan_to_num(np.divide(a1, np.linalg.norm(a1)))
v = (pt3 - pt1)
v = np.nan_to_num(np.divide(v, np.linalg.norm(v)))
a2 = np.cross(a1, v)
a2 = np.nan_to_num(np.divide(a2, np.linalg.norm(a2)))
x, y, z, R = frame.setFrameData(
a1, a2, dictRef[segName][tfName]['sequence'])
segPicked.referentials[
-1].static.m_axisX = x # work on the last TF in the list : thus index -1
segPicked.referentials[-1].static.m_axisY = y
segPicked.referentials[-1].static.m_axisZ = z
segPicked.referentials[-1].static.setRotation(R)
segPicked.referentials[-1].static.setTranslation(ptOrigin)
# - add Nodes in segmental static(technical)Frame -
for label in segPicked.m_markerLabels:
globalPosition = aquiStatic.GetPoint(
str(label)).GetValues()[frameInit:frameEnd, :].mean(axis=0)
segPicked.referentials[-1].static.addNode(
label, globalPosition, positionType="Global")
def getViconRmatrix(frameVal, acq, originLabel, proximalLabel, lateralLabel, sequence):
pt1 = acq.GetPoint(originLabel).GetValues()[frameVal,:]
pt2 = acq.GetPoint(proximalLabel).GetValues()[frameVal,:]
pt3 = acq.GetPoint(lateralLabel).GetValues()[frameVal,:]
a1 = (pt2-pt1)
a1 = a1/np.linalg.norm(a1)
v = (pt3-pt1)
v = v/np.linalg.norm(v)
a2 = np.cross(a1,v)
a2 = a2/np.linalg.norm(a2)
x,y,z,R = frame.setFrameData(a1,a2,sequence)
return R
def check(self):
frameNumber = self.acq.GetPointFrameNumber()
LASI_values = self.acq.GetPoint("LASI").GetValues()
RASI_values = self.acq.GetPoint("RASI").GetValues()
LPSI_values = self.acq.GetPoint("LPSI").GetValues()
RPSI_values = self.acq.GetPoint("RPSI").GetValues()
sacrum_values = (self.acq.GetPoint("LPSI").GetValues() +
self.acq.GetPoint("RPSI").GetValues()) / 2.0
midAsis_values = (self.acq.GetPoint("LASI").GetValues() +
self.acq.GetPoint("RASI").GetValues()) / 2.0
projectedLASI = np.array(
[LASI_values[:, 0], LASI_values[:, 1],
np.zeros((frameNumber))]).T
projectedRASI = np.array(
[RASI_values[:, 0], RASI_values[:, 1],
np.zeros((frameNumber))]).T
projectedLPSI = np.array(
[LPSI_values[:, 0], LPSI_values[:, 1],
np.zeros((frameNumber))]).T
projectedRPSI = np.array(
[RPSI_values[:, 0], RPSI_values[:, 1],
np.zeros((frameNumber))]).T
for i in range(0, frameNumber):
verts = [
projectedLASI[i, 0:2], # left, bottom
projectedRASI[i, 0:2], # left, top
projectedRPSI[i, 0:2], # right, top
projectedLPSI[i, 0:2], # right, bottom
projectedLASI[i, 0:2], # right, top
]
codes = [
Path.MOVETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.CLOSEPOLY,
]
path = Path(verts, codes)
intersection = geometry.LineLineIntersect(projectedLASI[i, :],
projectedLPSI[i, :],
projectedRASI[i, :],
projectedRPSI[i, :])
if path.contains_point(intersection[0]):
logging.error(
"wrong Labelling of pelvic markers at frame [%i]" % (i))
self.state = False
else:
# check marker side
pt1 = RASI_values[i, :]
pt2 = LASI_values[i, :]
pt3 = sacrum_values[i, :]
ptOrigin = midAsis_values[i, :]
a1 = (pt2 - pt1)
a1 = np.divide(a1, np.linalg.norm(a1))
v = (pt3 - pt1)
v = np.divide(v, np.linalg.norm(v))
a2 = np.cross(a1, v)
a2 = np.divide(a2, np.linalg.norm(a2))
x, y, z, R = frame.setFrameData(a1, a2, "YZX")
csFrame_L = frame.Frame()
csFrame_L.setRotation(R)
csFrame_L.setTranslation(RASI_values[i, :])
csFrame_R = frame.Frame()
csFrame_R.setRotation(R)
csFrame_R.setTranslation(LASI_values[i, :])
for marker in self.markers:
residual = self.acq.GetPoint(marker).GetResidual(i)
if marker[0] == "L":
local = np.dot(
csFrame_L.getRotation().T,
self.acq.GetPoint(marker).GetValues()[i, :] -
csFrame_L.getTranslation())
if marker[0] == "R":
local = np.dot(
csFrame_R.getRotation().T,
self.acq.GetPoint(marker).GetValues()[i, :] -
csFrame_R.getTranslation())
if residual > 0.0:
if marker[0] == "L" and local[1] < 0:
logging.error(
"check location of the marker [%s] at frame [%i]"
% (marker, i))
self.state = False
if marker[0] == "R" and local[1] > 0:
logging.error(
"check location of the marker [%s] at frame [%i]"
% (marker, i))
self.state = False
def _right_foreFoot_motion(self, aqui, dictRef, dictAnat, options=None):
"""
:Parameters:
- `aqui` (btkAcquisition) - acquisition
- `dictRef` (dict) - instance of Model
- `frameInit` (int) - starting frame
"""
seg = self.getSegment("Right Forefoot")
# --- motion of the technical referential
seg.getReferential("TF").motion = []
# additional markers
# NA
#computation
for i in range(0, aqui.GetPointFrameNumber()):
pt1 = aqui.GetPoint(
str(dictRef["Right Forefoot"]["TF"]['labels'][0])).GetValues()[
i, :]
pt2 = aqui.GetPoint(
str(dictRef["Right Forefoot"]["TF"]['labels'][1])).GetValues()[
i, :]
pt3 = aqui.GetPoint(
str(dictRef["Right Forefoot"]["TF"]['labels'][2])).GetValues()[
i, :]
if dictRef["Right Forefoot"]["TF"]['labels'][2] is not None:
pt3 = aqui.GetPoint(
str(dictRef["Right Forefoot"]["TF"]['labels']
[2])).GetValues()[i, :] # not used
v = (pt3 - pt1)
else:
v = self.getSegment(
"Right Shank").anatomicalFrame.motion[i].m_axisY
ptOrigin = aqui.GetPoint(
str(dictRef["Right Forefoot"]["TF"]['labels'][3])).GetValues()[
i, :]
a1 = (pt2 - pt1)
a1 = a1 / np.linalg.norm(a1)
v = (pt3 - pt1)
v = v / np.linalg.norm(v)
a2 = np.cross(a1, v)
a2 = a2 / np.linalg.norm(a2)
x, y, z, R = cfr.setFrameData(
a1, a2, dictRef["Right Forefoot"]["TF"]['sequence'])
frame = cfr.Frame()
frame.m_axisX = x
frame.m_axisY = y
frame.m_axisZ = z
frame.setRotation(R)
frame.setTranslation(ptOrigin)
seg.getReferential("TF").addMotionFrame(frame)
# --- motion of new markers
btkTools.smartAppendPoint(
aqui, "RvTOE",
seg.getReferential("TF").getNodeTrajectory("RvTOE"))
btkTools.smartAppendPoint(
aqui, "RvD5M",
seg.getReferential("TF").getNodeTrajectory("RvD5M"))
# --- motion of the anatomical referential
seg.anatomicalFrame.motion = []
for i in range(0, aqui.GetPointFrameNumber()):
ptOrigin = aqui.GetPoint(
str(dictAnat["Right Forefoot"]['labels'][3])).GetValues()[i, :]
#R = np.dot(seg.getReferential("TF").motion[i].getRotation(),relativeSegTech )
R = np.dot(
seg.getReferential("TF").motion[i].getRotation(),
seg.getReferential("TF").relativeMatrixAnatomic)
frame = cfr.Frame()
frame.update(R, ptOrigin)
seg.anatomicalFrame.addMotionFrame(frame)
def _rightHindFoot_anatomicalCalibrate(self,
aquiStatic,
dictAnatomic,
frameInit,
frameEnd,
options=None):
seg = self.getSegment("Right Hindfoot")
# --- Construction of the anatomical Referential
pt1 = aquiStatic.GetPoint(
str(dictAnatomic["Right Hindfoot"]['labels'][0])).GetValues()[
frameInit:frameEnd, :].mean(axis=0)
pt2 = aquiStatic.GetPoint(
str(dictAnatomic["Right Hindfoot"]['labels'][1])).GetValues()[
frameInit:frameEnd, :].mean(axis=0)
if dictAnatomic["Right Hindfoot"]['labels'][2] is not None:
pt3 = aquiStatic.GetPoint(
str(dictAnatomic["Right Hindfoot"]['labels'][2])).GetValues()[
frameInit:frameEnd, :].mean(axis=0) # not used
v = (pt3 - pt1)
else:
v = self.getSegment(
"Right Shank").anatomicalFrame.static.m_axisY #(pt3-pt1)
ptOrigin = aquiStatic.GetPoint(
str(dictAnatomic["Right Hindfoot"]['labels'][3])).GetValues()[
frameInit:frameEnd, :].mean(axis=0)
if ("rightFlatHindFoot" in options.keys()
and options["rightFlatHindFoot"]):
logging.warning("option (rightFlatHindFoot) enable")
#pt2[2] = pt1[2]
pt1[2] = pt2[2]
else:
logging.warning("option (rightFlatHindFoot) disable")
a1 = (pt2 - pt1)
a1 = a1 / np.linalg.norm(a1)
v = v / np.linalg.norm(v)
a2 = np.cross(a1, v)
a2 = a2 / np.linalg.norm(a2)
x, y, z, R = cfr.setFrameData(
a1, a2, dictAnatomic["Right Hindfoot"]['sequence'])
seg.anatomicalFrame.static.m_axisX = x
seg.anatomicalFrame.static.m_axisY = y
seg.anatomicalFrame.static.m_axisZ = z
seg.anatomicalFrame.static.setRotation(R)
seg.anatomicalFrame.static.setTranslation(ptOrigin)
# --- relative rotation Technical Anatomical
tf = seg.getReferential("TF")
# actual Relative Rotation
trueRelativeMatrixAnatomic = np.dot(
tf.static.getRotation().T,
seg.anatomicalFrame.static.getRotation())
# native CGM relative rotation
y, x, z = ceuler.euler_yxz(trueRelativeMatrixAnatomic)
rotX = np.array([[1, 0, 0], [0, np.cos(x), -np.sin(x)],
[0, np.sin(x), np.cos(x)]])
rotY = np.array([[np.cos(y), 0, np.sin(y)], [0, 1, 0],
[-np.sin(y), 0, np.cos(y)]])
relativeMatrixAnatomic = np.dot(rotY, rotX)
tf.setRelativeMatrixAnatomic(relativeMatrixAnatomic)
# --- node manager
for label in seg.m_markerLabels:
globalPosition = aquiStatic.GetPoint(
str(label)).GetValues()[frameInit:frameEnd, :].mean(axis=0)
seg.anatomicalFrame.static.addNode(label,
globalPosition,
positionType="Global")
def _rightForeFoot_calibrate(self,
aquiStatic,
dictRef,
frameInit,
frameEnd,
options=None):
nFrames = aquiStatic.GetPointFrameNumber()
seg = self.getSegment("Right Forefoot")
# --- additional markers and Update of the marker segment list
# new markers ( RvTOE - RvD5M)
toe = aquiStatic.GetPoint("RTOE").GetValues()
d5 = aquiStatic.GetPoint("RD5M").GetValues()
valuesVirtualToe = np.zeros((nFrames, 3))
valuesVirtualD5 = np.zeros((nFrames, 3))
for i in range(0, nFrames):
valuesVirtualToe[i, :] = np.array([
toe[i, 0], toe[i, 1], toe[i, 2] - self.mp["rightToeOffset"]
]) #valuesVirtualCun[i,2]])#
valuesVirtualD5[i, :] = np.array(
[d5[i, 0], d5[i, 1], valuesVirtualToe[i, 2]])
btkTools.smartAppendPoint(aquiStatic,
"RvTOE",
valuesVirtualToe,
desc="virtual")
btkTools.smartAppendPoint(aquiStatic,
"RvD5M",
valuesVirtualD5,
desc="virtual-flat ")
# update marker list
seg.addMarkerLabel("RMidFoot")
seg.addMarkerLabel("RvCUN")
seg.addMarkerLabel("RvTOE")
seg.addMarkerLabel("RvD5M")
# --- technical frame selection and construction
tf = seg.getReferential("TF")
pt1 = aquiStatic.GetPoint(
str(dictRef["Right Forefoot"]["TF"]['labels'][0])).GetValues()[
frameInit:frameEnd, :].mean(axis=0) # D5
pt2 = aquiStatic.GetPoint(
str(dictRef["Right Forefoot"]["TF"]['labels'][1])).GetValues()[
frameInit:frameEnd, :].mean(axis=0) # Toe
if dictRef["Right Forefoot"]["TF"]['labels'][2] is not None:
pt3 = aquiStatic.GetPoint(
str(dictRef["Right Forefoot"]["TF"]['labels'][2])).GetValues()[
frameInit:frameEnd, :].mean(axis=0)
v = (pt3 - pt1)
else:
v = 100 * np.array([0.0, 0.0, 1.0])
ptOrigin = aquiStatic.GetPoint(
str(dictRef["Right Forefoot"]["TF"]['labels'][3])).GetValues()[
frameInit:frameEnd, :].mean(axis=0)
a1 = (pt2 - pt1)
a1 = a1 / np.linalg.norm(a1)
v = v / np.linalg.norm(v)
a2 = np.cross(a1, v)
a2 = a2 / np.linalg.norm(a2)
x, y, z, R = cfr.setFrameData(
a1, a2, dictRef["Right Forefoot"]["TF"]['sequence'])
tf.static.m_axisX = x
tf.static.m_axisY = y
tf.static.m_axisZ = z
tf.static.setRotation(R)
tf.static.setTranslation(ptOrigin)
# --- node manager
for label in seg.m_markerLabels:
globalPosition = aquiStatic.GetPoint(
str(label)).GetValues()[frameInit:frameEnd, :].mean(axis=0)
tf.static.addNode(label, globalPosition, positionType="Global")
def _rightHindFoot_calibrate(self,
aquiStatic,
dictRef,
frameInit,
frameEnd,
options=None):
nFrames = aquiStatic.GetPointFrameNumber()
seg = self.getSegment("Right Hindfoot")
# --- additional markers and Update of the marker segment list
# new markers
# mid point NAV and RP5M
valMidFoot = (aquiStatic.GetPoint("RNAV").GetValues() +
aquiStatic.GetPoint("RP5M").GetValues()) / 2.0
btkTools.smartAppendPoint(aquiStatic, "RMidFoot", valMidFoot, desc="")
# virtual CUN
cun = aquiStatic.GetPoint("RCUN").GetValues()
valuesVirtualCun = np.zeros((nFrames, 3))
for i in range(0, nFrames):
valuesVirtualCun[i, :] = np.array(
[cun[i, 0], cun[i, 1], cun[i, 2] - self.mp["rightToeOffset"]])
btkTools.smartAppendPoint(aquiStatic,
"RvCUN",
valuesVirtualCun,
desc="cun Registrate")
# update marker list
seg.addMarkerLabel("RAJC") # required markers
seg.addMarkerLabel("RMidFoot")
seg.addMarkerLabel("RvCUN")
# --- technical frame selection and construction
tf = seg.getReferential("TF")
pt1 = aquiStatic.GetPoint(
str(dictRef["Right Hindfoot"]["TF"]['labels'][0])).GetValues()[
frameInit:frameEnd, :].mean(axis=0)
pt2 = aquiStatic.GetPoint(
str(dictRef["Right Hindfoot"]["TF"]['labels'][1])).GetValues()[
frameInit:frameEnd, :].mean(axis=0)
if dictRef["Right Hindfoot"]["TF"]['labels'][2] is not None:
pt3 = aquiStatic.GetPoint(
str(dictRef["Right Hindfoot"]["TF"]['labels'][2])).GetValues()[
frameInit:frameEnd, :].mean(axis=0) # not used
v = (pt3 - pt1)
else:
v = self.getSegment(
"Right Shank").anatomicalFrame.static.m_axisY #(pt3-pt1)
ptOrigin = aquiStatic.GetPoint(
str(dictRef["Right Hindfoot"]["TF"]['labels'][3])).GetValues()[
frameInit:frameEnd, :].mean(axis=0)
a1 = (pt2 - pt1)
a1 = a1 / np.linalg.norm(a1)
v = v / np.linalg.norm(v)
a2 = np.cross(a1, v)
a2 = a2 / np.linalg.norm(a2)
x, y, z, R = cfr.setFrameData(
a1, a2, dictRef["Right Hindfoot"]["TF"]['sequence'])
tf.static.m_axisX = x
tf.static.m_axisY = y
tf.static.m_axisZ = z
tf.static.setRotation(R)
tf.static.setTranslation(ptOrigin)
# --- node manager
for label in seg.m_markerLabels:
globalPosition = aquiStatic.GetPoint(
str(label)).GetValues()[frameInit:frameEnd, :].mean(axis=0)
tf.static.addNode(label, globalPosition, positionType="Global")