def apply_algorithm(traj, D, times, anchors, method='ours'):
"""
Apply method to given measurements.
:return: Chat, points, indices
Chat contains the trajectory fitted to the measurements.
points
"""
if method == 'ours-weighted':
basis = traj.get_basis(times=times)
Chat = trajectory_recovery(D, anchors, basis, weighted=True)
return Chat, None, None
elif method == 'ours':
basis = traj.get_basis(times=times)
Chat = trajectory_recovery(D, anchors, basis, weighted=False)
return Chat, None, None
elif method == 'srls':
indices = range(D.shape[0])[traj.dim + 2::3]
points, indices = pointwise_srls(D, anchors, traj, indices)
times = np.array(times)[indices]
Chat = None
if points.shape[0] >= traj.n_complexity:
Chat = fit_trajectory(points.T, times=times, traj=traj)
else:
print(f'Warning in apply_algorithm(srls): cannot fit trajectory to points of shape {points.shape}.')
return Chat, points, indices
elif method == 'rls':
indices = range(D.shape[0])[traj.dim + 2::3]
grid = get_grid(anchors, grid_size=0.5)
points, indices = pointwise_rls(D, anchors, traj, indices, grid=grid)
times = np.array(times)[indices]
Chat = None
if points.shape[0] >= traj.n_complexity:
Chat = fit_trajectory(points.T, times=times, traj=traj)
else:
print(f'Warning in apply_algorithm(rls): cannot fit trajectory to points of shape {points.shape}.')
return Chat, points, indices
elif method == 'lm-ellipse':
basis = traj.get_basis(times=times)
c0 = init_lm(traj.coeffs, method='ellipse').flatten()
Chat = least_squares_lm(D, anchors, basis, c0, cost='simple', jacobian=False)
return Chat, None, None
elif method == 'lm-line':
basis = traj.get_basis(times=times)
c0 = init_lm(traj.coeffs, method='line').flatten()
Chat = least_squares_lm(D, anchors, basis, c0, cost='simple', jacobian=False)
return Chat, None, None
elif method == 'lm-ours-weighted':
basis = traj.get_basis(times=times)
c0 = trajectory_recovery(D, anchors, basis, weighted=True)
Chat = None
if c0 is not None:
c0 = c0.flatten()
Chat = least_squares_lm(D, anchors, basis, c0, cost='simple', jacobian=False)
return Chat, None, None
else:
raise ValueError(method)
def create_complexities(traj, D, times, anchors, list_complexities):
""" Evaluate our algorithm and srls, srls for different complexities.
Used to create Figure 7, first row, in Relax and Recover paper.
"""
grid = get_grid(anchors, grid_size=0.5)
results = pd.DataFrame(columns=['method', 'result', 'n_complexity'])
num_seeds = 3
for n_complexity in list_complexities:
traj.set_n_complexity(n_complexity)
basis = traj.get_basis(times=times)
Chat = trajectory_recovery(D, anchors[:2, :], basis, weighted=True)
results.loc[len(results), :] = dict(n_complexity=n_complexity,
method='ours-weighted',
result=Chat)
indices = range(D.shape[0])[traj.dim + 2::3]
p_rls, __ = pointwise_rls(D, anchors, traj, indices, grid)
p_srls, __ = pointwise_srls(D, anchors, traj, indices)
results.loc[len(results), :] = dict(n_complexity=n_complexity,
method='srls',
result=p_srls)
results.loc[len(results), :] = dict(n_complexity=n_complexity,
method='rls',
result=p_rls)
return results
def plot_complexities_old(traj,
D,
times,
anchors,
full_df,
list_complexities,
srls=False,
rls=True):
if rls:
grid = get_grid(anchors, grid_size=0.5)
fig, axs = plt.subplots(1,
len(list_complexities),
sharex=True,
sharey=True)
for ax, n_complexity in zip(axs, list_complexities):
traj.set_n_complexity(n_complexity)
basis = traj.get_basis(times=times)
Chat = trajectory_recovery(D, anchors[:2, :], basis, weighted=True)
traj.set_coeffs(coeffs=Chat)
traj.plot_pretty(times=times, color="C0", label='ours', ax=ax)
ax.plot(full_df.px,
full_df.py,
color='black',
ls=':',
linewidth=1.,
label='GPS')
ax.set_title('K = {}'.format(traj.n_complexity))
remove_ticks(ax)
i = 1
if srls:
indices = range(D.shape[0])[::3]
points, used_indices = pointwise_srls(D, anchors, traj, indices)
label = 'SRLS'
for x in points:
ax.scatter(*x, color=f'C{i}', label=label, s=4.0)
label = None
i += 1
if rls:
indices = range(D.shape[0])[traj.dim + 2::3]
points, used_indices = pointwise_rls(D, anchors, traj, indices,
grid)
label = 'RLS'
for x in points:
ax.scatter(*x, color=f'C{i}', label=label, s=4.0)
label = None
add_scalebar(axs[0], 20, loc='lower left')
return fig, axs
def create_subsample(traj, D, times, anchors, n_measurements_list):
""" Evaluate our algorithm and srls, srls for different numbers of measurements.
Used to create Figure 7, second row, in Relax and Recover paper.
"""
grid = get_grid(anchors, grid_size=0.5)
results = pd.DataFrame(columns=['method', 'result', 'n_measurements'])
num_seeds = 3
for n_measurements in n_measurements_list:
for seed in range(num_seeds):
np.random.seed(seed)
indices = np.random.choice(D.shape[0],
n_measurements,
replace=False)
D_small = D[indices, :]
times_small = np.array(times)[indices]
basis_small = traj.get_basis(times=times_small)
Chat = trajectory_recovery(D_small,
anchors[:2, :],
basis_small,
weighted=True)
results.loc[len(results), :] = dict(n_measurements=n_measurements,
method='ours-weighted',
result=Chat)
p_rls, __ = pointwise_rls(D, anchors, traj, indices, grid)
p_srls, __ = pointwise_srls(D, anchors, traj, indices)
results.loc[len(results), :] = dict(n_measurements=n_measurements,
method='srls',
result=p_srls)
results.loc[len(results), :] = dict(n_measurements=n_measurements,
method='rls',
result=p_rls)
return results
def test_combination(self):
""" Test that if we do our method first
and then apply LM with split method, we do not
change the result.
"""
sigma = 0.01
tol = 1e-3 # found empirically.
for i in range(3): # works up to 10, but quite slow.
self.set_measurements(seed=i)
D_noisy = add_noise(self.D_topright, noise_sigma=sigma)
x0 = trajectory_recovery(D_noisy,
self.anchors,
self.basis,
weighted=False)
x1 = least_squares_lm(D_noisy,
self.anchors,
self.basis,
x0=x0.reshape((-1, )),
cost='split',
jacobian=False,
verbose=VERBOSE)
assert np.linalg.norm(
x0 - x1) < tol, f'for {i}: {np.linalg.norm(x0 - x1)}'
def run_simulation(parameters, outfolder=None, solver=None, verbose=False):
""" Run simulation.
:param parameters: Can be either the name of the folder where parameters.json
is stored, or a new dict of parameters.
"""
if type(parameters) == str:
fname = parameters + 'parameters.json'
parameters = read_params(fname)
print('read parameters from file {}.'.format(fname))
elif type(parameters) == dict:
parameters = parameters
# if we are trying new parameters and saving in a directory that already exists,
# we need to make sure that the saved parameters are actually the same.
if outfolder is not None:
try:
parameters_old = read_params(outfolder + 'parameters.json')
parameters['time'] = parameters_old['time']
assert parameters == parameters_old, 'found conflicting parameters file: {}'.format(
outfolder + 'parameters.json')
except FileNotFoundError:
print('no conflicting parameters file found.')
except AssertionError as error:
raise (error)
else:
raise TypeError('parameters needs to be folder name or dictionary.')
if 'noise_to_square' not in parameters:
parameters['noise_to_square'] = False
if 'measure_distances' not in parameters:
parameters['measure_distances'] = False
if 'sampling_strategy' not in parameters:
parameters['sampling_strategy'] = 'uniform'
complexities = parameters['complexities']
anchors = parameters['anchors']
positions = parameters['positions']
n_its = parameters['n_its']
noise_sigmas = parameters['noise_sigmas']
success_thresholds = parameters['success_thresholds']
assert len(success_thresholds) == len(noise_sigmas)
if parameters['sampling_strategy'] == 'single_time':
max_measurements = max(positions)
else:
max_measurements = max(positions) * max(anchors)
successes = np.full((len(complexities), len(anchors), len(positions),
len(noise_sigmas), max_measurements), np.nan)
errors = np.full(successes.shape, np.nan)
relative_errors = np.full(successes.shape, np.nan)
absolute_errors = np.full(successes.shape, np.nan)
num_not_solved = np.full(successes.shape, np.nan)
num_not_accurate = np.full(successes.shape, np.nan)
squared_distances = []
for c_idx, n_complexity in enumerate(complexities):
print('n_complexity', n_complexity)
for a_idx, n_anchors in enumerate(anchors):
print('n_anchors', n_anchors)
for p_idx, n_positions in enumerate(positions):
print('n_positions', n_positions)
if parameters['sampling_strategy'] == 'single_time':
n_measurements = n_positions
else:
n_measurements = n_positions * n_anchors
for m_idx, n_missing in enumerate(range(n_measurements)):
if verbose:
print('measurements idx', m_idx)
for noise_idx, noise_sigma in enumerate(noise_sigmas):
indexes = np.s_[c_idx, a_idx, p_idx, noise_idx, m_idx]
if verbose:
print("noise", noise_sigma)
# set all values to 0 since we have visited them.
if np.isnan(successes[indexes]):
successes[indexes] = 0.0
if np.isnan(num_not_solved[indexes]):
num_not_solved[indexes] = 0.0
if np.isnan(num_not_accurate[indexes]):
num_not_accurate[indexes] = 0.0
for _ in range(n_its):
trajectory = Trajectory(n_complexity, dim=DIM)
anchors_coord = create_anchors(DIM, n_anchors)
trajectory.set_coeffs(seed=None)
basis, D_topright = get_measurements(
trajectory,
anchors_coord,
n_samples=n_positions)
distances = np.sqrt(D_topright)
D_topright = add_noise(
D_topright, noise_sigma,
parameters["noise_to_square"])
mask = create_mask(
n_positions,
n_anchors,
strategy=parameters['sampling_strategy'],
n_missing=n_missing)
if parameters['measure_distances']:
squared_distances.extend(
D_topright.flatten().tolist())
D_topright = np.multiply(D_topright, mask)
try:
assert p.full_rank_condition(
np.sort(np.sum(mask, axis=0))[::-1],
DIM + 1, n_complexity), "insufficient rank"
if (solver is None) or (
solver
== "semidef_relaxation_noiseless"):
X = semidef_relaxation_noiseless(
D_topright,
anchors_coord,
basis,
chosen_solver=cvxpy.CVXOPT)
P_hat = X[:DIM, DIM:]
elif solver == 'trajectory_recovery':
P_hat = trajectory_recovery(
D_topright, anchors_coord, basis)
elif solver == 'weighted_trajectory_recovery':
P_hat = trajectory_recovery(D_topright,
anchors_coord,
basis,
weighted=True)
else:
raise ValueError(solver)
# calculate reconstruction error with respect to distances
trajectory_estimated = Trajectory(coeffs=P_hat)
_, D_estimated = get_measurements(
trajectory_estimated,
anchors_coord,
n_samples=n_positions)
estimated_distances = np.sqrt(D_estimated)
robust_add(
errors, indexes,
np.linalg.norm(P_hat - trajectory.coeffs))
robust_add(
relative_errors, indexes,
np.linalg.norm(
(distances - estimated_distances) /
(distances + 1e-10)))
robust_add(
absolute_errors, indexes,
np.linalg.norm(distances -
estimated_distances))
assert not np.linalg.norm(
P_hat - trajectory.coeffs
) > success_thresholds[noise_idx]
robust_increment(successes, indexes)
except cvxpy.SolverError:
logging.info(
"could not solve n_positions={}, n_missing={}"
.format(n_positions, n_missing))
robust_increment(num_not_solved, indexes)
except ZeroDivisionError:
logging.info(
"could not solve n_positions={}, n_missing={}"
.format(n_positions, n_missing))
robust_increment(num_not_solved, indexes)
except np.linalg.LinAlgError:
robust_increment(num_not_solved, indexes)
except AssertionError as e:
if str(e) == "insufficient rank":
robust_increment(num_not_solved, indexes)
else:
logging.info(
"result not accurate n_positions={}, n_missing={}"
.format(n_positions, n_missing))
robust_increment(num_not_accurate, indexes)
errors[indexes] = errors[indexes] / (
n_its - num_not_solved[indexes])
relative_errors[indexes] = relative_errors[
indexes] / (n_its - num_not_solved[indexes])
results = {
'successes': successes,
'num-not-solved': num_not_solved,
'num-not-accurate': num_not_accurate,
'errors': errors,
'relative-errors': relative_errors,
'absolute-errors': absolute_errors,
'distances': squared_distances
}
if outfolder is not None:
print('Done with simulation. Saving results...')
parameters['time'] = time.time()
if not os.path.exists(outfolder):
os.makedirs(outfolder)
save_params(outfolder + 'parameters.json', **parameters)
save_results(outfolder + 'result_{}_{}', results)
else:
return results
def build_up_algorithm(D, anchors, basis, times, eps=1, verbose=False):
""" Build-up algorithm for trajectory estimation.
Build up different trajectories as long as measurements "fit". When they
stop fitting (see eps parameter), start a new trajectory.
:param D: measurement matrix with squared distances (N x M)
:param anchors: anchor coordinates (dim x M)
:param basis: basis vectors (K x N)
:param times: list of measurement times (length N)
:param eps: error threshold for starting new trajectory.
"""
verify_dimensions(D, anchors, basis, times)
C_k = None
tk = []
D_k = np.empty((0, D.shape[1]))
basis_k = np.empty((basis.shape[0], 0))
C_list = []
t_list = []
def g(C_k):
r_n = C_k.dot(f_n).reshape((ams.shape[0], 1))
dist_estimates = np.linalg.norm(ams - r_n, axis=0)
distances = np.sqrt(d_mn_row.flatten()[d_mn_row.flatten() > 0])
return np.sum(np.abs(dist_estimates - distances)) / len(
distances) # MAE
for d_mn_row, t_n, f_n in zip(D, times, basis.T):
d_mn_row = d_mn_row.reshape((1, -1))
ams = anchors[:, d_mn_row[0] > 0]
if C_k is None or g(C_k) <= eps:
D_k = np.r_[D_k, d_mn_row]
basis_k = np.c_[basis_k, f_n]
tk.append(t_n)
try:
C_test = trajectory_recovery(D_k, anchors, basis_k)
assert C_test is not None
# TODO find a sensible general threshold here.
assert g(C_test) < 2 * eps
C_k = C_test
# We need this somehow for numercial reasons.
# Otherwise sometimes the tests fail because -0. != 0.
C_k[np.abs(C_k) <= 1e-10] = 0.0
except AssertionError as e:
if verbose:
print(
'skipping {:.2f} because only {} measurements.'.format(
t_n, len(np.array(tk))))
except np.linalg.LinAlgError:
if verbose:
print('skipping {:.2f} because failed'.format(t_n))
elif (C_k is not None) and g(C_k) > eps:
if verbose:
print('changing to new trajectory, because error is {:.4f}'.
format(g(C_k)))
basis_k = f_n
D_k = d_mn_row
C_list.append(C_k)
t_list.append(tk)
tk = [t_n]
C_k = None
if C_k is not None:
C_list.append(C_k)
t_list.append(tk)
return C_list, t_list
def averaging_algorithm(D,
anchors,
basis,
times,
t_window=1.0,
n_times=None,
verbose=False):
""" Iteratively compute estimates over fixed time window.
:param D: measurement matrix with squared distances (N x M)
:param anchors: anchor coordinates (dim x M)
:param basis: basis vectors (K x N)
:param times: list of measurement times (length N)
:param t_window: width of fixed time window.
"""
if type(times) == list:
times = np.array(times)
verify_dimensions(D, anchors, basis, times)
D_k = np.empty((0, D.shape[1]))
basis_k = np.empty((basis.shape[0], 0))
C_list = []
t_list = []
# make sure we always have overlap
if n_times is None:
t_start = np.arange(times[0], times[-1], step=t_window / 2)
else:
t_start = np.linspace(times[0], times[-1], n_times)
for t_s in t_start:
tk = times[(times >= t_s) & (times < t_s + t_window)]
for t_n in tk:
idx = np.where(times == t_n)[0]
d_mn_row = D[idx, :]
f_n = basis[:, idx]
D_k = np.r_[D_k, d_mn_row]
basis_k = np.c_[basis_k, f_n]
try:
C_k = trajectory_recovery(D_k, anchors, basis_k)
# We need this somehow for numercial reasons.
# Otherwise sometimes the tests fail because -0. != 0.
C_k[np.abs(C_k) <= 1e-10] = 0.0
C_list.append(C_k)
except AssertionError:
if verbose:
print('skipping {:.2f} because only {} measurements.'.format(
t_s, len(np.array(tk))))
except np.linalg.LinAlgError:
if verbose:
print('skipping {:.2f} because failed.'.format(t_s))
except ValueError:
raise
D_k = np.empty((0, D.shape[1]))
basis_k = np.empty((basis.shape[0], 0))
t_list.append(tk)
return C_list, t_list
def plot_subsample_old(traj,
D,
times,
anchors,
full_df,
n_measurements_list,
srls=False,
rls=True):
import hypothesis as h
basis = traj.get_basis(times=times)
fig, axs = plt.subplots(1,
len(n_measurements_list),
sharex=True,
sharey=True)
if rls:
grid = get_grid(anchors, grid_size=0.5)
alpha = 1.0
num_seeds = 3
for ax, n_measurements in zip(axs, n_measurements_list):
label = 'ours'
coeffs = np.empty([traj.dim, traj.n_complexity, 0])
colors = {0: 0, 1: 2, 2: 3}
for seed in range(num_seeds):
np.random.seed(seed)
indices = np.random.choice(D.shape[0],
n_measurements,
replace=False)
D_small = D[indices, :]
mask = (D_small > 0).astype(np.float)
p = np.sort(np.sum(mask, axis=0))[::-1]
if not h.limit_condition(list(p), traj.dim + 1, traj.n_complexity):
print("insufficient rank")
times_small = np.array(times)[indices]
basis_small = traj.get_basis(times=times_small)
Chat = trajectory_recovery(D_small,
anchors[:2, :],
basis_small,
weighted=True)
coeffs = np.dstack([coeffs, Chat])
traj.set_coeffs(coeffs=Chat)
traj.plot_pretty(times=times,
color='C{}'.format(colors[seed]),
ax=ax,
alpha=alpha)
Chat_avg = np.mean(coeffs, axis=2)
traj.set_coeffs(coeffs=Chat_avg)
traj.plot_pretty(times=times, color='C0', label=label, ax=ax)
i = 1
if srls:
points, used_indices = pointwise_srls(D, anchors, traj, indices)
label = 'SRLS'
for x in points:
ax.scatter(*x, color=f'C{i}', label=label, s=4.0)
label = None
i += 1
if rls:
points, used_indices = pointwise_rls(D, anchors, traj, indices,
grid)
label = 'RLS'
for x in points:
ax.scatter(*x, color=f'C{i}', label=label, s=4.0)
label = None
ax.plot(full_df.px,
full_df.py,
ls=':',
linewidth=1.,
color='black',
label='GPS')
remove_ticks(ax)
ax.set_title('N = {}'.format(n_measurements), y=-0.22)
add_scalebar(axs[0], 20, loc='lower left')
return fig, axs