def smartAppendPoint(acq, label, values,
pointType=btk.btkPoint.Marker, desc="",
residuals=None):
"""
Function to append a point into an acquisition object.
:param acq: (btkAcquisition) btk Acquisition instance
:param label: (str) point's label
:param values: (ndarray(n, 3)) point's values
:param pointType: (enums of btkPoint) type of Point
:param residuals:
:return: None
"""
values = np.nan_to_num(values)
if residuals is None:
residuals = np.zeros((values.shape[0], 1))
for i in np.arange(values.shape[0]):
if np.all(values[i, :] == 0.0):
residuals[i] = -1.0
new_btkPoint = btk.btkPoint(label, acq.GetPointFrameNumber())
new_btkPoint.SetValues(values)
new_btkPoint.SetDescription(desc)
new_btkPoint.SetType(pointType)
new_btkPoint.SetResiduals(residuals)
acq.AppendPoint(new_btkPoint)
def test_InsertItem(self):
test = btk.btkPointCollection()
m = btk.btkPoint("HEEL_R", 10)
self.assertEqual(test.InsertItem(0, m), True)
self.assertEqual(test.GetItemNumber(), 1)
m.SetValue(0, 0, 1.0)
self.assertEqual(test.GetItem(0).GetValue(0, 0), 1.0)
def test_InsertItem(self):
test = btk.btkPointCollection()
m = btk.btkPoint("HEEL_R", 10)
self.assertEqual(test.InsertItem(0, m), True)
self.assertEqual(test.GetItemNumber(), 1)
m.SetValue(0,0,1.0)
self.assertEqual(test.GetItem(0).GetValue(0,0), 1.0)
def test_Constructor(self):
test = btk.btkPoint("HEEL_R", 200)
self.assertEqual(test.GetLabel(), "HEEL_R")
self.assertEqual(test.GetDescription(), "")
self.assertEqual(test.GetValues().shape[0], 200)
self.assertEqual(test.GetValues().shape[1], 3)
self.assertEqual(test.GetResiduals().shape[0], 200)
self.assertEqual(test.GetType(), btk.btkPoint.Marker)
def test_Constructor(self):
test = btk.btkPoint("HEEL_R", 200)
self.assertEqual(test.GetLabel(), "HEEL_R")
self.assertEqual(test.GetDescription(), "")
self.assertEqual(test.GetValues().shape[0], 200)
self.assertEqual(test.GetValues().shape[1], 3)
self.assertEqual(test.GetResiduals().shape[0], 200)
self.assertEqual(test.GetType(), btk.btkPoint.Marker)
def test_SetValues(self):
p = btk.btkPoint("HEEL_R", 4)
values = numpy.array([[1.,2.,3.],[4.,5.,6.],[7.,8.,9.],[10.,11.,12.]])
p.SetValues(values);
values_extracted = p.GetValues()
for i in range(0,4):
self.assertEqual(values_extracted[i,0], values[i,0])
self.assertEqual(values_extracted[i,1], values[i,1])
self.assertEqual(values_extracted[i,2], values[i,2])
def test_SetValues(self):
p = btk.btkPoint("HEEL_R", 4)
values = numpy.array([[1., 2., 3.], [4., 5., 6.], [7., 8., 9.],
[10., 11., 12.]])
p.SetValues(values)
values_extracted = p.GetValues()
for i in range(0, 4):
self.assertEqual(values_extracted[i, 0], values[i, 0])
self.assertEqual(values_extracted[i, 1], values[i, 1])
self.assertEqual(values_extracted[i, 2], values[i, 2])
def save(self, new_tracks, new_labels, filename, acq, file_kind, wrist_angles=np.array([]), finger_angles=np.array([]), thumb_angles=np.array([])):
# saving to c3d
name = filename.split(".")[0] + '_' + file_kind + '.c3d'
processed_file = btk.btkAcquisition.Clone(acq)
processed_file.ClearPoints()
list_points = []
for i in range(len(new_labels)):
list_points.append(btk.btkPoint(new_labels[i], new_tracks.shape[1]))
list_points[-1].SetValues(new_tracks[i, :, :])
processed_file.AppendPoint(list_points[-1])
if wrist_angles.any():
list_points.append(btk.btkPoint("Wrist", wrist_angles.shape[1]))
list_points[-1].SetValues(wrist_angles.T)
list_points[-1].SetType(btk.btkPoint.Angle)
processed_file.AppendPoint(list_points[-1])
if finger_angles.any():
for i in range(4):
list_points.append(btk.btkPoint("MCP" + str(2 + i) + "_flexion_abduction", finger_angles.shape[1]))
list_points[-1].SetValues(finger_angles[i, :, :])
list_points[-1].SetType(btk.btkPoint.Angle)
processed_file.AppendPoint(list_points[-1])
list_points.append(btk.btkPoint("PIP" + str(2 + i) + "_flexion", finger_angles.shape[1]))
list_points[-1].SetValues(finger_angles[4 + i, :, :])
list_points[-1].SetType(btk.btkPoint.Angle)
processed_file.AppendPoint(list_points[-1])
list_points.append(btk.btkPoint("DIP" + str(2 + i) + "_flexion", finger_angles.shape[1]))
list_points[-1].SetValues(finger_angles[8 + i, :, :])
list_points[-1].SetType(btk.btkPoint.Angle)
processed_file.AppendPoint(list_points[-1])
if thumb_angles.any():
list_points.append(btk.btkPoint("THUMB_rotation_abduction", thumb_angles.shape[1]))
list_points[-1].SetValues(thumb_angles[0, :, :])
list_points[-1].SetType(btk.btkPoint.Angle)
processed_file.AppendPoint(list_points[-1])
list_points.append(btk.btkPoint("MCP1_flexion", thumb_angles.shape[1]))
list_points[-1].SetValues(thumb_angles[1, :, :])
list_points[-1].SetType(btk.btkPoint.Angle)
processed_file.AppendPoint(list_points[-1])
list_points.append(btk.btkPoint("IP1_flexion", thumb_angles.shape[1]))
list_points[-1].SetValues(thumb_angles[2, :, :])
list_points[-1].SetType(btk.btkPoint.Angle)
processed_file.AppendPoint(list_points[-1])
writer = btk.btkAcquisitionFileWriter()
writer.SetInput(processed_file)
writer.SetFilename(str(name))
writer.Update()
# reccording destinations for rendering in mokka
if file_kind == "pre_processed":
self.pre_processed_destination = name
elif file_kind == "labelled":
self.labelled_destination = name
def test_SetComponent(self):
test = btk.btkWrench()
p = btk.btkPoint("KneeJointCenter",1)
f = btk.btkPoint("KneeJointForce",1)
m = btk.btkPoint("KneeJointMoment",1)
test.SetPosition(p);
test.SetForce(f);
test.SetMoment(m);
test.SetFrameNumber(10);
self.assertEqual(test.GetPosition().GetLabel(), "KneeJointCenter")
self.assertEqual(test.GetPosition().GetType(), btk.btkPoint.Marker)
self.assertEqual(test.GetPosition().GetValues().shape[0], 10)
self.assertEqual(test.GetForce().GetLabel(), "KneeJointForce")
self.assertEqual(test.GetForce().GetType(), btk.btkPoint.Force)
self.assertEqual(test.GetForce().GetValues().shape[0], 10)
self.assertEqual(test.GetMoment().GetLabel(), "KneeJointMoment")
self.assertEqual(test.GetMoment().GetType(), btk.btkPoint.Moment)
self.assertEqual(test.GetMoment().GetValues().shape[0], 10)
self.assertEqual(p.GetType(), btk.btkPoint.Marker)
self.assertEqual(p.GetValues().shape[0], 10)
self.assertEqual(f.GetType(), btk.btkPoint.Force)
self.assertEqual(f.GetValues().shape[0], 10)
self.assertEqual(m.GetType(), btk.btkPoint.Moment)
self.assertEqual(m.GetValues().shape[0], 10)
def test_SetComponent(self):
test = btk.btkWrench()
p = btk.btkPoint("KneeJointCenter", 1)
f = btk.btkPoint("KneeJointForce", 1)
m = btk.btkPoint("KneeJointMoment", 1)
test.SetPosition(p)
test.SetForce(f)
test.SetMoment(m)
test.SetFrameNumber(10)
self.assertEqual(test.GetPosition().GetLabel(), "KneeJointCenter")
self.assertEqual(test.GetPosition().GetType(), btk.btkPoint.Marker)
self.assertEqual(test.GetPosition().GetValues().shape[0], 10)
self.assertEqual(test.GetForce().GetLabel(), "KneeJointForce")
self.assertEqual(test.GetForce().GetType(), btk.btkPoint.Force)
self.assertEqual(test.GetForce().GetValues().shape[0], 10)
self.assertEqual(test.GetMoment().GetLabel(), "KneeJointMoment")
self.assertEqual(test.GetMoment().GetType(), btk.btkPoint.Moment)
self.assertEqual(test.GetMoment().GetValues().shape[0], 10)
self.assertEqual(p.GetType(), btk.btkPoint.Marker)
self.assertEqual(p.GetValues().shape[0], 10)
self.assertEqual(f.GetType(), btk.btkPoint.Force)
self.assertEqual(f.GetValues().shape[0], 10)
self.assertEqual(m.GetType(), btk.btkPoint.Moment)
self.assertEqual(m.GetValues().shape[0], 10)
def convert(self, bvh, output_file):
acq = btk.btkAcquisition()
acq.Init(0, bvh.frame_count)
acq.SetPointFrequency(1 / bvh.frame_time)
self.points_dict = {}
marker_count = 0
for key in bvh.channel_dict.keys():
point = btk.btkPoint('Marker_' + key, bvh.frame_count)
point.SetLabel('Marker_' + key)
self.points_dict[key] = point
marker_count += 1
self.calculate_joint_position(bvh)
for key in self.points_dict.keys():
acq.AppendPoint(self.points_dict[key])
writer = btk.btkAcquisitionFileWriter()
writer.SetInput(acq)
writer.SetFilename(output_file)
writer.Update()
def combine_files(c3d_file, mat_file, postfix='_Updated'):
""" Combine a c3d file with the 'RefPoints' markers from a corresponding
mat file
"""
c3dacq = open_acqusition(c3d_file)
acq_frames = c3dacq.GetPointFrameNumber()
matobj = loadmat(mat_file, struct_as_record=False, squeeze_me=True)
for refname in matobj['RefPoint']._fieldnames:
value = getattr(matobj['RefPoint'],refname).value
if value.shape[0] != acq_frames:
raise ValueError('The number of frames in the C3D file ({}) and '
'the mat file ({}) does not match'
''.format(acq_frames, value.shape[0]))
point = btk.btkPoint(acq_frames)
point.SetLabel(refname)
point.SetValues(value)
c3dacq.AppendPoint(point)
updated_filename = write_acqusition(c3dacq, c3d_file, postfix=postfix)
return updated_filename
def test_FrameNumber(self):
test = btk.btkPoint("HEEL_R", 200)
test.SetFrameNumber(225)
self.assertEqual(test.GetValues().shape[0], 225)
self.assertEqual(test.GetResiduals().shape[0], 225)
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()
def test_FrameNumber(self):
test = btk.btkPoint("HEEL_R", 200)
test.SetFrameNumber(225)
self.assertEqual(test.GetValues().shape[0], 225)
self.assertEqual(test.GetResiduals().shape[0], 225)
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()
def addFunctionalJointCentres(trialC3D, shoulderC3D = None, elbowC3D = None, filtFreq = None):
if shoulderC3D != None:
#Add shoulder joint centre marker
#Initialise a file reader
reader = btk.btkAcquisitionFileReader()
#Start with the shoulder joint centre
#Load in SCoRE c3d file
reader.SetFilename(shoulderC3D)
#Update reader
reader.Update()
#Get the btk acquisition object
acq = reader.GetOutput()
#Get number of frames
nFrames = acq.GetPointFrameNumber()
#Get the shoulder joint centre marker
SJCmrkr = np.empty([nFrames,3])
SJCmrkr[:,0] = acq.GetPoint('Thorax_UpperArm.R_score').GetValues()[:,0]
SJCmrkr[:,1] = acq.GetPoint('Thorax_UpperArm.R_score').GetValues()[:,1]
SJCmrkr[:,2] = acq.GetPoint('Thorax_UpperArm.R_score').GetValues()[:,2]
#Get the three markers to use to calculate joint centre position
#These will be: R.PUA.Top, R.LUA.Top, R.LUA.Right
Amrkr = np.empty([nFrames,3])
Amrkr[:,0] = acq.GetPoint('R.PUA.Top').GetValues()[:,0]
Amrkr[:,1] = acq.GetPoint('R.PUA.Top').GetValues()[:,1]
Amrkr[:,2] = acq.GetPoint('R.PUA.Top').GetValues()[:,2]
Bmrkr = np.empty([nFrames,3])
Bmrkr[:,0] = acq.GetPoint('R.LUA.Top').GetValues()[:,0]
Bmrkr[:,1] = acq.GetPoint('R.LUA.Top').GetValues()[:,1]
Bmrkr[:,2] = acq.GetPoint('R.LUA.Top').GetValues()[:,2]
Cmrkr = np.empty([nFrames,3])
Cmrkr[:,0] = acq.GetPoint('R.LUA.Right').GetValues()[:,0]
Cmrkr[:,1] = acq.GetPoint('R.LUA.Right').GetValues()[:,1]
Cmrkr[:,2] = acq.GetPoint('R.LUA.Right').GetValues()[:,2]
#Filter marker data (if required)
if filtFreq != None:
#Get sampling frequency
fs = acq.GetPointFrequency()
#Create low pass digital filter
w = filtFreq / (fs / 2) #normalise filter frequency
b,a = signal.butter(filtFreq, w, 'low')
#Filter rotated marker data
SJCmrkr[:,0] = signal.filtfilt(b, a, SJCmrkr[:,0])
SJCmrkr[:,1] = signal.filtfilt(b, a, SJCmrkr[:,1])
SJCmrkr[:,2] = signal.filtfilt(b, a, SJCmrkr[:,2])
Amrkr[:,0] = signal.filtfilt(b, a, Amrkr[:,0])
Amrkr[:,1] = signal.filtfilt(b, a, Amrkr[:,1])
Amrkr[:,2] = signal.filtfilt(b, a, Amrkr[:,2])
Bmrkr[:,0] = signal.filtfilt(b, a, Bmrkr[:,0])
Bmrkr[:,1] = signal.filtfilt(b, a, Bmrkr[:,1])
Bmrkr[:,2] = signal.filtfilt(b, a, Bmrkr[:,2])
Cmrkr[:,0] = signal.filtfilt(b, a, Cmrkr[:,0])
Cmrkr[:,1] = signal.filtfilt(b, a, Cmrkr[:,1])
Cmrkr[:,2] = signal.filtfilt(b, a, Cmrkr[:,2])
#Calculate the distance between the markers at each frame
distA = np.empty([nFrames,1]); distB = np.empty([nFrames,1]); distC = np.empty([nFrames,1])
for p in range(0,nFrames-1):
distA[p,0] = math.sqrt((SJCmrkr[p,0] - Amrkr[p,0])**2 + (SJCmrkr[p,1] - Amrkr[p,1])**2 + (SJCmrkr[p,2] - Amrkr[p,2])**2)
distB[p,0] = math.sqrt((SJCmrkr[p,0] - Bmrkr[p,0])**2 + (SJCmrkr[p,1] - Bmrkr[p,1])**2 + (SJCmrkr[p,2] - Bmrkr[p,2])**2)
distC[p,0] = math.sqrt((SJCmrkr[p,0] - Cmrkr[p,0])**2 + (SJCmrkr[p,1] - Cmrkr[p,1])**2 + (SJCmrkr[p,2] - Cmrkr[p,2])**2)
#Calculate average distances
distA_avg = np.mean(distA); distB_avg = np.mean(distB); distC_avg = np.mean(distC)
#Read in the experimental data c3d file
readerExp = btk.btkAcquisitionFileReader()
#Load in experimental c3d trial
readerExp.SetFilename(trialC3D)
#Update reader
readerExp.Update()
#Get the btk acquisition object
acqExp = readerExp.GetOutput()
#Get number of frames
nFramesExp = acqExp.GetPointFrameNumber()
#Extract the same markers used for calculating distance from the experimental data
AmrkrExp = np.empty([nFramesExp,3])
AmrkrExp[:,0] = acqExp.GetPoint('R.PUA.Top').GetValues()[:,0]
AmrkrExp[:,1] = acqExp.GetPoint('R.PUA.Top').GetValues()[:,1]
AmrkrExp[:,2] = acqExp.GetPoint('R.PUA.Top').GetValues()[:,2]
BmrkrExp = np.empty([nFramesExp,3])
BmrkrExp[:,0] = acqExp.GetPoint('R.LUA.Top').GetValues()[:,0]
BmrkrExp[:,1] = acqExp.GetPoint('R.LUA.Top').GetValues()[:,1]
BmrkrExp[:,2] = acqExp.GetPoint('R.LUA.Top').GetValues()[:,2]
CmrkrExp = np.empty([nFramesExp,3])
CmrkrExp[:,0] = acqExp.GetPoint('R.LUA.Right').GetValues()[:,0]
CmrkrExp[:,1] = acqExp.GetPoint('R.LUA.Right').GetValues()[:,1]
CmrkrExp[:,2] = acqExp.GetPoint('R.LUA.Right').GetValues()[:,2]
#Filter marker data (if required)
if filtFreq != None:
#Get sampling frequency
fs = acq.GetPointFrequency()
#Create low pass digital filter
w = filtFreq / (fs / 2) #normalise filter frequency
b,a = signal.butter(filtFreq, w, 'low')
#Filter rotated marker data
AmrkrExp[:,0] = signal.filtfilt(b, a, AmrkrExp[:,0])
AmrkrExp[:,1] = signal.filtfilt(b, a, AmrkrExp[:,1])
AmrkrExp[:,2] = signal.filtfilt(b, a, AmrkrExp[:,2])
BmrkrExp[:,0] = signal.filtfilt(b, a, BmrkrExp[:,0])
BmrkrExp[:,1] = signal.filtfilt(b, a, BmrkrExp[:,1])
BmrkrExp[:,2] = signal.filtfilt(b, a, BmrkrExp[:,2])
CmrkrExp[:,0] = signal.filtfilt(b, a, CmrkrExp[:,0])
CmrkrExp[:,1] = signal.filtfilt(b, a, CmrkrExp[:,1])
CmrkrExp[:,2] = signal.filtfilt(b, a, CmrkrExp[:,2])
#Use the experimental marker data and calculated distances to solve the distance
#equations for the XYZ positions of the shoulder joint centre in the experimental
#trial. For the first frame, we'll use a marker that is close to the propose
#joint centre (i.e. R.ACR)
#Define the initial guess for shoulder joint centre on the first frame
initialGuess = [acqExp.GetPoint('R.ACR').GetValues()[0,0],
acqExp.GetPoint('R.ACR').GetValues()[0,1],
acqExp.GetPoint('R.ACR').GetValues()[0,2]]
#Initialise empty array for SJC experimental data
SJCmrkrExp = np.empty([nFramesExp,3])
#Loop through experimental trial frames and calculate SJC position
for frameNo in range(0,nFramesExp):
#Define the marker positions for below solver equations
xA = AmrkrExp[frameNo,0]; yA = AmrkrExp[frameNo,1]; zA = AmrkrExp[frameNo,2];
xB = BmrkrExp[frameNo,0]; yB = BmrkrExp[frameNo,1]; zB = BmrkrExp[frameNo,2];
xC = CmrkrExp[frameNo,0]; yC = CmrkrExp[frameNo,1]; zC = CmrkrExp[frameNo,2];
#Define a function that expresses the equations
def f(p):
x,y,z = p
#Define functions
fA = ((x - xA)**2 + (y - yA)**2 + (z - zA)**2) - distA_avg**2
fB = ((x - xB)**2 + (y - yB)**2 + (z - zB)**2) - distB_avg**2
fC = ((x - xC)**2 + (y - yC)**2 + (z - zC)**2) - distC_avg**2
return [fA,fB,fC]
#Test function with values
#print(f([30,40,50]))
#Check whether to update initial guess
if frameNo != 0:
#Update initial guess
initialGuess = [SJCmrkrExp[frameNo-1,0],SJCmrkrExp[frameNo-1,1],SJCmrkrExp[frameNo-1,2]]
#Solve equation for current marker position
SJCmrkrExp[frameNo,0],SJCmrkrExp[frameNo,1],SJCmrkrExp[frameNo,2] = fsolve(f,initialGuess)
#Cleanup
#del(xA,xB,xC,yA,yB,yC,zA,zB,zC)
#Add new marker back into c3d data as R.SJC
#Create new empty point
newPoint = btk.btkPoint(acqExp.GetPointFrameNumber())
#Set label on new point
newPoint.SetLabel('R.SJC')
#Set values for new marker from those calculated
newPoint.SetValues(SJCmrkrExp)
#Append new point to acquisition object
acqExp.AppendPoint(newPoint)
#Re-run the above process to do the elbow joint centre
if elbowC3D != None:
#Initialise a file reader
reader = btk.btkAcquisitionFileReader()
#Start with the shoulder joint centre
#Load in SCoRE c3d file
reader.SetFilename(elbowC3D)
#Update reader
reader.Update()
#Get the btk acquisition object
acq = reader.GetOutput()
#Get number of frames
nFrames = acq.GetPointFrameNumber()
#Get the shoulder joint centre marker
EJCmrkr = np.empty([nFrames,3])
EJCmrkr[:,0] = acq.GetPoint('UpperArm.R_ForeArm.R_score').GetValues()[:,0]
EJCmrkr[:,1] = acq.GetPoint('UpperArm.R_ForeArm.R_score').GetValues()[:,1]
EJCmrkr[:,2] = acq.GetPoint('UpperArm.R_ForeArm.R_score').GetValues()[:,2]
#Get the three markers to use to calculate joint centre position
#These will be: R.FA.Top, R.FA.Left, R.PUA.Right
Amrkr = np.empty([nFrames,3])
Amrkr[:,0] = acq.GetPoint('R.FA.Top').GetValues()[:,0]
Amrkr[:,1] = acq.GetPoint('R.FA.Top').GetValues()[:,1]
Amrkr[:,2] = acq.GetPoint('R.FA.Top').GetValues()[:,2]
Bmrkr = np.empty([nFrames,3])
Bmrkr[:,0] = acq.GetPoint('R.FA.Left').GetValues()[:,0]
Bmrkr[:,1] = acq.GetPoint('R.FA.Left').GetValues()[:,1]
Bmrkr[:,2] = acq.GetPoint('R.FA.Left').GetValues()[:,2]
Cmrkr = np.empty([nFrames,3])
Cmrkr[:,0] = acq.GetPoint('R.PUA.Right').GetValues()[:,0]
Cmrkr[:,1] = acq.GetPoint('R.PUA.Right').GetValues()[:,1]
Cmrkr[:,2] = acq.GetPoint('R.PUA.Right').GetValues()[:,2]
#Filter marker data (if required)
if filtFreq != None:
#Get sampling frequency
fs = acq.GetPointFrequency()
#Create low pass digital filter
w = filtFreq / (fs / 2) #normalise filter frequency
b,a = signal.butter(filtFreq, w, 'low')
#Filter rotated marker data
EJCmrkr[:,0] = signal.filtfilt(b, a, EJCmrkr[:,0])
EJCmrkr[:,1] = signal.filtfilt(b, a, EJCmrkr[:,1])
EJCmrkr[:,2] = signal.filtfilt(b, a, EJCmrkr[:,2])
Amrkr[:,0] = signal.filtfilt(b, a, Amrkr[:,0])
Amrkr[:,1] = signal.filtfilt(b, a, Amrkr[:,1])
Amrkr[:,2] = signal.filtfilt(b, a, Amrkr[:,2])
Bmrkr[:,0] = signal.filtfilt(b, a, Bmrkr[:,0])
Bmrkr[:,1] = signal.filtfilt(b, a, Bmrkr[:,1])
Bmrkr[:,2] = signal.filtfilt(b, a, Bmrkr[:,2])
Cmrkr[:,0] = signal.filtfilt(b, a, Cmrkr[:,0])
Cmrkr[:,1] = signal.filtfilt(b, a, Cmrkr[:,1])
Cmrkr[:,2] = signal.filtfilt(b, a, Cmrkr[:,2])
#Calculate the distance between the markers at each frame
distA = np.empty([nFrames,1]); distB = np.empty([nFrames,1]); distC = np.empty([nFrames,1])
for p in range(0,nFrames-1):
distA[p,0] = math.sqrt((EJCmrkr[p,0] - Amrkr[p,0])**2 + (EJCmrkr[p,1] - Amrkr[p,1])**2 + (EJCmrkr[p,2] - Amrkr[p,2])**2)
distB[p,0] = math.sqrt((EJCmrkr[p,0] - Bmrkr[p,0])**2 + (EJCmrkr[p,1] - Bmrkr[p,1])**2 + (EJCmrkr[p,2] - Bmrkr[p,2])**2)
distC[p,0] = math.sqrt((EJCmrkr[p,0] - Cmrkr[p,0])**2 + (EJCmrkr[p,1] - Cmrkr[p,1])**2 + (EJCmrkr[p,2] - Cmrkr[p,2])**2)
#Calculate average distances
distA_avg = np.mean(distA); distB_avg = np.mean(distB); distC_avg = np.mean(distC)
#Extract the same markers used for calculating distance from the experimental data
AmrkrExp = np.empty([nFramesExp,3])
AmrkrExp[:,0] = acqExp.GetPoint('R.FA.Top').GetValues()[:,0]
AmrkrExp[:,1] = acqExp.GetPoint('R.FA.Top').GetValues()[:,1]
AmrkrExp[:,2] = acqExp.GetPoint('R.FA.Top').GetValues()[:,2]
BmrkrExp = np.empty([nFramesExp,3])
BmrkrExp[:,0] = acqExp.GetPoint('R.FA.Left').GetValues()[:,0]
BmrkrExp[:,1] = acqExp.GetPoint('R.FA.Left').GetValues()[:,1]
BmrkrExp[:,2] = acqExp.GetPoint('R.FA.Left').GetValues()[:,2]
CmrkrExp = np.empty([nFramesExp,3])
CmrkrExp[:,0] = acqExp.GetPoint('R.PUA.Right').GetValues()[:,0]
CmrkrExp[:,1] = acqExp.GetPoint('R.PUA.Right').GetValues()[:,1]
CmrkrExp[:,2] = acqExp.GetPoint('R.PUA.Right').GetValues()[:,2]
#Filter marker data (if required)
if filtFreq != None:
#Get sampling frequency
fs = acq.GetPointFrequency()
#Create low pass digital filter
w = filtFreq / (fs / 2) #normalise filter frequency
b,a = signal.butter(filtFreq, w, 'low')
#Filter rotated marker data
AmrkrExp[:,0] = signal.filtfilt(b, a, AmrkrExp[:,0])
AmrkrExp[:,1] = signal.filtfilt(b, a, AmrkrExp[:,1])
AmrkrExp[:,2] = signal.filtfilt(b, a, AmrkrExp[:,2])
BmrkrExp[:,0] = signal.filtfilt(b, a, BmrkrExp[:,0])
BmrkrExp[:,1] = signal.filtfilt(b, a, BmrkrExp[:,1])
BmrkrExp[:,2] = signal.filtfilt(b, a, BmrkrExp[:,2])
CmrkrExp[:,0] = signal.filtfilt(b, a, CmrkrExp[:,0])
CmrkrExp[:,1] = signal.filtfilt(b, a, CmrkrExp[:,1])
CmrkrExp[:,2] = signal.filtfilt(b, a, CmrkrExp[:,2])
#Use the experimental marker data and calculated distances to solve the distance
#equations for the XYZ positions of the shoulder joint centre in the experimental
#trial. For the first frame, we'll use a marker that is close to the propose
#joint centre (i.e. R.PUA.Left)
#Define the initial guess for shoulder joint centre on the first frame
initialGuess = [acqExp.GetPoint('R.PUA.Left').GetValues()[0,0],
acqExp.GetPoint('R.PUA.Left').GetValues()[0,1],
acqExp.GetPoint('R.PUA.Left').GetValues()[0,2]]
#Initialise empty array for SJC experimental data
EJCmrkrExp = np.empty([nFramesExp,3])
#Loop through experimental trial frames and calculate SJC position
for frameNo in range(0,nFramesExp):
#Define the marker positions for below solver equations
xA = AmrkrExp[frameNo,0]; yA = AmrkrExp[frameNo,1]; zA = AmrkrExp[frameNo,2];
xB = BmrkrExp[frameNo,0]; yB = BmrkrExp[frameNo,1]; zB = BmrkrExp[frameNo,2];
xC = CmrkrExp[frameNo,0]; yC = CmrkrExp[frameNo,1]; zC = CmrkrExp[frameNo,2];
#Define a function that expresses the equations
def f(p):
x,y,z = p
#Define functions
fA = ((x - xA)**2 + (y - yA)**2 + (z - zA)**2) - distA_avg**2
fB = ((x - xB)**2 + (y - yB)**2 + (z - zB)**2) - distB_avg**2
fC = ((x - xC)**2 + (y - yC)**2 + (z - zC)**2) - distC_avg**2
return [fA,fB,fC]
#Test function with values
#print(f([30,40,50]))
#Check whether to update initial guess
if frameNo != 0:
#Update initial guess
initialGuess = [EJCmrkrExp[frameNo-1,0],EJCmrkrExp[frameNo-1,1],EJCmrkrExp[frameNo-1,2]]
#Solve equation for current marker position
EJCmrkrExp[frameNo,0],EJCmrkrExp[frameNo,1],EJCmrkrExp[frameNo,2] = fsolve(f,initialGuess)
#Cleanup
#del(xA,xB,xC,yA,yB,yC,zA,zB,zC)
#Add new marker back into c3d data as R.EJC
#Create new empty point
newPoint2 = btk.btkPoint(acqExp.GetPointFrameNumber())
#Set label on new point
newPoint2.SetLabel('R.EJC')
#Set values for new marker from those calculated
newPoint2.SetValues(EJCmrkrExp)
#Append new point to acquisition object
acqExp.AppendPoint(newPoint2)
#Write new c3d file
writerExp = btk.btkAcquisitionFileWriter()
writerExp.SetInput(acqExp)
writerExp.SetFilename(trialC3D[0:-4] + '_withJointCentres.c3d')
writerExp.Update()
return('Functional joint centres added')
RFootProgressAngles[:, index[9]] = value
index[9] = index[9] + 1
if "RFootProgressAnglesZ" in line:
temp = line.split('=')
temp = temp[1].split('\n')
value = temp[0].split(',')
RFootProgressAngles[:, index[9]] = value
index[9] = index[9] + 1
if NbLeftFiles < 1 and NbRightFiles < 1:
newAcq = btk.btkAcquisition()
newAcq.Init(0, 101, 0, 1)
newAcq.SetPointFrequency(100)
for num in range(int(sys.argv[1])):
label = "LPelvisAngles" + str(num + 1)
newpoint = btk.btkPoint(label, newAcq.GetPointFrameNumber())
newpoint.SetValues(LPelvisAngles[:, range(num * 3, num * 3 + 2 + 1)])
newpoint.SetType(btk.btkPoint.Angle)
newAcq.AppendPoint(newpoint)
label = "LHipAngles" + str(num + 1)
newpoint = btk.btkPoint(label, newAcq.GetPointFrameNumber())
newpoint.SetValues(LHipAngles[:, range(num * 3, num * 3 + 2 + 1)])
newpoint.SetType(btk.btkPoint.Angle)
newAcq.AppendPoint(newpoint)
label = "LKneeAngles" + str(num + 1)
newpoint = btk.btkPoint(label, newAcq.GetPointFrameNumber())
newpoint.SetValues(LKneeAngles[:, range(num * 3, num * 3 + 2 + 1)])
newpoint.SetType(btk.btkPoint.Angle)
newAcq.AppendPoint(newpoint)