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")