Application Examples
This page shows examples of how the qmt toolbox can be used to analyze inertial sensor data. You can find
the code files and the example data in the
examples/ folder of the repository.
Full Body Motion Tracking – Basic Example
This example shows how the qmt framework can be used to perform offline full body 6D motion tracking and animate a 3D avatar with just a few lines of code. Sensor-to-segment orientations and an initial heading offset are determined by manual tuning – which can easily be done in the webapp.
examples/full_body_tracking_basic_example.py
#!/usr/bin/env python3
# SPDX-FileCopyrightText: 2021 Bo Yang <b.yang@campus.tu-berlin.de>
# SPDX-FileCopyrightText: 2021 Daniel Laidig <laidig@control.tu-berlin.de>
#
# SPDX-License-Identifier: MIT
"""
Offline full body motion tracking example.
This example shows how the qmt framework can be used to perform offline full body 6D motion tracking and animate a 3D
avatar with just a few lines of code. This script is kept as short and simple as possible. For a more complex example,
see the file full_body_tracking_advanced_example.py.
"""
import qmt
import numpy as np
# manually tuned sensor-to-segment orientations and heading offsets
# (use the "copy config changes" button in the boxmodel webapp to obtain those values)
tunedParams = qmt.Struct({
'hip': {'heading_offset': 0.0, 'q_segment2sensor': [0.542, 0.542, 0.455, -0.455]},
'lower_back': {'heading_offset': -30.0, 'q_segment2sensor': [0.455, 0.455, 0.542, -0.542]},
'upper_back': {'heading_offset': 15.0, 'q_segment2sensor': [0.579, 0.579, 0.406, -0.406]},
'head': {'heading_offset': -45.0, 'q_segment2sensor': [0.406, 0.406, 0.579, -0.579]},
'upper_arm_left': {'heading_offset': -89.5, 'q_segment2sensor': [0.488, 0.354, 0.449, -0.66]},
'forearm_left': {'heading_offset': -95.0, 'q_segment2sensor': [0.482, 0.517, 0.517, -0.482]},
'hand_left': {'heading_offset': -85.0, 'q_segment2sensor': [0.5, 0.5, 0.5, -0.5]},
'upper_arm_right': {'heading_offset': 60.0, 'q_segment2sensor': [0.43, 0.467, 0.617, -0.465]},
'forearm_right': {'heading_offset': 80.0, 'q_segment2sensor': [0.542, 0.455, 0.455, -0.542]},
'hand_right': {'heading_offset': 54.0, 'q_segment2sensor': [0.46, 0.478, 0.622, -0.416]},
'upper_leg_left': {'heading_offset': -140.0, 'q_segment2sensor': [0.172, -0.73, -0.661, 0.014]},
'lower_leg_left': {'heading_offset': 105.0, 'q_segment2sensor': [0.641, -0.299, -0.242, -0.664]},
'foot_left': {'heading_offset': -150.0, 'q_segment2sensor': [-0.205, 0.158, 0.766, 0.588]},
'upper_leg_right': {'heading_offset': 80.0, 'q_segment2sensor': [0.703, -0.182, 0.048, -0.686]},
'lower_leg_right': {'heading_offset': -155.0, 'q_segment2sensor': [0.282, -0.681, -0.624, -0.259]},
'foot_right': {'heading_offset': -150.0, 'q_segment2sensor': [-0.158, 0.205, 0.588, 0.766]},
})
tunedParams.createArrays() # convert all q_segment2sensor quaternions from lists to numpy arrays
if __name__ == '__main__':
# load the raw IMU data
data = qmt.Struct.load('full_body_example_data.mat')
# the data only contains a time vector. calculate the sampling time from it.
timediff = np.diff(data['t'])
Ts = timediff[0]
assert np.allclose(timediff, Ts) # make sure the sampling time is constant to avoid surprises
# list of segment names that IMUs are attached to
segments = [k for k in data.keys() if k != 't']
# run orientation estimation for each IMU
params = {'Ts': Ts, 'tauAcc': 1, 'zeta': 0, 'accRating': 1}
for name in segments:
quat = qmt.oriEstIMU(gyr=data[f'{name}.gyr'], acc=data[f'{name}.acc'], params=params)
data[f'{name}.quat_imu'] = quat
# apply those quaternions to each segment
for name in segments:
qSeg2Imu = qmt.normalized(tunedParams.get(f'{name}.q_segment2sensor', [1, 0, 0, 0]))
data[f'{name}.quat_seg'] = qmt.qmult(data[f'{name}.quat_imu'], qSeg2Imu)
# correct heading for each segment
for name in segments:
deltaQuat = qmt.deltaQuat(np.deg2rad(tunedParams.get(f'{name}.heading_offset', 0)))
data[f'{name}.quat_seg_adjusted'] = qmt.qmult(deltaQuat, data[f'{name}.quat_seg'])
# create config for visualization
data['config'] = {
'base': 'human',
'segments': {
name: {
'signal': f'{name}.quat_seg_adjusted',
'heading_offset': tunedParams.get(f'{name}.heading_offset', 0),
'q_segment2sensor': tunedParams.get(f'{name}.q_segment2sensor', [1, 0, 0, 0]),
'imubox_cs': 'FLU', # the x-axis of the IMU is pointing forward, the y-axis to the left
} for name in segments
},
'debug_mode': True,
}
# save the processed data (including the config) to a mat file
data.save('example_output/full_body_results.mat', makedirs=True)
# visualize the motion with a box model
webapp = qmt.Webapp('/view/boxmodel', data=data, quiet=True)
webapp.run()
Full Body Motion Tracking – Advanced Example
This example builds upon the previous example and replaces the manual tuning with constraint-based methods for magnetometer-free motion tracking.
examples/full_body_tracking_advanced_example.py
#!/usr/bin/env python3
# SPDX-FileCopyrightText: 2021 Bo Yang <b.yang@campus.tu-berlin.de>
# SPDX-FileCopyrightText: 2021 Daniel Laidig <laidig@control.tu-berlin.de>
#
# SPDX-License-Identifier: MIT
"""
Offline full body motion tracking example.
This example performs offline full body 6D motion tracking using more advanced algorithms. For a simpler example,
see the file full_body_tracking_basic_example.py.
"""
import qmt
import numpy as np
# all body segments with the respective parents
parents = {
'hip': None,
'lower_back': 'hip',
'upper_back': 'lower_back',
'head': 'upper_back',
'upper_arm_left': 'upper_back',
'forearm_left': 'upper_arm_left',
'hand_left': 'forearm_left',
'upper_arm_right': 'upper_back',
'forearm_right': 'upper_arm_right',
'hand_right': 'forearm_right',
'upper_leg_left': 'hip',
'lower_leg_left': 'upper_leg_left',
'foot_left': 'lower_leg_left',
'upper_leg_right': 'hip',
'lower_leg_right': 'upper_leg_right',
'foot_right': 'lower_leg_right',
}
# time markers to show in the playback timeline
markers = [
dict(pos=40, end=44, name='standing', col='C2'),
dict(pos=52, end=79, name='sitting', col='C2'),
dict(pos=95, end=115, name='walking', col='C0'),
dict(pos=115, name='turn', col='C3'),
dict(pos=116, end=138.5, name='walking (2)', col='C0'),
dict(pos=138.5, name='turn (2)', col='C3'),
]
# settings for qmt.resetAlignment
resetAlignmentSettings = {
'hip': dict(x=[0, 0, -1], xCs=0, y=[0, 0, 1], yCs=-1, exactAxis='y'),
'lower_back': dict(x=[0, 0, -1], xCs=0, y=[0, 0, 1], yCs=-1, exactAxis='y'),
'upper_back': dict(x=[0, 0, -1], xCs=0, y=[0, 0, 1], yCs=-1, exactAxis='y'),
'head': dict(x=[0, 0, -1], xCs=0, y=[0, 0, 1], yCs=-1, exactAxis='y'),
'upper_arm_left': dict(x=[0, -1, -1], xCs=0, y=[0, 0, 1], yCs=-1, exactAxis='y'),
'upper_arm_right': dict(x=[0, 1, -1], xCs=0, y=[0, 0, 1], yCs=-1, exactAxis='y'),
'forearm_left': dict(x=[0, 0, -1], xCs=0, y=[0, 0, 1], yCs=-1),
'forearm_right': dict(x=[0, 0, -1], xCs=0, y=[0, 0, 1], yCs=-1),
'hand_left': dict(x=[0, 0, -1], xCs=0, y=[0, 0, 1], yCs=-1),
'hand_right': dict(x=[0, 0, -1], xCs=0, y=[0, 0, 1], yCs=-1),
}
# settings for qmt.jointAxisEstHingeOlsson, for the joint between the given segment and the respective parent
jointEstSettings = {
'foot_left': dict(wa=100, wg=0.1, useSampleSelection=True, angRateEnergyThreshold=51, winSize=41, dataSize=7500,
tol=1e-8, flip=False, quiet=True),
'lower_leg_left': dict(wa=10, wg=3, useSampleSelection=True, angRateEnergyThreshold=51, winSize=41, dataSize=7500,
tol=1e-8, flip=True, applyToParent=True, flipParent=True, quiet=True),
'foot_right': dict(wa=100, wg=0.1, useSampleSelection=True, angRateEnergyThreshold=51, winSize=41, dataSize=7500,
tol=1e-8, flip=False, quiet=True),
'lower_leg_right': dict(wa=10, wg=3, useSampleSelection=True, angRateEnergyThreshold=51, winSize=41, dataSize=7500,
tol=1e-8, flip=True, applyToParent=True, flipParent=True, quiet=True)
}
# settings for qmt.resetHeading, for the joint between the given segment and the respective parent
resetHeadingSettings = {
'upper_arm_left': dict(deltaOffset=np.deg2rad(-90)),
'upper_arm_right': dict(deltaOffset=np.deg2rad(90)),
}
# settings for qmt.headingCorrection, for the joint between the given segment and the respective parent
deltaCorrectionSettings = {
'hip': dict(),
'lower_back': dict(joint=[0, 0, 1]),
'upper_back': dict(joint=[0, 0, 1]),
'upper_leg_left': dict(joint=[0, 0, 1]),
'lower_leg_left': dict(joint=[0, 0, 1]),
'foot_left': dict(joint=[0, 0, 1], est_settings={'startRating': 0.5, 'stillnessRating': 0.5}),
'upper_leg_right': dict(joint=[0, 0, 1]),
'lower_leg_right': dict(joint=[0, 0, 1]),
'foot_right': dict(joint=[0, 0, 1]),
'head': dict(
joint=[[1, 0, 0], [0, 0, 1]],
est_settings=dict(angleRanges=np.deg2rad(np.array([[-80, 80], [-70, 70], [-30, 80]], float)),
convention='yxz', useRomConstraints=True)
),
'upper_arm_left': dict(
joint=[[1, 0, 0], [0, 1, 0], [0, 0, 1]],
joint_info=dict(angle_ranges=np.deg2rad(np.array([[-5, 40], [-90, -70], [-10, 40]], float)), convention='xyz')
),
'forearm_left': dict(
joint=[[1, 0, 0], [0, 1, 0]],
est_settings={'ratingMin': 0.1, 'startRating': 0, 'stillnessRating': 0}
),
'hand_left': dict(
joint=[[0, 0, 1], [0, 1, 0]],
est_settings={'ratingMin': 0.1, 'startRating': 0.3, 'stillnessRating': 0.3}
),
'hand_right': dict(
joint=[[0, 0, 1], [0, 1, 0]],
est_settings={'ratingMin': 0.1, 'startRating': 0.3, 'stillnessRating': 0.3}
),
'forearm_right': dict(
joint=[[1, 0, 0], [0, 1, 0]],
est_settings={'ratingMin': 0.1, 'startRating': 0, 'stillnessRating': 0}
),
'upper_arm_right': dict(
joint=[[1, 0, 0], [0, 1, 0], [0, 0, 1]],
joint_info=dict(angle_ranges=np.deg2rad(np.array([[-30, 0], [80, 105], [-10, 40]], float)), convention='xyz')
),
}
def mergeSettings():
return {
name: {
'resetAlignmentSettings': resetAlignmentSettings.get(name, {}),
'jointEstSettings': jointEstSettings.get(name, {}),
'resetHeadingSettings': resetHeadingSettings.get(name, {}),
'deltaCorrectionSettings': deltaCorrectionSettings.get(name, {}),
} for name, parent in parents.items()
}
def estimateOrientations(data, settings, plot=False):
timediff = np.diff(data['t']) # the data only contains a time vector. calculate the sampling rate from it
assert np.allclose(timediff, timediff[0]) # make sure the sampling time is constant to avoid surprises
# run orientation estimation for each IMU
defaults = {'Ts': timediff[0], 'tauAcc': 1, 'zeta': 0, 'accRating': 1}
for name in settings:
qmt.setupDebugPlots(title=name) # show the segment name in the plot title
params = qmt.setDefaults(settings[name].get('oriEst', {}), defaults)
quat = qmt.oriEstIMU(gyr=data[f'{name}.gyr'], acc=data[f'{name}.acc'], params=params, plot=plot)
quat[0:100] = quat[100] # remove initial convergence phase
data[f'{name}.quat_imu'] = quat
def resetAlignment(data, settings, plot=False):
reset = np.zeros(data['t'].shape)
reset[0] = 1
for name in settings:
params = settings[name].get('resetAlignmentSettings', {})
if np.allclose(params.get('x', [0, 0, 0]), 0) and np.allclose(params.get('y', [0, 0, 0]), 0) and \
np.allclose(params.get('z', [0, 0, 0]), 0):
continue
qmt.setupDebugPlots(title=name)
q_out = qmt.resetAlignment(data[f'{name}.quat_imu'][None], reset, **params, plot=plot)
data[f'{name}.quat_seg'] = q_out[0]
data[f'{name}.q_segment2sensor'] = qmt.qrel(data[f'{name}.quat_imu'][0], data[f'{name}.quat_seg'][0])
def estimateJointAxes(data, settings, plot=False):
defaults = {'tol': 1e-8, 'quiet': True}
for name, parent in parents.items():
estSettings = qmt.setDefaults(settings[name].get('jointEstSettings', {}), defaults, check=False)
if 'wa' not in estSettings:
continue
qmt.setupDebugPlots(title=f'{parent}/{name}')
jhat1, jhat2 = qmt.jointAxisEstHingeOlsson(data[f'{parent}.acc'], data[f'{name}.acc'], data[f'{parent}.gyr'],
data[f'{name}.gyr'], estSettings=estSettings, plot=plot)
flip = estSettings.get('flip')
flipParent = estSettings.get('flipParent')
for segment, jhat, flip in ((parent, jhat1, flipParent), (name, jhat2, flip)):
if segment == parent and not estSettings.get('applyToParent'):
continue
jhat = -jhat.squeeze() if flip else jhat.squeeze()
qBS = qmt.quatFrom2Axes(z=jhat, y=qmt.normalized(data[f'{segment}.acc'][100, :]), exactAxis='y')
data[f'{segment}.quat_seg'] = qmt.qmult(data[f'{segment}.quat_imu'], qBS)
data[f'{segment}.q_segment2sensor'] = qBS
def resetHeading(data, settings, plot=False):
reset = np.zeros(data['t'].shape)
reset[0] = 1
for name, parent in parents.items():
if parent is None:
data[f'{name}.quat_seg_resetHeading'] = data[f'{name}.quat_seg']
continue
q = [data[f'{parent}.quat_seg_resetHeading'], data[f'{name}.quat_seg']]
deltaOffset = settings[name].get('resetHeadingSettings', {}).get('deltaOffset', 0)
qmt.setupDebugPlots(title=f'{parent}/{name}')
out = qmt.resetHeading(q, reset, base=0, deltaOffset=deltaOffset, plot=plot)
data[f'{name}.quat_seg_resetHeading'] = out[1]
def headingCorrection(data, settings, plot=False, saveDebug=True):
delta = {}
t = data['t']
for name, parent in parents.items():
if parent is None:
delta[name] = np.zeros(t.shape[0])
continue
gyr1 = data[f'{parent}.gyr']
gyr2 = data[f'{name}.gyr']
quat1 = data[f'{parent}.quat_seg']
quat2 = data[f'{name}.quat_seg']
params = settings[name]['deltaCorrectionSettings']
joint = params['joint']
joint_info = params.get('joint_info', {})
est_settings = params.get('est_settings', {})
print(f'headingCorrection: {parent}/{name}')
qmt.setupDebugPlots(title=f'{parent}/{name}')
out = qmt.headingCorrection(gyr1, gyr2, quat1, quat2, t, joint, joint_info, est_settings, plot=plot)
delta_filt = out[2]
if saveDebug:
data[f'{name}.delta'] = out[1]
data[f'{name}.delta_filt'] = out[2]
data[f'{name}.rating'] = out[3]
delta[name] = delta[parent] + delta_filt
for name in parents:
deltaQuat = qmt.deltaQuat(delta[name])
data[f'{name}.quat_seg_deltaCorr'] = qmt.qmult(deltaQuat, data[f'{name}.quat_seg'])
def main():
# disable/enable the creation of debug plots
plot = False
# plot = True
# disable/enable continuous constraint-based heading correction (if False, headingReset is used)
useHeadingCorrection = False
# useHeadingCorrection = True
if plot:
# ensure that the webapp viewer is initialized before any plots are created
qmt.Webapp.initialize()
# setup debug plots: save to one multipage pdf and increase the size
qmt.setupDebugPlots(mode='pdfpages', filename='example_output/full_body_debug_plots.pdf', figsize_cm=(29.7, 21))
data = qmt.Struct.load('full_body_example_data.mat')
settings = mergeSettings()
estimateOrientations(data, settings, plot)
resetAlignment(data, settings, plot)
estimateJointAxes(data, settings, plot)
if not useHeadingCorrection:
resetHeading(data, settings, plot)
else:
headingCorrection(data, settings, plot)
qmt.setupDebugPlots(mode='show') # close the PDF file before opening the visualization
quatSignal = 'quat_seg_deltaCorr' if useHeadingCorrection else 'quat_seg_resetHeading'
data['config'] = {
'base': 'human',
'segments': {
name: {
'signal': f'{name}.{quatSignal}',
'q_segment2sensor': data[f'{name}.q_segment2sensor'],
'imubox_cs': 'FLU', # the x-axis of the IMU is pointing forward, the y-axis to the left
} for name in settings
},
'debug_mode': True,
'markers': markers,
}
data.save('example_output/full_body_results_advanced.mat', makedirs=True)
webapp = qmt.Webapp('/view/boxmodel', data=data, quiet=True)
webapp.run()
if __name__ == '__main__':
main()
Full Body Motion Tracking Demo
The full body tracking demo shows how a custom webapp can be used to realize an interactive demo application. The Python code performs full body 6D motion tracking (as in the two previous examples). The custom webapp allows the user to change parameters. Those parameters are then sent to the Python code, which re-processes the data and sends the resulting orientation back to the webapp for 3D visualization.