def setUp(self):
"""
There are 3 trajectories of 4 frames each, each going along a separate
axis.
"""
self.tmpl = "tests/testing_fodder/ptvis/ptv_is.%d"
self.first = 10001
self.last = 10004
# ptv_is files are in [mm], [mm/s], etc.
correct_pos = np.r_[0.1, 0.2, 0.3, 0.5] / 1000.
correct_vel = np.r_[0.1, 0.1, 0.2, 0.] / 1000.
correct_accel = np.r_[0., 0.1, 0., 0.] / 1000.
t = np.r_[1:5] + 10000
self.correct = []
for axis in [0, 1, 2]:
pos = np.zeros((4, 3))
pos[:, axis] = correct_pos
vel = np.zeros((4, 3))
vel[:, axis] = correct_vel
accel = np.zeros((4, 3))
accel[:, axis] = correct_accel
self.correct.append(
Trajectory(pos, vel, t, len(self.correct), accel=accel))
def savitzky_golay(trajs, fps, window_size, order):
r"""Smooth (and optionally differentiate) data with a Savitzky-Golay filter.
The Savitzky-Golay filter removes high frequency noise from data.
It has the advantage of preserving the original shape and
features of the signal better than other types of filtering
approaches, such as moving averages techniques.
Parameters:
trajs - a list of Trajectory objects
window_size - int,
the length of the window. Must be an odd integer number.
fps - frames per second, used for calculating velocity and acceleration.
order - int,
the order of the polynomial used in the filtering.
Must be less then `window_size` - 1.
Returns:
new_trajs - a list of Trajectory objects representing the smoothed
trajectories. Trajectories shorter than the window size are discarded.
Notes:
The Savitzky-Golay is a type of low-pass filter, particularly
suited for smoothing noisy data. The main idea behind this
approach is to make for each point a least-square fit with a
polynomial of high order over a odd-sized window centered at
the point.
References:
.. [#] A. Savitzky, M. J. E. Golay, Smoothing and Differentiation of \
Data by Simplified Least Squares Procedures. Analytical \
Chemistry, 1964, 36 (8), pp 1627-1639.
.. [#] Numerical Recipes 3rd Edition: The Art of Scientific Computing \
W.H. Press, S.A. Teukolsky, W.T. Vetterling, B.P. Flannery \
Cambridge University Press ISBN-13: 9780521880688
.. [#] http://wiki.scipy.org/Cookbook/SavitzkyGolay
"""
try:
window_size = np.abs(np.int(window_size))
order = np.abs(np.int(order))
except ValueError:
raise ValueError("window_size and order have to be of type int")
if window_size % 2 != 1 or window_size < 1:
raise TypeError("window_size size must be a positive odd number")
if window_size < order + 2:
raise TypeError("window_size is too small for the polynomials order")
order_range = range(order + 1)
half_window = (window_size - 1) // 2
# Properties that should not be copied from the old trajectory because
# they are obtained otherwise (or copied elsewhere).
smoothed_keys = ['pos', 'velocity', 'accel', 'acc_pp', 'time', 'trajid']
# precompute coefficients
b = np.mat([[k**i for i in order_range]
for k in range(-half_window, half_window + 1)])
m = np.linalg.pinv(b).A
m_pos = m[0]
m_vel = m[1] * fps
m_acc = m[2] * (fps**2 * 2)
m_jerk = m[3] * (fps**3 * 6)
new_trajs = []
for traj in trajs:
if len(traj) < window_size:
continue
newpos = []
newvel = []
newacc = []
jerk = []
nextacc = []
nextvel = []
for y in traj.pos().T: # For each component of pos
# pad the signal at the extremes with
# values taken from the signal itself
firstvals = y[0] - np.abs(y[1:half_window + 1][::-1] - y[0])
lastvals = y[-1] + np.abs(y[-half_window - 1:-1][::-1] - y[-1])
y = np.concatenate((firstvals, y, lastvals))
newpos.append(np.convolve(m_pos[::-1], y, mode='valid'))
newvel.append(np.convolve(m_vel[::-1], y, mode='valid'))
newacc.append(np.convolve(m_acc[::-1], y, mode='valid'))
jerk.append(np.convolve(m_jerk[::-1], y, mode='valid'))
newpos = np.r_[newpos].T
newvel = np.r_[newvel].T
newacc = np.r_[newacc].T
jerk = np.r_[jerk].T
# Velocity and acceleration evaluated at i = 1 rather than i = 0,
# for comparison with the i = 0 values from next polynomial.
# Delta t treatment is in m_*.
# Assumed that the first point is trimmed, the zeros are just for
# alignment.
nextvel = np.vstack(
(np.zeros(3), newvel + newacc / fps + jerk / 2. / fps**2))[:-1]
nextacc = np.vstack((np.zeros(3), newacc + jerk / fps))[:-1]
newtraj = Trajectory(newpos,
newvel,
traj.time(),
traj.trajid(),
accel=newacc,
vel_pp=nextvel,
acc_pp=nextacc)
# Copy unsmoothed properties from old trajectory:
for k, v in traj.as_dict().iteritems():
if k not in smoothed_keys:
newtraj.create_property(k, v)
new_trajs.append(newtraj)
return new_trajs
# Weld the final best candidates.
out_trajects = []
used_trids = set() # don't repeat taken candidates as masters.
for trid, cand in list(links.items()):
if trid in used_trids:
continue
trj_weld = scn.trajectory_by_id(trid)
while cand[0] is not None:
used_trids.add(cand[0])
trj1 = trj_weld
trj2 = scn.trajectory_by_id(cand[0])
trj_weld = Trajectory(
np.vstack((trj1.pos(), trj2.pos())),
np.vstack((trj1.velocity(), trj2.velocity())),
trajid=trj1.trajid(),
time=np.hstack((trj1.time(), trj2.time())),
accel=np.vstack((trj1.accel(), trj2.accel())),
)
if cand[0] not in links:
break
cand = links[cand[0]]
out_trajects.append(trj_weld)
# Check wheter we link correctly the trajs
# plot the trajs
fig = plt.figure(figsize=(7, 7))
for trj in out_trajects:
pos = trj.pos()
def savitzky_golay(trajs, fps, window_size, order, deriv=0, rate=1):
r"""Smooth (and optionally differentiate) data with a Savitzky-Golay filter.
The Savitzky-Golay filter removes high frequency noise from data.
It has the advantage of preserving the original shape and
features of the signal better than other types of filtering
approaches, such as moving averages techniques.
Parameters
----------
trajs : a list of Trajectory objects
window_size : int
the length of the window. Must be an odd integer number.
fps : frames per second, used for calculating velocity and acceleration.
order : int
the order of the polynomial used in the filtering.
Must be less then `window_size` - 1.
deriv: int
the order of the derivative to compute (default = 0 means only smoothing)
Returns
-------
new_trajs : a list of Trajectory objects representing the smoothed
trajectories. Trajectories shorter than the window size are discarded.
Notes
-----
The Savitzky-Golay is a type of low-pass filter, particularly
suited for smoothing noisy data. The main idea behind this
approach is to make for each point a least-square fit with a
polynomial of high order over a odd-sized window centered at
the point.
References
----------
.. [1] A. Savitzky, M. J. E. Golay, Smoothing and Differentiation of
Data by Simplified Least Squares Procedures. Analytical
Chemistry, 1964, 36 (8), pp 1627-1639.
.. [2] Numerical Recipes 3rd Edition: The Art of Scientific Computing
W.H. Press, S.A. Teukolsky, W.T. Vetterling, B.P. Flannery
Cambridge University Press ISBN-13: 9780521880688
.. [3] http://wiki.scipy.org/Cookbook/SavitzkyGolay
"""
from math import factorial
try:
window_size = np.abs(np.int(window_size))
order = np.abs(np.int(order))
except ValueError:
raise ValueError("window_size and order have to be of type int")
if window_size % 2 != 1 or window_size < 1:
raise TypeError("window_size size must be a positive odd number")
if window_size < order + 2:
raise TypeError("window_size is too small for the polynomials order")
order_range = range(order + 1)
half_window = (window_size - 1) // 2
# precompute coefficients
b = np.mat([[k**i for i in order_range]
for k in range(-half_window, half_window + 1)])
m = np.linalg.pinv(b).A[deriv] * rate**deriv * factorial(deriv)
new_trajs = []
for traj in trajs:
if len(traj) < window_size:
continue
newpos = []
for y in traj.pos().T:
# pad the signal at the extremes with
# values taken from the signal itself
firstvals = y[0] - np.abs(y[1:half_window + 1][::-1] - y[0])
lastvals = y[-1] + np.abs(y[-half_window - 1:-1][::-1] - y[-1])
y = np.concatenate((firstvals, y, lastvals))
newpos.append(np.convolve(m[::-1], y, mode='valid'))
newpos = np.r_[newpos].T
newvel = np.vstack((np.diff(newpos, axis=0) * fps, np.zeros((1, 3))))
newacc = np.vstack((np.diff(newvel[:-1], axis=0) * fps, np.zeros(
(2, 3))))
new_trajs.append(
Trajectory(newpos,
newvel,
traj.time(),
traj.trajid(),
accel=newacc))
return new_trajs
def savitzky_golay(trajs, fps, window_size, order):
r"""Smooth (and optionally differentiate) data with a Savitzky-Golay filter.
The Savitzky-Golay filter removes high frequency noise from data.
It has the advantage of preserving the original shape and
features of the signal better than other types of filtering
approaches, such as moving averages techniques.
Parameters:
trajs - a list of Trajectory objects
window_size - int,
the length of the window. Must be an odd integer number.
fps - frames per second, used for calculating velocity and acceleration.
order - int,
the order of the polynomial used in the filtering.
Must be less then `window_size` - 1.
Returns:
new_trajs - a list of Trajectory objects representing the smoothed
trajectories. Trajectories shorter than the window size are discarded.
Notes:
The Savitzky-Golay is a type of low-pass filter, particularly
suited for smoothing noisy data. The main idea behind this
approach is to make for each point a least-square fit with a
polynomial of high order over a odd-sized window centered at
the point.
References:
.. [#] A. Savitzky, M. J. E. Golay, Smoothing and Differentiation of \
Data by Simplified Least Squares Procedures. Analytical \
Chemistry, 1964, 36 (8), pp 1627-1639.
.. [#] Numerical Recipes 3rd Edition: The Art of Scientific Computing \
W.H. Press, S.A. Teukolsky, W.T. Vetterling, B.P. Flannery \
Cambridge University Press ISBN-13: 9780521880688
.. [#] http://wiki.scipy.org/Cookbook/SavitzkyGolay
"""
try:
window_size = np.abs(np.int(window_size))
order = np.abs(np.int(order))
except ValueError:
raise ValueError("window_size and order have to be of type int")
if window_size % 2 != 1 or window_size < 1:
raise TypeError("window_size size must be a positive odd number")
if window_size < order + 2:
raise TypeError("window_size is too small for the polynomials order")
order_range = range(order+1)
half_window = (window_size -1) // 2
# Properties that should not be copied from the old trajectory because
# they are obtained otherwise (or copied elsewhere).
smoothed_keys = ['pos', 'velocity', 'accel', 'acc_pp', 'time',
'trajid']
# precompute coefficients
b = np.mat([[k**i for i in order_range] for k in range(-half_window, half_window+1)])
m = np.linalg.pinv(b).A
m_pos = m[0]
m_vel = m[1] * fps
m_acc = m[2] * (fps**2 * 2)
m_jerk = m[3] * (fps**3 * 6)
new_trajs = []
for traj in trajs:
if len(traj) < window_size:
continue
newpos = []
newvel = []
newacc = []
jerk = []
nextacc = []
nextvel = []
for y in traj.pos().T: # For each component of pos
# pad the signal at the extremes with
# values taken from the signal itself
firstvals = y[0] - np.abs( y[1:half_window+1][::-1] - y[0] )
lastvals = y[-1] + np.abs(y[-half_window-1:-1][::-1] - y[-1])
y = np.concatenate((firstvals, y, lastvals))
newpos.append(np.convolve( m_pos[::-1], y, mode='valid'))
newvel.append(np.convolve( m_vel[::-1], y, mode='valid'))
newacc.append(np.convolve( m_acc[::-1], y, mode='valid'))
jerk.append(np.convolve( m_jerk[::-1], y, mode='valid'))
newpos = np.r_[newpos].T
newvel = np.r_[newvel].T
newacc = np.r_[newacc].T
jerk = np.r_[jerk].T
# Velocity and acceleration evaluated at i = 1 rather than i = 0,
# for comparison with the i = 0 values from next polynomial.
# Delta t treatment is in m_*.
# Assumed that the first point is trimmed, the zeros are just for
# alignment.
nextvel = np.vstack((np.zeros(3), newvel + newacc/fps + jerk/2./fps**2))[:-1]
nextacc = np.vstack((np.zeros(3), newacc + jerk/fps))[:-1]
newtraj = Trajectory(newpos, newvel, traj.time(), traj.trajid(),
accel=newacc, vel_pp=nextvel, acc_pp = nextacc)
# Copy unsmoothed properties from old trajectory:
for k, v in traj.as_dict().iteritems():
if k not in smoothed_keys:
newtraj.create_property(k, v)
new_trajs.append(newtraj)
return new_trajs