def adam_solve(lambda_flows,
grad_energy_bound,
samples,
u_func,
h,
m=1000,
step_size=0.001):
'''
Uses adam solver to optimize the energy bound
'''
output = np.copy(
lambda_flows) # Copies so original parameters are not modified
print("BEFORE LEARNING:\n{}".format(output))
grad_energy_bound = autograd.grad(
energy_bound) # Autograd gradient of energy bound
g_eb = lambda lambda_flows, i: grad_energy_bound(
lambda_flows,
samples,
h,
u_func,
#beta= (0.1 + i/1000))
beta=min(1, 0.01 + i / 10000)) # Annealing
output = adam(g_eb,
output,
num_iters=m,
callback=callback,
step_size=step_size)
print("AFTER LEARNING:\n{}".format(output))
# Resample and flow a larger number of samples to better show fit
samples = np.random.randn(20000)[:, np.newaxis]
samples_flowed = flow_samples(output, samples, h)
np.savetxt("./linear_plots/flow_params.txt", output)
return samples_flowed
def runSB(psf, psf_k, imageArray):
nImages = np.shape(imageArray)[2]
results = imageArray * 0
for imageIdx in range(0, nImages):
if imageIdx < start:
continue
grndpath = '/home/moss/SMLM/data/fluorophores/frames/' + str(
imageIdx + 1).zfill(5) + '.csv'
grnd = Table.read(grndpath, format='ascii')
no_source = len(grnd['xnano'])
img = imageArray[:, :, imageIdx]
sub = img - np.average(img[img < np.average(img) + 3 * np.std(img)])
subnorm = sub / np.max(sub)
mock = makeMock(grnd)
#mocknorm = mock/np.max(mock);
sb = SparseBayes(subnorm, psf, psf_k, no_source)
#sb = SparseBayes_alpha(mock,psf,psf_k,no_source);
#sb = SparseBayes_nofft(mock,psf,psf_k,sig_psf,no_source);
#sb = SparseBayes_gaussian(subnorm,psf,psf_k);
results[:, :, imageIdx] = sb.res
s = 'nopri' + str(imageIdx + 1).zfill(5) + '.out'
np.savetxt(s, sb.res)
plt.imshow(results[:, :, imageIdx])
plt.show()
return results
def p_abs(dofold,nG,bproj,method='RT'):
dof = b_filter(dofold,bproj)
planewave={'p_amp':0,'s_amp':1,'p_phase':0,'s_phase':0}
theta = 0.
phi = 0.
t1 = time.time()
obj= rcwa_assembly(dof,nG,bproj,theta,phi,planewave)
vals = 0.
R = 0.
T = 0.
if method == 'V':
for i in range(Nlayer):
Mv=get_conv(1./Mx/My,dof[i*Mx*My:(i+1)*Mx*My].reshape((Mx,My)),obj.G)
vals = vals + np.real(obj.Volume_integral(1+i,Mv,Mv,Mv,normalize=1))*np.real(obj.omega)*np.imag(epsdiff)
elif method == 'RT':
R,T = obj.RT_Solve(normalize=1)
vals = 1-R-T
t2 = time.time()
if 'autograd' not in str(type(vals)):
global ctrl
np.savetxt('dof.txt',dof)
if ctrl<=1:
print(t2-t1)
if method == 'V':
print(ctrl,vals)
elif method == 'RT':
print(ctrl, 'R =',R,'T =',T,'Abs=',vals,'time=',t2-t1)
ctrl +=1
return vals
def evaluate(self, y_test , y_pred):
y_test = y_test.flatten()
y_test = self.normalize_Y(y_test)
np.savetxt("pred", y_pred, newline=" ")
np.savetxt("test", y_test, newline=" ")
error_rate = (np.sum(y_test == y_pred)/ y_test.size)
print(error_rate)
return error_rate
def adam_solve(lambda_flows, grad_energy_bound, samples, u_func, h, m=1000, step_size=0.001,
bnn=False):
'''
Uses adam solver to optimize the energy bound
'''
output = np.copy(lambda_flows) # Copies to avoid changing initial conditions
print("BEFORE LEARNING:\n{}".format(output))
grad_energy_bound = autograd.grad(energy_bound) # Autograd gradient of energy
g_eb = lambda lambda_flows, i: grad_energy_bound(lambda_flows, samples, h, u_func,
#beta= (0.1 + i/1000))
beta=min(2, i/1000), # Annealing
#beta=min(1, 0.01+i/10000),
bnn=bnn) # Annealing
output = adam(g_eb, output, num_iters=m, callback=callback, step_size=step_size)
print("\nAFTER LEARNING:\n{}".format(output))
#samples = np.random.randn(30000)[:,np.newaxis] # Plot with more samples for better clarity
q_0_mu = np.array([0,0])
q_0_sigma = 1
D = q_0_mu.shape[0]
#samples = np.random.multivariate_normal(q_0_mu, q_0_sigma*np.eye(D), 20000)
samples_flowed = flow_samples(output, samples, h)
#np.savetxt("./data_fit_1d/flow_params.txt", output)
np.savetxt("./nn_fit/flow_params.txt", output)
if(bnn):
np.savetxt("./nn_fit/energy_bound.txt", e_bound)
fig, ax = plt.subplots()
ax.plot(e_bound)
ax.set(title="Energy Bound")
plt.savefig("./nn_fit/energy_bound.png")
plt.close()
np.savetxt("./nn_fit/joint_probs.txt", joint_probs)
fig, ax = plt.subplots()
ax.plot(joint_probs)
ax.set(title="Joint Probability")
plt.savefig("./nn_fit/joint_probs.png")
plt.close()
np.savetxt("./nn_fit/flow_probs.txt", flow_probs)
fig, ax = plt.subplots()
ax.plot(flow_probs)
ax.set(title="Flow Probs")
plt.savefig("./nn_fit/flow_probs.png")
plt.close()
np.savetxt("./nn_fit/grad_norms.txt", grad_norms)
fig, ax = plt.subplots()
ax.plot(grad_norms)
ax.set(title="Gradient Norms")
plt.savefig("./nn_fit/grad_norms.png")
plt.close()
return samples_flowed
def print_perf(combined_params, iter, grad):
if iter % 10 == 0:
model_params, prop_params = combined_params
bound = -objective(combined_params, iter)
message = "{:15}|{:20}".format(iter, bound)
with open(f_head + '_ELBO.csv', 'a') as f_handle:
np.savetxt(f_handle, [[iter, bound]], fmt='%i,%f')
print(message)
def dump_state(self, xk):
'''
callback to save the state to disk during optimization
'''
filename = 'state.txt'
if not os.path.exists(filename):
past = np.zeros((0, xk.shape[0]))
else:
past = np.loadtxt(filename)
if past.ndim < 2:
past = past.reshape(1, -1)
np.savetxt(filename, np.append(past, xk.reshape(1, -1), axis=0))
def save_checkpoint(i_epoch,
i_batch,
output_folder,
shared_file_object=True,
obj_array=None,
optimizer=None):
np.savetxt(os.path.join(output_folder, 'checkpoint.txt'),
np.array([i_epoch, i_batch]),
fmt='%d')
if not shared_file_object:
np.save(os.path.join(output_folder, 'obj_checkpoint.npy'), obj_array)
optimizer.save_param_arrays_to_checkpoint()
return
def runSB(imageDir, saveDir, psf, psf_k, imageArray):
nImages = np.shape(imageArray)[2]
results = imageArray * 0
for imageIdx in range(0, nImages):
if imageIdx < start:
continue
grndpath = imageDir + str(imageIdx) + '.truth'
grnd = np.loadtxt(grndpath)
no_source = np.shape(grnd)[0]
if len(np.shape(grnd)) < 2:
no_source = 1
img = imageArray[:, :, imageIdx]
sb = SparseBayes(img, psf, psf_k, no_source)
results[:, :, imageIdx] = sb.res
s = saveDir + str(imageIdx) + '.out'
np.savetxt(s, sb.res)
#plt.imshow(results[:,:,imageIdx]);
#plt.show();
return results
print 'choice: ', solve_choice
smooth_choice = sys.argv[10]
recursive_choice = sys.argv[11]
global order_file_name
order_file_name = sys.argv[12]
if order_file_name != None:
order = np.loadtxt(order_file_name)
order = order.astype(np.uint8)
print 'order', order
### save for application
np.savetxt(save_for_application_path_prefix + order_file_name,
order)
#### reorder the primary pigments
H_ordered = H[order, :]
H = H_ordered.copy()
### save reordered primary pigments.
np.savetxt(
os.path.splitext(KS_file_name)[0] + "-" +
os.path.splitext(order_file_name)[0] + ".txt", H)
R_vector = equations_in_RealPigments(H[:, :L],
H[:, L:],
r=1.0,
h=1.0)
P_vector = R_vector * Illuminantnew[:, 1].reshape(
(1, -1)) ### shape is N*L
def save_layers(x0, arr, H, output_prefix):
shape = arr.shape
original_shape = shape
img_size = shape[:2]
N = shape[0] * shape[1]
M = H.shape[0]
L = H.shape[1] / 2
Thickness = x0.reshape((N, M))
K0 = H[:, :L]
S0 = H[:, L:]
print Thickness.sum(axis=1).min()
print Thickness.sum(axis=1).max()
R_vector = np.ones((N, L))
for i in range(M):
R_vector = equations_in_RealPigments(K0[i:i + 1, :],
S0[i:i + 1, :],
r=R_vector,
h=Thickness[:, i:i + 1])
### from R spectrum x wavelength spectrums to linear rgb colors
P_vector = R_vector * Illuminantnew[:, 1].reshape(
(1, -1)) ### shape is N*L
R_xyz = (P_vector.reshape((N, 1, L)) * R_xyzcoeff.reshape(
(1, 3, L))).sum(axis=2) ###shape N*3*L to shape N*3
Normalize = (Illuminantnew[:, 1] *
R_xyzcoeff[1, :]).sum() ### scalar value.
R_xyz = R_xyz / Normalize ####xyz value shape is N*3
R_rgb = np.dot(
xyztorgb,
R_xyz.transpose()).transpose() ###linear rgb value, shape is N*3
R_rgb = Gamma_trans_img(R_rgb.clip(0, 1)) ##clip and gamma correction
print 'RGB RMSE: ', np.sqrt(
np.square(255 * (arr.reshape((-1, 3)) - R_rgb)).sum() / N)
filename = output_prefix + "-fixed_KS-reconstructed.png"
plt.imsave(filename, (R_rgb.reshape(original_shape) * 255.0).clip(
0, 255).round().astype(np.uint8))
np.savetxt(output_prefix + "-thickness.txt", Thickness)
#### save for applications
filename = save_for_application_path_prefix + os.path.splitext(
img_file)[0] + "-" + str(M) + "-KM_layers-" + os.path.splitext(
order_file_name)[0] + "-reconstructed.png"
plt.imsave(filename, (R_rgb.reshape(original_shape) * 255.0).clip(
0, 255).round().astype(np.uint8))
### compute sparsity
sparsity_thres_array = np.array(
[0.000001, 0.00001, 0.0001, 0.001, 0.01, 0.1])
Thickness_sparsity_list = np.ones(len(sparsity_thres_array))
for thres_ind in xrange(len(sparsity_thres_array)):
Thickness_sparsity_list[thres_ind] = len(Thickness[
Thickness <= sparsity_thres_array[thres_ind]]) * 1.0 / (N * M)
print "Thickness_sparsity_list: ", Thickness_sparsity_list
np.savetxt(output_prefix + "-Thickness-Sparsity.txt",
Thickness_sparsity_list)
# normalized_Thickness=Thickness/Thickness.sum(axis=1).reshape((-1,1))
# for i in xrange(M):
# #### save normalized_weights_map for each pigment.
# normalized_thickness_map_name=output_prefix+"-normalized_thickness_map-%02d.png" % i
# normalized_thickness_map=normalized_Thickness[:,i].reshape(img_size).copy()
# Image.fromarray((normalized_thickness_map*255.0).clip(0,255).round().astype(np.uint8)).save(normalized_thickness_map_name)
Thickness_sum_map = Thickness.sum(axis=1).reshape(img_size)
T_min = Thickness_sum_map.min()
T_max = Thickness_sum_map.max()
Thickness_sum_map = Thickness_sum_map / T_max
Image.fromarray((Thickness_sum_map * 255.0).round().astype(
np.uint8)).save(output_prefix + "-thickness_sum_map-min-" +
str(T_min) + "-max-" + str(T_max) + ".png")
for i in xrange(M):
thickness_map_name = output_prefix + "-layer_thickness_map-%02d.png" % i
Thickness_map = Thickness[:, i].reshape(img_size).copy()
Large_than_one = len(Thickness_map[Thickness_map > 1.0])
if Large_than_one > 0:
print "Number of Thickness value that is larger than 1.0 is : ", Large_than_one
Image.fromarray((Thickness_map * 255.0).clip(0, 255).round().astype(
np.uint8)).save(thickness_map_name)
####save for application
thickness_map_name = save_for_application_path_prefix + os.path.splitext(
img_file)[0] + "-" + str(M) + "-KM_layers-" + os.path.splitext(
order_file_name)[0] + "-thickness_map-%02d.png" % i
Image.fromarray((Thickness_map * 255.0).clip(0, 255).round().astype(
np.uint8)).save(thickness_map_name)
history.append((loss, ws, thetas))
# get gradient of loss function
grad = gradlossFn(ws, thetas, yobs, **lossfn_kwargs)
# compute new support
theta = lmo(grad)
# update signal parameters
if i == 0: _thetas = np.append(thetas, theta)
else:
_thetas = np.append(np.atleast_2d(thetas),
np.atleast_2d(theta),
axis=0)
ws, thetas = local_update(_thetas, yobs, lossFn, **lossfn_kwargs)
# calculate output
output = Psi(ws, thetas)
if (i > 2) and (history[-2][0] - history[-1][0] < 1.):
return loss, ws, thetas
return loss, ws, thetas
# run adcg
loss, ws, thetas = adcg(image.flatten(), ell, gradell, coordinate_descent,
n_adcg)
# writeout ADCG output
np.savetxt(foutput, np.vstack([thetas.T, ws.T]).T)
P0 = (logprob(ws.reshape(1, num_weights), inputs, targets))
print("P0", P0)
alpha = (Pprime - P0)
print("alpha", alpha)
bb = np.log(rs.rand(1))
print(bb)
accp[i] = 1 if bb <= alpha else 0
print("accept", bb <= alpha, "value", accp[i])
wi = wi_prime if bb <= alpha else wi
print("new wi", wi)
ws[i] = wi
# Let the first 9999 to burn-in
# for m in range(500):
# wi = ws[i]
# wi_prime = rs.randn(1) + wi
# ws_tmp = ws
# ws_tmp[i] = wi_prime
# Pprime = (logprob(ws_tmp.reshape(1,num_weights), inputs, targets))
# P0 = (logprob(ws.reshape(1, num_weights), inputs, targets))
# alpha = np.exp(Pprime - P0)
# wi = wi_prime if rs.rand(1) < alpha else wi
# ws[i] = wi
ws_df = np.concatenate([ws_df, ws.reshape(num_weights, 1)], axis=1)
print("accp vec", accp.reshape(num_weights, 1))
accp_df = np.concatenate(
[accp_df, accp.reshape(num_weights, 1)], axis=1)
filename = "gibbs_wts2" + "K" + str(k) + ".csv"
accp_file = "accp" + "K" + str(k) + ".csv"
np.savetxt(filename, ws_df, delimiter=',', fmt='%f')
np.savetxt(accp_file, accp_df, delimiter=',', fmt='%d')
D1, D2, D3, G, nodes_index = gsm.get_similarity_matrix(dotpath)
#D3=np.zeros((len(D1),len(D1)))
#D1[D1==0]=100
#D2[D2==0]=100
#D3[D3==0]=100
print(len(np.where(D1 == 0)))
A = np.random.rand(len(D1) * dim, 1)
mview = multiview(D1, D2, D3, dim, eps)
pos1, costs = mview.multiview_mds(A, steps, alpha, stopping_eps, outdir,
'collaboration')
pos1 = pos1.reshape(int(len(pos1) / dim), dim)
np.savetxt("data_pos_" + graphname + ".csv", pos1, delimiter=",")
#write to file
jsdata = "var points=" + str(pos1.tolist()) + ";"
file_path = outdir + "data_pos_" + graphname + ".js"
f = open(file_path, "w")
f.write(jsdata)
f.close()
d = json_graph.node_link_data(G)
#json.dump(d, open(outdir+'graph_'+graphname+'.json', 'w'))
j = open(outdir + 'graph_' + graphname + '.json', 'w')
j.write("edges='" + str(d['links']).replace("\'", "\"") + "';")
j.write("nodes='" + str(d['nodes']).replace("\'", "\"") + "';")
j.close()
init_var_params = np.concatenate([init_mean, init_log_std])
print("Optimizing variational parameters...")
variational_params = adam(gradient,
init_var_params,
step_size=0.1,
num_iters=200,
callback=callback)
# print(variational_params)
#
# Sample functions from the final posterior.
rs = npr.RandomState(0)
mean, log_std = unpack_params(variational_params)
# rs = npr.RandomState(0)
sample_weights = rs.randn(1000, num_weights) * np.exp(log_std) + mean
np.savetxt("VIwts.csv", sample_weights, delimiter=',', fmt='%f')
plot_inputs = np.linspace(-2, 2, num=400)
outputs_final = predictions(sample_weights,
np.expand_dims(plot_inputs, 1))
lowerbd = numpy.quantile(outputs_final, 0.05, axis=0)
upperbd = numpy.quantile(outputs_final, 0.95, axis=0)
inconint = np.logical_and(lowerbd < tot_targets,
upperbd > tot_targets).ravel()
con_ind = np.zeros(len(lowerbd))
con_ind[inconint] = 1
con_ind = con_ind.reshape(len(con_ind), 1)
coverage_df = np.concatenate([coverage_df, con_ind], axis=1)
# # Plot data and functions.
fig = plt.figure(figsize=(12, 8), facecolor='white')
ax = fig.add_subplot(111, frameon=False)
ax.plot(tot_inputs.ravel(),
Nr = TrackNormal(rx)
# psie is sort of backwards: higher angles go to the left
return np.angle(Nx) - np.angle(Nr)
if __name__ == '__main__':
TRACK_SPACING = 19.8 # cm
x = SVGPathToTrackPoints("oakwarehouse.path", TRACK_SPACING)[:-1]
xm = np.array(x)[:, 0] / 50 # 50 pixels / meter
track_k = TrackCurvature(xm)
Nx = TrackNormal(xm)
u = 1j * Nx
np.savetxt("track_x.txt",
np.vstack([np.real(xm), np.imag(xm)]).T.reshape(-1),
newline=",\n")
np.savetxt("track_u.txt",
np.vstack([np.real(u), np.imag(u)]).T.reshape(-1),
newline=",\n")
np.savetxt("track_k.txt", track_k, newline=",\n")
ye, val, stuff = OptimizeTrack(xm, 1.4, 0.1)
psie = RelativePsie(ye, xm)
rx = u*ye + xm
raceline_k = TrackCurvature(rx)
np.savetxt("raceline_k.txt", raceline_k, newline=",\n")
np.savetxt("raceline_ye.txt", ye, newline=",\n")
np.savetxt("raceline_psie.txt", psie, newline=",\n")
print "vector + loop: ", t2-t1
w_map = funProj_mat(param)
t3 = time.time()
print "mat : ", t3 - t2
print "diff: ", np.linalg.norm(w_loop - w_map)
elif flag_test == 3:
#v0 = np.random.randn(100)
v0 = np.complex(0,1)
print isLegal(v0)
elif flag_test == 4:
# test polyinterp
points = np.random.randn(2, 3)
print polyinterp(points)
np.savetxt(\
"./Mark_Schmidt/minConf/minFunc/test_data/polyinterp_input_1.csv",\
points)
elif flag_test == 5:
# test lbfgsUpdate
[p, m, corrections, debug, Hdiag] = [2, 5, 2, 0, 1e-3]
y = np.random.randn(p)
s = y + 0.1 * np.random.randn(p)
old_dirs = np.random.randn(p, m)
old_stps = np.random.randn(p, m)
data_mat = {'p':p, 'm':m, 'corrections':corrections, 'debug':debug, \
'Hdiag':Hdiag, 'y':y, 's':s, 'old_dirs':old_dirs, \
'old_stps':old_stps}
savemat("./Mark_Schmidt/minConf/minFunc/test_data/lbfgsUpdate_input_1.csv",\
data_mat)
[new_dirs, new_stps, new_Hdiag] = lbfgsUpdate(y, s, corrections, \
debug, old_dirs, old_stps, Hdiag)
if __name__ == "__main__":
# Get the Data
File = "yelp_train.txt"
(rating_matrix, opinion_matrix, opinion_matrixI) = getRatingMatrix(File)
print("Dimensions of the training dataset: ", rating_matrix.shape)
# Set the number of Factors
NUM_OF_FACTORS = 20
MAX_DEPTH = 6
# Build decision tree on training set
(decisionTree, decisionTreeI, user_vectors,
item_vectors) = alternateOptimization(opinion_matrix, opinion_matrixI,
rating_matrix, NUM_OF_FACTORS,
MAX_DEPTH, File)
Predicted_Rating = np.dot(user_vectors, item_vectors.T)
np.savetxt('/results/item_vector.txt', item_vectors, fmt='%0.8f')
np.savetxt('/results/user_vectors.txt', user_vectors, fmt='%0.8f')
np.savetxt('/results/rating_predict.txt', Predicted_Rating, fmt='%0.8f')
TestFile = "yelp_test.txt"
(test_r, test_opinion, test_opinionI) = getRatingMatrix(TestFile)
Predicted_Rating[np.where[rating_matrix > 0]] = 0.0
# print("Predicted_Rating for Test: ", Predicted_Rating)
# print("Test Rating: ", test)
'''NDCG = getNDCG(Predicted_Rating, test_r, 10)
print("NDCG@10: ", NDCG)
NDCG = getNDCG(Predicted_Rating, test_r, 20)
print("NDCG@20: ", NDCG)
NDCG = getNDCG(Predicted_Rating, test_r, 50)
print("NDCG@50: ", NDCG)'''
print("print user tree")
# Metropolis within Gibbs is used.
for k in range(1):
rs = npr.RandomState(k)
ws = rs.randn(num_weights)
ws_df = np.zeros(num_weights).reshape(num_weights, 1)
for b in range(B):
print("b", b)
for i in range(num_weights):
# wiprime|wi ~ N(wi, 1)
print("i", i)
# Let the first 9999 to burn-in
for m in range(50):
wi = ws[i]
wi_prime = rs.randn(1) + wi
ws_tmp = copy.deepcopy(ws)
ws_tmp[i] = wi_prime
Pprime = (logprob(ws_tmp.reshape(1, num_weights), inputs,
targets))
P0 = (logprob(ws.reshape(1, num_weights), inputs, targets))
alpha = (Pprime - P0)
print("alpha", alpha)
bb = np.log(rs.rand(1))
print("b", bb)
wi = wi_prime if bb < alpha else wi
ws[i] = wi
ws_df = np.concatenate([ws_df, ws.reshape(num_weights, 1)], axis=1)
filename = "gibbs_wts" + "K" + str(k) + ".csv"
np.savetxt(filename, ws_df, delimiter=',', fmt='%d')
theta_dict["C"] = np.repeat(25., pecmy.N)
theta_x = pecmy.unwrap_theta(theta_dict)
# opt.root(pecmy.pp_wrap_alpha, .5, args=(.99, ))['x']
# pecmy.W ** - .75
np.reshape(
np.repeat(np.max(pecmy.ecmy.tau + pecmy.tau_buffer, axis=1), pecmy.N),
(pecmy.N, pecmy.N)) / pecmy.ecmy.tau
np.reshape(np.repeat(np.min(tau_min_mat - pecmy.tau_buffer, axis=1), pecmy.N),
(pecmy.N, pecmy.N)) / pecmy.ecmy.tau
v = np.mean(pecmy.ecmy.tau, axis=1)
pecmy.estimator_bounds(theta_x, v, "lower")
# x, obj, status = pecmy.estimator(v, theta_x, pecmy.m, sv=sv, nash_eq=False)
x, obj, status = pecmy.estimator(v, theta_x, pecmy.m, sv=None, nash_eq=False)
x_dict = pecmy.rewrap_xlhvt(x)
ge_dict = pecmy.ecmy.rewrap_ge_dict(x_dict["ge_x"])
theta_dict = pecmy.rewrap_theta(x_dict["theta"])
print(ge_dict["tau_hat"] * pecmy.ecmy.tau)
print("-----")
for i in theta_dict.keys():
print(i)
print(theta_dict[i])
np.savetxt(out_fname, x, delimiter=",")
print('D_(r||p)', true_divg(x0))
print('Acceptance', len(Accep) / Num_sample)
Result[0, index] = loss(x0)
Result[1, index] = true_divg(x0)
Result[2, index] = len(Accep) / Num_sample
index = index + 1
X1 = np.arange(-7, 7, 0.01)
p1 = plt.scatter(X1,
stats.t.pdf(X1, 10, x0[0], np.exp(x0[1])),
label="t_" + str(10))
np.savetxt('GMM_Results', Result)
X1 = np.arange(-7, 5, 0.01)
p2 = plt.scatter(X1,
(stats.norm.pdf(X1, 2, 0.8) + stats.norm.pdf(X1, 0, 0.8) +
stats.norm.pdf(X1, -2, 0.8) + stats.norm.pdf(X1, -4, 0.8)) /
4,
label="real",
color='red')
p3 = plt.scatter(X1, learned_dist(x0), label="learn", color='black')
plt.legend(fontsize='xx-large')
plt.savefig('GMM_toy_ex.png')
train_images = mnist.train.next_batch(100)[0]
with bl.Model() as m:
X = bl.Placeholder('X', dimensions=agnp.array([100, 784]))
encoder = bl.ml.neural_network.DenseNeuralNetwork(
'Encoder',
X,
layer_dims=[784, 50, 20],
nonlinearity=bl.math.utils.sigmoid)
decoder = bl.ml.neural_network.DenseNeuralNetwork(
'Decoder',
encoder,
layer_dims=[20, 50, 784],
nonlinearity=bl.math.utils.sigmoid,
last_layer_nonlinearity=bl.math.utils.sigmoid)
y = bl.rvs.Bernoulli('obs', decoder, observed=X)
init = agnp.random.normal(0, 3, size=m.n_params)
optimizer = bl.math.optimizers.ADAM(learning_rate=5e-1)
def iter_func(t, p, o):
print("Objective value: %f" % o)
res = optimizer.run(
lambda x: -m.log_density(x, feed_dict={X: train_images}),
lambda x: -m.grad_log_density(x, feed_dict={X: train_images}),
init,
iter_func=iter_func,
iter_interval=5)
agnp.savetxt("pos.txt", res.position, delimiter=',')
def __init__(
self,
X,
Y,
init_state=None,
parameter_sampling_flag=0,
noiseless_flag=False,
):
self.dataXshape = X.shape
self.dataYshape = Y.shape
self.init_state_mean = np.array((0., 0., np.pi, 0.))
'''
if init_state is None:
self.init_state_mean=np.array((0.,0.,np.pi,0.))
else:
self.init_state_mean=init_state
'''
w_init = 0.1 * np.ones(dimw)
#lam_init = np.array([10000., 10000., 10000., 10000.])
lam_init = np.array([100., 100., 100., 100.])
alpha_init = np.array([5.])
unload_flag = 1
import os
if os.path.isfile("./result_apply/temp_param_cartpole.csv"):
param_load = np.loadtxt('./result_apply/temp_param_cartpole.csv',
delimiter=',')
if abs(param_load[0] - self.dataXshape[0]) < 1:
unload_flag = 0
w = np.array([param_load[1], param_load[2]])
lam = np.array([
param_load[3], param_load[4], param_load[5], param_load[6]
])
alpha = np.array([param_load[7]])
if unload_flag:
w, lam, alpha = laplace_approximation.maximized_approximate_log_marginal_likelihood(
Y, X, mo_model, w_init, lam_init, alpha_init)
param_save = np.array([
self.dataXshape[0], w[0], w[1], lam[0], lam[1], lam[2], lam[3],
alpha[0]
])
np.savetxt('./result_apply/temp_param_cartpole.csv',
param_save,
delimiter=',')
self.prec_weight = alpha
self.post_mean = w
self.post_var = inv(
laplace_approximation.hesse_w(Y, X, w, mo_model, lam, alpha))
self.noise_var1 = 1. / lam[0]
self.noise_var2 = 1. / lam[1]
self.noise_var3 = 1. / lam[2]
self.noise_var4 = 1. / lam[3]
self.lam_memo = lam
self.log_evidence_memo = laplace_approximation.approximate_log_marginal_likelihood(
Y, X, w, mo_model, lam, alpha)
self.parameter_sampling_flag = parameter_sampling_flag
self.noiseless_flag = noiseless_flag
if 2 == self.parameter_sampling_flag:
self.parameter_sample = np.random.multivariate_normal(
self.post_mean, self.post_var)
print("self.parameter_sample =", self.parameter_sample)
if 3 == self.parameter_sampling_flag:
self.parameter_sample = self.post_mean
self.sqrt_noise_var1 = np.sqrt(self.noise_var1)
self.sqrt_noise_var2 = np.sqrt(self.noise_var2)
self.sqrt_noise_var3 = np.sqrt(self.noise_var3)
self.sqrt_noise_var4 = np.sqrt(self.noise_var4)
if self.noiseless_flag:
self.sqrt_noise_var1 = 0.
self.sqrt_noise_var2 = 0.
self.sqrt_noise_var3 = 0.
self.sqrt_noise_var4 = 0.
ind_trn = np.where((t >= 1075) & (t < 1165))[0] # Interval of blocked flow
ind_vld = ind_trn
a = np.floor(len(ind_trn) / (npts - 1))
ind_trn = ind_trn[::int(a)]
ind_vld = np.setxor1d(ind_trn, ind_vld)
ind_tst = np.where((t >= 500))[0]
ind_tst = np.setxor1d(np.setxor1d(ind_tst, ind_trn), ind_vld)
ind_tst = ind_tst[:-1]
tM, zM, zdM, LM = gen.dataset(t, Z, ind_trn, **kwargs)
tV, zV, zdV, LV = gen.dataset(t, Z, ind_vld, **kwargs)
tS, zS, zdS, LS = gen.dataset(t, Z, ind_tst, **kwargs)
### Set up network, train, and predict
layer_sizes = [ndim, 128, 128, ndim]
step_size = 0.001
max_iters = 5000
lyap_off = 2000
dOTD = dOTDModel(layer_sizes, step_size, max_iters, lyap_off)
dOTD.train((zM, zdM, LM), notd)
dOTD.comp_error(trn_set=(zM, zdM, LM), vld_set=(zV, zdV, LV), \
tst_set=(zS, zdS, LS))
dOTD.comp_avgdist(ind_trn, ind_vld, ind_tst, Z, U)
dOTD_tst = dOTD.predict(Z) # Predict everywhere for plotting
### Save for plotting
for kk in range(notd):
filename = 'dOTD_tst' + str(kk + 1) + '.out'
data = np.hstack((np.atleast_2d(t).T, Z, \
dOTD_tst[kk], U[:,:,kk]))
np.savetxt(filename, data, fmt="%16.8e")
def save_gen_params(gen_params, gp_out_dir, nm):
np.savetxt(gp_out_dir + nm + '_00.csv', gen_params[0][0], delimiter=',')
np.savetxt(gp_out_dir + nm + '_01.csv', gen_params[0][1], delimiter=',')
np.savetxt(gp_out_dir + nm + '_10.csv', gen_params[1][0], delimiter=',')
np.savetxt(gp_out_dir + nm + '_11.csv', gen_params[1][1], delimiter=',')
return
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
Nr = TrackNormal(rx)
# psie is sort of backwards: higher angles go to the left
return np.angle(Nx) - np.angle(Nr)
if __name__ == '__main__':
TRACK_SPACING = 19.8 # cm
x = SVGPathToTrackPoints("oakwarehouse.path", TRACK_SPACING)[:-1]
xm = np.array(x)[:, 0] / 50 # 50 pixels / meter
track_k = TrackCurvature(xm)
Nx = TrackNormal(xm)
u = 1j * Nx
np.savetxt("track_x.txt",
np.vstack([np.real(xm), np.imag(xm)]).T.reshape(-1),
newline=",\n")
np.savetxt("track_u.txt",
np.vstack([np.real(u), np.imag(u)]).T.reshape(-1),
newline=",\n")
np.savetxt("track_k.txt", track_k, newline=",\n")
ye, val, stuff = OptimizeTrack(xm, 1.4, 0.1)
psie = RelativePsie(ye, xm)
rx = u * ye + xm
raceline_k = TrackCurvature(rx)
np.savetxt("raceline_k.txt", raceline_k, newline=",\n")
np.savetxt("raceline_ye.txt", ye, newline=",\n")
np.savetxt("raceline_psie.txt", psie, newline=",\n")
print ('KNN')
print ( np.sum(res2) / 100.)
# Make neural net functions
N_weights, pred_fun, loss_fun, \
frac_err = make_nn_funs(input_shape, layer_specs, L2_reg)
loss_grad = grad(loss_fun)
# Initialize weights
rs = npr.RandomState()
W = rs.randn(N_weights) * param_scale
print(" Epoch | Train err | Test error ")
def print_perf(epoch, W):
test_perf = frac_err(W, test_images, test_labels)
train_perf = frac_err(W, train_images, train_labels)
print("{0:15}|{1:15}|{2:15}".format(epoch, train_perf, test_perf))
# Train with sgd
batch_idxs = make_batches(N_data, batch_size)
cur_dir = np.zeros(N_weights)
for epoch in range(num_epochs):
print_perf(epoch, W)
for idxs in batch_idxs:
grad_W = loss_grad(W, train_images[idxs], train_labels[idxs])
cur_dir = momentum * cur_dir + (1.0 - momentum) * grad_W
W -= learning_rate * cur_dir
np.savetxt('W',W)
words_all = cv.fit_transform(words).toarray()
featname_all = cv.get_feature_names()
# choose the alpha features
idx_words = []
for j in np.arange(len(featname_all)):
if featname_all[j].isalpha():
idx_words.append(j)
featname_alpha = np.array(featname_all)[idx_words]
words_alpha = words_all[:, idx_words]
#words_std = np.std(words_alpha, axis=0)
words_std = np.sum(words_alpha, axis=0)
# choose the high-variance words
t1 = np.argsort(words_std)
idx_highstd = t1[-500:][::-1]
featname_highstd = featname_alpha[idx_highstd]
words_highstd = words_alpha[:, idx_highstd]
# save the data into csv files
np.savetxt("webkbRaw_word.csv", words_highstd, delimiter=',')
np.savetxt("webkbRaw_label_univ.csv", label_univ.astype(int), delimiter=',')
np.savetxt("webkbRaw_label_topic.csv", label_topic.astype(int), delimiter=',')
csvfile = open("webkbRaw_wordnames.csv", "wb")
csvwriter = csv.writer(csvfile, delimiter=',')
for i in np.arange(len(featname_highstd)):
csvwriter.writerow([featname_highstd[i]])
csvfile.close()
# rs = npr.RandomState(0)
covariance = get_covariance(variational_params)
cov_decomp = np.linalg.cholesky(covariance)
sample_weights = np.matmul(rs.randn(1000, len(log_std)), cov_decomp) + mean
plot_inputs = np.linspace(-2, 2, num=400)
outputs_final = predictions(sample_weights, np.expand_dims(plot_inputs, 1))
lowerbd = numpy.quantile(outputs_final, 0.05, axis=0)
upperbd = numpy.quantile(outputs_final, 0.95, axis=0)
inconint = np.logical_and(lowerbd < tot_targets, upperbd > tot_targets).ravel()
con_ind = np.zeros(len(lowerbd))
con_ind[inconint] = 1
con_ind = con_ind.reshape(len(con_ind), 1)
coverage_df = np.concatenate([coverage_df, con_ind], axis=1)
# # Plot data and functions.
fig = plt.figure(figsize=(12, 8), facecolor='white')
ax = fig.add_subplot(111, frameon=False)
ax.plot(tot_inputs.ravel(), tot_targets.ravel(), "bx", label = "testing data")
ax.plot(inputs.ravel(), targets.ravel(), 'rx', label = "training data")
ax.plot(tot_inputs.ravel(), lowerbd.ravel(), "k-")
ax.plot(tot_inputs.ravel(), upperbd.ravel(), "k-")
ax.set_ylim([-2, 3])
ax.legend()
plt.show()
filename = "blockVar" + "B" + str(B) + "t"+str(t) + "noise"+str(tau) + ".csv"
np.savetxt(filename, coverage_df, delimiter=',', fmt='%d')
# csv.reader("diagVar_QuantUQ.csv")
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("_________________________________________________________")
# Run the chain (with burn-in).
samples, is_accepted = tfp.mcmc.sample_chain(
num_results=num_results,
num_burnin_steps=num_burnin_steps,
current_state=start_point,
#current_state=np.random.randn(1, num_weights),
kernel=adaptive_hmc,
trace_fn=lambda _, pkr: pkr.inner_results.is_accepted)
sample_mean = tf.reduce_mean(samples)
sample_stddev = tf.math.reduce_std(samples)
is_accepted = tf.reduce_mean(tf.cast(is_accepted, dtype=tf.float32))
return sample_mean, sample_stddev, is_accepted, samples
tmp = pyreadr.read_r('hmcstartpoints')
tmp1 = tmp['D1'].to_numpy()
tmp2 = tmp1.reshape(13, 100)
for start in range(100):
spts = tmp2[:, start]
sample_mean, sample_stddev, is_accepted, samples = run_chain(
spts.reshape(1, 13))
print('mean:{:.4f} stddev:{:.4f} acceptance:{:.4f}'.format(
sample_mean.numpy(), sample_stddev.numpy(), is_accepted.numpy()))
sam = samples.numpy()
sam = sam.reshape(num_results, 13)
sam = np.concatenate([sam, sam])
np.savetxt("multiple_hmc.csv", sam, delimiter=',', fmt='%f')
