def convert_and_write(from_c3d_filename, to_c3d_filename):
# Makes reader from filename
reader = btk.btkAcquisitionFileReader()
reader.SetFilename(from_c3d_filename)
reader.Update()
acq=reader.GetOutput()
points=acq.GetPoints()
n_points=points.GetItemNumber()
print '\n \n Updating values in c3d object'
n=0
for f in xrange(acq.GetPointFrameNumber()):
for i in xrange(n_points):
point=acq.GetPoint(i)
data=point.GetValues()[f,:]
point.SetDataSlice(f, *[1000*x for x in data])
if not f%5000:
n=n+1
print '\n So far updated {} frames'.format(n*5000)
writer = btk.btkAcquisitionFileWriter()
writer.SetInput(acq)
writer.SetFilename(to_c3d_filename)
writer.Update()
print '\n Converted c3d file from: \n {}'.format(from_c3d_filename)
print '\n Wrote converted c3d file to: \n {}'.format(to_c3d_filename)
def write_acqusition(acq, c3d_file, postfix=''):
""" Writes an acqusition back to a C3D file """
writer = btk.btkAcquisitionFileWriter()
writer.SetInput(acq)
c3dfile = postfix.join(os.path.splitext(c3d_file))
writer.SetFilename(c3dfile)
writer.Update()
return c3dfile
def smartWriter(acq, filename):
"""Function to write a c3d file with BTK.
:parameters:
`acq` : (btkAcquisition) a btk Acquisition instance
`filename` : path and filename of the c3d
:return:
None
"""
writer = btk.btkAcquisitionFileWriter()
writer.SetInput(acq)
writer.SetFilename(str(filename))
writer.Update()
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 save_record_c3d(acq, file_name):
"""Writes the data of an acquisition object to a c3d file.
Args:
acq: Data acquisition object
file_name(str): Name of the c3d file
Note:
The file name extension should be .c3d
"""
writer = btk.btkAcquisitionFileWriter()
writer.SetInput(acq)
writer.SetFilename(file_name)
writer.Update()
def save_record_c3d(acq, file_name):
"""Writes the data of an acquisition object to a c3d file.
Args:
acq: Data acquisition object
file_name(str): Name of the c3d file
Note:
The file name extension should be .c3d
"""
writer = btk.btkAcquisitionFileWriter()
writer.SetInput(acq)
writer.SetFilename(file_name)
writer.Update()
def process(filename_in, filename_out, sacr=True, debug=True):
filename_in = filename_in.encode()
filename_out = filename_out.encode()
curves, markers, kin, first_frame = extract_kinematics('L',
filename_in,
sacr=sacr)
events = {}
events[("Foot Strike", "Left")] = ml_method(curves, modelHS, first_frame,
filename_out)
events[("Foot Off", "Left")] = ml_method(curves, modelFO, first_frame,
filename_out)
if debug:
plt.show()
reader = btk.btkAcquisitionFileReader()
reader.SetFilename(filename_in)
reader.Update()
acq = reader.GetOutput()
acq.ClearEvents()
print("First_frame: " + str(first_frame))
for k, v in events.items():
for frame in v:
fps = 100.0
event = btk.btkEvent()
event.SetLabel(k[0])
event.SetContext(k[1])
event.SetId(2 - (k[0] == "Foot Strike"))
event.SetFrame(int(frame))
event.SetTime(frame / fps)
acq.AppendEvent(event)
writer = btk.btkAcquisitionFileWriter()
writer.SetInput(acq)
writer.SetFilename(filename_out)
writer.Update()
return
def process(filename_in, filename_out):
idxL = [(i / 3) * 3 + i for i in range(30)]
idxR = [3 + (i / 3) * 3 + i for i in range(30)]
inputs = extract_kinematics('L', filename_in)
inputsL = inputs[:, idxL]
inputsR = inputs[:, idxR]
XL, YL = convert_data(inputsL)
XR, YR = convert_data(inputsR)
events = {}
events[("Foot Strike", "Left")] = neural_method(XR, modelFO)
events[("Foot Strike", "Right")] = neural_method(XL, modelFO)
events[("Foot Off", "Left")] = neural_method(XR, modelHS)
events[("Foot Off", "Right")] = neural_method(XL, modelHS)
reader = btk.btkAcquisitionFileReader()
reader.SetFilename(filename_in)
reader.Update()
acq = reader.GetOutput()
first_frame = acq.GetFirstFrame()
acq.ClearEvents()
print(first_frame)
for k, v in events.items():
for frame in v:
frame_true = first_frame + frame
fps = 120.0
event = btk.btkEvent()
event.SetLabel(k[0])
event.SetContext(k[1])
event.SetId(2 - (k[0] == "Foot Strike"))
event.SetFrame(np.round(frame_true))
event.SetTime(frame_true / fps)
acq.AppendEvent(event)
writer = btk.btkAcquisitionFileWriter()
writer.SetInput(acq)
writer.SetFilename(filename_out)
writer.Update()
return
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 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 compute(FilenameIn, FilenameOut):
json_file = open('deepevent/data/DeepEventModel.json', 'r')
loaded_model_json = json_file.read()
json_file.close()
load_model = model_from_json(loaded_model_json)
load_model.load_weights("deepevent/data/DeepEventWeight.h5")
#Read the FilenameIn.c3d
reader = btk.btkAcquisitionFileReader()
reader.SetFilename(FilenameIn)
reader.Update()
acq = reader.GetOutput()
md = acq.GetMetaData()
SubjectInfo = md.FindChild("SUBJECTS").value().FindChild(
"NAMES").value().GetInfo()
SubjectValue = SubjectInfo.ToString()
markers = ["LANK", "RANK", "LTOE", "RTOE", "LHEE", "RHEE"]
nframes = 1536
nb_data_in = 36 #6 markers x 3
# Filter and derivate the input
inputs = np.zeros((1, nframes, nb_data_in))
for k in range(6):
inputs[0, 0:acq.GetPointFrameNumber(), k * 3:(k + 1) * 3] = filter(
acq,
acq.GetPoint(markers[k]).GetValues(), 6)
inputs[0, 0:acq.GetPointFrameNumber(),
3 * len(markers) + k * 3:3 * len(markers) +
(k + 1) * 3] = derive_centre(acq, inputs[0, :,
k * 3:(k + 1) * 3])
# Prediction with the model
predicted = load_model.predict(
inputs
) #shape[1,nb_frames,5] 0: no event, 1: Left Foot Strike, 2: Right Foot Strike, 3:Left Toe Off, 4: Right Toe Off
#Threshold to set the gait events
predicted_seuil = predicted
for j in range(nframes):
if predicted[0, j, 1] <= 0.01:
predicted_seuil[0, j, 1] = 0
if predicted[0, j, 2] <= 0.01:
predicted_seuil[0, j, 2] = 0
if predicted[0, j, 3] <= 0.01:
predicted_seuil[0, j, 3] = 0
if predicted[0, j, 4] <= 0.01:
predicted_seuil[0, j, 4] = 0
predicted_seuil_max = np.zeros((1, nframes, 5))
for j in range(1, 5):
predicted_seuil_max[0,
argrelextrema(predicted_seuil[0, :,
j], np.greater)[0],
j] = 1
for j in range(nframes):
if np.sum(predicted_seuil_max[0, j, :]) == 0:
predicted_seuil_max[0, j, 0] = 1
eventLFS = np.argwhere(predicted_seuil_max[0, :, 1])
for ind_indice in range(eventLFS.shape[0]):
newEvent = btk.btkEvent()
newEvent.SetLabel("Foot Strike")
newEvent.SetContext("Left")
newEvent.SetTime((acq.GetFirstFrame() - 1) / acq.GetPointFrequency() +
float(eventLFS[ind_indice] / acq.GetPointFrequency()))
newEvent.SetSubject(SubjectValue[0])
newEvent.SetId(1)
acq.AppendEvent(newEvent)
eventRFS = np.argwhere(predicted_seuil_max[0, :, 2])
for ind_indice in range(eventRFS.shape[0]):
newEvent = btk.btkEvent()
newEvent.SetLabel("Foot Strike")
newEvent.SetContext("Right")
newEvent.SetTime((acq.GetFirstFrame() - 1) / acq.GetPointFrequency() +
float(eventRFS[ind_indice] / acq.GetPointFrequency()))
newEvent.SetSubject(SubjectValue[0])
newEvent.SetId(1)
acq.AppendEvent(newEvent)
eventLFO = np.argwhere(predicted_seuil_max[0, :, 3])
for ind_indice in range(eventLFO.shape[0]):
newEvent = btk.btkEvent()
newEvent.SetLabel("Foot Off")
newEvent.SetContext("Left") #
newEvent.SetTime((acq.GetFirstFrame() - 1) / acq.GetPointFrequency() +
float(eventLFO[ind_indice] / acq.GetPointFrequency()))
newEvent.SetSubject(SubjectValue[0])
newEvent.SetId(2)
acq.AppendEvent(newEvent)
eventRFO = np.argwhere(predicted_seuil_max[0, :, 4])
for ind_indice in range(eventRFO.shape[0]):
newEvent = btk.btkEvent()
newEvent.SetLabel("Foot Off")
newEvent.SetContext("Right") #
newEvent.SetTime((acq.GetFirstFrame() - 1) / acq.GetPointFrequency() +
float(eventRFO[ind_indice] / acq.GetPointFrequency()))
newEvent.SetSubject(SubjectValue[0])
newEvent.SetId(2)
acq.AppendEvent(newEvent)
#Write the FilenameOut.c3d
writer = btk.btkAcquisitionFileWriter()
writer.SetInput(acq)
writer.SetFilename(FilenameOut)
writer.Update()
def addEvents(trialC3D):
#Read in the C3D file
readerC3D = btk.btkAcquisitionFileReader()
#Load in experimental c3d trial
readerC3D.SetFilename(trialC3D)
#Update reader
readerC3D.Update()
#Get the btk acquisition object
acqC3D = readerC3D.GetOutput()
if 'UpwardReach' in trialC3D:
#Use the horizontal velocity of the hand marker to see where it starts
#moving forward and backwards (X-axis) to identify movement segments
#Get the X-axis hand marker data
xHand = acqC3D.GetPoint('R.HAND').GetValues()[:,0]
#Calculate the difference between subsequent marker positions
xHandDiff = np.diff(xHand)
#Find the first element with a negative value (skipping over the first
#few frames to avoid any movements at the beginning). This will indicate
#the index where the weight has been picked off the shelf. This also
#uses a threshold of 1 to avoid any small movements
liftOff1 = np.where(xHandDiff[100:-1] < -1)[0][0] + 100
#The next point where positive data occurs will be the reach to put the
#weight back on the shelf
putBack1 = np.where(xHandDiff[liftOff1:-1] > 1)[0][0] + liftOff1
#The next negative point after this will be the point where the weight
#has been placed back on the shelf
liftOff2 = np.where(xHandDiff[putBack1:-1] < -1)[0][0] + putBack1
#Add the events back into the C3D file with relevant labels
#Get the start frame to add to the identified row index
firstFrame = acqC3D.GetFirstFrame()
#Get sampling rate to calculate time
fs = acqC3D.GetPointFrequency()
#Concentric start point
conStart = btk.btkEvent();
conStart.SetFrame(int(putBack1) + firstFrame-1); conStart.SetLabel('conStart')
conStart.SetTime((putBack1 + firstFrame-1) * (1/fs))
acqC3D.AppendEvent(conStart)
#Concentric end point
conEnd = btk.btkEvent();
conEnd.SetFrame(int(liftOff2) + firstFrame-1); conEnd.SetLabel('conEnd')
conEnd.SetTime((liftOff2 + firstFrame-1) * (1/fs))
acqC3D.AppendEvent(conEnd)
#Eccentric start point
eccStart = btk.btkEvent();
eccStart.SetFrame(int(liftOff1) + firstFrame-1); eccStart.SetLabel('eccStart')
eccStart.SetTime((liftOff1 + firstFrame-1) * (1/fs))
acqC3D.AppendEvent(eccStart)
#Eccentric end point
eccEnd = btk.btkEvent();
eccEnd.SetFrame(int(putBack1-1) + firstFrame-1); eccEnd.SetLabel('eccEnd')
eccEnd.SetTime((putBack1-1 + firstFrame-1) * (1/fs))
acqC3D.AppendEvent(eccEnd)
#Update acquisition object
acqC3D.Update()
#Re-write C3D file
writerC3D = btk.btkAcquisitionFileWriter()
writerC3D.SetInput(acqC3D)
writerC3D.SetFilename(trialC3D)
writerC3D.Update()
return('Events added to C3D file')
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 save_c3d(acq, f_path):
if f_path is None: return None
writer = btk.btkAcquisitionFileWriter()
writer.SetInput(acq)
writer.SetFilename(f_path)
return writer.Update()
# print(force/scale)
plt.figure(figsize=(10, 4))
# plt.figure(num=1)
plt.plot(t, fz1, label="Fz1")
plt.plot(t, fz2, label="Fz2")
plt.plot(t, fz3, label="Fz3")
plt.legend()
plt.xlabel("Time [s]")
plt.ylabel("GRF vertical [N]")
# plt.figure(0)
plt.savefig("forceZaxis.png")
# create a modified c3d file
writer = btk.btkAcquisitionFileWriter()
writer.SetInput(clone)
writer.SetFilename(sys.argv[1] + ".c3d")
writer.Update()
# plot the modified c3d file
reader2 = btk.btkAcquisitionFileReader() # build a btk reader object
reader2.SetFilename(sys.argv[1] + ".c3d") # set a filename to the reader
acq2 = reader2.GetOutput() # btk aquisition object
acq2.Update() # Update ProcessObject associated with DataObject
ana = acq2.GetAnalog("Fz1")
fz1 = ana.GetValues() / ana.GetScale()
ana = acq2.GetAnalog("Fz2")
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')
label = "RAnkleAngles" + str(num + 1)
newpoint = btk.btkPoint(label, newAcq.GetPointFrameNumber())
newpoint.SetValues(RAnkleAngles[:, range(num * 3, num * 3 + 2 + 1)])
newpoint.SetType(btk.btkPoint.Angle)
newAcq.AppendPoint(newpoint)
label = "RFootProgressAngles" + str(num + 1)
newpoint = btk.btkPoint(label, newAcq.GetPointFrameNumber())
newpoint.SetValues(RFootProgressAngles[:,
range(num * 3, num * 3 + 2 +
1)])
newpoint.SetType(btk.btkPoint.Angle)
newAcq.AppendPoint(newpoint)
writer = btk.btkAcquisitionFileWriter()
writer.SetInput(newAcq)
writer.SetFilename("output.c3d")
writer.Update()
if NbLeftFiles >= 1:
for L in range(NbLeftFiles):
newAcq = btk.btkAcquisition()
newAcq.Init(0, 101, 0, 1)
newAcq.SetPointFrequency(100)
for num in range(L * NbStep, (L + 1) * NbStep):
label = "LPelvisAngles" + str(num + 1)
newpoint = btk.btkPoint(label, newAcq.GetPointFrameNumber())
newpoint.SetValues(LPelvisAngles[:,
range(num * 3, num * 3 + 2 + 1)])
