def test_least_squares_lm(self):
mask = create_mask(*self.D_gt.shape, strategy='single_time')
D_sparse = self.D_gt * mask
C_gt_vec = self.traj.coeffs.reshape((-1, ))
cost_gt = cost_function(C_gt_vec, D_sparse, self.anchors[:2, :],
self.basis)
self.assertTrue(np.sum(np.abs(cost_gt)) < eps)
Chat = self.traj.coeffs
x0 = Chat.copy().reshape((-1, ))
Cref = least_squares_lm(D_sparse, self.anchors, self.basis, x0)
self.assertLess(error_measure(Cref, self.traj.coeffs), eps)
def add_gt_fitting(traj, times_small, points_small, current_results, n_it=0):
# fit ground truth to chosen points.
n_complexity = traj.n_complexity
n_measurements = len(times_small)
coeffs = fit_trajectory(points_small.T, times=times_small, traj=traj)
traj_gt = traj.copy()
traj_gt.set_coeffs(coeffs=coeffs)
points_fitted = traj_gt.get_sampling_points(times=times_small).T
mse = error_measure(points_fitted, points_small, 'mse')
mae = error_measure(points_fitted, points_small, 'mae')
current_results.loc[len(current_results)] = dict(
plotting=(coeffs, points_fitted),
n_complexity=n_complexity,
n_measurements=n_measurements,
method='gt',
n_it=n_it,
mae=mae,
mse=mse,
cost_rls=None,
cost_srls=None)
return points_fitted
def plot_complexities(traj,
times,
results,
points_gt=None,
ax=None,
verbose=False):
""" Create complexity plots.
:param results: results dict of form {
'label1': (coeffs1, points1(if applicable)),
'label2': (coeffs2, points2(if applicable))
}
:param points_gt: points of shape (N x dim).
"""
from other_algorithms import error_measure
if ax is None:
fig, ax = plt.subplots()
if points_gt is not None:
assert points_gt.shape[1] == traj.dim
ax.plot(points_gt[:, 0],
points_gt[:, 1],
color='black',
ls=':',
linewidth=1.,
label='GPS')
i = 0
measure = 'mse'
for label, (Chat, points) in results.items():
color = f'C{i}'
i += 1
if Chat is not None:
traj.set_coeffs(coeffs=Chat)
traj.plot_pretty(times=times, color=color, label=label, ax=ax)
if verbose:
points_est = traj.get_sampling_points(times=times).T
err = error_measure(points_gt, points_est, measure=measure)
print(f'{label} {measure} from coeffs: {err:.2e}')
if points is not None:
for x in points:
ax.scatter(*x, color=color, label=label, s=4.0)
label = None
remove_ticks(ax)
return ax
def generate_results(traj,
D_small,
times_small,
anchors,
points_small,
methods=METHODS,
n_it=0):
n_complexity = traj.n_complexity
n_measurements = np.sum(D_small > 0)
current_results = pd.DataFrame(columns=[
'n_it', 'n_complexity', 'n_measurements', 'mae', 'mse', 'method',
'plotting', 'cost_rls', 'cost_srls'
])
basis_small = traj.get_basis(times=times_small)
for method in methods:
C_hat, p_hat, lat_idx = apply_algorithm(traj,
D_small,
times_small,
anchors,
method=method)
plotting = (C_hat, p_hat)
mae = mse = cost_rls = cost_slrs = None
if C_hat is not None:
traj.set_coeffs(coeffs=C_hat)
p_fitted = traj.get_sampling_points(times=times_small).T
mae = error_measure(p_fitted, points_small, 'mae')
mse = error_measure(p_fitted, points_small, 'mse')
cost_rls = np.sum(
cost_function(C_hat.reshape((-1, )),
D_small,
anchors,
basis_small,
squared=False))
cost_srls = np.sum(
cost_function(C_hat.reshape((-1, )),
D_small,
anchors,
basis_small,
squared=True))
current_results.loc[len(current_results)] = dict(
plotting=plotting,
n_complexity=n_complexity,
n_measurements=n_measurements,
method=method,
n_it=n_it,
mae=mae,
mse=mse,
cost_rls=cost_rls,
cost_srls=cost_srls)
# do raw version if applicable
if method in ['rls', 'srls']:
points_small_lat = points_small[lat_idx]
mae = error_measure(p_hat, points_small_lat, 'mae')
mse = error_measure(p_hat, points_small_lat, 'mse')
current_results.loc[len(current_results)] = dict(
plotting=(None, None),
n_complexity=n_complexity,
n_measurements=n_measurements,
method=method + ' raw',
n_it=n_it,
mae=mae,
mse=mse,
cost_rls=cost_rls,
cost_srls=cost_srls)
return current_results