def NG_dr1(X, verbosity = 0):
"""
X: array of N points on Gr(n, p); N x n x p array
aim to represent X by X_hat (N points on Gr(n-1, p))
where X_hat_i = A^T X_i, A \in St(n, n-1)
minimizing the projection error (using projection F-norm)
"""
N, n, p = X.shape
cpx = np.iscomplex(X).any() # true if X is complex-valued
if cpx:
man = Product([ComplexGrassmann(n, 1), Euclidean(p, 2)])
else:
man = Product([Grassmann(n, 1), Euclidean(p)])
X_ = torch.from_numpy(X)
@pymanopt.function.PyTorch
def cost(v, b):
vvT = torch.matmul(v, v.conj().t()) # n x n
if cpx:
b_ = b[:,0] + b[:,1]*1j
b_ = torch.unsqueeze(b_, axis=1)
else:
b_ = torch.unsqueeze(b, axis=1)
vbt = torch.matmul(v, b_.t()) # n x p
IvvT = torch.eye(n, dtype=X_.dtype) - vvT
d2 = 0
for i in range(N):
d2 = d2 + dist_proj(X_[i], torch.matmul(IvvT, X_[i]) + vbt)**2/N
#d2 = d2 + dist_proj(X_[i], torch.matmul(AAT, X_[i]))**2/N
return d2
solver = SteepestDescent()
problem = Problem(manifold=man, cost=cost, verbosity=verbosity)
theta = solver.solve(problem)
v = theta[0]
b_ = theta[1]
if cpx:
b = b_[:,0] + b_[:,1]*1j
b = np.expand_dims(b, axis=1)
else:
b = np.expand_dims(b_, axis=1)
R = ortho_complement(v)
tmp = np.array([R.conj().T for i in range(N)])
X_low = multiprod(tmp, X)
X_low = np.array([qr(X_low[i])[0] for i in range(N)])
return X_low, R, v, b
def NG_dr(X, m, verbosity=0, *args, **kwargs):
"""
X: array of N points on Gr(n, p); N x n x p array
aim to represent X by X_hat (N points on Gr(m, p), m < n)
where X_hat_i = R^T X_i, R \in St(n, m)
minimizing the projection error (using projection F-norm)
"""
N, n, p = X.shape
cpx = np.iscomplex(X).any() # true if X is complex-valued
if cpx:
man = Product([ComplexGrassmann(n, m), Euclidean(n, p, 2)])
else:
man = Product([Grassmann(n, m), Euclidean(n, p)])
X_ = torch.from_numpy(X)
@pymanopt.function.PyTorch
def cost(A, B):
AAT = torch.matmul(A, A.conj().t()) # n x n
if cpx:
B_ = B[:,:,0] + B[:,:,1]*1j
else:
B_ = B
IAATB = torch.matmul(torch.eye(n, dtype=X_.dtype) - AAT, B_) # n x p
d2 = 0
for i in range(N):
d2 = d2 + dist_proj(X_[i], torch.matmul(AAT, X_[i]) + IAATB)**2/N
#d2 = d2 + dist_proj(X_[i], torch.matmul(AAT, X_[i]))**2/N
return d2
#solver = ConjugateGradient()
solver = SteepestDescent()
problem = Problem(manifold=man, cost=cost, verbosity=verbosity)
theta = solver.solve(problem)
A = theta[0]
B = theta[1]
if cpx:
B_ = B[:,:,0] + B[:,:,1]*1j
else:
B_ = B
#tmp = np.array([A.T for i in range(N)])
tmp = np.array([A.conj().T for i in range(N)])
X_low = multiprod(tmp, X)
X_low = np.array([qr(X_low[i])[0] for i in range(N)])
return X_low, A, B_
def setUp(self):
self.m = m = 100
self.n = n = 50
self.euclidean = Euclidean(m, n)
self.sphere = Sphere(n)
self.manifold = Product([self.euclidean, self.sphere])
point = self.manifold.random_point()
@pymanopt.function.autograd(self.manifold)
def cost(*x):
return np.sum([np.linalg.norm(a - b)**2 for a, b in zip(x, point)])
self.cost = cost
def run(backend=SUPPORTED_BACKENDS[0], quiet=True):
num_samples, num_weights = 200, 3
solver = TrustRegions()
manifold = Euclidean(3)
for k in range(5):
samples = rnd.randn(num_samples, num_weights)
targets = rnd.randn(num_samples)
cost, egrad, ehess = create_cost_egrad_ehess(backend, samples, targets)
problem = pymanopt.Problem(manifold,
cost,
egrad=egrad,
ehess=ehess,
verbosity=0)
estimated_weights = solver.solve(problem)
if not quiet:
print("Run {}".format(k + 1))
print("Weights found by pymanopt (top) / "
"closed form solution (bottom)")
print(estimated_weights)
print(la.pinv(samples) @ targets)
print("")
def run(backend=SUPPORTED_BACKENDS[0], quiet=True):
num_samples, num_weights = 200, 3
optimizer = TrustRegions(verbosity=0)
manifold = Euclidean(3)
for k in range(5):
samples = np.random.normal(size=(num_samples, num_weights))
targets = np.random.normal(size=num_samples)
(
cost,
euclidean_gradient,
euclidean_hessian,
) = create_cost_and_derivates(manifold, samples, targets, backend)
problem = pymanopt.Problem(
manifold,
cost,
euclidean_gradient=euclidean_gradient,
euclidean_hessian=euclidean_hessian,
)
estimated_weights = optimizer.run(problem).point
if not quiet:
print(f"Run {k + 1}")
print(
"Weights found by pymanopt (top) / "
"closed form solution (bottom)"
)
print(estimated_weights)
print(np.linalg.pinv(samples) @ targets)
print("")
def setUp(self):
self.m = m = 20
self.n = n = 10
self.rank = rank = 3
A = np.random.normal(size=(m, n))
self.manifold = Product([FixedRankEmbedded(m, n, rank), Euclidean(n)])
@pymanopt.function.autograd(self.manifold)
def cost(u, s, vt, x):
return np.linalg.norm(((u * s) @ vt - A) @ x) ** 2
self.cost = cost
self.gradient = self.cost.get_gradient_operator()
self.hessian = self.cost.get_hessian_operator()
self.problem = pymanopt.Problem(self.manifold, self.cost)
def setUp(self):
self.m = m = 20
self.n = n = 10
self.rank = rank = 3
A = np.random.randn(m, n)
@pymanopt.function.Autograd
def cost(u, s, vt, x):
return np.linalg.norm(((u * s) @ vt - A) @ x)**2
self.cost = cost
self.gradient = self.cost.compute_gradient()
self.hvp = self.cost.compute_hessian_vector_product()
self.manifold = Product([FixedRankEmbedded(m, n, rank), Euclidean(n)])
self.problem = pymanopt.Problem(self.manifold, self.cost)
def fit(self, T, Y, init, maxIter=100):
self.init_fit(T, Y, None)
D = self.D + self.L
K = self.K
# (1) Instantiate the manifold
manifold = Product([PositiveDefinite(D + 1, k=K), Euclidean(K - 1)])
cost = self.get_cost_function(T, Y)
problem = Problem(manifold=manifold, cost=cost, verbosity=1)
# (3) Instantiate a Pymanopt solver
solver = SteepestDescent(maxiter=3 * maxIter)
# let Pymanopt do the rest
Xopt = solver.solve(problem)
self.Xopt_to_theta(Xopt)
def evaluate(self):
# Perform reconstruction
if self.recon_type == 'tp': # Estimate theta and phi
manifold = Sphere(3)
def cost(X, data=self.data):
ll = self.multiframe.noise_model.loglikelihood(
util.xyz2tp(*X), data)
return -ll
problem = Problem(manifold=manifold, cost=cost, verbosity=0)
start_pts = [
np.array(util.tp2xyz(*x)) for x in util.sphere_profile(20)
]
solver = ParticleSwarm(maxcostevals=200)
Xopt = solver.solve(problem, x=start_pts)
self.estimated_fluorophore = fluorophore.Fluorophore(*util.xyz2tp(
*Xopt))
elif self.recon_type == 'tpc': # Estimate theta, phi, constant
# Create manifold and cost function
manifold = Product((Sphere(3), Euclidean(1)))
def cost(X, data=self.data):
estimate = np.hstack([util.xyz2tp(*X[0]), X[1]])
ll = self.multiframe.noise_model.loglikelihood(estimate, data)
return -ll
problem = Problem(manifold=manifold, cost=cost, verbosity=0)
# Generate start_pts and format
xyz_start_pts = 3 * [
np.array(util.tp2xyz(*x)) for x in util.sphere_profile(10)
]
c_start_pts = np.expand_dims(np.hstack(
(10 * [0.1], 10 * [2], 10 * [10])),
axis=1)
start_pts = np.hstack((xyz_start_pts, c_start_pts))
pts = []
for start_pt in start_pts:
pts.append([np.array(start_pt[0:3]), np.array(start_pt[3:5])])
# Solve
solver = ParticleSwarm(maxcostevals=250)
Xopt = solver.solve(problem, x=pts)
self.estimated_fluorophore = fluorophore.Fluorophore(
*np.hstack([util.xyz2tp(*Xopt[0]), Xopt[1]]).flatten())
elif self.recon_type == 'tpck': # Estimate theta, phi, constant, kappa
# Create manifold and cost function
manifold = Product((Sphere(3), Euclidean(2)))
def cost(X, data=self.data):
estimate = np.array([
util.xyz2tp(*X[0]), X[1]
]).flatten() # Reshape data for loglikelihood function
ll = self.multiframe.noise_model.loglikelihood(estimate, data)
print(estimate, ll)
return -ll
problem = Problem(manifold=manifold, cost=cost, verbosity=0)
# Generate start_pts and format
xyz_start_pts = 3 * [
np.array(util.tp2xyz(*x)) for x in util.sphere_profile(10)
]
k_start_pts = np.expand_dims(np.hstack(
(10 * [-100], 10 * [0], 10 * [100])),
axis=1)
c_start_pts = np.expand_dims(np.hstack(
(10 * [0.1], 10 * [1], 10 * [10])),
axis=1)
start_pts = np.hstack((xyz_start_pts, c_start_pts, k_start_pts))
pts = []
for start_pt in start_pts:
pts.append([np.array(start_pt[0:3]), np.array(start_pt[3:5])])
# Solve
solver = ParticleSwarm(maxcostevals=200)
Xopt = solver.solve(problem, x=pts)
self.estimated_fluorophore = fluorophore.Fluorophore(
*np.array([util.xyz2tp(*Xopt[0]), Xopt[1]]).flatten())
with tf.name_scope("test") as scope:
correct_prediction = tf.equal(tf.argmax(y,1), tf.argmax(y_,1))
accuracy = tf.reduce_mean(tf.cast(correct_prediction, "float"))
accuracy_summary =tf.summary.scalar("accuracy", accuracy)
#with tf.name_scope("e_test") as scope:
#e_correct_prediction = tf.equal(tf.argmax(e_y, 1), tf.argmax(y_, 1))
#e_accuracy = tf.reduce_mean(tf.cast(e_correct_prediction, "float"))
#e_accuracy_summary =tf.summary.scalar("e_accuracy", e_accuracy)
summaries = tf.summary.merge_all()
manifold_b = Euclidean(1,10)
if use_parameterization:
manifold_A = Euclidean(784, k)
manifold_B = Euclidean(k, 10)
manifold_W = Product([manifold_A,manifold_B])
arg = [A, B, b]
else:
manifold_W = FixedRankEmbedded(784, 10, k)
#manifold_W = Product([Stiefel(784,k), Euclidean(k), Stiefel(k,10)])
arg = [A, M, B, b]
manifold = Product([manifold_W, manifold_b])
e_manifold_W = Euclidean(784, 10)
e_arg = [eW, b]
def main():
"""Main function to run nonlinear manifold optimization on SE(3) to estimate
an optimal relative pose transformation between coordinate frames given by
the different lidar sensors."""
# Extract and process the CSVs
main_odometry = relative_pose_processing.process_df(MAIN_ODOM_CSV)
front_odometry = relative_pose_processing.process_df(FRONT_ODOM_CSV)
rear_odometry = relative_pose_processing.process_df(REAR_ODOM_CSV)
# Process poses
(main_aligned, front_aligned,
rear_aligned) = relative_pose_processing.align_df(
[main_odometry, front_odometry, rear_odometry])
# Get ICP covariance matrices
# Main lidar odometry
main_icp, main_trans_cov, main_trans_cov_max, \
main_trans_cov_avg, main_rot_cov, main_rot_cov_max, \
main_rot_cov_avg, main_reject = parse_icp_cov(main_odometry, type="main",
reject_thr=REJECT_THR)
# Front lidar odometry
front_icp, front_trans_cov, front_trans_cov_max, \
front_trans_cov_avg, front_rot_cov, front_rot_cov_max, \
front_rot_cov_avg, front_reject = parse_icp_cov(front_odometry, type="front",
reject_thr=REJECT_THR)
# Rear lidar odometry
rear_icp, rear_trans_cov, rear_trans_cov_max, \
rear_trans_cov_avg, rear_rot_cov, rear_rot_cov_max, \
rear_rot_cov_avg, rear_reject = parse_icp_cov(rear_odometry, type="rear",
reject_thr=REJECT_THR)
# Calculate relative poses
(main_aligned,
main_rel_poses) = relative_pose_processing.calc_rel_poses(main_aligned)
(front_aligned,
front_rel_poses) = relative_pose_processing.calc_rel_poses(front_aligned)
(rear_aligned,
rear_rel_poses) = relative_pose_processing.calc_rel_poses(rear_aligned)
cov_t_main, cov_R_main = compute_weights_euler(main_aligned)
cov_t_front, cov_R_front = compute_weights_euler(front_aligned)
cov_t_rear, cov_R_rear = compute_weights_euler(rear_aligned)
# Extract a single scalar using the average value from rotation and translation
var_t_main = extract_variance(cov_t_main, mode="max")
var_R_main = extract_variance(cov_R_main, mode="max")
var_t_front = extract_variance(cov_t_front, mode="max")
var_R_front = extract_variance(cov_R_front, mode="max")
var_t_rear = extract_variance(cov_t_main, mode="max")
var_R_rear = extract_variance(cov_R_rear, mode="max")
# Optimization (1) Instantiate a manifold
translation_manifold = Euclidean(3) # Translation vector
so3 = Rotations(3) # Rotation matrix
manifold = Product((so3, translation_manifold)) # Instantiate manifold
# Get initial guesses for our estimations
initial_poses = {}
if os.path.exists(PKL_POSES_PATH): # Check to make sure path exists
transforms_dict = load_transforms(
PKL_POSES_PATH) # Loads relative transforms
# Now get initial guesses from the relative poses
initial_guess_main_front = transforms_dict[
"velodyne_front"] # Get relative transform from main to front (T^{V}_{F})
initial_guess_main_rear = transforms_dict[
"velodyne_rear"] # Get relative transform from front to main T^{V}_{B})
initial_guess_front_rear = np.linalg.inv(
initial_guess_main_front
) @ initial_guess_main_rear # Get relative transform from front to rear T^{B}_{W})
direct_initial_guess_front_rear = transforms_dict[
"direct_front_rear"] # Transform directly computed
# Print out all the initial estimates as poses
print(
"INITIAL GUESS MAIN FRONT: \n {} \n".format(initial_guess_main_front))
print("INITIAL GUESS MAIN REAR: \n {} \n".format(initial_guess_main_rear))
print(
"INITIAL GUESS FRONT REAR: \n {} \n".format(initial_guess_front_rear))
print("INITIAL GUESS DIRECT FRONT REAR: \n {} \n".format(
direct_initial_guess_front_rear))
# Get rotation matrices for initial guesses
R0_main_front, t0_main_front = initial_guess_main_front[:3, :
3], initial_guess_main_front[:
3,
3]
X0_main_front = (R0_main_front, t0_main_front)
print("INITIAL GUESS MAIN FRONT: \n R0: \n {} \n\n t0: \n {} \n".format(
R0_main_front, t0_main_front))
R0_main_rear, t0_main_rear = initial_guess_main_rear[:3, :
3], initial_guess_main_rear[:
3,
3]
X0_main_rear = (R0_main_rear, t0_main_rear)
print("INITIAL GUESS MAIN REAR: \n R0: \n {} \n\n t0: \n {} \n".format(
R0_main_rear, t0_main_rear))
R0_front_rear, t0_front_rear = initial_guess_front_rear[:3, :
3], initial_guess_front_rear[:
3,
3]
X0_front_rear = (R0_front_rear, t0_front_rear)
print("INITIAL GUESS FRONT REAR: \n R0: \n {} \n\n t0: \n {} \n".format(
R0_front_rear, t0_front_rear))
######################## MAIN FRONT CALIBRATION ################################
# Carry out optimization for main-front homogeneous transformations
### PARAMETERS ###
A = np.array(front_rel_poses) # First set of poses
B = np.array(main_rel_poses) # Second set of poses
N = min(A.shape[0], B.shape[0])
r = np.logical_or(np.array(main_reject[:N]), np.array(
front_reject[:N])) # If either has high variance, reject the sample
omega = np.max([var_R_main,
var_R_front]) # Take average across different odometries
rho = np.max([var_t_main,
var_t_front]) # Take average across different odometries
### PARAMETERS ###
cost_main_front = lambda x: cost(x, A, B, r, rho, omega, WEIGHTED)
problem_main_front = Problem(
manifold=manifold, cost=cost_main_front
) # (2a) Compute the optimization between main and front
solver_main_front = CustomSteepestDescent(
) # (3) Instantiate a Pymanopt solver
Xopt_main_front = solver_main_front.solve(problem_main_front,
x=X0_main_front)
print("Initial Guess for Main-Front Transformation: \n {}".format(
initial_guess_main_front))
print("Optimal solution between main and front reference frames: \n {}".
format(Xopt_main_front))
# Take intermediate values for plotting
estimates_x_main_front = solver_main_front.estimates
errors_main_front = solver_main_front.errors
iters_main_front = solver_main_front.iterations
# Metrics dictionary
estimates_dict_main_front = {
i: T
for i, T in zip(iters_main_front, estimates_x_main_front)
}
error_dict_main_front = {
i: e
for i, e in zip(iters_main_front, errors_main_front)
}
# Save intermediate results to a pkl file
estimates_fname_main_front = os.path.join(ANALYSIS_RESULTS_PATH,
"estimates_main_front.pkl")
error_fname_main_front = os.path.join(ANALYSIS_RESULTS_PATH,
"error_main_front.pkl")
# Save estimates to pickle file
with open(estimates_fname_main_front, "wb") as pkl_estimates:
pickle.dump(estimates_dict_main_front, pkl_estimates)
pkl_estimates.close()
# Save error to pickle file
with open(error_fname_main_front, "wb") as pkl_error:
pickle.dump(error_dict_main_front, pkl_error)
pkl_error.close()
# Calculate difference between initial guess and final
XOpt_T_main_front = construct_pose(Xopt_main_front[0],
Xopt_main_front[1].reshape((3, 1)))
print("DIFFERENCE IN MATRICES: \n {}".format(
np.subtract(XOpt_T_main_front, initial_guess_main_front)))
# Compute the weighted and unweighted RMSE
rmse_init_weighted, rmse_final_weighted, rmse_init_R_weighted, \
rmse_init_t_weighted, rmse_final_R_weighted, \
rmse_final_t_weighted = compute_rmse_weighted(initial_guess_main_front,
XOpt_T_main_front, A, B, rho,
omega)
rmse_init_unweighted, rmse_final_unweighted, rmse_init_R_unweighted, \
rmse_init_t_unweighted, rmse_final_R_unweighted, \
rmse_final_t_unweighted = compute_rmse_unweighted(initial_guess_main_front,
XOpt_T_main_front, A, B)
rmses = [
rmse_init_unweighted, rmse_final_unweighted, rmse_init_weighted,
rmse_final_weighted, rmse_init_R_unweighted, rmse_init_t_unweighted,
rmse_final_R_unweighted, rmse_final_t_unweighted, rmse_init_R_weighted,
rmse_init_t_weighted, rmse_final_R_weighted, rmse_final_t_weighted
]
# Display and save RMSEs
outpath = os.path.join(ANALYSIS_RESULTS_PATH, "main_front_rmse.txt")
display_and_save_rmse(rmses, outpath)
# Save final estimates
final_estimate_outpath = os.path.join(FINAL_ESTIMATES_PATH,
"main_front_final.txt")
np.savetxt(final_estimate_outpath, XOpt_T_main_front)
################################################################################
######################## MAIN REAR CALIBRATION #################################
### PARAMETERS ###
A = np.array(rear_rel_poses) # First set of poses
B = np.array(main_rel_poses) # Second set of poses
N = min(A.shape[0], B.shape[0])
r = np.logical_or(np.array(main_reject[:N]), np.array(
rear_reject[:N])) # If either has high variance, reject the sample
omega = np.max([var_R_main,
var_R_rear]) # Take average across different odometries
rho = np.max([var_t_main,
var_t_rear]) # Take average across different odometries
### PARAMETERS ###
cost_main_rear = lambda x: cost(x, A, B, r, rho, omega, WEIGHTED)
# Carry out optimization for main-rear homogeneous transformations
problem_main_rear = Problem(
manifold=manifold, cost=cost_main_rear
) # (2a) Compute the optimization between main and front
solver_main_rear = CustomSteepestDescent(
) # (3) Instantiate a Pymanopt solver
Xopt_main_rear = solver_main_rear.solve(problem_main_rear, x=X0_main_rear)
print("Initial Guess for Main-Rear Transformation: \n {}".format(
initial_guess_main_rear))
print("Optimal solution between main and rear reference frames: \n {}".
format(Xopt_main_rear))
# Take intermediate values for plotting
estimates_x_main_rear = solver_main_rear.estimates
errors_main_rear = solver_main_rear.errors
iters_main_rear = solver_main_rear.iterations
# Metrics dictionary
estimates_dict_main_rear = {
i: T
for i, T in zip(iters_main_rear, estimates_x_main_rear)
}
error_dict_main_rear = {
i: e
for i, e in zip(iters_main_rear, errors_main_rear)
}
# Save intermediate results to a pkl file
estimates_fname_main_rear = os.path.join(ANALYSIS_RESULTS_PATH,
"estimates_main_rear.pkl")
error_fname_main_rear = os.path.join(ANALYSIS_RESULTS_PATH,
"error_main_rear.pkl")
# Save estimates to pickle file
with open(estimates_fname_main_rear, "wb") as pkl_estimates:
pickle.dump(estimates_dict_main_rear, pkl_estimates)
pkl_estimates.close()
# Save error to pickle file
with open(error_fname_main_rear, "wb") as pkl_error:
pickle.dump(error_dict_main_rear, pkl_error)
pkl_error.close()
# Calculate difference between initial guess and final
XOpt_T_main_rear = construct_pose(Xopt_main_rear[0],
Xopt_main_rear[1].reshape((3, 1)))
print("DIFFERENCE IN MATRICES: \n {}".format(
np.subtract(XOpt_T_main_rear, initial_guess_main_rear)))
# Compute the weighted and unweighted RMSE
rmse_init_weighted, rmse_final_weighted, rmse_init_R_weighted, \
rmse_init_t_weighted, rmse_final_R_weighted, \
rmse_final_t_weighted = compute_rmse_weighted(initial_guess_main_rear, XOpt_T_main_rear, A, B, rho, omega)
rmse_init_unweighted, rmse_final_unweighted, rmse_init_R_unweighted, \
rmse_init_t_unweighted, rmse_final_R_unweighted, \
rmse_final_t_unweighted = compute_rmse_unweighted(initial_guess_main_rear, XOpt_T_main_rear, A, B)
rmses = [
rmse_init_unweighted, rmse_final_unweighted, rmse_init_weighted,
rmse_final_weighted, rmse_init_R_unweighted, rmse_init_t_unweighted,
rmse_final_R_unweighted, rmse_final_t_unweighted, rmse_init_R_weighted,
rmse_init_t_weighted, rmse_final_R_weighted, rmse_final_t_weighted
]
# Display and save RMSEs
outpath = os.path.join(ANALYSIS_RESULTS_PATH, "main_rear_rmse.txt")
display_and_save_rmse(rmses, outpath)
# Save final estimates
final_estimate_outpath = os.path.join(FINAL_ESTIMATES_PATH,
"main_rear_final.txt")
np.savetxt(final_estimate_outpath, XOpt_T_main_rear)
################################################################################
######################## FRONT REAR CALIBRATION ################################
### PARAMETERS ###
A = np.array(rear_rel_poses) # First set of poses
B = np.array(front_rel_poses) # Second set of poses
N = min(A.shape[0], B.shape[0])
r = np.logical_or(np.array(front_reject[:N]), np.array(
rear_reject[:N])) # If either has high variance, reject the sample
omega = np.max([var_R_front,
var_R_rear]) # Take average across different odometries
rho = np.max([var_t_front,
var_t_rear]) # Take average across different odometries
### PARAMETERS ###
cost_front_rear = lambda x: cost(x, A, B, r, rho, omega, WEIGHTED)
# Carry out optimization for front-rear homogeneous transformations
problem_front_rear = Problem(
manifold=manifold, cost=cost_front_rear
) # (2a) Compute the optimization between main and front
solver_front_rear = CustomSteepestDescent(
) # (3) Instantiate a Pymanopt solver
Xopt_front_rear = solver_front_rear.solve(problem_front_rear,
x=X0_front_rear)
print("Initial Guess for Front-Rear Transformation: \n {}".format(
initial_guess_front_rear))
print("Optimal solution between front and rear reference frames: \n {}".
format(Xopt_front_rear))
# Take intermediate values for plotting
estimates_x_front_rear = solver_front_rear.estimates
errors_front_rear = solver_front_rear.errors
iters_front_rear = solver_front_rear.iterations
# Metrics dictionary
estimates_dict_front_rear = {
i: T
for i, T in zip(iters_front_rear, estimates_x_front_rear)
}
error_dict_front_rear = {
i: e
for i, e in zip(iters_front_rear, errors_front_rear)
}
# Save intermediate results to a pkl file
estimates_fname_front_rear = os.path.join(ANALYSIS_RESULTS_PATH,
"estimates_front_rear.pkl")
error_fname_front_rear = os.path.join(ANALYSIS_RESULTS_PATH,
"error_front_rear.pkl")
# Save estimates to pickle file
with open(estimates_fname_front_rear, "wb") as pkl_estimates:
pickle.dump(estimates_dict_front_rear, pkl_estimates)
pkl_estimates.close()
# Save error to pickle file
with open(error_fname_front_rear, "wb") as pkl_error:
pickle.dump(error_dict_front_rear, pkl_error)
pkl_error.close()
# Calculate difference between initial guess and final
XOpt_T_front_rear = construct_pose(Xopt_front_rear[0],
Xopt_front_rear[1].reshape((3, 1)))
print("DIFFERENCE IN MATRICES: \n {}".format(
np.subtract(XOpt_T_front_rear, initial_guess_front_rear)))
# Compute the weighted and unweighted RMSE
rmse_init_weighted, rmse_final_weighted, rmse_init_R_weighted, \
rmse_init_t_weighted, rmse_final_R_weighted, \
rmse_final_t_weighted = compute_rmse_weighted(initial_guess_front_rear,
XOpt_T_front_rear, A, B, rho,
omega)
rmse_init_unweighted, rmse_final_unweighted, rmse_init_R_unweighted, \
rmse_init_t_unweighted, rmse_final_R_unweighted, \
rmse_final_t_unweighted = compute_rmse_unweighted(initial_guess_front_rear,
XOpt_T_front_rear, A, B)
rmses = [
rmse_init_unweighted, rmse_final_unweighted, rmse_init_weighted,
rmse_final_weighted, rmse_init_R_unweighted, rmse_init_t_unweighted,
rmse_final_R_unweighted, rmse_final_t_unweighted, rmse_init_R_weighted,
rmse_init_t_weighted, rmse_final_R_weighted, rmse_final_t_weighted
]
# Display and save RMSEs
outpath = os.path.join(ANALYSIS_RESULTS_PATH, "front_rear_rmse.txt")
display_and_save_rmse(rmses, outpath)
# Save final estimates
final_estimate_outpath = os.path.join(FINAL_ESTIMATES_PATH,
"front_rear_final.txt")
np.savetxt(final_estimate_outpath, XOpt_T_front_rear)
################################################################################
# Display all results
print("_________________________________________________________")
print("_____________________ALL RESULTS_________________________")
print("_________________________________________________________")
print("Initial Guess for Main-Front Transformation: \n {}".format(
initial_guess_main_front))
print("Optimal solution between main and front reference frames: \n {}".
format(Xopt_main_front))
print("_________________________________________________________")
print("Initial Guess for Main-Rear Transformation: \n {}".format(
initial_guess_main_rear))
print("Optimal solution between main and rear reference frames: \n {}".
format(Xopt_main_rear))
print("_________________________________________________________")
print("Initial Guess for Front-Rear Transformation: \n {}".format(
initial_guess_front_rear))
print("Optimal solution between front and rear reference frames: \n {}".
format(Xopt_front_rear))
print("_________________________________________________________")
def fit_gpytorch_manifold(
mll: MarginalLogLikelihood,
bounds: Optional[ParameterBounds] = None,
solver: Solver = pyman_solvers.ConjugateGradient(maxiter=500),
nb_init_candidates: int = 200,
last_x_as_candidate_prob: float = 0.9,
options: Optional[Dict[str, Any]] = None,
track_iterations: bool = True,
approx_mll: bool = False,
module_to_array_func: TModToArray = module_to_list_of_array,
module_from_array_func: TArrayToMod = set_params_with_list_of_array,
) -> Tuple[MarginalLogLikelihood, Dict[str, Union[
float, List[OptimizationIteration]]]]:
"""
This function fits a gpytorch model by maximizing MLL with a pymanopt optimizer.
The model and likelihood in mll must already be in train mode.
This method requires that the model has `train_inputs` and `train_targets`.
Parameters
----------
:param mll: MarginalLogLikelihood to be maximized.
Optional parameters
-------------------
:param nb_init_candidates: number of random initial candidates for the GP parameters
:param last_x_as_candidate_prob: probability that the last set of parameter is among the initial candidates
:param bounds: A dictionary mapping parameter names to tuples of lower and upper bounds.
:param solver: Pymanopt solver.
:param options: Dictionary of solver options, passed along to scipy.minimize.
:param track_iterations: Track the function values and wall time for each iteration.
:param approx_mll: If True, use gpytorch's approximate MLL computation. This is disabled by default since the
stochasticity is an issue for determistic optimizers). Enabling this is only recommended when working with
large training data sets (n>2000).
Returns
-------
:return: 2-element tuple containing
- MarginalLogLikelihood with parameters optimized in-place.
- Dictionary with the following key/values:
"fopt": Best mll value.
"wall_time": Wall time of fitting.
"iterations": List of OptimizationIteration objects with information on each iteration.
If track_iterations is False, will be empty.
Example:
gp = SingleTaskGP(train_X, train_Y)
mll = ExactMarginalLogLikelihood(gp.likelihood, gp)
mll.train()
fit_gpytorch_scipy(mll)
mll.eval()
"""
options = options or {}
# Current parameters
x0, property_dict, bounds = module_to_array_func(module=mll,
bounds=bounds,
exclude=options.pop(
"exclude", None))
x0 = [x0i.astype(np.float64) for x0i in x0]
if bounds is not None:
warnings.warn(
'Bounds handling not supported yet in fit_gpytorch_manifold')
# bounds = Bounds(lb=bounds[0], ub=bounds[1], keep_feasible=True)
t1 = time.time()
# Define cost function
def cost(x):
param_dict = OrderedDict(mll.named_parameters())
idx = 0
for p_name, attrs in property_dict.items():
# Construct the new tensor
if len(attrs.shape) == 0: # deal with scalar tensors
# new_data = torch.tensor(x[0], dtype=attrs.dtype, device=attrs.device)
new_data = torch.tensor(x[idx][0],
dtype=attrs.dtype,
device=attrs.device)
else:
# new_data = torch.tensor(x, dtype=attrs.dtype, device=attrs.device).view(*attrs.shape)
new_data = torch.tensor(x[idx],
dtype=attrs.dtype,
device=attrs.device).view(*attrs.shape)
param_dict[p_name].data = new_data
idx += 1
# mllx = set_params_with_array(mll, x, property_dict)
train_inputs, train_targets = mll.model.train_inputs, mll.model.train_targets
mll.zero_grad()
output = mll.model(*train_inputs)
args = [output, train_targets] + _get_extra_mll_args(mll)
loss = -mll(*args).sum()
return loss
def egrad(x):
loss = cost(x)
loss.backward()
param_dict = OrderedDict(mll.named_parameters())
grad = []
for p_name in property_dict:
t = param_dict[p_name].grad
if t is None:
# this deals with parameters that do not affect the loss
if len(property_dict[p_name].shape
) > 1 and property_dict[p_name].shape[0] > 1:
# if the variable is a matrix, keep its shape
grad.append(np.zeros(property_dict[p_name].shape))
else:
grad.append(np.zeros(property_dict[p_name].shape))
else:
if t.ndim > 1 and t.shape[
0] > 1: # if the variable is a matrix, keep its shape
grad.append(t.detach().cpu().double().clone().numpy())
else: # Vector case
grad.append(
t.detach().view(-1).cpu().double().clone().numpy())
return grad
# Define the manifold (product of manifolds)
manifolds_list = []
for p_name, t in mll.named_parameters():
try:
# If a manifold is given add it
manifolds_list.append(attrgetter(p_name + "_manifold")(mll))
except AttributeError:
# Otherwise, default: Euclidean
manifolds_list.append(
Euclidean(int(np.prod(property_dict[p_name].shape))))
# Product of manifolds
manifold = Product(manifolds_list)
# Instanciate the problem on the manifold
if track_iterations:
verbosity = 2
else:
verbosity = 0
problem = Problem(manifold=manifold,
cost=cost,
egrad=egrad,
verbosity=verbosity,
arg=torch.Tensor()) #, precon=precon)
# For cases where the Hessian is hard/long to compute, we approximate it with finite differences of the gradient.
# Typical cases: the Hessian can be hard to compute due to the 2nd derivative of the eigenvalue decomposition,
# e.g. in the SPD affine-invariant distance.
problem._hess = types.MethodType(get_hessianfd, problem)
# Choose initial parameters
# Do not always consider x0, to encourage variations of the parameters.
if np.random.rand() < last_x_as_candidate_prob:
x0_candidates = [x0]
x0_candidates += [
manifold.rand() for i in range(nb_init_candidates - 1)
]
else:
x0_candidates = []
x0_candidates += [manifold.rand() for i in range(nb_init_candidates)]
for i in range(int(3 * nb_init_candidates / 4)):
x0_candidates[i][0:4] = x0[0:4] #TODO remove hard-coding
y0_candidates = [cost(x0_candidates[i]) for i in range(nb_init_candidates)]
y_init, x_init_idx = torch.Tensor(y0_candidates).min(0)
x_init = x0_candidates[x_init_idx]
with gpt_settings.fast_computations(log_prob=approx_mll):
# Logverbosity of the solver to 1
solver._logverbosity = 1
# Solve
opt_x, opt_log = solver.solve(problem, x=x_init)
# Construct info dict
info_dict = {
"fopt": float(cost(opt_x).detach().numpy()),
"wall_time": time.time() - t1,
"opt_log": opt_log,
}
# if not res.success: # TODO update
# try:
# # Some res.message are bytes
# msg = res.message.decode("ascii")
# except AttributeError:
# # Others are str
# msg = res.message
# warnings.warn(
# f"Fitting failed with the optimizer reporting '{msg}'", OptimizationWarning
# )
# Set to optimum
mll = module_from_array_func(mll, opt_x, property_dict)
return mll, info_dict
def _update_classifier(self, data, labels, w, classes):
"""Update the classifier parameters theta and bias
Parameters
----------
data : list of 2D arrays, element i has shape=[voxels_i, samples_i]
Each element in the list contains the fMRI data of one subject for
the classification task.
labels : list of arrays of int, element i has shape=[samples_i]
Each element in the list contains the labels for the data samples
in data_sup.
w : list of 2D array, element i has shape=[voxels_i, features]
The orthogonal transforms (mappings) :math:`W_i` for each subject.
classes : int
The number of classes in the classifier.
Returns
-------
theta : array, shape=[features, classes]
The MLR parameter for the class planes.
bias : array shape=[classes,]
The MLR parameter for class biases.
"""
# Stack the data and labels for training the classifier
data_stacked, labels_stacked, weights = \
SSSRM._stack_list(data, labels, w)
features = w[0].shape[1]
total_samples = weights.size
data_th = S.shared(data_stacked.astype(theano.config.floatX))
val_ = S.shared(labels_stacked)
total_samples_S = S.shared(total_samples)
theta_th = T.matrix(name='theta', dtype=theano.config.floatX)
bias_th = T.col(name='bias', dtype=theano.config.floatX)
constf2 = S.shared(self.alpha / self.gamma, allow_downcast=True)
weights_th = S.shared(weights)
log_p_y_given_x = \
T.log(T.nnet.softmax((theta_th.T.dot(data_th.T)).T + bias_th.T))
f = -constf2 * T.sum(
(log_p_y_given_x[T.arange(total_samples_S), val_]) /
weights_th) + 0.5 * T.sum(theta_th**2)
manifold = Product((Euclidean(features,
classes), Euclidean(classes, 1)))
problem = Problem(manifold=manifold,
cost=f,
arg=[theta_th, bias_th],
verbosity=0)
solver = ConjugateGradient(mingradnorm=1e-6)
solution = solver.solve(problem)
theta = solution[0]
bias = solution[1]
del constf2
del theta_th
del bias_th
del data_th
del val_
del solver
del solution
return theta, bias
def setUp(self):
self.m = m = 10
self.n = n = 5
self.man = Euclidean(m, n)
from pymanopt.manifolds import Euclidean
from pymanopt.solvers import TrustRegions
if __name__ == "__main__":
# Cost function is the squared reconstruction error
X = torch.from_numpy(np.zeros((200, 3)))
y = torch.from_numpy(np.zeros((200, 3)))
def cost(w):
return (y - X.matmul(w)).pow(2).sum()
# A solver that involves the hessian
solver = TrustRegions()
# R^3
manifold = Euclidean(3, 1)
# Create the problem with extra cost function arguments
problem = Problem(manifold=manifold,
cost=cost,
verbosity=0,
arg=torch.Tensor())
# Solve 5 instances of the same type of problem for different data input
for k in range(0, 5):
# Generate random data
X = torch.from_numpy(np.random.randn(200, 3))
y = torch.from_numpy(np.random.randn(200, 1))
wopt = solver.solve(problem)
print('Run {}'.format(k + 1))
class TestProductManifold(unittest.TestCase):
def setUp(self):
self.m = m = 100
self.n = n = 50
self.euclidean = Euclidean(m, n)
self.sphere = Sphere(n)
self.man = Product([self.euclidean, self.sphere])
def test_dim(self):
np_testing.assert_equal(self.man.dim, self.m*self.n+self.n-1)
def test_typicaldist(self):
np_testing.assert_equal(self.man.typicaldist,
np.sqrt((self.m*self.n)+np.pi**2))
def test_dist(self):
X = self.man.rand()
Y = self.man.rand()
np_testing.assert_equal(self.man.dist(X, Y),
np.sqrt(
self.euclidean.dist(X[0], Y[0])**2 +
self.sphere.dist(X[1], Y[1])**2))
# def test_inner(self):
# def test_proj(self):
# def test_ehess2rhess(self):
# def test_retr(self):
# def test_egrad2rgrad(self):
# def test_norm(self):
# def test_rand(self):
# def test_randvec(self):
# def test_transp(self):
def test_exp_log_inverse(self):
s = self.man
X = s.rand()
Y = s.rand()
Yexplog = s.exp(X, s.log(X, Y))
np_testing.assert_almost_equal(s.dist(Y, Yexplog), 0)
def test_log_exp_inverse(self):
s = self.man
X = s.rand()
U = s.randvec(X)
Ulogexp = s.log(X, s.exp(X, U))
np_testing.assert_array_almost_equal(U[0], Ulogexp[0])
np_testing.assert_array_almost_equal(U[1], Ulogexp[1])
def test_pairmean(self):
s = self.man
X = s.rand()
Y = s.rand()
Z = s.pairmean(X, Y)
np_testing.assert_array_almost_equal(s.dist(X, Z), s.dist(Y, Z))
def setUp(self):
self.m = m = 100
self.n = n = 50
self.euclidean = Euclidean(m, n)
self.sphere = Sphere(n)
self.man = Product([self.euclidean, self.sphere])
def setUp(self):
self.m = m = 100
self.n = n = 50
self.euclidean = Euclidean(m, n)
self.sphere = Sphere(n)
self.man = Product([self.euclidean, self.sphere])
class TestProductManifold(unittest.TestCase):
def setUp(self):
self.m = m = 100
self.n = n = 50
self.euclidean = Euclidean(m, n)
self.sphere = Sphere(n)
self.man = Product([self.euclidean, self.sphere])
def test_dim(self):
np_testing.assert_equal(self.man.dim, self.m * self.n + self.n - 1)
def test_typicaldist(self):
np_testing.assert_equal(self.man.typicaldist,
np.sqrt((self.m * self.n) + np.pi**2))
def test_dist(self):
X = self.man.rand()
Y = self.man.rand()
np_testing.assert_equal(
self.man.dist(X, Y),
np.sqrt(
self.euclidean.dist(X[0], Y[0])**2 +
self.sphere.dist(X[1], Y[1])**2))
# def test_inner(self):
# def test_proj(self):
# def test_ehess2rhess(self):
# def test_retr(self):
# def test_egrad2rgrad(self):
# def test_norm(self):
# def test_rand(self):
# def test_randvec(self):
# def test_transp(self):
def test_exp_log_inverse(self):
s = self.man
X = s.rand()
Y = s.rand()
Yexplog = s.exp(X, s.log(X, Y))
np_testing.assert_almost_equal(s.dist(Y, Yexplog), 0)
def test_log_exp_inverse(self):
s = self.man
X = s.rand()
U = s.randvec(X)
Ulogexp = s.log(X, s.exp(X, U))
np_testing.assert_array_almost_equal(U[0], Ulogexp[0])
np_testing.assert_array_almost_equal(U[1], Ulogexp[1])
def test_pairmean(self):
s = self.man
X = s.rand()
Y = s.rand()
Z = s.pairmean(X, Y)
np_testing.assert_array_almost_equal(s.dist(X, Z), s.dist(Y, Z))
def manifold_fit(self, Y, X):
from pymanopt.manifolds import Rotations, Euclidean, Product
from pymanopt import Problem
from pymanopt.solvers import TrustRegions
# from minimal_varx import make_normalized_G
cov_res, cov_xlag, cov_y_xlag = calc_extended_covariance(Y, X, self.p)
self.set_covs(cov_res, cov_xlag)
# m = X.shape[0]
with_c = (self.mm_degree > self.agg_rnk) and (self.m - self.agg_rnk)
C = Euclidean(self.mm_degree - self.agg_rnk, self.m - self.agg_rnk)
RG = Rotations(self.m)
if with_c:
raise (ValueError(
"This option is implemented only for self.agg_rnk == m"))
if with_c:
manifold = Product([RG, C])
else:
manifold = RG
if not with_c:
c_null = np.zeros(
(self.mm_degree - self.agg_rnk, self.m - self.agg_rnk))
else:
c_null = None
if with_c:
def cost(x):
o, c = x
G = make_normalized_G(self, self.m, o, c)
self.calc_states(G)
return self.neg_log_llk
else:
def cost(x):
G = make_normalized_G(self, self.m, x, c_null)
self.calc_states(G)
return self.neg_log_llk
if with_c:
def egrad(x):
o, c = x
grad_o, grad_c = self.map_o_c_grad(self._gradient_tensor, o, c)
return [grad_o, grad_c]
else:
def egrad(x):
grad_o, grad_c = self.map_o_c_grad(self._gradient_tensor, x,
c_null)
return grad_o
if with_c:
def ehess(x, Heta):
o, c = x
eta = make_normalized_G(self, self.m, Heta[0], Heta[1])
hess_raw = self.hessian_prod(eta)
hess_o, hess_c = self.map_o_c_grad(hess_raw, o, c)
return [hess_o, hess_c]
else:
def ehess(x, Heta):
eta = make_normalized_G(self, self.m, Heta, c_null)
hess_raw = self.hessian_prod(eta)
hess_o, hess_c = self.map_o_c_grad(hess_raw, x, c_null)
return hess_o
if with_c:
min_mle = Problem(manifold, cost, egrad=egrad, ehess=ehess)
else:
min_mle = Problem(manifold, cost, egrad=egrad, ehess=ehess)
solver = TrustRegions()
opt = solver.solve(min_mle)
if with_c:
G_opt = make_normalized_G(self, self.m, opt[0], opt[1])
else:
G_opt = make_normalized_G(self, self.m, opt, c_null)
self.calc_H_F_Phi(G_opt, cov_y_xlag)
return opt
# +
import sys
sys.path.insert(0, "../..")
from autograd.scipy.special import logsumexp
import pymanopt
from pymanopt import Problem
from pymanopt.manifolds import Euclidean, Product, SymmetricPositiveDefinite
from pymanopt.optimizers import SteepestDescent
# (1) Instantiate the manifold
manifold = Product([SymmetricPositiveDefinite(D + 1, k=K), Euclidean(K - 1)])
# (2) Define cost function
# The parameters must be contained in a list theta.
@pymanopt.function.autograd(manifold)
def cost(S, v):
# Unpack parameters
nu = np.append(v, 0)
logdetS = np.expand_dims(np.linalg.slogdet(S)[1], 1)
y = np.concatenate([samples.T, np.ones((1, N))], axis=0)
# Calculate log_q
y = np.expand_dims(y, 0)
# 'Probability' of y belonging to each cluster
def run(self,
S,
F,
v=None,
C=None,
fs=None,
omega=None,
maxiter=500,
tol=1e-10,
variant='bp'):
'''
Run MERLiN algorithm.
Whether to run a scalar variant, i.e. S -> C -> w'F, or a
timeseries variant, i.e. S -> C -> bp(w'F) is determined by the
dimensionality of the input F.
Input (default)
- S
(m x 1) np.array that contains the samples of S
- F
either a (d x m) np.array that contains the linear mixture
samples or a (d x m x n) np.array that contains the linearly
mixed timeseries of length n (d channels, m trials)
- v
(d x 1) np.array holding the linear combination that
extracts middle node C from F
- C
(m x 1) np.array that contains the samples of the middle
node C
- fs
sampling rate in Hz
- omega
tuple of (low, high) cut-off of desired frequency band
- maxiter (500)
maximum iterations to run the optimisation algorithm for
- tol (1e-16)
terminate optimisation if step size < tol or grad norm < tol
- variant ('bp')
determines which MERLiN variant to use on timeseries data
('bp' = MERLiNbp algorithm ([1], Algorithm 4),
'bpicoh' = MERLiNbpicoh algorithm ([1], Algorithm 5),
'nlbp' = MERLiNnlbp)
Output
- w
linear combination that was found and should extract the
effect of C from F
- converged
boolean that indicates whether the stopping criterion was
met before the maximum number of iterations was performed
- curob
objecive functions value at w
'''
self._S = S
self._Forig = F
self._fs = fs
self._omega = omega
self._d = F.shape[0]
self._m = F.shape[1]
# scalar or timeseries mode
if F.ndim == 3:
self._mode = 'timeseries'
self._n = F.shape[2]
if not (fs and omega):
raise ValueError('Both the optional arguments fs and omega '
'need to be provided.')
if self._verbosity:
print('Launching MERLiN' + variant + ' for iid sampled '
'timeseries chunks.')
elif F.ndim == 2:
self._mode = 'scalar'
if self._verbosity:
print('Launching MERLiN for iid sampled scalars.')
else:
raise ValueError('F needs to be a 2-dimensional numpy array '
'(iid sampled scalars) or a 3-dimensional '
'numpy array (iid sampled timeseries chunks).')
self._prepare(v, C)
if self._mode is 'scalar':
problem = self._problem_MERLiN()
elif variant is 'bp':
problem = self._problem_MERLiNbp()
elif variant is 'bpicoh':
problem = self._problem_MERLiNbpicoh()
elif variant is 'nlbp':
problem = self._problem_MERLiNnlbp()
else:
raise NotImplementedError
if variant is not 'nlbp':
problem.manifold = Sphere(self._d, 1)
elif variant is 'nlbp':
problem.manifold = Product(
[Sphere(self._d, 1),
Euclidean(1, 1),
Euclidean(1, 1)])
# choose best out of ten 10-step runs as initialisation
solver = SteepestDescent(maxiter=10, logverbosity=1)
res = [solver.solve(problem) for k in range(0, 10)]
obs = [-r[1]['final_values']['f(x)'] for r in res]
w0 = res[obs.index(max(obs))][0]
solver = SteepestDescent(maxtime=float('inf'),
maxiter=maxiter,
mingradnorm=tol,
minstepsize=tol,
logverbosity=1)
if self._verbosity:
print('Running optimisation algorithm.')
w, info = solver.solve(problem, x=w0)
if variant is 'nlbp':
w = w[0]
converged = maxiter != info['final_values']['iterations']
curob = -float(info['final_values']['f(x)'])
if self._verbosity:
print('DONE.')
return self._P.T.dot(w), converged, curob
def setUp(self):
self.m = m = 10
self.n = n = 5
self.manifold = Euclidean(m, n)
super().setUp()
def cross_validation(odom_1,
aligned_1,
odom_2,
aligned_2,
type_1,
type_2,
K=10):
"""Function to run cross-validation to run nonlinear optimization for optimal
pose estimation and evaluation. Performs cross-validation K times and splits
the dataset into K (approximately) even splits, to be used for in-sample
training and out-of-sample evaluation.
This function estimates a relative transformation between two lidar frames
using nonlinear optimization, and evaluates the robustness of this estimate
through K-fold cross-validation performance of our framework. Though this
function does not return any values, it saves all results in the
'results' relative path.
Parameters:
odom_1 (pd.DataFrame): DataFrame corresponding to odometry data for the
pose we wish to transform into the odom_2 frame of reference. See
data/main_odometry.csv for an example of the headers/columns/data
types this function expects this DataFrame to have.
aligned_1 (pd.DataFrame): DataFrame corresponding to aligned odometry
data given the 3 sets of odometry data for the 3 lidar sensors. This
data corresponds to the odom_1 sensor frame.
odom_2 (pd.DataFrame): DataFrame corresponding to odometry data for the
pose we wish to transform the odom_1 frame of reference into. See
data/main_odometry.csv for an example of the headers/columns/data
types this function expects this DataFrame to have.
aligned_2 (pd.DataFrame): DataFrame corresponding to aligned odometry
data given the 3 sets of odometry data for the 3 lidar sensors. This
data corresponds to the odom_2 sensor frame.
type_1 (str): String denoting the lidar type. Should be in the set
{'main', 'front', 'rear'}. This type corresponds to the data type
for the odom_1 frame.
type_2 (str): String denoting the lidar type. Should be in the set
{'main', 'front', 'rear'}. This type corresponds to the data type
for the odom_2 frame.
K (int): The number of folds to be used for cross-validation. Defaults
to 10.
"""
# Get ICP covariance matrices
# Odom 1 lidar odometry
odom1_icp, odom1_trans_cov, odom1_trans_cov_max, \
odom1_trans_cov_avg, odom1_rot_cov, odom1_rot_cov_max, \
odom1_rot_cov_avg, odom1_reject = parse_icp_cov(odom_1, type=type_1,
reject_thr=REJECT_THR)
# Odom 2 lidar odometry
odom2_icp, odom2_trans_cov, odom2_trans_cov_max, \
odom2_trans_cov_avg, odom2_rot_cov, odom2_rot_cov_max, \
odom2_rot_cov_avg, odom2_reject = parse_icp_cov(odom_2, type=type_2,
reject_thr=REJECT_THR)
# Calculate relative poses
(odom1_aligned,
odom1_rel_poses) = relative_pose_processing.calc_rel_poses(aligned_1)
(odom2_aligned,
odom2_rel_poses) = relative_pose_processing.calc_rel_poses(aligned_2)
# Compute weights for weighted estimate
cov_t_odom1, cov_R_odom1 = compute_weights_euler(odom1_aligned)
cov_t_odom2, cov_R_odom2 = compute_weights_euler(odom2_aligned)
# Extract a single scalar using the average value from rotation and translation
var_t_odom1 = extract_variance(cov_t_odom1, mode="max")
var_R_odom1 = extract_variance(cov_R_odom1, mode="max")
var_t_odom2 = extract_variance(cov_t_odom2, mode="max")
var_R_odom2 = extract_variance(cov_R_odom2, mode="max")
# Optimization (1) Instantiate a manifold
translation_manifold = Euclidean(3) # Translation vector
so3 = Rotations(3) # Rotation matrix
manifold = Product((so3, translation_manifold)) # Instantiate manifold
# Get initial guesses for our estimations
if os.path.exists(PKL_POSES_PATH): # Check to make sure path exists
transforms_dict = load_transforms(
PKL_POSES_PATH) # Relative transforms
# Map types to sensor names to access initial estimate relative transforms
types2sensors = {"main": "velodyne", "front": "front", "rear": "rear"}
# Now get initial guesses from the relative poses
initial_guess_odom1_odom2 = transforms_dict["{}_{}".format(
types2sensors[type_1], types2sensors[type_2])]
# Print out all the initial estimates as poses
print("INITIAL GUESS {} {}: \n {} \n".format(types2sensors[type_1],
types2sensors[type_2],
initial_guess_odom1_odom2))
# Get rotation matrices for initial guesses
R0_odom1_odom2, t0_odom1_odom2 = initial_guess_odom1_odom2[:3, :3], \
initial_guess_odom1_odom2[:3, 3]
X0_odom1_odom2 = (R0_odom1_odom2, t0_odom1_odom2) # Pymanopt estimate
print("INITIAL GUESS {} {}: \n R0: \n {} \n\n t0: \n {} \n".format(
types2sensors[type_1], types2sensors[type_2], R0_odom1_odom2,
t0_odom1_odom2))
# Create KFold xval object to get training/validation indices
kf = KFold(n_splits=K, random_state=None, shuffle=False)
k = 0 # Set fold counter to 0
# Dataset
A = np.array(odom2_rel_poses) # First set of poses
B = np.array(odom1_rel_poses) # Second set of poses
N = len(A)
assert len(A) == len(B) # Sanity check to ensure odometry data matches
r = np.logical_or(np.array(odom1_reject)[:N],
np.array(odom2_reject)[:N]) # Outlier rejection
print("NUMBER OF CROSS-VALIDATION FOLDS: {}".format(K))
# Iterate over 30 second intervals of the poses
for train_index, test_index in kf.split(
A): # Perform K-fold cross-validation
# Path for results from manifold optimization
analysis_results_path = os.path.join(ANALYSIS_RESULTS_PATH,
"k={}".format(k))
final_estimates_path = os.path.join(FINAL_ESTIMATES_PATH,
"k={}".format(k))
odometry_plots_path = os.path.join(ODOMETRY_PLOTS_PATH,
"k={}".format(k))
# Make sure all paths exist - if they don't create them
for path in [
analysis_results_path, final_estimates_path,
odometry_plots_path
]:
check_dir(path)
# Get training data
A_train = A[train_index]
B_train = B[train_index]
N_train = min(A_train.shape[0], B_train.shape[0])
r_train = r[train_index]
print("FOLD NUMBER: {}, NUMBER OF TRAINING SAMPLES: {}".format(
k, N_train))
omega = np.max([var_R_odom1, var_R_odom2
]) # Take average across different odometries
rho = np.max([var_t_odom1,
var_t_odom2]) # Take average across different odometries
cost_lambda = lambda x: cost(x, A_train, B_train, r_train, rho, omega,
WEIGHTED) # Create cost function
problem = Problem(manifold=manifold,
cost=cost_lambda) # Create problem
solver = CustomSteepestDescent() # Create custom solver
X_opt = solver.solve(problem, x=X0_odom1_odom2) # Solve problem
print("Initial Guess for Main-Front Transformation: \n {}".format(
initial_guess_odom1_odom2))
print("Optimal solution between {} and {} "
"reference frames: \n {}".format(types2sensors[type_1],
types2sensors[type_2], X_opt))
# Take intermediate values for plotting
estimates_x = solver.estimates
errors = solver.errors
iters = solver.iterations
# Metrics dictionary
estimates_dict = {i: T for i, T in zip(iters, estimates_x)}
error_dict = {i: e for i, e in zip(iters, errors)}
# Save intermediate results to a pkl file
estimates_fname = os.path.join(
analysis_results_path,
"estimates_{}_{}.pkl".format(types2sensors[type_1],
types2sensors[type_2], X_opt))
error_fname = os.path.join(
analysis_results_path,
"error_{}_{}.pkl".format(types2sensors[type_1],
types2sensors[type_2], X_opt))
# Save estimates to pickle file
with open(estimates_fname, "wb") as pkl_estimates:
pickle.dump(estimates_dict, pkl_estimates)
pkl_estimates.close()
# Save error to pickle file
with open(error_fname, "wb") as pkl_error:
pickle.dump(error_dict, pkl_error)
pkl_error.close()
# Calculate difference between initial guess and final
X_opt_T = construct_pose(X_opt[0], X_opt[1].reshape((3, 1)))
print("DIFFERENCE IN MATRICES: \n {}".format(
np.subtract(X_opt_T, initial_guess_odom1_odom2)))
# Compute the weighted RMSE (training/in-sample)
train_rmse_init_weighted, train_rmse_final_weighted, train_rmse_init_R_weighted, \
train_rmse_init_t_weighted, train_rmse_final_R_weighted, \
train_rmse_final_t_weighted = compute_rmse_weighted(
initial_guess_odom1_odom2, X_opt_T, A_train, B_train, rho, omega)
# Compute the unweighted RMSE (training/in-sample)
train_rmse_init_unweighted, train_rmse_final_unweighted, train_rmse_init_R_unweighted, \
train_rmse_init_t_unweighted, train_rmse_final_R_unweighted, \
train_rmse_final_t_unweighted = compute_rmse_unweighted(
initial_guess_odom1_odom2, X_opt_T, A_train, B_train)
# Concatenate all RMSE values for training/in-sample
train_rmses = [
train_rmse_init_unweighted, train_rmse_final_unweighted,
train_rmse_init_weighted, train_rmse_final_weighted,
train_rmse_init_R_unweighted, train_rmse_init_t_unweighted,
train_rmse_final_R_unweighted, train_rmse_final_t_unweighted,
train_rmse_init_R_weighted, train_rmse_init_t_weighted,
train_rmse_final_R_weighted, train_rmse_final_t_weighted
]
# Display and save RMSEs
outpath = os.path.join(
analysis_results_path,
"train_rmse_{}_{}.txt".format(types2sensors[type_1],
types2sensors[type_2]))
display_and_save_rmse(train_rmses, outpath)
# Get test data
A_test = A[test_index]
B_test = B[test_index]
N_test = min(A_test.shape[0], B_test.shape[0])
print("NUMBER OF TEST SAMPLES: {}".format(N_test))
# Compute the weighted RMSE (testing/out-of-sample)
test_rmse_init_weighted, test_rmse_final_weighted, test_rmse_init_R_weighted, \
test_rmse_init_t_weighted, test_rmse_final_R_weighted, \
test_rmse_final_t_weighted = compute_rmse_weighted(initial_guess_odom1_odom2,
X_opt_T, A_test, B_test, rho, omega)
# Compute the unweighted RMSE (testing/out-of-sample)
test_rmse_init_unweighted, test_rmse_final_unweighted, test_rmse_init_R_unweighted, \
test_rmse_init_t_unweighted, test_rmse_final_R_unweighted, \
test_rmse_final_t_unweighted = compute_rmse_unweighted(initial_guess_odom1_odom2,
X_opt_T, A_test, B_test)
# Concatenate all RMSE values for testing/out-of-sample
test_rmses = [
test_rmse_init_unweighted, test_rmse_final_unweighted,
test_rmse_init_weighted, test_rmse_final_weighted,
test_rmse_init_R_unweighted, test_rmse_init_t_unweighted,
test_rmse_final_R_unweighted, test_rmse_final_t_unweighted,
test_rmse_init_R_weighted, test_rmse_init_t_weighted,
test_rmse_final_R_weighted, test_rmse_final_t_weighted
]
# Display and save RMSEs
outpath = os.path.join(
analysis_results_path,
"test_rmse_{}_{}.txt".format(types2sensors[type_1],
types2sensors[type_2]))
display_and_save_rmse(test_rmses, outpath)
# Save final estimates
final_estimate_outpath = os.path.join(
final_estimates_path, "{}_{}.txt".format(types2sensors[type_1],
types2sensors[type_2]))
np.savetxt(final_estimate_outpath, X_opt_T)
# Finally, increment k
k += 1
class TestProductManifold(ManifoldTestCase):
def setUp(self):
self.m = m = 100
self.n = n = 50
self.euclidean = Euclidean(m, n)
self.sphere = Sphere(n)
self.manifold = Product([self.euclidean, self.sphere])
point = self.manifold.random_point()
@pymanopt.function.autograd(self.manifold)
def cost(*x):
return np.sum([np.linalg.norm(a - b)**2 for a, b in zip(x, point)])
self.cost = cost
def test_dim(self):
np_testing.assert_equal(self.manifold.dim,
self.m * self.n + self.n - 1)
def test_typical_dist(self):
np_testing.assert_equal(self.manifold.typical_dist,
np.sqrt((self.m * self.n) + np.pi**2))
def test_dist(self):
X = self.manifold.random_point()
Y = self.manifold.random_point()
np_testing.assert_equal(
self.manifold.dist(X, Y),
np.sqrt(
self.euclidean.dist(X[0], Y[0])**2 +
self.sphere.dist(X[1], Y[1])**2),
)
def test_tangent_vector_multiplication(self):
# Regression test for https://github.com/pymanopt/pymanopt/issues/49.
manifold = Product((Euclidean(12), Grassmann(12, 3)))
x = manifold.random_point()
eta = manifold.random_tangent_vector(x)
np.float64(1.0) * eta
# def test_inner_product(self):
# def test_projection(self):
# def test_euclidean_to_riemannian_hessian(self):
# def test_retraction(self):
def test_first_order_function_approximation(self):
self.run_gradient_approximation_test()
def test_second_order_function_approximation(self):
self.run_hessian_approximation_test()
# def test_norm(self):
# def test_random_point(self):
# def test_random_tangent_vector(self):
# def test_transport(self):
def test_exp_log_inverse(self):
s = self.manifold
X = s.random_point()
Y = s.random_point()
Yexplog = s.exp(X, tangent_vector=s.log(X, Y))
np_testing.assert_almost_equal(s.dist(point_a=Y, point_b=Yexplog), 0)
def test_log_exp_inverse(self):
s = self.manifold
X = s.random_point()
U = s.random_tangent_vector(X)
Ulogexp = s.log(X, s.exp(X, U))
np_testing.assert_array_almost_equal(U[0], Ulogexp[0])
np_testing.assert_array_almost_equal(U[1], Ulogexp[1])
def test_pair_mean(self):
s = self.manifold
X = s.random_point()
Y = s.random_point()
Z = s.pair_mean(X, Y)
np_testing.assert_array_almost_equal(s.dist(X, Z), s.dist(Y, Z))
class TestProductManifold(unittest.TestCase):
def setUp(self):
self.m = m = 100
self.n = n = 50
self.euclidean = Euclidean(m, n)
self.sphere = Sphere(n)
self.man = Product([self.euclidean, self.sphere])
def test_dim(self):
np_testing.assert_equal(self.man.dim, self.m * self.n + self.n - 1)
def test_typicaldist(self):
np_testing.assert_equal(self.man.typicaldist,
np.sqrt((self.m * self.n) + np.pi**2))
def test_dist(self):
X = self.man.rand()
Y = self.man.rand()
np_testing.assert_equal(
self.man.dist(X, Y),
np.sqrt(
self.euclidean.dist(X[0], Y[0])**2 +
self.sphere.dist(X[1], Y[1])**2))
def test_tangent_vector_multiplication(self):
# Regression test for https://github.com/pymanopt/pymanopt/issues/49.
man = Product((Euclidean(12), Grassmann(12, 3)))
x = man.rand()
eta = man.randvec(x)
np.float64(1.0) * eta
# def test_inner(self):
# def test_proj(self):
# def test_ehess2rhess(self):
# def test_retr(self):
# def test_egrad2rgrad(self):
# def test_norm(self):
# def test_rand(self):
# def test_randvec(self):
# def test_transp(self):
def test_exp_log_inverse(self):
s = self.man
X = s.rand()
Y = s.rand()
Yexplog = s.exp(X, s.log(X, Y))
np_testing.assert_almost_equal(s.dist(Y, Yexplog), 0)
def test_log_exp_inverse(self):
s = self.man
X = s.rand()
U = s.randvec(X)
Ulogexp = s.log(X, s.exp(X, U))
np_testing.assert_array_almost_equal(U[0], Ulogexp[0])
np_testing.assert_array_almost_equal(U[1], Ulogexp[1])
def test_pairmean(self):
s = self.man
X = s.rand()
Y = s.rand()
Z = s.pairmean(X, Y)
np_testing.assert_array_almost_equal(s.dist(X, Z), s.dist(Y, Z))
if __name__ == "__main__":
# Generate random data
X = np.random.randn(3, 100)
Y = X[0:1, :] - 2 * X[1:2, :] + np.random.randn(1, 100) + 5
# Cost function is the sqaured test error
w = T.matrix()
b = T.matrix()
cost = T.sum((Y - w.T.dot(X) - b[0, 0])**2)
# A solver that involves the hessian
solver = TrustRegions()
# R^3 x R^1
manifold = Product([Euclidean(3, 1), Euclidean(1, 1)])
# Solve the problem with pymanopt
problem = Problem(manifold=manifold, cost=cost, arg=[w, b], verbosity=0)
wopt = solver.solve(problem)
print('Weights found by pymanopt (top) / ' 'closed form solution (bottom)')
print(wopt[0].T)
print(wopt[1])
X1 = np.concatenate((X, np.ones((1, 100))), axis=0)
wclosed = np.linalg.inv(X1.dot(X1.T)).dot(X1).dot(Y.T)
print(wclosed[0:3].T)
print(wclosed[3])
def test_tangent_vector_multiplication(self):
# Regression test for https://github.com/pymanopt/pymanopt/issues/49.
man = Product((Euclidean(12), Grassmann(12, 3)))
x = man.rand()
eta = man.randvec(x)
np.float64(1.0) * eta
Sigma_norm_vec = np.reshape(Sigma_norm_B,(n*n,1))
Wcd_vec = np.dot(np.linalg.inv(lyap_tmp),Sigma_norm_vec)
Wcd = np.reshape(Wcd_vec,(n,n))
Wnum = np.dot(Wo,Wcd)
Wden = np.dot(Wo,Wcd-Sigma_norm_B)
num = np.linalg.det(Wnum+sigma2*I)
den = np.linalg.det(Wden+sigma2*I)
return -np.log2(num/den)/(2.*T)
def make_shannon_capacity_fact_fixed_time(T):
def shannon_capacity_fact_fixed_time(L):
return shannon_rate(L, T)
return shannon_capacity_fact_fixed_time
# instantiate manifold for Pymanopt
manifold_fact = Euclidean(n,n)
# instantiate solver for Pymanopt
solver = TrustRegions()
#### find R_max via bisection method ####
# min value T
T1=1e-8
shannon_capacity_fact_fixed_time = make_shannon_capacity_fact_fixed_time(T1)
problem = Problem(manifold=manifold_fact, cost=shannon_capacity_fact_fixed_time, verbosity=0)
L_opt1 = solver.solve(problem)
p_1 = -shannon_capacity_fact_fixed_time(L_opt1)
#print p_1
Sigma_opt1 = np.dot(L_opt1,L_opt1.transpose())
# Set up Estimate
W1_hatA = tf.Variable(tf.ones([dx,dx]))
W2_hatA = tf.Variable(tf.ones([dx,dy]))
# Cost function is the sqaured test error
W_hatA = tf.matmul(W1_hatA, W2_hatA)
Y_hatA = tf.matmul(X, W_hatA)
costA = tf.reduce_mean(tf.square(Y - Y_hatA))
# first-order, second-order
solver = StochasticGradient(stepsize=lr, logverbosity=2)
# Product Manifold
manifold = Product(( Euclidean(dx, dx), Euclidean(dx, dy) ))
# Solve the problem with pymanopt
problem = Problem(manifold=manifold, cost=cost, arg=[W1_hat, W2_hat], verbosity=2)
# tf standard GD
tf_gd_solver = tf.train.GradientDescentOptimizer(learning_rate=lr).minimize(costA)
niters = 10
wopt = [np.ones([dx,dx]), np.ones([dx,dy])] # initial point
sess = tf.Session()
sess.run(tf.global_variables_initializer())
print('SGD-PYMANOPT\tSGD-STANDARDTF')
for i in range(0,niters):
