def computeMotionTechnicalFrame(self,
aqui,
segName,
dictRef,
method,
options=None):
segPicked = self.getSegment(segName)
segPicked.getReferential("TF").motion = []
if method == enums.motionMethod.Sodervisk:
tms = segPicked.m_tracking_markers
for i in range(0, aqui.GetPointFrameNumber()):
visibleMarkers = btkTools.getVisibleMarkersAtFrame(
aqui, tms, i)
# constructuion of the input of sodervisk
arrayStatic = np.zeros((len(visibleMarkers), 3))
arrayDynamic = np.zeros((len(visibleMarkers), 3))
j = 0
for vm in visibleMarkers:
arrayStatic[j, :] = segPicked.getReferential(
"TF").static.getNode_byLabel(vm).m_global
arrayDynamic[j, :] = aqui.GetPoint(vm).GetValues()[i, :]
j += 1
Ropt, Lopt, RMSE, Am, Bm = motion.segmentalLeastSquare(
arrayStatic, arrayDynamic)
R = np.dot(Ropt,
segPicked.getReferential("TF").static.getRotation())
tOri = np.dot(
Ropt,
segPicked.getReferential(
"TF").static.getTranslation()) + Lopt
cframe = frame.Frame()
cframe.setRotation(R)
cframe.setTranslation(tOri)
cframe.m_axisX = R[:, 0]
cframe.m_axisY = R[:, 1]
cframe.m_axisZ = R[:, 2]
segPicked.getReferential("TF").addMotionFrame(
copy.deepcopy(cframe))
else:
raise Exception("[pyCGM2] : motion method doesn t exist")
def computeMotionAnatomicalFrame(self,
aqui,
segName,
dictAnatomic,
options=None):
segPicked = self.getSegment(segName)
segPicked.anatomicalFrame.motion = []
ndO = str(dictAnatomic[segName]['labels'][3])
ptO = segPicked.getReferential("TF").getNodeTrajectory(ndO)
csFrame = frame.Frame()
for i in range(0, aqui.GetPointFrameNumber()):
R = np.dot(
segPicked.getReferential("TF").motion[i].getRotation(),
segPicked.getReferential("TF").relativeMatrixAnatomic)
csFrame.update(R, ptO)
segPicked.anatomicalFrame.addMotionFrame(copy.deepcopy(csFrame))
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 _rightHindFoot_motion_optimize(self, aqui, dictRef, dictAnat,
motionMethod):
seg = self.getSegment("Right Hindfoot")
# --- add RAJC if marker list < 2 - check presence of tracking markers in the acquisition
if seg.m_tracking_markers != []:
if len(seg.m_tracking_markers) == 2:
if "RAJC" not in seg.m_tracking_markers:
seg.m_tracking_markers.append("RAJC")
logging.debug("RAJC added to tracking marker list")
# --- Motion of the Technical frame
seg.getReferential("TF").motion = []
# part 1: get global location in Static
if seg.m_tracking_markers != []: # work with tracking markers
staticPos = np.zeros((len(seg.m_tracking_markers), 3))
i = 0
for label in seg.m_tracking_markers:
staticPos[i, :] = seg.getReferential(
"TF").static.getNode_byLabel(label).m_global
i += 1
# part 2 : get dynamic position ( look out i pick up value in the btkAcquisition)
for i in range(0, aqui.GetPointFrameNumber()):
if seg.m_tracking_markers != []: # work with traking markers
dynPos = np.zeros((len(seg.m_tracking_markers), 3)) # use
k = 0
for label in seg.m_tracking_markers:
dynPos[k, :] = aqui.GetPoint(label).GetValues()[i, :]
k += 1
if motionMethod == pyCGM2Enums.motionMethod.Sodervisk:
Ropt, Lopt, RMSE, Am, Bm = cmot.segmentalLeastSquare(
staticPos, dynPos)
R = np.dot(Ropt, seg.getReferential("TF").static.getRotation())
tOri = np.dot(
Ropt,
seg.getReferential("TF").static.getTranslation()) + Lopt
frame = cfr.Frame()
frame.setRotation(R)
frame.setTranslation(tOri)
frame.m_axisX = R[:, 0]
frame.m_axisY = R[:, 1]
frame.m_axisZ = R[:, 2]
seg.getReferential("TF").addMotionFrame(frame)
# --- RvCUN
btkTools.smartAppendPoint(
aqui,
"RvCUN",
seg.getReferential("TF").getNodeTrajectory("RvCUN"),
desc="opt")
# --- Motion of the Anatomical frame
seg.anatomicalFrame.motion = []
for i in range(0, aqui.GetPointFrameNumber()):
ptOrigin = aqui.GetPoint(
str(dictAnat["Right Hindfoot"]['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 _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 __init__(self):
self.static = frame.Frame()
self.motion = []
self.relativeMatrixAnatomic = np.zeros((3, 3))
self.additionalInfos = dict()