def gen_data():
offset = 14
num_poses = 50
num_cases = 1
# Name the relevant filenames
data_filename = "unscaled_pose_data.mat"
#Load the data
T_vki, T_ci, extcal = utils.load_data_1(data_filename)
# Convert inertial poses to relative
T_v_rel = utils.inertial_to_relative(T_vki, offset)
T_c_rel = utils.inertial_to_relative(T_ci, offset)
trans_sigma = 0.1
rot_sigma = 0.1
scale = 2
T_v_rel_noisy = utils.add_noise(T_v_rel, trans_sigma, rot_sigma)
T_c_rel_noisy = utils.add_noise(T_c_rel, trans_sigma, rot_sigma)
my_solver = solver()
my_solver.set_T_c_rel(T_c_rel_noisy, scale)
data_dict = {
"T1": T_v_rel_noisy,
"T2": my_solver.T_c_rel,
"extcal": extcal,
"scale": 2
}
sio.savemat("failure_case.mat", data_dict)
test = 1
def main():
#Make all numpy errors stop the program
np.seterr(all='raise')
#Set important values
offset = 14
num_cases = 100
brookshire = False
if not brookshire:
# Name the relevant filenames
data_filename = "unscaled_pose_data.mat"
#Load the data
T_vki, T_ci, extcal = utils.load_data_1(data_filename)
# Convert inertial poses to relative
T_v_rel = utils.inertial_to_relative(T_vki, offset)
T_c_rel = utils.inertial_to_relative(T_ci, offset)
trans_sigma = 0.01
rot_sigma = 0
scale = 2
data_filename = "./results/per{}t{}r_".format(int(
trans_sigma * 1000), int(rot_sigma * 1000)) + data_filename
set_of_cons = ['R', 'RC', 'RH', 'RCH']
#set_of_cons = ['RCH']
else:
# Name the relevant filenames
data_filename = "brookshire_data.mat"
T_v_rel, T_c_rel, extcal = utils.load_brookshire_data(data_filename)
scale = 1
trans_sigma = 0
rot_sigma = 0
data_filename = "./results/brookshire_data.mat"
set_of_cons = ['RCH']
# Initialize solver
my_solver = solver()
my_solver.set_extcal(extcal)
my_solver.set_scale(scale)
results = limit_constraints(my_solver, T_v_rel, T_c_rel, scale, num_cases,
trans_sigma, rot_sigma, set_of_cons)
#Save the results
utils.results_saver(data_filename, results)
# #Choose a subset of the data
# num_poses_total = len(T_v_rel)
# num_poses = 10
# subset_indices = np.random.choice(num_poses_total, num_poses, replace=False).tolist()
# T_v_rel_sub = [T_v_rel[index] for index in subset_indices]
# T_c_rel_sub = [T_c_rel[index] for index in subset_indices]
# my_solver.set_T_v_rel(T_v_rel_sub)
# my_solver.set_T_c_rel(T_c_rel_sub, 2)
return
def main(rotation_error, trans_error):
#Set important values
offset = 14
num_poses = -1
num_cases = 100
brookshire = False
if not brookshire:
# Name the relevant filenames
data_filename = "unscaled_pose_data.mat"
#Load the data
T_vki, T_ci, extcal = utils.load_data_1(data_filename)
# Convert inertial poses to relative
T_v_rel = utils.inertial_to_relative(T_vki, offset)
T_c_rel = utils.inertial_to_relative(T_ci, offset)
trans_sigma = trans_error
rot_sigma = rotation_error
scale = .5 # 2.
data_filename = "./results/matt_N_{:}_per{}t{}r_".format(
num_cases, int(trans_sigma * 1000), int(
rot_sigma * 1000)) + data_filename
else:
# Name the relevant filenames
data_filename = "brookshire_data.mat"
T_v_rel, T_c_rel, extcal = utils.load_brookshire_data(data_filename)
scale = 1
trans_sigma = trans_error
rot_sigma = rotation_error
data_filename = "./results/brookshire_data.mat"
# Initialize solver
my_solver = solver()
my_solver.set_extcal(extcal)
my_solver.set_scale(scale)
# test = limit_constraints(my_solver, T_v_rel, T_c_rel, scale, num_cases, trans_sigma, rot_sigma)
test_andreff = limit_constraints_with_andreff(my_solver, T_v_rel, T_c_rel,
scale, num_cases,
trans_sigma, rot_sigma)
#Save the results
utils.results_saver(data_filename, test_andreff)
# #Choose a subset of the data
# num_poses_total = len(T_v_rel)
# num_poses = 10
# subset_indices = np.random.choice(num_poses_total, num_poses, replace=False).tolist()
# T_v_rel_sub = [T_v_rel[index] for index in subset_indices]
# T_c_rel_sub = [T_c_rel[index] for index in subset_indices]
# my_solver.set_T_v_rel(T_v_rel_sub)
# my_solver.set_T_c_rel(T_c_rel_sub, 2)
return test_andreff
def matlab_comparison():
data = sio.loadmat('simple_case.mat')
T1 = data['T1']
T2 = data['T2']
T_v_rel = [SE3.from_matrix(T1[:, :, 0]), SE3.from_matrix(T1[:, :, 1])]
T_c_rel = [SE3.from_matrix(T2[:, :, 0]), SE3.from_matrix(T2[:, :, 1])]
my_solver = solver()
my_solver.set_T_v_rel(T_v_rel)
my_solver.set_T_c_rel(T_c_rel)
dual_time, dual_gt, dual_primal, dual_gap, dual_opt, dual_solution, dual_flag = my_solver.dual_solve(
'R', verbose=True)
rel_time, rel_gt, rel_primal, rel_gap, relax_opt, relax_solution, rel_flag = my_solver.relax_solve(
'R')
print(1 / dual_opt[3])
print(1 / relax_opt[3])
print(dual_solution)
print(relax_solution)
return
from solver.ext_solver import solver from utility.utils import load_data_1, inertial_to_relative, add_noise # Name the relevant filenames data_filename = "unscaled_pose_data.mat" # Load the data T_vki, T_cki, extcal = load_data_1(data_filename) scale = 2 #Set noise levels trans_noise = 0.01 #Percentage of translation rot_noise = 0.01 #Percentage of rotation # Initialize solver my_solver = solver() # Convert inertial poses to relative T_v_rel = inertial_to_relative(T_vki) T_c_rel = inertial_to_relative(T_cki) # Choose a subset of the data num_poses_total = len(T_v_rel) num_poses = 100 subset_indices = np.random.choice(num_poses_total, num_poses, replace=False).tolist() T_v_rel_sub = add_noise([T_v_rel[index] for index in subset_indices], trans_noise, rot_noise) T_c_rel_sub = add_noise([T_c_rel[index] for index in subset_indices], trans_noise, rot_noise) #Load the data into the solver my_solver.set_T_v_rel(T_v_rel_sub) #Load relative egomotion sensor poses my_solver.set_T_c_rel(T_c_rel_sub, scale=scale) # Load and scale relative camera sensor poses