Trajectory Deviation Perk Tutor metric

TrajectoryDeviation.py

import math
import vtk
# import numpy as np
# from datetime import datetime
from PythonMetricsCalculator import PerkEvaluatorMetric

class TrajectoryDeviation( PerkEvaluatorMetric ):
  
  # Static methods
  @staticmethod
  def GetMetricName():
    return "Trajectory Deviation"
  
  @staticmethod  
  def GetMetricUnit():
    return "deg"
  
  @staticmethod
  def GetTransformRoles():
    return [ "Needle" ]
  
  @staticmethod
  def GetAnatomyRoles():
    return { "Targets": "vtkMRMLMarkupsFiducialNode" }
    
    
  # Instance methods    
  def __init__( self ):
    PerkEvaluatorMetric.__init__( self )
    self.R_deviation = []
    self.A_deviation = []
    self.deviation_magnitude = []
    self.allErrors = []
    self.timeStamps = []
    self.timestr = 'blerg'
    self.fname = "C:/Users/P/Documents/tool-Trajectory-Entry-"+self.timestr+".csv"
    
    
  def SetAnatomy( self, role, node ):   
    if ( role == "Targets" ):
      self.targets = node  
      return True
      
    return False
    
  
  def AddTimestamp( self, time, matrix, point, role ):
    currTargetPositionEntry = [ 0, 0, 0 ]
    currTargetPositionEnd = [0, 0, 0]
    self.targets.GetNthFiducialPosition(0, currTargetPositionEntry) #Assume first fid point is the entry point
    self.targets.GetNthFiducialPosition(1, currTargetPositionEnd ) #Assume second fid point is the end point
    targetVector = (currTargetPositionEnd - currTargetPositionEntry)/math.hypot(currTargetPositionEnd - currTargetPositionEntry)
    
    # Find the needle tip in RAS
    toolTip_RAS = point[0:3]
    toolTipEnd = [0.,0.,10., 1]
    
    toolTipEnd_RAS = [0., 0., 0., 0.]
    matrix.MultiplyPoint(toolTipEnd, toolTipEnd_RAS)
    toolTipEnd_RAS = toolTipEnd_RAS[0:3]
    
    toolTrajectory = (toolTipEnd_RAS - toolTip_RAS)/math.hypot(toolTipEnd_RAS - toolTip_RAS)
    
    self.R_deviation = toolTrajectory[0]
    
    # self.R_deviation.append(needleTip_RAS[0] - currTargetPosition[0])
    # self.A_deviation.append(needleTip_RAS[1] - currTargetPosition[1])
    # self.S_deviation.append(needleTip_RAS[2] - currTargetPosition[2])
    
    # self.needleTipTargetDistance.append(math.sqrt( vtk.vtkMath.Distance2BetweenPoints( currTargetPosition_RAS, needleTip_RAS ) ))
    
    # self.timeStamps.append(time)
    
    # self.allErrors.append([self.timeStamps[-1], self.R_deviation[-1], self.A_deviation[-1], self.S_deviation[-1], self.needleTipTargetDistance[-1]])
    
    # with open(self.fname, "a+") as myfile:
    #   myfile.write(self.GetMetric()+"\n")
    
  def GetMetric( self ):
    return self.R_deviation
    # separator = ','
    # return separator.join(map(str,self.allErrors[-1]))